{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Welding Example #02: TCP movements and Weaving\n",
    "In this example we will focus on more complex tcp movements along the workpiece and how to combine different motion shapes like weaving."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Imports"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "nbsphinx": "hidden"
   },
   "outputs": [],
   "source": [
    "# enable interactive plots on Jupyterlab with ipympl and jupyterlab-matplotlib installed\n",
    "# %matplotlib widget"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import pandas as pd\n",
    "\n",
    "from weldx import (\n",
    "    Q_,\n",
    "    CoordinateSystemManager,\n",
    "    Geometry,\n",
    "    LinearHorizontalTraceSegment,\n",
    "    LocalCoordinateSystem,\n",
    "    Trace,\n",
    "    WXRotation,\n",
    "    get_groove,\n",
    ")\n",
    "from weldx.welding.util import sine"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## General setup\n",
    "We will use the same workpiece geometry as defined in the previous example."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### groove shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "groove = get_groove(\n",
    "    groove_type=\"VGroove\",\n",
    "    workpiece_thickness=\"0.5 cm\",\n",
    "    groove_angle=\"50 deg\",\n",
    "    root_face=\"1 mm\",\n",
    "    root_gap=\"1 mm\",\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### workpiece geometry"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# define the weld seam length in mm\n",
    "seam_length = Q_(150, \"mm\")\n",
    "\n",
    "# create a linear trace segment a the complete weld seam trace\n",
    "trace_segment = LinearHorizontalTraceSegment(seam_length)\n",
    "trace = Trace(trace_segment)\n",
    "\n",
    "# create 3d workpiece geometry from the groove profile and trace objects\n",
    "geometry = Geometry(groove.to_profile(width_default=Q_(5, \"mm\")), trace)\n",
    "\n",
    "# rasterize geometry\n",
    "profile_raster_width = \"2 mm\"  # resolution of each profile in mm\n",
    "trace_raster_width = \"30 mm\"  # space between profiles in mm\n",
    "geometry_data_sp = geometry.rasterize(\n",
    "    profile_raster_width=profile_raster_width, trace_raster_width=trace_raster_width\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Coordinate system manager"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# crete a new coordinate system manager with default base coordinate system\n",
    "csm = CoordinateSystemManager(\"base\")\n",
    "\n",
    "# add the workpiece coordinate system\n",
    "csm.create_cs(\n",
    "    coordinate_system_name=\"workpiece\",\n",
    "    reference_system_name=\"base\",\n",
    "    orientation=trace.coordinate_system.orientation.data,\n",
    "    coordinates=Q_(trace.coordinate_system.coordinates.data, \"mm\"),\n",
    ")\n",
    "\n",
    "# add the geometry data of the specimen\n",
    "csm.assign_data(\n",
    "    geometry.spatial_data(profile_raster_width, trace_raster_width),\n",
    "    \"specimen\",\n",
    "    \"workpiece\",\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Movement definitions\n",
    "Like in the previous example we start by defining the general linear movement along the weld seam with a constant welding speed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tcp_start_point = Q_([5.0, 0.0, 2.0], \"mm\")\n",
    "tcp_end_point = Q_([seam_length.m - 5.0, 0.0, 2.0], \"mm\")\n",
    "\n",
    "v_weld = Q_(10, \"mm/s\")\n",
    "s_weld = (tcp_end_point - tcp_start_point)[0]  # length of the weld\n",
    "t_weld = s_weld / v_weld\n",
    "\n",
    "t_start = pd.Timedelta(\"0s\")\n",
    "t_end = pd.Timedelta(str(t_weld))\n",
    "\n",
    "rot = WXRotation.from_euler(\"x\", 180, degrees=True)\n",
    "\n",
    "coords = np.stack((tcp_start_point, tcp_end_point))\n",
    "\n",
    "tcp_wire = LocalCoordinateSystem(\n",
    "    coordinates=coords, orientation=rot, time=[t_start, t_end]\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's add the linear movement to the coordinate system manager and see a simple plot:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.add_cs(\n",
    "    coordinate_system_name=\"tcp_wire\", reference_system_name=\"workpiece\", lcs=tcp_wire\n",
    ")\n",
    "csm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def ax_setup(ax, rotate=170):\n",
    "    ax.legend()\n",
    "    ax.set_xlabel(\"x / mm\")\n",
    "    ax.set_ylabel(\"y / mm\")\n",
    "    ax.set_zlabel(\"z / mm\")\n",
    "    ax.view_init(30, -10)\n",
    "    ax.set_ylim([-5.5, 5.5])\n",
    "    ax.view_init(30, rotate)\n",
    "    ax.legend()\n",
    "\n",
    "\n",
    "color_dict = {\n",
    "    \"tcp_sine\": (255, 0, 0),\n",
    "    \"tcp_wire_sine\": (255, 0, 0),\n",
    "    \"tcp_wire_sine2\": (255, 0, 0),\n",
    "    \"tcp_wire\": (0, 150, 0),\n",
    "    \"specimen\": (0, 0, 255),\n",
    "}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = csm.plot(\n",
    "    coordinate_systems=[\"tcp_wire\"],\n",
    "    colors=color_dict,\n",
    "    limits=[(0, 140), (-5, 5), (0, 12)],\n",
    "    show_vectors=False,\n",
    "    show_wireframe=True,\n",
    ")\n",
    "ax_setup(ax)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## add a sine wave to the TCP movement\n",
    "We now want to add a weaving motion along the y-axis (horizontal plane) of our TCP motion. We can define a general weaving motion using the `weldx.utility.sine` function that creates `TimeSeries` class."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ts_sine = sine(f=Q_(0.5 * 2 * np.pi, \"Hz\"), amp=Q_([0, 0.75, 0], \"mm\"))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now define a simple coordinate system that contains only the weaving motion."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tcp_sine = LocalCoordinateSystem(coordinates=ts_sine)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "One approach to combine the weaving motion with the existing linear `tcp_wire` movement is to use the coordinate system manager. We can add the `tcp_sine` coordinate system relative to the `tcp_wire` system:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.add_cs(\n",
    "    coordinate_system_name=\"tcp_sine\", reference_system_name=\"tcp_wire\", lcs=tcp_sine\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets see the result:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "t = pd.timedelta_range(start=t_start, end=t_end, freq=\"10ms\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = csm.plot(\n",
    "    coordinate_systems=[\"tcp_wire\", \"tcp_sine\"],\n",
    "    colors=color_dict,\n",
    "    limits=[(0, 140), (-5, 5), (0, 12)],\n",
    "    show_origins=False,\n",
    "    show_vectors=False,\n",
    "    show_wireframe=True,\n",
    "    time=t,\n",
    ")\n",
    "ax_setup(ax)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here a little bit closer to see the actual sine wave:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = csm.plot(\n",
    "    coordinate_systems=[\"tcp_wire\", \"tcp_sine\"],\n",
    "    colors=color_dict,\n",
    "    limits=[(0, 5), (-2, 2), (0, 12)],\n",
    "    show_origins=False,\n",
    "    show_vectors=False,\n",
    "    show_wireframe=False,\n",
    "    time=t,\n",
    ")\n",
    "ax_setup(ax)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Another approach would be to combine both systems before adding them to the coordinate system manager. We can combine both coordinate systems using the __+__ operator to generate the superimposed weaving coordinate system."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tcp_wire_sine = tcp_sine.interp_time(t) + tcp_wire"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note the difference in reference coordinate system compared to the first example."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.add_cs(\"tcp_wire_sine\", \"workpiece\", tcp_wire_sine)\n",
    "csm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We get the same result:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = csm.plot(\n",
    "    coordinate_systems=[\"tcp_wire\", \"tcp_wire_sine\"],\n",
    "    colors=color_dict,\n",
    "    limits=[(0, 140), (-5, 5), (0, 12)],\n",
    "    show_origins=False,\n",
    "    show_vectors=False,\n",
    "    show_wireframe=True,\n",
    ")\n",
    "ax_setup(ax)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = csm.plot(\n",
    "    coordinate_systems=[\"tcp_wire\", \"tcp_sine\"],\n",
    "    colors=color_dict,\n",
    "    limits=[(0, 5), (-2, 2), (0, 12)],\n",
    "    show_origins=False,\n",
    "    show_vectors=False,\n",
    "    show_wireframe=False,\n",
    ")\n",
    "ax_setup(ax)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Adding every single superposition step in the coordinate system manager can be more flexible and explicit, but will clutter the CSM instance for complex movements."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## plot with time interpolation\n",
    "Sometimes we might only be interested in a specific time range of the experiment or we want to change the time resolution. For this we can use the time interpolation methods of the CSM (or the coordinate systems).\n",
    "\n",
    "Let's say we want to weave only 8 seconds of our experiment (starting from 2020-04-20 10:03:00) but interpolate steps of 1 ms."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "t_interp = pd.timedelta_range(start=\"3s\", end=\"11s\", freq=\"1ms\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = csm.interp_time(t_interp).plot(\n",
    "    coordinate_systems=[\"tcp_wire\", \"tcp_wire_sine\"],\n",
    "    colors=color_dict,\n",
    "    limits=[(0, 140), (-5, 5), (0, 12)],\n",
    "    show_origins=False,\n",
    "    show_vectors=False,\n",
    "    show_wireframe=True,\n",
    ")\n",
    "ax_setup(ax)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Adding a second weaving motion\n",
    "We now want to add a second weaving motion along the z-axis that only exists for a limited time. Lets generate the motion first:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ts_sine = sine(f=Q_(1 / 8 * 2 * np.pi, \"Hz\"), amp=Q_([0, 0, 1], \"mm\"))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We define a new `LocalCoordinateSystem` and interpolate it our specified timestamps."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "t = pd.timedelta_range(start=\"0s\", end=\"8s\", freq=\"25ms\")\n",
    "tcp_sine2 = LocalCoordinateSystem(coordinates=ts_sine).interp_time(t)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tcp_sine2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "adding all the movements together. We have to be careful with the time-axis in this case !"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "t_interp = pd.timedelta_range(\n",
    "    start=tcp_wire.time.index[0], end=tcp_wire.time.index[-1], freq=\"20ms\"\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tcp_wire_sine2 = (\n",
    "    tcp_sine2.interp_time(t_interp) + tcp_sine.interp_time(t_interp)\n",
    ") + tcp_wire"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "csm.add_cs(\"tcp_wire_sine2\", \"workpiece\", tcp_wire_sine2)\n",
    "csm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = csm.plot(\n",
    "    coordinate_systems=[\"tcp_wire\", \"tcp_wire_sine2\"],\n",
    "    colors=color_dict,\n",
    "    limits=[(0, 140), (-5, 5), (0, 12)],\n",
    "    show_origins=False,\n",
    "    show_vectors=False,\n",
    ")\n",
    "ax_setup(ax, rotate=110)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ax = csm.plot(\n",
    "    coordinate_systems=[\"tcp_wire\", \"tcp_wire_sine2\"],\n",
    "    colors=color_dict,\n",
    "    limits=[(60, 100), (-2, 2), (0, 12)],\n",
    "    show_origins=False,\n",
    "    show_vectors=False,\n",
    ")\n",
    "ax_setup(ax, rotate=110)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "nbsphinx": "hidden"
   },
   "source": [
    "## K3D Visualization"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "nbsphinx": "hidden"
   },
   "outputs": [],
   "source": [
    "csm.plot(\n",
    "    backend=\"k3d\",\n",
    "    coordinate_systems=[\"tcp_wire_sine2\"],\n",
    "    colors=color_dict,\n",
    "    show_vectors=False,\n",
    "    show_traces=True,\n",
    "    show_data_labels=False,\n",
    "    show_labels=False,\n",
    "    show_origins=True,\n",
    ")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "",
   "language": "python",
   "name": ""
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}