{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "536150a3",
   "metadata": {},
   "source": [
    "# Action Designator\n",
    "This example will show the different kinds of Action Designator that are available. We will see how to create Action Designators and what they do.\n",
    "\n",
    "\n",
    "Action Designator are high-level descriptions of actions which the robot should execute. \n",
    "\n",
    "Action Deisgnators are created from a Action Designator Descritpion, which describes the type of action as well as the parameter for this action. Parameter are given as a list of possible parameters.\n",
    "For example, if you want to describe the robot moving to a table you would need a ```NavigateAction``` and a list of poses that are near the table. The Action Designator Description will then pick one of the poses and return a performable Action Designator which contains the picked pose. \n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4767750f",
   "metadata": {},
   "source": [
    "## Navigate Action\n",
    "We will start with a simple example of the ```NavigateAction```. \n",
    "\n",
    "First, we need a BulletWorld with a robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "55d76329",
   "metadata": {},
   "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",
      "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 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",
      "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.bullet_world import BulletWorld, Object\n",
    "from pycram.pose import Pose\n",
    "from pycram.enums import ObjectType\n",
    "\n",
    "world = BulletWorld()\n",
    "pr2 = Object(\"pr2\", ObjectType.ROBOT, \"pr2.urdf\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "id": "418343b7",
   "metadata": {},
   "outputs": [],
   "source": [
    "world.exit()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ed57b47d",
   "metadata": {},
   "source": [
    "To move the robot we need to create a description and resolve it to an actual Designator. The description of navigation only needs a list of possible poses."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "d5960cc9",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pycram.designators.action_designator import NavigateAction\n",
    "from pycram.pose import Pose\n",
    "\n",
    "pose = Pose([1, 0, 0], [0, 0, 0, 1])\n",
    "\n",
    "# This is the Designator Description\n",
    "navigate_description = NavigateAction(target_locations=[pose])\n",
    "\n",
    "# This is the performable Designator\n",
    "navigate_designator = navigate_description.resolve()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "30bd6765",
   "metadata": {},
   "source": [
    "What we now did was create the pose were we want to move the robot, create a description describing a navigation with a list of possible poses (in this case the list contains only one pose) and create an action designator from the description. The action designator contains the pose picked from the list of possible poses and can be performed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "4a51088f",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pycram.process_module import simulated_robot\n",
    "\n",
    "with simulated_robot:\n",
    "    navigate_designator.perform()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7d4e02c0",
   "metadata": {},
   "source": [
    "Every designator that is performed needs to be in an environment that specifies where to perform the designator either on the real robot or the simulated one. This environment is called ```simulated_robot``` similar there is also a ```real_robot``` environment. \n",
    "\n",
    "There are also decorators which do the same thing but for whole methods, they are called ```with_real_robot``` and ```with_simulated_robor```"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a1d1fd15",
   "metadata": {},
   "source": [
    "## Move Torso\n",
    "This action designator moves the torso up or down, specifically it sets the torso joint to a given value.\n",
    "\n",
    "We start again by creating a description and resolving it to a designator. Afterwards, the designator is perfomed in a ```simulated_robot``` environment. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "b3bb514b",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pycram.designators.action_designator import MoveTorsoAction\n",
    "from pycram.process_module import simulated_robot\n",
    "\n",
    "torso_pose = 0.2\n",
    "\n",
    "torso_desig = MoveTorsoAction([torso_pose]).resolve()\n",
    "\n",
    "with simulated_robot:\n",
    "    torso_desig.perform()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "aa20c0b0",
   "metadata": {},
   "source": [
    "## Set Gripper\n",
    "As the name implies, this action designator is used to open or close the gripper. \n",
    "\n",
    "The procedure is similar to the last time, but this time we will shorten it a bit."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "3927240c",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pycram.designators.action_designator import SetGripperAction\n",
    "from pycram.process_module import simulated_robot\n",
    "\n",
    "gripper = \"right\"\n",
    "motion = \"open\"\n",
    "\n",
    "with simulated_robot:\n",
    "    SetGripperAction(grippers=[gripper], motions=[motion]).resolve().perform()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1b0e1cfe",
   "metadata": {},
   "source": [
    "## Park Arms\n",
    "Park arms is used to move one or both arms into the default parking position."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "4d11f9db",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pycram.designators.action_designator import ParkArmsAction\n",
    "from pycram.process_module import simulated_robot\n",
    "from pycram.enums import Arms\n",
    "\n",
    "with simulated_robot:\n",
    "    ParkArmsAction([Arms.BOTH]).resolve().perform()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2ea31ae9",
   "metadata": {},
   "source": [
    "## Pick Up and Place\n",
    "Since these are depending on each other, meaning you can only place something when you picked it up beforehand, they will be shown together. \n",
    "\n",
    "These action designators use object designators, which will not be further explained so please check the example on object designators for more details. \n",
    "\n",
    "To start we need an environment in which we can pick up and place things as well as an object to pick up."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "6e60ef22",
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Scalar element defined multiple times: limit\n",
      "Scalar element defined multiple times: limit\n"
     ]
    }
   ],
   "source": [
    "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n",
    "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))\n",
    "\n",
    "world.reset_bullet_world()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "e30fee45",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pycram.designators.action_designator import PickUpAction, PlaceAction, ParkArmsAction, MoveTorsoAction, NavigateAction\n",
    "from pycram.designators.object_designator import BelieveObject\n",
    "from pycram.process_module import simulated_robot\n",
    "from pycram.enums import Arms\n",
    "from pycram.pose import Pose\n",
    "\n",
    "milk_desig = BelieveObject(names=[\"milk\"])\n",
    "arm =\"right\"\n",
    "\n",
    "with simulated_robot:\n",
    "    ParkArmsAction([Arms.BOTH]).resolve().perform()\n",
    "    \n",
    "    MoveTorsoAction([0.3]).resolve().perform()\n",
    "    \n",
    "    NavigateAction([Pose([0.72, 0.98, 0.0], \n",
    "                     [0.0, 0.0, 0.014701099828940344, 0.9998919329926708])]).resolve().perform()\n",
    "    \n",
    "    PickUpAction(object_designator_description=milk_desig, \n",
    "                     arms=[arm], \n",
    "                     grasps=[\"right\"]).resolve().perform()\n",
    "    \n",
    "    NavigateAction([Pose([-1.90, 0.78, 0.0], \n",
    "                     [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])]).resolve().perform()\n",
    "    \n",
    "    PlaceAction(object_designator_description=milk_desig, \n",
    "                target_locations=[Pose([-1.20, 1.0192, 0.9624], \n",
    "                                   [0.0, 0.0, 0.6339889056055381, 0.7733421413379024])], \n",
    "                arms=[arm]).resolve().perform()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "9519886b",
   "metadata": {},
   "outputs": [],
   "source": [
    "world.reset_bullet_world()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "72e04d08",
   "metadata": {},
   "source": [
    "## Look At\n",
    "Look at lets the robot look at a specific point, for example if it should look at an object for detecting. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "aa86a53d",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pycram.designators.action_designator import LookAtAction\n",
    "from pycram.process_module import simulated_robot\n",
    "from pycram.pose import Pose\n",
    "\n",
    "target_location = Pose([1, 0, 0.5], [0, 0, 0, 1])\n",
    "with simulated_robot:\n",
    "    LookAtAction(targets=[target_location]).resolve().perform()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2f1838d1",
   "metadata": {},
   "source": [
    "## Detect\n",
    "Detect is used to detect objects in the field of vision (FOV) of the robot. We will use the milk used in the pick up/place example, if you didn't execute that example you can spawn the milk with the following cell. The detect designator will return a resolved instance of an ObjectDesignatorDescription. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "e52a5062",
   "metadata": {},
   "outputs": [],
   "source": [
    "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "368e1902",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "ObjectDesignatorDescription.Object(name=milk, type=ObjectType.MILK, bullet_world_object=Object(world=<pycram.bullet_world.BulletWorld object at 0x7f73f87738b0>, \n",
      "local_transformer=<pycram.local_transformer.LocalTransformer object at 0x7f73f8773b80>, \n",
      "name=milk, \n",
      "type=ObjectType.MILK, \n",
      "color=[1, 1, 1, 1], \n",
      "id=4, \n",
      "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n",
      "joints: ..., \n",
      "links: ..., \n",
      "attachments: ..., \n",
      "cids: ..., \n",
      "original_pose=header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699445647\n",
      "    nsecs: 368098735\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: 1.3\n",
      "    y: 1.0\n",
      "    z: 0.9\n",
      "  orientation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0, \n",
      "tf_frame=milk_4, \n",
      "urdf_object: ..., \n",
      "_current_pose=header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699445653\n",
      "    nsecs: 726317167\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: -1.1999999986110241\n",
      "    y: 1.019199981411649\n",
      "    z: 0.9623999834060677\n",
      "  orientation: \n",
      "    x: 6.574869882871473e-09\n",
      "    y: 2.9171242826262372e-09\n",
      "    z: 0.6339889522913499\n",
      "    w: 0.7733421030646893, \n",
      "_current_link_poses={'milk_main': header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699445647\n",
      "    nsecs: 475535869\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: 1.3\n",
      "    y: 1.0\n",
      "    z: 0.9\n",
      "  orientation: \n",
      "    x: 0\n",
      "    y: 0\n",
      "    z: 1\n",
      "    w: 1}, \n",
      "_current_link_transforms={'milk_main': header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699445665\n",
      "    nsecs:  21037817\n",
      "  frame_id: \"map\"\n",
      "child_frame_id: \"milk_4\"\n",
      "transform: \n",
      "  translation: \n",
      "    x: 1.3\n",
      "    y: 1.0\n",
      "    z: 0.9\n",
      "  rotation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0}, \n",
      "_current_joint_states={}, \n",
      "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05  8.96554102e-02], \n",
      "link_to_geometry={'milk_main': <urdf_parser_py.urdf.Mesh object at 0x7f73f594b9a0>}), _pose=<bound method Object.get_pose of Object(world=<pycram.bullet_world.BulletWorld object at 0x7f73f87738b0>, \n",
      "local_transformer=<pycram.local_transformer.LocalTransformer object at 0x7f73f8773b80>, \n",
      "name=milk, \n",
      "type=ObjectType.MILK, \n",
      "color=[1, 1, 1, 1], \n",
      "id=4, \n",
      "path=/home/jdech/workspace/ros/src/pycram-1/src/pycram/../../resources/cached/milk.urdf, \n",
      "joints: ..., \n",
      "links: ..., \n",
      "attachments: ..., \n",
      "cids: ..., \n",
      "original_pose=header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699445647\n",
      "    nsecs: 368098735\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: 1.3\n",
      "    y: 1.0\n",
      "    z: 0.9\n",
      "  orientation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0, \n",
      "tf_frame=milk_4, \n",
      "urdf_object: ..., \n",
      "_current_pose=header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699445653\n",
      "    nsecs: 726317167\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: -1.1999999986110241\n",
      "    y: 1.019199981411649\n",
      "    z: 0.9623999834060677\n",
      "  orientation: \n",
      "    x: 6.574869882871473e-09\n",
      "    y: 2.9171242826262372e-09\n",
      "    z: 0.6339889522913499\n",
      "    w: 0.7733421030646893, \n",
      "_current_link_poses={'milk_main': header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699445647\n",
      "    nsecs: 475535869\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: 1.3\n",
      "    y: 1.0\n",
      "    z: 0.9\n",
      "  orientation: \n",
      "    x: 0\n",
      "    y: 0\n",
      "    z: 1\n",
      "    w: 1}, \n",
      "_current_link_transforms={'milk_main': header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699445665\n",
      "    nsecs:  21037817\n",
      "  frame_id: \"map\"\n",
      "child_frame_id: \"milk_4\"\n",
      "transform: \n",
      "  translation: \n",
      "    x: 1.3\n",
      "    y: 1.0\n",
      "    z: 0.9\n",
      "  rotation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0}, \n",
      "_current_joint_states={}, \n",
      "base_origin_shift=[ 4.15300950e-04 -6.29518181e-05  8.96554102e-02], \n",
      "link_to_geometry={'milk_main': <urdf_parser_py.urdf.Mesh object at 0x7f73f594b9a0>})>, pose=header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1699445653\n",
      "    nsecs: 726317167\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: -1.1999999986110241\n",
      "    y: 1.019199981411649\n",
      "    z: 0.9623999834060677\n",
      "  orientation: \n",
      "    x: 6.574869882871473e-09\n",
      "    y: 2.9171242826262372e-09\n",
      "    z: 0.6339889522913499\n",
      "    w: 0.7733421030646893)\n"
     ]
    }
   ],
   "source": [
    "from pycram.designators.action_designator import DetectAction, LookAtAction, ParkArmsAction, NavigateAction\n",
    "from pycram.designators.object_designator import BelieveObject\n",
    "from pycram.enums import Arms\n",
    "from pycram.process_module import simulated_robot\n",
    "from pycram.pose import Pose\n",
    "\n",
    "milk_desig = BelieveObject(names=[\"milk\"])\n",
    "\n",
    "with simulated_robot:\n",
    "    ParkArmsAction([Arms.BOTH]).resolve().perform()\n",
    "    \n",
    "    NavigateAction([Pose([0, 1, 0], [0, 0, 0, 1])]).resolve().perform()\n",
    "    \n",
    "    LookAtAction(targets=[milk_desig.resolve().pose]).resolve().perform()\n",
    "    \n",
    "    obj_desig = DetectAction(milk_desig).resolve().perform()\n",
    "    \n",
    "    print(obj_desig)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2480ea22",
   "metadata": {},
   "source": [
    "## Transporting\n",
    "Transporting can transport an object from its current position to another target position. It is similar to the Pick and Place plan used in the Pick-up and Place example. Since we need an Object which we can transport we spawn a milk, you don't need to do this if you alredy spawned it in a previous example."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "101d3968",
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Scalar element defined multiple times: limit\n",
      "Scalar element defined multiple times: limit\n"
     ]
    }
   ],
   "source": [
    "kitchen = Object(\"kitchen\", ObjectType.ENVIRONMENT, \"kitchen.urdf\")\n",
    "milk = Object(\"milk\", ObjectType.MILK, \"milk.stl\", pose=Pose([1.3, 1, 0.9]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "86a085a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pycram.designators.action_designator import *\n",
    "from pycram.designators.object_designator import *\n",
    "from pycram.process_module import simulated_robot\n",
    "from pycram.pose import Pose\n",
    "\n",
    "milk_desig = BelieveObject(names=[\"milk\"])\n",
    "\n",
    "with simulated_robot:\n",
    "    MoveTorsoAction([0.3]).resolve().perform()\n",
    "    TransportAction(milk_desig, [\"left\"], [Pose([-0.9, 0.9, 0.95], [0, 0, 1, 0])]).resolve().perform()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "de2eb218",
   "metadata": {},
   "source": [
    "## Opening\n",
    "Opening allows the robot to open a drawer, the drawer is identified by an ObjectPart designator which describes the handle of the drawer that should be grasped. \n",
    "\n",
    "For the moment this designator works only in the apartment environment, therefore we remove the kitchen and spawn the apartment."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "85fde530",
   "metadata": {},
   "outputs": [],
   "source": [
    "kitchen.remove()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "27937aa5",
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Unknown tag \"material\" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]\n",
      "Unknown tag \"material\" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]\n"
     ]
    }
   ],
   "source": [
    "apartment = Object(\"apartment\", ObjectType.ENVIRONMENT, \"apartment.urdf\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "ec8817a5",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pycram.designators.action_designator import *\n",
    "from pycram.designators.object_designator import *\n",
    "from pycram.enums import Arms\n",
    "from pycram.process_module import simulated_robot\n",
    "from pycram.pose import Pose\n",
    "\n",
    "apartment_desig = BelieveObject(names=[\"apartment\"]).resolve()\n",
    "handle_deisg = ObjectPart(names=[\"handle_cab10_t\"], part_of=apartment_desig)\n",
    "\n",
    "with simulated_robot:\n",
    "    MoveTorsoAction([0.25]).resolve().perform()\n",
    "    ParkArmsAction([Arms.BOTH]).resolve().perform()\n",
    "    NavigateAction([Pose([1.7474915981292725, 2.6873629093170166, 0.0],\n",
    "                         [-0.0, 0.0, 0.5253598267689507, -0.850880163370435])]).resolve().perform()\n",
    "    OpenAction(handle_deisg, [\"right\"]).resolve().perform()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3a0f2fb3",
   "metadata": {},
   "source": [
    "## Closing\n",
    "Closing lets the robot close an open drawer, like opening the drawer is identified by an ObjectPart designator describing the handle to be grasped. \n",
    "\n",
    "This action designator only works in the apartment environment for the moment, therefore we remove the kitchen and spawn the apartment. Additionally, we open the drawer such that we can close it with the action designator."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b5721512",
   "metadata": {},
   "outputs": [],
   "source": [
    "kitchen.remove()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "187176a3",
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Unknown tag \"material\" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]\n",
      "Unknown tag \"material\" in /robot[@name='apartment']/link[@name='coffe_machine']/collision[1]\n"
     ]
    }
   ],
   "source": [
    "apartment = Object(\"apartment\", ObjectType.ENVIRONMENT, \"apartment.urdf\")\n",
    "apartment.set_joint_state(\"cabinet10_drawer_top_joint\", 0.4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "be6c0bf5",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pycram.designators.action_designator import *\n",
    "from pycram.designators.object_designator import *\n",
    "from pycram.enums import Arms\n",
    "from pycram.process_module import simulated_robot\n",
    "from pycram.pose import Pose\n",
    "\n",
    "apartment_desig = BelieveObject(names=[\"apartment\"]).resolve()\n",
    "handle_deisg = ObjectPart(names=[\"handle_cab10_t\"], part_of=apartment_desig)\n",
    "\n",
    "with simulated_robot:\n",
    "    MoveTorsoAction([0.25]).resolve().perform()\n",
    "    ParkArmsAction([Arms.BOTH]).resolve().perform()\n",
    "    NavigateAction([Pose([1.7474915981292725, 2.8073629093170166, 0.0],\n",
    "                         [-0.0, 0.0, 0.5253598267689507, -0.850880163370435])]).resolve().perform()\n",
    "    CloseAction(handle_deisg, [\"right\"]).resolve().perform()"
   ]
  }
 ],
 "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
}