{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "5307e0c0",
   "metadata": {},
   "source": [
    "# Exercise Introduction\n",
    "\n",
    "To conclude the introduction to robotics session, we will simulate a robotic manipulation task.\n",
    "\n",
    "In this task a robotic arm is presented with a table top environment, where it has to pick up an object and place it at a desired location. With a real world environment the robot has to percieve its environment through sensors. Instead, in this exercise we will rely on environment information provided by the simulator.\n",
    "\n",
    "The exercise is structured as follows:\n",
    "\n",
    "**Understanding a Robotic Task** - Understand how a robotic problem is formulated.\n",
    "\n",
    "**Working Through a Solution** - Step-by-step undertsand how to approach a robotic task.\n",
    "\n",
    "**Performing a Task** - Stitch together the understanding and solution to perform a task.\n",
    "\n",
    "The above objectives will improve your understanding on how to work on robotics in general. \n",
    "\n",
    "The **Exercise** shall then follow as:\n",
    "\n",
    "**1.** *Perform a simple pick-place task in the table top environment with a cube object.*\n",
    "\n",
    "<img src=\"simple_motion.gif\" width=\"300\">\n",
    "\n",
    "**2.** *Utilise pick-place from previous task to build a small three level stack of cubes.*\n",
    "\n",
    "<img src=\"stacking.gif\" width=\"300\">"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3624fe1c",
   "metadata": {},
   "source": [
    "### Installation and Setup (one-time)\n",
    "\n",
    "We will work with the Pybullet simulator. It is a light weight simulator and can be run in local laptops (windows, linux, mac) and workstation. You don't need CUDA compatible GPU. However, note that your system shall support OpenGL for visualization. \n",
    "\n",
    "1. [Optional] If you are familiar with a packages/dependancies manager (virtual environment manager) such as conda, create an environment of your choice.\n",
    "\n",
    "2. Install pybullet as `pip install pybullet`\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "360998d0",
   "metadata": {},
   "source": [
    "## 0. Getting Started\n",
    "This section provides an overview of PyBuller simulator. Before proceeding with the exercise, you are encouraged to go through the PyBullet documentation to get an outline of its functionalities."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "725cd510",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Importing required packages\n",
    "\n",
    "import pybullet as p\n",
    "import pybullet_data\n",
    "import time, math\n",
    "import numpy as np\n",
    "import random\n",
    "import os\n",
    "from enum import Enum\n",
    "from copy import deepcopy"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "db04bbec",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "4bdfea54",
   "metadata": {},
   "source": [
    "\n",
    "PyBullet and most of robotics follows a standard *urdf* format to describe an object, a robot and other entities. Once loaded, these descriptions allow the simulator to handle the geometry and physics of the objects/robot automatically."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "29ac0f28",
   "metadata": {},
   "source": [
    "Coordinate frames is an important aspect of robotics which helps describe motions and environment to a robot. Hence, when adding an asset (such as a robot or an object) into a simulator, you need to specify its position and orientation in the coordinate frame."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "98659e99",
   "metadata": {},
   "source": [
    "\n",
    "Note that when objects are first added to a simulator, they remain static and no physics is applied automatically. You may need to simulate physics such as gravity, collisions, and motion."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e4c52e0d",
   "metadata": {},
   "source": [
    "# 1. Pick and Place with a Manipulator\n",
    "Now that you are familiar with the basic principles of the simulator, let us move on to creating a simple environment. In this section, we will set up a tabletop scenario with a manipulator arm and one or more objects. Once the environment is ready, we will implement a basic pick-and-place operation.\n",
    "\n",
    "The goal here is to understand how to (1) define an environment, (2) position both the robot and objects within it, and (3) use low-level control to move the robot’s gripper so that it can grasp an object and place it at a desired location."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "adbf6153",
   "metadata": {},
   "source": [
    "### 1.1 Setting up the environment"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "94345cea",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Connect to physics client\n",
    "# disconnect any physics client running already\n",
    "if p.isConnected():\n",
    "    p.disconnect()\n",
    "\n",
    "physics_client = p.connect(p.GUI)\n",
    "p.setAdditionalSearchPath(pybullet_data.getDataPath())\n",
    "p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1)\n",
    "p.setGravity(0,0,-9.8)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f148368c",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Load ground plane (using pybullet_data)\n",
    "base_position = [0,0,-1.1615/2]\n",
    "planeId = p.loadURDF(\"plane.urdf\", basePosition = base_position)\n",
    "\n",
    "# Load a table\n",
    "tablePosition = np.array([0.3,0.1,0])+np.array(base_position) #[0.5,0,-1.1615]\n",
    "table_orientation = p.getQuaternionFromEuler([0, 0, -math.pi/2])\n",
    "tableId = p.loadURDF(\"table/table.urdf\", tablePosition, table_orientation, globalScaling=1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c5fd8411",
   "metadata": {},
   "source": [
    "PyBullet allows you to configure parameters for the simulation like the step size etc. Simulating the environment can also be automated, but this might sometimes conflict with our environment setup structure. Hence, we choose to customize simulation with our own function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "02884c7d",
   "metadata": {},
   "outputs": [],
   "source": [
    "# frequency of steps\n",
    "NUM_STEPS_PER_SECOND = 240\n",
    "\n",
    "# set the step size in simulator using the parameter NUM_STEPS_PER_SECOND\n",
    "p.setTimeStep(1/NUM_STEPS_PER_SECOND)\n",
    "\n",
    "\n",
    "def simulate_world(num_steps_per_second):\n",
    "    for i in range(num_steps_per_second):\n",
    "        p.stepSimulation()\n",
    "        time.sleep(1./num_steps_per_second) #This will make the simulation run at {num_steps}Hz. \n",
    "    #You can change this value to make the simulation run faster or slower."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9eb8e333",
   "metadata": {},
   "source": [
    "### 1.2 Understanding spwaned assets\n",
    "A simulator allows you to understand an object by querying its attributes such as:\n",
    "\n",
    " - Base pose: the position and orientation of the object in the world frame.\n",
    "\n",
    " - Joint information: if the asset has joints (e.g., in robots), you can get their type, limits, and current state.\n",
    "\n",
    " - Dynamics: such as mass, friction, and other physical parameters.\n",
    "\n",
    "Understanding these properties is important because interacting with any object in the simulator, whether static like a table or dynamic like a robot arm, requires precise knowledge of its geometry, pose, and possible degrees of freedom.\n",
    "\n",
    "Now that you have an idea of what type of information can be gathered, you can try to access this with the help from documentation."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e9b24c60",
   "metadata": {},
   "source": [
    "### 1.3 Adding a Robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2d32dfeb",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Spawn franka emika ROBOT robot using pybullet_data\n",
    "panda_position, panda_orientation = [0,0,0], p.getQuaternionFromEuler([0, 0, 0])\n",
    "robotId = p.loadURDF(\"franka_panda/panda.urdf\", panda_position, panda_orientation, useFixedBase = 1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4433f175",
   "metadata": {},
   "source": [
    "We have spawned a 7-Degree-of-Freedom (7-DoF) Franka Emika ROBOT robot. This means that the robot has 7 revolute joints, which define its primary arm motions. To better understand the robot model in the simulator, refer to the ```Controlling a Robot``` section in the PyBullet documentation. In this exercise, we will inspect the robot and check how many joints it contains, what types of joints they are, and what properties they have."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "797a6adf",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "## understand the joint types\n",
    "\n",
    "# first we need to understand how many joints are there\n",
    "ROBOT_NUM_JOINTS = p.getNumJoints(robotId)\n",
    "print(\"num joints in ROBOT: \", ROBOT_NUM_JOINTS)\n",
    "\n",
    "# for each joint in the robot, get the joint name, joint_type, physical parameters: damping and friction, and joint limits\n",
    "\n",
    "JointTypes = Enum(\"JointTypes\", [\"JOINT_REVOLUTE\", \"JOINT_PRISMATIC\", \"JOINT_SPHERICAL\", \"JOINT_PLANAR\", \"JOINT_FIXED\"], start= 0)\n",
    "\n",
    "for i in range(ROBOT_NUM_JOINTS):\n",
    "    joints_info = p.getJointInfo(robotId, i)\n",
    "    jname = joints_info[1].decode(\"utf-8\")\n",
    "    jtype = joints_info[2]\n",
    "    print(f\"linkIdx: {i}, \\t Joint name: {jname}, \\t Joint type: {JointTypes(jtype).name}, \\t damping: {joints_info[6]}, \\t friction: {joints_info[7]}, joint limits: {joints_info[8:10]}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0728c895",
   "metadata": {},
   "source": [
    "#### 1.4 Configure robot parameters\n",
    "In many cases, we need to configure certain robot parameters before performing tasks. These parameters include the robot's **home position** (the default pose from which it starts), **the end-effector ID** (which identifies the gripper link), and the **lower and upper joint limits** that define the feasible range of motion for each joint. Setting these parameters correctly is important for safe and effective control of the robot in the simulator. You can also refer to examples provided by pybullet in their [repository](https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_robots)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "709848fb",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Store ids of moving joints (Revolute joints, prismatic joints[for fingers])\n",
    "REVOLUTE_JOINTS = [0,1,2,3,4,5,6]\n",
    "PRISMATIC_JOINTS = [9,10] \n",
    "\n",
    "# configure the robots end effector id, end effector is the part of the robot which will interact with the objetcs when commanded\n",
    "ROBOT_END_EFFECTOR_INDEX = 11\n",
    "\n",
    "# configuring home position for robot: in default configuration what shall be the states of each joints of the robot\n",
    "ROBOT_HOME_POSITION = [0.002434193097806552, -0.7811217832664136, 0.009772560588829905, -2.062867921360752, -0.007282385107511599, 1.4879767515924243, 0.8049546850317025, 0.0] #joint angles in radians for the panda home position. These values can be obtained from the Moveit ROS or Franka Control Inteface.\n",
    "\n",
    "# configure the joint limits\n",
    "JOINT_LOWER_LIMITS = [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973] \n",
    "JOINT_UPPER_LIMITS = [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973] \n",
    "\n",
    "\n",
    "# The fingers of the robot are already configured for you. You don't need to do anything extra here. \n",
    "# finger parameters\n",
    "ROBOT_FINGER_OPEN = 0.04\n",
    "ROBOT_FINGER_CLOSED = 0\n",
    "# creating a constraint to keep the fingers centered\n",
    "def create_finger_constraint(p, robotId):\n",
    "\tfinger_constraint = p.createConstraint(\n",
    "\t\t\t\trobotId, 9, robotId, 10,\n",
    "\t\t\t\tjointType=p.JOINT_GEAR,\n",
    "\t\t\t\tjointAxis=[1, 0, 0],\n",
    "\t\t\t\tparentFramePosition=[0, 0, 0],\n",
    "\t\t\t\tchildFramePosition=[0, 0, 0])\n",
    "\n",
    "\tp.changeConstraint(finger_constraint, gearRatio=-1, erp=0.1, maxForce=50) #These values are taken from the pybullet examples https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_robots/panda/panda_sim_grasp.py\n",
    "\treturn p, robotId\n",
    "\n",
    "p, robotId = create_finger_constraint(p, robotId)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bd176c63",
   "metadata": {},
   "source": [
    "Great! Now that you understand the robot and its parameters, let’s start building code to interact with it. The first step is to determine whether the robot is currently moving. This can be done by checking the velocity or momentum of each joint. If all joint velocities are close to zero, the robot could be considered stationary. Monitoring this is important because many control commands (such as planning a new trajectory) should only be executed when the robot is not already moving."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8908dffb",
   "metadata": {},
   "outputs": [],
   "source": [
    "def is_robot_moving(p, robotid, joints):\n",
    "    '''\n",
    "    args\n",
    "        p: pybullet_client\n",
    "        robotid: id assigned to the spawned robot\n",
    "        joints: the joint ids of the robot which can freely move\n",
    "    returns\n",
    "        bool (moving/steady)\n",
    "    This function will help in identifying if our robot is in motion so we could bring it to rest before stopping the simulation\n",
    "    '''\n",
    "    for j in joints:\n",
    "        state = p.getJointState(robotid, j)\n",
    "        this_joint_velocity = state[1]\n",
    "        if not math.isclose(this_joint_velocity, 0, abs_tol=1e-5):\n",
    "            return True\n",
    "    return False\n",
    "\n",
    "is_robot_moving(p, robotId, REVOLUTE_JOINTS)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b8628da6",
   "metadata": {},
   "source": [
    "### 1.5 Controlling the robot motions\n",
    "\n",
    "Lets now build towards moving the robot. We should remember that a robot motion is always performed in the robots joint space. Using the same principle we would design a function that achieves this."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b025f096",
   "metadata": {},
   "outputs": [],
   "source": [
    "def move_robot_to_position(p, robotid, target_positions:list):\n",
    "    '''\n",
    "    args\n",
    "        p: pybullet_client\n",
    "        robotID: namespace id given to the robot\n",
    "        target_positions: the target values for each joint\n",
    "    returns\n",
    "        It moves the robot to the mentioned target position and should return \n",
    "        true if succeeded and false if failed\n",
    "    this function will be used to set joint commands on the robot to guide it's joints to the target_positions\n",
    "    '''\n",
    "    for j in range(len(target_positions)):\n",
    "        joint_type = p.getJointInfo(robotid, j)[2]\n",
    "        if joint_type == p.JOINT_REVOLUTE:\n",
    "            p.setJointMotorControl2(bodyIndex=robotid, jointIndex=j, controlMode=p.POSITION_CONTROL, targetPosition=target_positions[j], maxVelocity=.6)\n",
    "    status = execute_action()\n",
    "    return status\n",
    "\n",
    "\n",
    "def execute_action(max_steps=1000, p=p, robotid=robotId, joints=REVOLUTE_JOINTS):\n",
    "    p.stepSimulation()\n",
    "    step = 0\n",
    "    while is_robot_moving(p, robotid, joints) and step < max_steps:\n",
    "        p.stepSimulation()\n",
    "        time.sleep(1/NUM_STEPS_PER_SECOND)\n",
    "        step+=1\n",
    "    if is_robot_moving(p, robotid, joints):\n",
    "        return False\n",
    "    return True\n",
    "\n",
    "move_robot_to_position(p, robotId, ROBOT_HOME_POSITION)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "76189ee8",
   "metadata": {},
   "source": [
    "You can utilise this function to move robot to a defined default position. With provided values the homing action shall look like\n",
    "\n",
    "\n",
    "<img src=\"home_position.png\" width=\"300\">"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e980db1a",
   "metadata": {},
   "source": [
    "Opening or closing the gripper works in a very similar way to moving any other joint. The main difference is that instead of controlling the arm joints, you set the target positions for the gripper joints. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8dcfd1ae",
   "metadata": {},
   "outputs": [],
   "source": [
    "def command_fingers(p, robotid, open:bool):\n",
    "    '''\n",
    "    args\n",
    "        p: pybullet_client\n",
    "        robotID: namespace id given to the robot\n",
    "        open: do we want the robot to open its fingers, otherwise close them\n",
    "    returns\n",
    "        bool (success/failure)\n",
    "\n",
    "    This function will simply close or open robots gripper, so to grasp or release the object\n",
    "    '''\n",
    "    command = ROBOT_FINGER_OPEN if open else ROBOT_FINGER_CLOSED\n",
    "    p.setJointMotorControl2(robotid, 9, p.POSITION_CONTROL, targetPosition=command, force=100)\n",
    "    p.setJointMotorControl2(robotid, 10, p.POSITION_CONTROL, targetPosition=command, force=100)\n",
    "    status = execute_action(max_steps = 50)\n",
    "    return status\n",
    "\n",
    "command_fingers(p, robotId, open=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d1baab5d",
   "metadata": {},
   "source": [
    "### 1.6 Manipulating the objects\n",
    "\n",
    "#### Picking up the object"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b9c3d9f6",
   "metadata": {},
   "source": [
    "Spawn the cube object"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6ae7751f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# using pybullet_data\n",
    "\n",
    "cube_position = [0.52,0.0,0.09]\n",
    "cube_orientation = p.getQuaternionFromEuler([random.uniform(-math.pi/2, math.pi/2), random.uniform(-math.pi/2, math.pi/2), random.uniform(-math.pi/2, math.pi/2)]) #random cube orientation\n",
    "cubeId = p.loadURDF(\"cube_small.urdf\", cube_position, cube_orientation)\n",
    "\n",
    "simulate_world(num_steps_per_second = NUM_STEPS_PER_SECOND)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ec5d5b72",
   "metadata": {},
   "source": [
    "Now that we can control both the robot arm and the gripper, let's perform a pick operation on the cube. The process can be broken down into the following steps:\n",
    "\n",
    "1. Get the cube's pose through the simulator.\n",
    "\n",
    "2. **Command robotic motion using inverse kinematics**: The joint space is not very simple to understand hence we would specify target position in task space and use inverse kinematics (IK) to get the corresponding target joints.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "78c69a7c",
   "metadata": {},
   "outputs": [],
   "source": [
    "def pick_object(p, robotid, objectid):\n",
    "    '''\n",
    "    args\n",
    "        p: pybullet_client\n",
    "        robotid: namespace id given to the robot\n",
    "        objectid: namespace id given to the object\n",
    "    returns\n",
    "        bool (success/failure)\n",
    "\n",
    "    This function shall command physics client to pick objectid with robotid\n",
    "    '''\n",
    "    # First use the objectid to get its position and orientation,\n",
    "    # Use pybullets inverse kinematics to get robots target joint positions corresponding to the object pose\n",
    "    object_position, object_orientation = p.getBasePositionAndOrientation(objectid)\n",
    "    joint_positions = p.calculateInverseKinematics(robotid, ROBOT_END_EFFECTOR_INDEX, targetPosition=object_position, lowerLimits=JOINT_LOWER_LIMITS, upperLimits=JOINT_UPPER_LIMITS,\n",
    "                                                   restPoses=ROBOT_HOME_POSITION, maxNumIterations=10000, residualThreshold=1e-4)\n",
    "    \n",
    "    move_robot_to_position(p, robotid, ROBOT_HOME_POSITION)\n",
    "    command_fingers(p, robotid, open=True)\n",
    "    move_robot_to_position(p, robotid, target_positions=joint_positions[:7])\n",
    "    command_fingers(p, robotid, open=False)\n",
    "    move_robot_to_position(p, robotid, ROBOT_HOME_POSITION)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4a04c6da",
   "metadata": {},
   "source": [
    "#### Place the object\n",
    "\n",
    "Similarly we will use inverse kinematics to place the object at the desired position"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "66472e0f",
   "metadata": {},
   "outputs": [],
   "source": [
    "def place_at_target_position(p, robotid, target_pose):\n",
    "    '''\n",
    "    args\n",
    "        p: pybullet_client\n",
    "        robotid: namespace id given to the robot\n",
    "        target_pose: location at which the object in robots hand is to be placed\n",
    "    returns\n",
    "        bool (success/failure)\n",
    "\n",
    "    this function shall command the physics cliet to place the object at the target_pose\n",
    "    '''\n",
    "    joint_positions = p.calculateInverseKinematics(robotid, ROBOT_END_EFFECTOR_INDEX, targetPosition=target_pose, lowerLimits=JOINT_LOWER_LIMITS, upperLimits=JOINT_UPPER_LIMITS,\n",
    "                                                   restPoses=ROBOT_HOME_POSITION, maxNumIterations=10000, residualThreshold=1e-4)\n",
    "    \n",
    "    move_robot_to_position(p, robotid, target_positions=joint_positions[:7])\n",
    "    command_fingers(p, robotid, open=True)\n",
    "    move_robot_to_position(p, robotid, ROBOT_HOME_POSITION)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f13ee1dd",
   "metadata": {},
   "outputs": [],
   "source": [
    "target_position = [.38, -.15, .08]\n",
    "pick_object(p, robotId, cubeId)\n",
    "place_at_target_position(p, robotId, target_position)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3483c8f1",
   "metadata": {},
   "source": [
    "### 1.7 Pick and place with crane like operations\n",
    "\n",
    "We can see that the motion with previous operations is not very smooth. The robot might sometimes collide with the object. To reduce this kind of inconsistency in robotic motion, instead we will provide the robot some **waypoints** which will allow more controlled motions.\n",
    "\n",
    "\n",
    "<img src=\"waypoints_motion.gif\" width=\"200\">\n",
    "As you can see, this waypoint-based approach provides safer and more reliable pick-and-place operations, reducing the chances of collisions and ensuring more stable placements. \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "05168a54",
   "metadata": {},
   "outputs": [],
   "source": [
    "def crane_pick_object(p, robotid, objectid):\n",
    "    '''\n",
    "    args\n",
    "        p: pybullet_client\n",
    "        robotid: namespace id given to the robot\n",
    "        objectid: namespace id given to the object\n",
    "    returns\n",
    "        bool (success/failure)\n",
    "\n",
    "    There are many ways a robot can reach target location and sometimes this may lead to undesirable behaviors\n",
    "    for this purpose, we define an approximate path (through `waypoints`) so robot doesn't take absurd paths\n",
    "    '''\n",
    "    object_position, object_orientation = p.getBasePositionAndOrientation(objectid)\n",
    "    orientation = p.getQuaternionFromEuler([0, math.pi, -math.pi])\n",
    "\n",
    "    move_up_joint_positions = p.calculateInverseKinematics(robotId, ROBOT_END_EFFECTOR_INDEX,  targetPosition=[object_position[0], object_position[1], object_position[2]+0.09], targetOrientation = orientation, \n",
    "                                                   lowerLimits=JOINT_LOWER_LIMITS, upperLimits=JOINT_UPPER_LIMITS, restPoses=ROBOT_HOME_POSITION, maxNumIterations=10000, residualThreshold=1e-4)\n",
    "    \n",
    "    grasp_position = p.calculateInverseKinematics(robotId, ROBOT_END_EFFECTOR_INDEX,  targetPosition = object_position, targetOrientation = orientation, lowerLimits=JOINT_LOWER_LIMITS, upperLimits=JOINT_UPPER_LIMITS, \n",
    "                                                  restPoses=ROBOT_HOME_POSITION, maxNumIterations=10000, residualThreshold=1e-4)\n",
    "\n",
    "    move_robot_to_position(p, robotid, ROBOT_HOME_POSITION)\n",
    "    move_robot_to_position(p, robotid, move_up_joint_positions[:7])\n",
    "    move_robot_to_position(p, robotid, grasp_position[:7])\n",
    "    command_fingers(p, robotid, open=False)\n",
    "    move_robot_to_position(p, robotid, move_up_joint_positions[:7])\n",
    "    move_robot_to_position(p, robotid, ROBOT_HOME_POSITION)\n",
    "\n",
    "\n",
    "\n",
    "def crane_place_object(p, robotid, target_pose):\n",
    "    '''\n",
    "    args\n",
    "        p: pybullet_client\n",
    "        robotid: namespace id given to the robot\n",
    "        target_pose: location at which the object in robots hand is to be placed\n",
    "    returns\n",
    "        bool (success/failure)\n",
    "    '''\n",
    "    orientation = p.getQuaternionFromEuler([0, math.pi, -math.pi])\n",
    "    move_up_joint_positions = p.calculateInverseKinematics(robotId, ROBOT_END_EFFECTOR_INDEX,  targetPosition=[target_pose[0], target_pose[1], target_pose[2]+0.09], targetOrientation = orientation, \n",
    "                                                   lowerLimits=JOINT_LOWER_LIMITS, upperLimits=JOINT_UPPER_LIMITS, restPoses=ROBOT_HOME_POSITION, maxNumIterations=10000, residualThreshold=1e-4)\n",
    "\n",
    "    place_positions = p.calculateInverseKinematics(robotid, ROBOT_END_EFFECTOR_INDEX, targetPosition=target_pose, targetOrientation = orientation, lowerLimits=JOINT_LOWER_LIMITS, upperLimits=JOINT_UPPER_LIMITS,\n",
    "                                                   restPoses=ROBOT_HOME_POSITION, maxNumIterations=10000, residualThreshold=1e-4)\n",
    "    \n",
    "    move_robot_to_position(p, robotid, move_up_joint_positions[:7])\n",
    "    move_robot_to_position(p, robotid, target_positions=place_positions[:7])\n",
    "    command_fingers(p, robotid, open=True)\n",
    "    move_robot_to_position(p, robotid, move_up_joint_positions[:7])\n",
    "    move_robot_to_position(p, robotid, ROBOT_HOME_POSITION)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "24ce3694",
   "metadata": {},
   "source": [
    "#### 1.8 Perform a pick and place operation \n",
    "1. with normal pick and place functions and \n",
    "2. with crane like pick and place functioning\n",
    "\n",
    "try to analyze the difference in motion paths with both methods"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "321cd26e",
   "metadata": {},
   "outputs": [],
   "source": [
    "target_position = [.36, .2, .08]\n",
    "crane_pick_object(p, robotId, cubeId)\n",
    "crane_place_object(p, robotId, target_position)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "385c2ef7",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "ea2d06fd",
   "metadata": {},
   "source": [
    "\n",
    "\n",
    "\n",
    "## 2. Stacking\n",
    "In this part of the assignment, you need to build a three-level tower by stacking cube objects on top of each other. You can reuse your pick-and-place implementation from the previous part to perform the action sequences.\n",
    "\n",
    "It is recommended to use cubes of decreasing sizes for the upper levels. This is because pybullet’s object physics makes it challenging to maintain stability when stacking cubes of the same size—the tower may collapse due to small simulation inaccuracies."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "496f8a22",
   "metadata": {},
   "source": [
    "#### 2.1 Setting up the environment"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "81c97f0f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# remove previous objects from the scene\n",
    "p.removeBody(cubeId)\n",
    "\n",
    "# spawn three cubes to be stacked\n",
    "cubeb_position = [0.52,0.3,0.09]\n",
    "cubem_position = [0.48,0.41,0.09]\n",
    "cubes_position = [0.4,0.35,0.09]\n",
    "cube_orientation = p.getQuaternionFromEuler([random.uniform(-math.pi/2, math.pi/2), random.uniform(-math.pi/2, math.pi/2), random.uniform(-math.pi/2, math.pi/2)]) #random cube orientation\n",
    "cubebID = p.loadURDF(\"cube_small.urdf\", cubeb_position, cube_orientation) #, globalScaling=6/5)\n",
    "cubemID = p.loadURDF(\"cube_small.urdf\", cubem_position, cube_orientation)\n",
    "cubesID = p.loadURDF(\"cube_small.urdf\", cubes_position, cube_orientation) #, globalScaling=4/5)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "020f3cbf",
   "metadata": {},
   "source": [
    "#### 2.2 Perform stacking with pick-place operations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9bacc56e",
   "metadata": {},
   "outputs": [],
   "source": [
    "# For each cubeID configure target location and perform pick-place to build a stack\n",
    "# TODO"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1d601618",
   "metadata": {},
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.9.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}