{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "696fa6c3",
   "metadata": {},
   "source": [
    "# Pose\n",
    "Poses in PyCRAM are represented by the Pose class which inherits from the PoseStamped message of ROS. This makes PyCRAMs poses compatible with everything in ROS like services, topics or TF. \n",
    "\n",
    "This notebook will provide an overview about poses, how to use them and what they can do. We will start by simply creating a pose. "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7cb45ea7",
   "metadata": {},
   "source": [
    "Before we start a few words about naming convention of Poses in PyCRAM. Naming convention is similar to the PoseStamped message so if you are familiar with that this should be easy. \n",
    "\n",
    "* **Position:** A position means the position in cartisian space, so the x, y, and z coordinates. \n",
    "* **Orientation:** An orientation is the rotation in all thre axes represented as a quaternion with x, y, z, w.\n",
    "* **Pose:** A pose is the combination of a position and an orientation. Poses in PyCRAM also contain a frame of reference to which the position and orientation are relative."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "a161dd90",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699448767\n",
      "    nsecs: 355911970\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: 1\n",
      "    y: 2\n",
      "    z: 3\n",
      "  orientation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Pose\n",
    "\n",
    "example_pose = Pose([1, 2, 3], [0, 0, 0, 1], \"map\")\n",
    "print(example_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a6b50354",
   "metadata": {},
   "source": [
    "As you can see we created the ```example_pose``` with a position of ```[1, 2, 3]``` and an orientation of ```[0, 0, 0, 1]``` in the frame ```map```. But we don't need to provide all these parameters for a Pose, in case there is no parameter the Pose will use default parameter. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "cf5c1aaf",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699448769\n",
      "    nsecs: 128770112\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "  orientation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Pose\n",
    "\n",
    "default_pose = Pose()\n",
    "print(default_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9e599a66",
   "metadata": {},
   "source": [
    "In case no parameter is provided the defualt parameter are:\n",
    "\n",
    "* position: ```[0, 0, 0]```\n",
    "* orientation: ```[o, 0, 0, 1]```\n",
    "* frame: ```map```\n",
    "\n",
    "The following example will show how to access the data stored in a pose. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "a1587669",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Access to a component of the position: 2\n",
      "Access to a component of the rotation: 0.0\n",
      "Get the whole position as geometry_msgs/Pose:\n",
      "x: 1\n",
      "y: 2\n",
      "z: 3\n",
      "You can also get position or orientation as a list: [1, 2, 3]\n",
      "Same with the whole pose: [[1, 2, 3], [0.0, 0.0, 0.0, 1.0]]\n",
      "Access the reference frame: map\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Pose\n",
    "\n",
    "example_pose = Pose([1, 2, 3], [0, 0, 0, 1], \"map\")\n",
    "\n",
    "print(f\"Access to a component of the position: {example_pose.position.y}\")\n",
    "\n",
    "print(f\"Access to a component of the rotation: {example_pose.orientation.x}\")\n",
    "\n",
    "print(f\"Get the whole position as geometry_msgs/Pose:\\n{example_pose.position}\")\n",
    "\n",
    "print(f\"You can also get position or orientation as a list: {example_pose.position_as_list()}\")\n",
    "\n",
    "print(f\"Same with the whole pose: {example_pose.to_list()}\")\n",
    "\n",
    "print(f\"Access the reference frame: {example_pose.frame}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a4788cd2",
   "metadata": {},
   "source": [
    "## Editing a pose\n",
    "You can also edit the data saved in a Pose, similar to how you access it.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "f54533d0",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Edit only one component:\n",
      "x: 3\n",
      "y: 2\n",
      "z: 3 \n",
      "\n",
      "Edit the whole position:\n",
      "x: 0\n",
      "y: 0\n",
      "z: 1 \n",
      "\n",
      "Set a new frame:\n",
      "new_frame \n",
      "\n",
      "Set the position via method:\n",
      "x: 3\n",
      "y: 2\n",
      "z: 1 \n",
      "\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Pose\n",
    "\n",
    "example_pose = Pose([1, 2, 3], [0, 0, 0, 1], \"map\")\n",
    "\n",
    "# Edit a single component of the position \n",
    "example_pose.position.x = 3\n",
    "print(f\"Edit only one component:\\n{example_pose.position}\", \"\\n\")\n",
    "\n",
    "# Edit the whole position\n",
    "example_pose.position = [0, 0, 1]\n",
    "print(f\"Edit the whole position:\\n{example_pose.position}\", \"\\n\")\n",
    "\n",
    "example_pose.frame = \"new_frame\"\n",
    "print(f\"Set a new frame:\\n{example_pose.frame}\", \"\\n\")\n",
    "\n",
    "example_pose.set_position([3, 2, 1])\n",
    "print(f\"Set the position via method:\\n{example_pose.position}\", \"\\n\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "998585a8",
   "metadata": {},
   "source": [
    "## Copy Poses\n",
    "You can also copy Poses to create a new Pose with the same data. This can be useful if you have a method which would need to alter the Pose, since poses are passed by reference to a method every change done to the Pose in the method would affect the instanced passed to the method. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "a5fcd945",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699448775\n",
      "    nsecs: 284231901\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: 1\n",
      "    y: 2\n",
      "    z: 3\n",
      "  orientation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0 \n",
      "\n",
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699448775\n",
      "    nsecs: 284231901\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: 1\n",
      "    y: 2\n",
      "    z: 3\n",
      "  orientation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Pose\n",
    "\n",
    "example_pose = Pose([1, 2, 3], [0, 0, 0, 1], \"map\")\n",
    "\n",
    "copy_pose = example_pose.copy()\n",
    "\n",
    "print(example_pose, \"\\n\")\n",
    "print(copy_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a3e9ab69",
   "metadata": {},
   "source": [
    "## Convert to Transform\n",
    "PyCRAM also has its own transform at which we will take a look in the next section. However, here we will take a look at how to convert a Pose into a Transform. \n",
    "\n",
    "For this example we will take a Pose which represents the current pose of a milk object and convert it into a Transform which represents the transformation from the ```map``` frame to the ```milk``` frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "6ac8728d",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699448777\n",
      "    nsecs: 693948984\n",
      "  frame_id: \"map\"\n",
      "child_frame_id: \"milk\"\n",
      "transform: \n",
      "  translation: \n",
      "    x: 3\n",
      "    y: 4\n",
      "    z: 1\n",
      "  rotation: \n",
      "    x: 0.7071067811865476\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 0.7071067811865476\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Pose\n",
    "\n",
    "milk_pose = Pose([3, 4, 1], [1, 0, 0, 1], \"map\")\n",
    "\n",
    "milk_transform = milk_pose.to_transform(\"milk\")\n",
    "\n",
    "print(milk_transform)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f00e1daa",
   "metadata": {},
   "source": [
    "# Transforms\n",
    "Transforms are similar to Poses but instead of representing a Pose in a frame of reference they represent a transformation from one frame of reference to another. For this purpose Transforms have an additioinal parameter called ```child_frame_id``` which is the frame of reference to which the Transform is pointing. \n",
    "\n",
    "Transforms in PyCRAM inherit from the TransformStamped message of ROS which makes them, like Poses, compatible to ROS services and topics that expect a TransformStamped message. Therefore, the naming conventions of Transforms are the same as of TransformStamped which. \n",
    "\n",
    "* **Translation:** The vector describing the transformation in cartesian space. \n",
    "* **Rotation:** The quaternion describing the transformation of rotation.\n",
    "* **Transform:** The combination of translation and rotation "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "c868a704",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699448779\n",
      "    nsecs: 614031314\n",
      "  frame_id: \"map\"\n",
      "child_frame_id: \"object\"\n",
      "transform: \n",
      "  translation: \n",
      "    x: 1\n",
      "    y: 2\n",
      "    z: 2\n",
      "  rotation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Transform\n",
    "\n",
    "example_transform = Transform([1, 2, 2], [0, 0, 0, 1], \"map\", \"object\")\n",
    "\n",
    "print(example_transform)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "acfe3d8c",
   "metadata": {},
   "source": [
    "Transforms have the same methods to get and set values as Poses have, theresfore only a short showcase will be given. For more details please look at the Pose example or the API documentation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "0941365f",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Access the rotation:\n",
      "x: 0.0\n",
      "y: 0.0\n",
      "z: 0.7071067811865475\n",
      "w: 0.7071067811865475 \n",
      "\n",
      "Access the child_frane: object \n",
      "\n",
      "New translation:\n",
      "x: 1\n",
      "y: 1\n",
      "z: 1\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Transform\n",
    "\n",
    "example_transform = Transform([2, 5, 1], [0, 0, 1, 1], \"map\", \"object\")\n",
    "\n",
    "print(f\"Access the rotation:\\n{example_transform.rotation}\", \"\\n\")\n",
    "\n",
    "print(f\"Access the child_frane: {example_transform.child_frame_id}\", \"\\n\")\n",
    "\n",
    "# changing translation and rotation is exactly like with Poses.\n",
    "\n",
    "example_transform.translation = [1, 1, 1]\n",
    "print(f\"New translation:\\n{example_transform.translation}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "52a88e9f",
   "metadata": {},
   "source": [
    "## Convert to Pose and Copy\n",
    "Analog to Poses Transforms have a method that converts a Transform to a Pose, in this process the ```child_frame_id``` will be lost. \n",
    "\n",
    "Also like in Poses Transforms have a ```copy``` method which creates an exact copy of this Transform."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "e2ea3258",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The converted pose:\n",
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699448783\n",
      "    nsecs: 267217397\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: 1\n",
      "    y: 1\n",
      "    z: 1\n",
      "  orientation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0 \n",
      "\n",
      "The copied transform:\n",
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699448783\n",
      "    nsecs: 268019437\n",
      "  frame_id: \"map\"\n",
      "child_frame_id: \"milk\"\n",
      "transform: \n",
      "  translation: \n",
      "    x: 1\n",
      "    y: 1\n",
      "    z: 1\n",
      "  rotation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Transform\n",
    "\n",
    "milk_transform = Transform([1, 1, 1], [0, 0, 0, 1], \"map\", \"milk\")\n",
    "\n",
    "milk_pose = milk_transform.to_pose()\n",
    "\n",
    "print(f\"The converted pose:\\n{milk_pose}\", \"\\n\")\n",
    "\n",
    "example_transform = Transform([1, 1, 1], [0, 0, 0, 1], \"map\", \"milk\")\n",
    "\n",
    "copy_transform = example_transform.copy()\n",
    "\n",
    "print(f\"The copied transform:\\n{copy_transform}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d0b523b7",
   "metadata": {},
   "source": [
    "## Operations on Transforms\n",
    "Transforms have, unlike Poses, operations that can be done. These operations are: \n",
    "\n",
    "* Multiplication \n",
    "* Invert\n",
    "* InverseTimes\n",
    "\n",
    "### Multiplication\n",
    "We will first take a look at the multiplication of Transforms. We will use an example were we have two Transforms, the first from ```map``` to a ```hand``` frame and the second from the ```hand``` to a ```milk``` frame. By multiplicating these two we get the Transform from ```map``` to ```milk``` frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "f408288b",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699448785\n",
      "    nsecs: 359950304\n",
      "  frame_id: \"map\"\n",
      "child_frame_id: \"milk\"\n",
      "transform: \n",
      "  translation: \n",
      "    x: 1.1\n",
      "    y: 1.05\n",
      "    z: 1.0\n",
      "  rotation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Transform\n",
    "\n",
    "map_to_hand = Transform([1, 1, 1], [0, 0, 0, 1], \"map\", \"hand\")\n",
    "\n",
    "hand_to_milk = Transform([0.1, 0.05, 0], [0, 0, 0, 1], \"hand\", \"milk\")\n",
    "\n",
    "map_to_milk = map_to_hand * hand_to_milk\n",
    "\n",
    "print(map_to_milk)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "722483a8",
   "metadata": {},
   "source": [
    "### Invert \n",
    "This inverts a Transform, so in we have a transform from ```map``` to ```milk``` then inverting it results in a Transform from ```milk``` to ```map``` . "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "1596f595",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699448787\n",
      "    nsecs:   1776695\n",
      "  frame_id: \"milk\"\n",
      "child_frame_id: \"map\"\n",
      "transform: \n",
      "  translation: \n",
      "    x: -1.0\n",
      "    y: -1.0\n",
      "    z: -0.5\n",
      "  rotation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Transform\n",
    "\n",
    "map_to_milk = Transform([1, 1, 0.5], [0, 0, 0, 1], \"map\", \"milk\")\n",
    "\n",
    "milk_to_map = map_to_milk.invert()\n",
    "\n",
    "print(milk_to_map)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "392ff064",
   "metadata": {},
   "source": [
    "### Inverse Times\n",
    "Inverse times combines the inverting and multiplication of Transforms, this results in a 'minus' for Transforms. We will again use the examle of a hand holding a milk, but this time we have the Transforms from ```map``` to ```milk``` and ```hand``` to ```milk```. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "813e698f",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699448788\n",
      "    nsecs: 592707633\n",
      "  frame_id: \"map\"\n",
      "child_frame_id: \"hand\"\n",
      "transform: \n",
      "  translation: \n",
      "    x: 1.0\n",
      "    y: 1.0\n",
      "    z: 1.0\n",
      "  rotation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0\n"
     ]
    }
   ],
   "source": [
    "from pycram.pose import Transform\n",
    "\n",
    "map_to_milk = Transform([1.1, 1.05, 1], [0, 0, 0, 1], \"map\", \"milk\")\n",
    "\n",
    "hand_to_milk = Transform([0.1, 0.05, 0], [0, 0, 0, 1], \"hand\", \"milk\")\n",
    "\n",
    "map_to_milk = map_to_milk.inverse_times(hand_to_milk)\n",
    "\n",
    "print(map_to_milk)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}