{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Bullet World\n",
    "This Notebook will show you the basics of working with the PyCRAM BulletWorld.\n",
    "\n",
    "First we need to import and create a BulletWorld."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "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"
     ]
    }
   ],
   "source": [
    "from pycram.bullet_world import BulletWorld\n",
    "from pycram.pose import Pose\n",
    "\n",
    "world = BulletWorld()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This new window is the BulletWorld, PyCRAMs internal physics simulation. You can use the mouse to move the camera around:\n",
    "\n",
    "  * Press the left mouse button to rotate the camera\n",
    "  * Press the right mouse button to move the camera \n",
    "  * Press the middle mouse button (scroll wheel) and move the mouse up or down to zoom\n",
    "    \n",
    "At the moment the BulletWorld only contains a floor, this is spawned by default when creating the BulletWorld. Furthermore, the gravity is set to 9.8 m^2, which is the same gravitation as the one on earth. \n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To close the BulletWorld again please use the ```exit``` method since it will also terminate threads running in the background"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "world.exit()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To spawn new things in the BulletWorld we need to import the Object class and create and instance of it. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pycram.bullet_world import Object\n",
    "\n",
    "milk = Object(\"milk\", \"milk\", \"milk.stl\", pose=Pose([0, 0, 1]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see this spawns a milk floating in the air. What we did here was create a new Object which has the name \"milk\" as well as the type \"milk\", is spawned from the file \"milk.stl\" and is at the position [0, 0, 1]. \n",
    "\n",
    "The first three of these parameters are required while the position is optional. As you can see it was sufficent to only specify the filename for PyCRAM to spawn the milk mesh. When only providing a filename PyCRAM will search in its resource directory for a matching file and use it. \n",
    "\n",
    "For a complete list of all parameters that can be used to crate an Object please check the documentation. \n",
    "\n",
    "\n",
    "\n",
    "Since the Object is spawned we can now interact with it. First we want to move it around and change it's orientation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "milk.set_position(Pose([1, 1, 1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "milk.set_orientation(Pose(orientation=[1,0, 0, 1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "milk.set_pose(Pose([0, 0, 1], [0, 0, 0, 1]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the same sense as setting the position or orientation you can also get the position and orientation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Position: \n",
      "x: 0.0\n",
      "y: 0.0\n",
      "z: 1.0\n",
      "Orientation: \n",
      "x: 0.0\n",
      "y: 0.0\n",
      "z: 0.0\n",
      "w: 1.0\n",
      "Pose: \n",
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1690203002\n",
      "    nsecs:  93374252\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 1.0\n",
      "  orientation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0\n"
     ]
    }
   ],
   "source": [
    "print(f\"Position: \\n{milk.get_position()}\")\n",
    "\n",
    "print(f\"Orientation: \\n{milk.get_orientation()}\")\n",
    "\n",
    "print(f\"Pose: \\n{milk.get_pose()}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Attachments\n",
    "You can attach Objects to each other simply by calling the attach method on one of them and providing the other as parameter. Since attachments are bi-directional it doesn't matter on which Object you call the method. \n",
    "\n",
    "First we neeed another Object"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "cereal = Object(\"cereal\", \"cereal\", \"breakfast_cereal.stl\", pose=Pose([1, 0, 1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "milk.attach(cereal)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now since they are attached to each other, if we move one of them the other will move in conjunction."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "milk.set_position(Pose([1,1,1]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the same way the Object can also be detached, just call the detach method on one of the two attached Objects."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "cereal.detach(milk)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Links and Joints\n",
    "Objects spawned from mesh files do not have links or joints, but if you spawn things from a URDF like a robot they will have a lot of links and joints. Every Object has two dictionaries as attributes namley ```links``` and ```joints``` which contain every link or joint as key and a unique id, used by PyBullet, as value. \n",
    "\n",
    "We will see this at the example of the PR2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "{'base_link': 0, 'base_bellow_link': 1, 'base_laser_link': 2, 'fl_caster_rotation_link': 3, 'fl_caster_l_wheel_link': 4, 'fl_caster_r_wheel_link': 5, 'fr_caster_rotation_link': 6, 'fr_caster_l_wheel_link': 7, 'fr_caster_r_wheel_link': 8, 'bl_caster_rotation_link': 9, 'bl_caster_l_wheel_link': 10, 'bl_caster_r_wheel_link': 11, 'br_caster_rotation_link': 12, 'br_caster_l_wheel_link': 13, 'br_caster_r_wheel_link': 14, 'torso_lift_link': 15, 'l_torso_lift_side_plate_link': 16, 'r_torso_lift_side_plate_link': 17, 'imu_link': 18, 'head_pan_link': 19, 'head_tilt_link': 20, 'head_plate_frame': 21, 'sensor_mount_link': 22, 'high_def_frame': 23, 'high_def_optical_frame': 24, 'double_stereo_link': 25, 'wide_stereo_link': 26, 'wide_stereo_optical_frame': 27, 'wide_stereo_l_stereo_camera_frame': 28, 'wide_stereo_l_stereo_camera_optical_frame': 29, 'wide_stereo_r_stereo_camera_frame': 30, 'wide_stereo_r_stereo_camera_optical_frame': 31, 'narrow_stereo_link': 32, 'narrow_stereo_optical_frame': 33, 'narrow_stereo_l_stereo_camera_frame': 34, 'narrow_stereo_l_stereo_camera_optical_frame': 35, 'narrow_stereo_r_stereo_camera_frame': 36, 'narrow_stereo_r_stereo_camera_optical_frame': 37, 'projector_wg6802418_frame': 38, 'projector_wg6802418_child_frame': 39, 'laser_tilt_mount_link': 40, 'laser_tilt_link': 41, 'r_shoulder_pan_link': 42, 'r_shoulder_lift_link': 43, 'r_upper_arm_roll_link': 44, 'r_upper_arm_link': 45, 'r_elbow_flex_link': 46, 'r_forearm_roll_link': 47, 'r_forearm_link': 48, 'r_wrist_flex_link': 49, 'r_wrist_roll_link': 50, 'r_gripper_palm_link': 51, 'r_gripper_led_frame': 52, 'r_gripper_motor_accelerometer_link': 53, 'r_gripper_tool_frame': 54, 'r_gripper_motor_slider_link': 55, 'r_gripper_motor_screw_link': 56, 'r_gripper_l_finger_link': 57, 'r_gripper_l_finger_tip_link': 58, 'r_gripper_r_finger_link': 59, 'r_gripper_r_finger_tip_link': 60, 'r_gripper_l_finger_tip_frame': 61, 'r_forearm_cam_frame': 62, 'r_forearm_cam_optical_frame': 63, 'l_shoulder_pan_link': 64, 'l_shoulder_lift_link': 65, 'l_upper_arm_roll_link': 66, 'l_upper_arm_link': 67, 'l_elbow_flex_link': 68, 'l_forearm_roll_link': 69, 'l_forearm_link': 70, 'l_wrist_flex_link': 71, 'l_wrist_roll_link': 72, 'l_gripper_palm_link': 73, 'l_gripper_led_frame': 74, 'l_gripper_motor_accelerometer_link': 75, 'l_gripper_tool_frame': 76, 'l_gripper_motor_slider_link': 77, 'l_gripper_motor_screw_link': 78, 'l_gripper_l_finger_link': 79, 'l_gripper_l_finger_tip_link': 80, 'l_gripper_r_finger_link': 81, 'l_gripper_r_finger_tip_link': 82, 'l_gripper_l_finger_tip_frame': 83, 'l_forearm_cam_frame': 84, 'l_forearm_cam_optical_frame': 85, 'torso_lift_motor_screw_link': 86, 'base_footprint': -1}\n"
     ]
    },
    {
     "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",
      "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": [
    "pr2 = Object(\"pr2\", \"pr2\", \"pr2.urdf\")\n",
    "print(pr2.links)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For links there are similar methods available as for the pose. However, you can only **get** the position and orientation of a link. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Position: \n",
      "x: -0.05000000447034836\n",
      "y: 0.0\n",
      "z: 0.7906750440597534\n",
      "Orientation: \n",
      "x: 0.0\n",
      "y: 0.0\n",
      "z: 0.0\n",
      "w: 1.0\n",
      "Pose: \n",
      "header: \n",
      "  seq: 0\n",
      "  stamp: \n",
      "    secs: 1690203043\n",
      "    nsecs: 303540229\n",
      "  frame_id: \"map\"\n",
      "pose: \n",
      "  position: \n",
      "    x: -0.05000000447034836\n",
      "    y: 0.0\n",
      "    z: 0.7906750440597534\n",
      "  orientation: \n",
      "    x: 0.0\n",
      "    y: 0.0\n",
      "    z: 0.0\n",
      "    w: 1.0\n"
     ]
    }
   ],
   "source": [
    "print(f\"Position: \\n{pr2.get_link_position('torso_lift_link')}\")\n",
    "\n",
    "print(f\"Orientation: \\n{pr2.get_link_orientation('torso_lift_link')}\")\n",
    "\n",
    "print(f\"Pose: \\n{pr2.get_link_pose('torso_lift_link')}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Methods available for joints are:\n",
    "\n",
    "  * ```get_joint_state```\n",
    "  * ```set_joint_state```\n",
    "  * ```get_joint_limits```\n",
    "  \n",
    "We will see how these methods work at the example of the torso_lift_joint"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Joint limits: (0.0, 0.33)\n",
      "Current Joint state: 0.0\n",
      "New Joint state: 0.2\n"
     ]
    }
   ],
   "source": [
    "print(f\"Joint limits: {pr2.get_joint_limits('torso_lift_joint')}\")\n",
    "\n",
    "print(f\"Current Joint state: {pr2.get_joint_state('torso_lift_joint')}\")\n",
    "\n",
    "pr2.set_joint_state(\"torso_lift_joint\", 0.2)\n",
    "\n",
    "print(f\"New Joint state: {pr2.get_joint_state('torso_lift_joint')}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Misc Methods\n",
    "There are a few methods that don't fit any category but could be helpful anyways. The first two are ```get_color``` and ```set_color```, as the name implies they can be used to get or set the color for specific links or the whole Object. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Pr2 forearm color: (0.699999988079071, 0.699999988079071, 0.699999988079071, 1.0)\n"
     ]
    }
   ],
   "source": [
    "print(f\"Pr2 forearm color: {pr2.get_color('r_forearm_link')}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "pr2.set_color([1, 0, 0], \"r_forearm_link\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lastly, there is ```get_AABB``` AABB stands for axis aligned bounding box. This method returns two points in world coordinates which span a rectangle representing the AABB."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "((-0.0015000000000000005, -0.0015000000000000005, 0.06949999999999999),\n",
       " (0.0015000000000000005, 0.0015000000000000005, 0.0725))"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "pr2.get_AABB()"
   ]
  }
 ],
 "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
}