{
 "cells": [
  {
   "cell_type": "markdown",
   "source": [
    "\n",
    "## Setting up the Environment\n",
    "\n",
    "This step involves importing the required modules and initializing key components for our tasks.\n"
   ],
   "metadata": {
    "collapsed": false
   },
   "id": "b258b9598f821833"
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "1ef7ee85a79929e6",
   "metadata": {
    "collapsed": false,
    "ExecuteTime": {
     "end_time": "2023-09-08T16:51:08.901104329Z",
     "start_time": "2023-09-08T16:51:08.139879012Z"
    }
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n",
      "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n",
      "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n",
      "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n"
     ]
    }
   ],
   "source": [
    "from pycram.designators.action_designator import Object, LocalTransformer, BulletWorld, Transform"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b44c2297a7b987b2",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "\n",
    "## Initializing the World\n",
    "\n",
    "Every robot simulation requires a world where it can interact. This world serves as the playground where the robot performs tasks. \n",
    "Let's start by creating this world and setting the appropriate gravitational force to simulate Earth's gravity.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "bb9446c4ef3ba8bc",
   "metadata": {
    "collapsed": false,
    "ExecuteTime": {
     "end_time": "2023-09-08T16:51:11.202198685Z",
     "start_time": "2023-09-08T16:51:10.916529902Z"
    }
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Exception in thread Thread-10:\n",
      "Traceback (most recent call last):\n",
      "  File \"/usr/lib/python3.8/threading.py\", line 932, in _bootstrap_inner\n",
      "    self.run()\n",
      "  File \"/home/oem/pycram_ws/src/pycram/src/pycram/bullet_world.py\", line 536, in run\n",
      "    basePosition=cameraTargetPosition)\n",
      "UnboundLocalError: local variable 'cameraTargetPosition' referenced before assignment\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "startThreads creating 1 threads.\n",
      "starting thread 0\n",
      "started thread 0 \n",
      "argc=2\n",
      "argv[0] = --unused\n",
      "argv[1] = --start_demo_name=Physics Server\n",
      "ExampleBrowserThreadFunc started\n",
      "X11 functions dynamically loaded using dlopen/dlsym OK!\n",
      "X11 functions dynamically loaded using dlopen/dlsym OK!\n",
      "Creating context\n",
      "Created GL 3.3 context\n",
      "Direct GLX rendering context obtained\n",
      "Making context current\n",
      "GL_VENDOR=NVIDIA Corporation\n",
      "GL_RENDERER=NVIDIA GeForce RTX 2060/PCIe/SSE2\n",
      "GL_VERSION=3.3.0 NVIDIA 525.125.06\n",
      "GL_SHADING_LANGUAGE_VERSION=3.30 NVIDIA via Cg compiler\n",
      "pthread_getconcurrency()=0\n",
      "Version = 3.3.0 NVIDIA 525.125.06\n",
      "Vendor = NVIDIA Corporation\n",
      "Renderer = NVIDIA GeForce RTX 2060/PCIe/SSE2\n",
      "b3Printf: Selected demo: Physics Server\n",
      "startThreads creating 1 threads.\n",
      "starting thread 0\n",
      "started thread 0 \n",
      "MotionThreadFunc thread started\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n",
      "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n",
      "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n",
      "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n"
     ]
    }
   ],
   "source": [
    "# Create an instance of the BulletWorld\n",
    "world = BulletWorld()\n",
    "# Set the gravitational force to simulate Earth's gravity\n",
    "world.set_gravity([0, 0, -9.8])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8783f80add4e64c7",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "\n",
    "## Adding Objects to the World\n",
    "\n",
    "For our robot to perform meaningful tasks, we need to populate its world with objects. \n",
    "In this section, we'll add a variety of objects, from a simple floor plane to kitchen setups and items like milk and bowls. \n",
    "These objects will be used in subsequent tasks.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "e1f9e28084fc99dc",
   "metadata": {
    "collapsed": false,
    "ExecuteTime": {
     "end_time": "2023-09-08T16:51:38.439962987Z",
     "start_time": "2023-09-08T16:51:33.675944073Z"
    }
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n",
      "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n",
      "Scalar element defined multiple times: limit\n",
      "Unknown tag \"material\" in /robot[@name='plane']/link[@name='planeLink']/collision[1]\n",
      "Unknown tag \"contact\" in /robot[@name='plane']/link[@name='planeLink']\n",
      "Scalar element defined multiple times: limit\n"
     ]
    }
   ],
   "source": [
    "plane = Object(\"floor\", \"environment\", \"plane.urdf\", world=world)\n",
    "from pycram.designators.object_designator import *\n",
    "\n",
    "kitchen = Object(\"kitchen\", \"environment\", \"kitchen.urdf\")\n",
    "milk = Object(\"milk\", \"milk\", \"milk.stl\", Pose([0.9, 1, 0.95]))\n",
    "bowl = Object(\"bowl\", \"bowl\", \"bowl.stl\", Pose([1.6, 1, 0.90]))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7768a7d47579b365",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "Transformations with LocalTransformer\n",
    "Now that we have our world set up, let's perform some transformations. We'll use the LocalTransformer to transform poses relative to our objects."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "181a9386fbfafc4a",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-09-08T16:42:15.478051994Z",
     "start_time": "2023-09-08T16:42:15.472743693Z"
    },
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1694191335\n",
      "    nsecs: 125374555\n",
      "  frame_id: \"milk_3\"\n",
      "pose: \n",
      "  position: \n",
      "    x: -0.9\n",
      "    y: -1.0\n",
      "    z: -0.95\n",
      "  orientation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0\n",
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1694191335\n",
      "    nsecs: 126070022\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": [
    "l = LocalTransformer()\n",
    "test_pose = Pose([1, 1, 1], [0, 0, 0, 1], \"map\")\n",
    "p = Pose()\n",
    "\n",
    "transformed_pose = l.transform_to_object_frame(p, milk)\n",
    "print(transformed_pose)\n",
    "\n",
    "new_pose = l.transform_pose(transformed_pose, \"map\")\n",
    "print(new_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8f5a5b3a2897cb52",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "In the above code, we first transformed a pose to the object frame of the milk object, and then we transformed it back to the map frame. This demonstrates how we can easily manipulate poses relative to objects in our environment.\n",
    "You can also transform poses relative to other poses. by using the transform_pose method. Further u can set a Transform."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8645f3e590e49a6d",
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "l.setTransform(Transform([1, 1, 1], [0, 0, 0, 1], \"map\", \"test_frame\"))\n",
    "p = Pose()\n",
    "\n",
    "transformed_pose = l.transform_pose(p, \"test_frame\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b83d7f2e06bc0dc0",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "You can also set a Pose to an object and update the transforms for that object."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "137f4cab85ab19de",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-09-08T16:47:16.375950749Z",
     "start_time": "2023-09-08T16:47:16.332420624Z"
    },
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "milk.set_pose(Pose([1, 2, 1]))\n",
    "l.update_transforms_for_object(milk)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b3aca70fc87b20fa",
   "metadata": {
    "collapsed": false
   },
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "ac8096a6bf2ee85a",
   "metadata": {
    "collapsed": false
   },
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}