lab_instructions.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "d252d842",
   "metadata": {},
   "source": [
    "# Table Of Content:\n",
    "* Preliminaries\n",
    "    * [Structure of the lab](#structure)\n",
    "    * [Objective of the lab](#objective)\n",
    "    * [How to work for the lab?](#howto)\n",
    "    * [Setting up the scene](#scene)\n",
    "* [Part 1: Geometry](#part1)\n",
    "    * [I. Computing the target configurations (Inverse Geometry)](#IG)\n",
    "    * [II. Motion planning](#motion_planning)\n",
    "* [Part 2: Dynamics](#part2)\n",
    "    * [I. From a path to a trajectory](#TO)\n",
    "    * [II. Implementing a control law](#control)\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a061912f",
   "metadata": {},
   "source": [
    "# Structure of the lab:  <a class=\"anchor\" id=\"structure\"></a>\n",
    "The lab is divided into separate python files, each designed for you to address a sub-problem atomically. These instructions will indicate where you should implement each task. At the end of each file, in the 'main' \n",
    "section you can locally test your functions. It is important that you **do not modify the names and signatures** of the methods provided: When marking the lab, I will in first instance run code that will use these functions to evaluate quantitatively the methods you proposed. "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "86d24413",
   "metadata": {},
   "source": [
    "# Objective of the lab <a class=\"anchor\" id=\"objective\"></a>\n",
    "Use both effectors of the nextage robot to grab a box and bring it to a target location.\n",
    "You will first plan a valid motion that brings the robot to a grasping configuration, then moves the box to \n",
    "a desired location while avoiding collisions. For this you will use a combination of motion planning and numerical optimisation.\n",
    "\n",
    "Once this motion plan will be computed, you will test it in a dynamics simulator using a control method of your choice.\n",
    "This will be the objective of part 2 of the lab.\n",
    "\n",
    "In the optional part 3 of the lab (for exceptional marks), you will be asked to self-propose a more complex task to achieve, which will require you to implement additional features to your framework. "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2b1d1455",
   "metadata": {},
   "source": [
    "# How to work for the lab? <a class=\"anchor\" id=\"howto\"></a>\n",
    "You have been used to notebooks for the tutorials, and these instructions also take the form of a notebook. To implement this lab I personally chose to work directly with a python IDE and I recommend to do the same. I worked with spyder but any python IDE might work as well. You can decide to work using notebooks if you prefer, this is not a problem, as long as your final deliverables meet the requirement specification.\n",
    "\n",
    "**In any case, keep your code in a versioning system**. You are free to use github gitlab or whatever service you are more comfortable working with. The easiest way to work is to \"fork\" the lab repository from github into your own account and adding this new repository as a remote server.\n",
    "\n",
    "\n",
    "\n",
    "## Code production\n",
    "You are free to reuse code from the tutorials or any other source as long as you explicitely **cite its origin both in the code and in your report**. You are free to use any method from the pinocchio API and to create as many methods as you would like. **If you want to use non-native python libraries**, we must discuss this. Remember that I will have to run your code to assess your lab!\n",
    "\n",
    "\n",
    "## I don't like the approach you have proposed to solve the problem. Can I do my own thing?\n",
    "Yes... and no. First of all, I would suggest that you discuss this with the TAs / myself before going for it. Secondly, you will see in the submission requirements that I only need some methods to be implemented for me to assess quantitatively your work. I consider that if all of these methods are implemented you followed the instructions. The report will then give you a chance to justify your approach. This should give you a lot of freedom. In particular at step 2 I propose to use motion planning to compute a reference path for the robot. If you choose a different approach it does not matter to me as long as we discussed it before and that it is not hard-coded somehow. "
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6da0e38c",
   "metadata": {},
   "source": [
    "# Setting up the scene <a class=\"anchor\" id=\"scene\"></a>\n",
    "\n",
    "To ensure efficient use of resources and to prevent the unintentional creation of multiple Meshcat server instances, I recommend initializing the server through the command line. Here's how you can do it:\n",
    "\n",
    "- Open a terminal. If you're on Ubuntu, you can quickly do this by pressing `ctrl + alt + t`.\n",
    "- Enter the following command and press `Enter`:\n",
    "   ```\n",
    "   meshcat-server \n",
    "   ```\n",
    "- Upon running the command, you'll receive an output that includes the \"zmq_url\". This is the address to which you will connect.\n",
    "\n",
    "**Tip**: You can run terminal commands directly from the Jupyter Notebook by prefixing them with `!`. However, for the purpose of this lab, I recommend initializing the Meshcat server directly from the terminal.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "3cfdd871",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Port: 6000 in use, trying another...\n",
      "Port: 7000 in use, trying another...\n",
      "zmq_url=tcp://127.0.0.1:6001\n",
      "web_url=http://127.0.0.1:7001/static/\n",
      "^C\n"
     ]
    }
   ],
   "source": [
    "!meshcat-server"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "6cb4d8dc",
   "metadata": {},
   "source": [
    "A helper function has been implemented to automatically connect to meshcat server, load the scene and the robot, and setup the collision handler in pinocchio for you."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "60f7a783",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Wrapper tries to connect to server <tcp://127.0.0.1:6000>\n",
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7000/static/\n"
     ]
    }
   ],
   "source": [
    "from tools import setupwithmeshcat\n",
    "robot, cube, viz = setupwithmeshcat(url=\"tcp://127.0.0.1:6000\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5b84c85f",
   "metadata": {},
   "source": [
    "```setupwithmeshcat``` takes an optional parameter corresponding to the url (zmq_url) of the meshcat server (given as a string). If no url is provided, it uses the variable ```MESHCAT_URL``` defined in config.py . If for some reason the default url does not match the one you are using, you can either provide this url to ```setupwithmeshcat``` or simply replace the value of ```MESHCAT_URL```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "id": "5a3385f4",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "tcp://127.0.0.1:6000\n"
     ]
    }
   ],
   "source": [
    "from config import MESHCAT_URL\n",
    "print(MESHCAT_URL)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "58f37692",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7000/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "hasattr(viz.viewer, 'jupyter_cell') and viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f1784092",
   "metadata": {},
   "source": [
    "## Description of the environment.\n",
    "The environment is composed of a fixed table and an obstacle, as well as a cube, which you are supposed to bring \n",
    "to the green target.\n",
    "\n",
    "## Description of the robot. \n",
    "The robot you will use for the lab is the Nextage robot from Kawada industries.\n",
    "You can use both pinocchio and the [URDF files](https://github.com/ediaro23/lab/blob/9a2b0c74b4ba1702da8f86fc1fa845e513ab4cfd/models/nextagea_description/urdf/NextageaOpen.urdf) to check the dimension of the robot configuration space.\n",
    "The configuration velocity space has the same dimension as Nextage is only composed of revolute joints."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "id": "953a9d43",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Nb joints = 16 (nq=15,nv=15)\n",
      "  Joint 0 universe: parent=0\n",
      "  Joint 1 CHEST_JOINT0: parent=0\n",
      "  Joint 2 HEAD_JOINT0: parent=1\n",
      "  Joint 3 HEAD_JOINT1: parent=2\n",
      "  Joint 4 LARM_JOINT0: parent=1\n",
      "  Joint 5 LARM_JOINT1: parent=4\n",
      "  Joint 6 LARM_JOINT2: parent=5\n",
      "  Joint 7 LARM_JOINT3: parent=6\n",
      "  Joint 8 LARM_JOINT4: parent=7\n",
      "  Joint 9 LARM_JOINT5: parent=8\n",
      "  Joint 10 RARM_JOINT0: parent=1\n",
      "  Joint 11 RARM_JOINT1: parent=10\n",
      "  Joint 12 RARM_JOINT2: parent=11\n",
      "  Joint 13 RARM_JOINT3: parent=12\n",
      "  Joint 14 RARM_JOINT4: parent=13\n",
      "  Joint 15 RARM_JOINT5: parent=14\n",
      "\n"
     ]
    }
   ],
   "source": [
    "print(robot.model)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "019167b4",
   "metadata": {},
   "source": [
    "You can also verify that in its default configuration the robot is in collision (with the table)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "d35965de",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "from tools import collision\n",
    "collision(robot, robot.q0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8aa20b41",
   "metadata": {},
   "source": [
    "# Configuration and helper functions  <a class=\"anchor\" id=\"config\"></a>\n",
    "\n",
    "I have modified the URDF files to add frames that are relevant for the tasks you need to accomplish.\n",
    "On the robot, I have created fixed joints attached to the tip of each effectors, called ```LARM_EFF``` and ```RARM_EFF```. Note that because they are fixed joints, they do not appear in the robot model and do not change the dimension of the configuration space. These names are defined for you in the config.py file as well. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "47f46aee",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Left hand joint name:  LARM_EFF\n",
      "Left hand joint placement: \n",
      "  R =\n",
      "-3.67321e-06           -1            0\n",
      "           1 -3.67321e-06            0\n",
      "           0            0            1\n",
      "  p = 0.452  0.28 0.851\n",
      "\n"
     ]
    }
   ],
   "source": [
    "from config import LEFT_HAND, RIGHT_HAND\n",
    "\n",
    "print (\"Left hand joint name: \", LEFT_HAND)\n",
    "\n",
    "\n",
    "import pinocchio as pin\n",
    "q = robot.q0.copy()\n",
    "\n",
    "#update the frame positions in robot.data given q\n",
    "pin.framesForwardKinematics(robot.model,robot.data,q)\n",
    "\n",
    "#now let's print the placement attached to the right hand\n",
    "print (\"Left hand joint placement: \")\n",
    "pin.computeJointJacobians(robot.model,robot.data,q)\n",
    "frameid = robot.model.getFrameId(LEFT_HAND)\n",
    "oMframe = robot.data.oMf[frameid] \n",
    "print(oMframe)\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "31dd8e66",
   "metadata": {},
   "source": [
    "Likewise the cube urdf (models/cubes/cube_small.urdf) also contains helpers joint that set a target location for the effectors. They are called ```LEFT_HOOK``` and ```RIGHT_HOOK``` in config.py.\n",
    "\n",
    "I added meshcat helper functions that will allow you to display the associated frames if you wish."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "7ba2ce30",
   "metadata": {},
   "outputs": [],
   "source": [
    "from config import LEFT_HOOK, RIGHT_HOOK, CUBE_PLACEMENT, CUBE_PLACEMENT_TARGET\n",
    "\n",
    "from tools import getcubeplacement, setcubeplacement\n",
    "from setup_meshcat import updatevisuals\n",
    "\n",
    "#We can access the current cube position using\n",
    "oMcube  = getcubeplacement(cube) #origin of the cube\n",
    "oMcubeL = getcubeplacement(cube, LEFT_HOOK) #placement of the left hand hook\n",
    "oMcubeR = getcubeplacement(cube, RIGHT_HOOK) #placement of the right hand hook\n",
    "\n",
    "\n",
    "#the cube position is updated using the following function:\n",
    "setcubeplacement(robot, cube, CUBE_PLACEMENT)\n",
    "#to update the frames for both the robot and the cube you can call\n",
    "updatevisuals(viz, robot, cube, q)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "23849de9",
   "metadata": {},
   "source": [
    "# Part 1: Geometry <a class=\"anchor\" id=\"part1\"></a>\n",
    "In this first part of the lab, we are concerned with the geometry of the robot and in motion planning in general.\n",
    "We are going to compute a geometric path that will serve as a guide for the dynamic part. Concretely, your objective is to compute a collision free path for the robot such that it \"grasps\" the cube and carries it over to its target position. Again, no dynamic computations are required in this phase, we are only interested in configurations that should be collision free, respect joint limits and geometrically consistent in terms of placement; there is already some work to be done here!\n",
    "\n",
    "## I. Computing the target configurations (Inverse Geometry) <a class=\"anchor\" id=\"IG\"></a>\n",
    "\n",
    "Your first task is the following: write the functions that generate an initial and a goal configuration for the nextage robot, such that the ```LEFT_HAND``` and ```RIGHT_HAND```are aligned respectively with the ```LEFT_HOOK``` and ```RIGHT_HOOK``` frames when the cube is located at its starting position  (```CUBE_PLACEMENT```) and at its goal position (```CUBE_PLACEMENT_TARGET```)\n",
    "\n",
    "For this implement the method ```computeqgrasppose``` in inverse_geometry.py \n",
    "\n",
    "The main method indicates how the method is going to be called to obtain the q0 and qe configurations:\n",
    "```\n",
    "q0 = computeqgrasppose(robot, q, cube, CUBE_PLACEMENT, viz)\n",
    "qe = computeqgrasppose(robot, q, cube, CUBE_PLACEMENT_TARGET,  viz)\n",
    "```\n",
    "\n",
    "Of course, q0 and qe should be collision-free and respect the joint limits of the robot. Do not hard-code anything here. In my tests I will use different targets to test the generality of ```computeqgrasppose```.\n",
    "\n",
    "q0 should look somehow like this:\n",
    "<img src=\"./images/q0.png\" alt=\"drawing\" width=\"200\"/>\n",
    "\n",
    "\n",
    "\n",
    "**hints:** \n",
    "* If your configurations look unnatural, you probably want to somehow introduce a \"postural bias\" in whatever method you are using \n",
    "* From the obtained configurations, you can easily obtain the relative placement between both hands expressed in a specific frame. This might prove relevant later in the lab so you may want to store it somewhere (maybe in solution.py)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bfbf350c",
   "metadata": {},
   "source": [
    "## II. Motion planning <a class=\"anchor\" id=\"motion_planning\"></a>\n",
    "\n",
    "For section II, I am proposing one course of action to successfully achieve the planning of a motion such that the robot carries the cube from its initial configuration to the end configuration. You are free to choose another course of action, but I do not advise it unless you really know what you are doing. In any case talk to me before making that decision. No matter what approach you choose, the method ```computepath``` from path.py must be implemented as specified for you to get the points.\n",
    "\n",
    "To compute a geometric path that represents a collision-free motion of our robot carrying the cube from q0 to qe, I suggest to use a sampling based motion planner. \n",
    "\n",
    "We will use the path.py file to write the motion planning algorithm\n",
    "\n",
    "### II.a Sampling configurations\n",
    "To generate configurations for the planner, I suggest a 2 step process: randomly sample configurations for the cube, then solve an inverse geometry problem to generate a valid pose on that location. Of course, check that the joint limits are respected and the configuration is collision-free before returning it.\n",
    "\n",
    "**hint:** you are free to bound your problem by ensuring that the cube placements that you generate only occur at positions / orientations that you think are interesting.\n",
    "\n",
    "### II.b Path projection\n",
    "However to do this we need to enforce the constraint that every sampled configuration is such that the effectors are holding the cube.\n",
    "\n",
    "Furthermore, the grasping constraint will apply to the complete path: a standard linear interpolation between two grasping configurations is not enough to guarantee that every interpolated configuration is such that the cube is grasped.\n",
    "Write a function that, given two configurations q0 and q1 and a discretisation step, returns an interpolated path between q0 and q1 such that every configuration in the path corresponds to a grasping pose. If it is not possible to generate such path, it will return a flag indicating so and, depending on your own decision, either return the part of the path that is valid, or nothing\n",
    "\n",
    "### II.c Solution path generation\n",
    "With the methods produced in II.1 and II.2, you should now be able to implement a motion planner that generates a geometrically valid path between q0 and qe. You can probably reuse code from the motion planning tutorial here, or implement you own. \n",
    "\n",
    "**requirement:** For me to assess this part you are required to implement the method ```computepath``` according to its documentation"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "93d77cb5",
   "metadata": {},
   "source": [
    "# Part 2: Dynamics <a class=\"anchor\" id=\"part2\"></a>\n",
    "Now that you have computed a reference path for your robot, it is time to try controlling it in a dynamics simulator. We will use pybullet for this. In theory pybullet is already installed but if not you should install it with pip:\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "192009a7",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Defaulting to user installation because normal site-packages is not writeable\n",
      "Requirement already satisfied: pybullet in /home/stonneau/.local/lib/python3.8/site-packages (3.2.5)\n",
      "\u001b[33mDEPRECATION: distro-info 0.23ubuntu1 has a non-standard version number. pip 23.3 will enforce this behaviour change. A possible replacement is to upgrade to a newer version of distro-info or contact the author to suggest that they release a version with a conforming version number. Discussion can be found at https://github.com/pypa/pip/issues/12063\u001b[0m\u001b[33m\n",
      "\u001b[0m\u001b[33mDEPRECATION: python-debian 0.1.36ubuntu1 has a non-standard version number. pip 23.3 will enforce this behaviour change. A possible replacement is to upgrade to a newer version of python-debian or contact the author to suggest that they release a version with a conforming version number. Discussion can be found at https://github.com/pypa/pip/issues/12063\u001b[0m\u001b[33m\n",
      "\u001b[0m\n",
      "\u001b[1m[\u001b[0m\u001b[34;49mnotice\u001b[0m\u001b[1;39;49m]\u001b[0m\u001b[39;49m A new release of pip is available: \u001b[0m\u001b[31;49m23.2.1\u001b[0m\u001b[39;49m -> \u001b[0m\u001b[32;49m23.3.1\u001b[0m\n",
      "\u001b[1m[\u001b[0m\u001b[34;49mnotice\u001b[0m\u001b[1;39;49m]\u001b[0m\u001b[39;49m To update, run: \u001b[0m\u001b[32;49mpython3 -m pip install --upgrade pip\u001b[0m\n"
     ]
    }
   ],
   "source": [
    "!pip install pybullet"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b28c2532",
   "metadata": {},
   "source": [
    "\n",
    "This second part consists in two tasks. First, converting your path into a time-parametrised trajectory, then tracking this trajectory using the control law of your choice.\n",
    "\n",
    "Again, you are free to use any method that you want to both tasks. \n",
    "\n",
    "\n",
    "## Week 6 advice:\n",
    "We have not yet covered everything needed to fully achieve the lab below, though next week we will cover everything that is missing. I would suggest working in the following way for week 6:\n",
    "\n",
    "+ for 2.I, use the level 0 approach for now. We will cover trajectory optimisation in next week's lecture. If you are already familiar with this you can go for level 1 of course if you want to.\n",
    "\n",
    "+ For 2.II, you already have everything needed to write a torque controller for moving without grasping the cube. You can thus get started on the task 0 described in 2.II You will need this step anyway to move on to the grasping."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "07e18de4",
   "metadata": {},
   "source": [
    "## I. From a path to a trajectory <a class=\"anchor\" id=\"TO\"></a>\n",
    "The first step is to parametrise your path into a time-varying trajectory that you may want to track.\n",
    "Depending on your choice of a control law, this trajectory could be in the configuration space, the task space, or even something else.\n",
    "\n",
    "I do not provide a template for this step, you are free to use any method that you see fit, so if you don't want to be bothered you can simply manually parametrise your trajectory (which is the level 0 described thereafter). However, you can score bonus marks if you go with more advanced methods. This will be described in the coming marking criteria. \n",
    "\n",
    "\n",
    "### 1. level 0: Manually set a velocity profile\n",
    "Come up with a time parametrisation for your path that intuitively makes sense and move on to the control part! To do this write an interpolation function that given a certain $t$ will return a configuration in your path as well as the corresponding velocity and accelerations desired at that time. I advise to work out something that is such that the starting and final velocities are 0. \n",
    "\n",
    "### 2. level 1: QP programming without collision constraints (covered in next week's lecture)\n",
    "What I would recommend is to use quadratic programming to solve a least square problem that fits at best the path that you computed while satisfying constraints.\n",
    "\n",
    "The class Bezier defined in [bezier.py](https://github.com/ediaro23/lab/blob/main/bezier.py) might prove useful.\n",
    "Take a look at the [primer on Bezier curves](https://pomax.github.io/bezierinfo/) if you want more info on these methods. An example of use is provided in [control.py](https://github.com/ediaro23/lab/blob/9a2b0c74b4ba1702da8f86fc1fa845e513ab4cfd/control.py#L47).\n",
    "\n",
    "The motivations for using Bezier curves are multiple:\n",
    "\n",
    "+ First, they are strictly equivalent to polynomials, which means that the trajectory you will compute will be continuous and infinitely differentiable.\n",
    "\n",
    "+ Secondly, the initial and terminal conditions are easy to specify: the velocity at the start / end only depends on the first/last two control points, and the accelerations only on the the first/last three control points. If you choose your first three control points to be strictly equal for instance, the initial velocity and acceleration of your trajectory will be 0.\n",
    "\n",
    "+ Thirdly, a Bezier curve lies completely in the convex hull of its control points. What this means for optimisation is that if you define linear inequality constraints on the control points of the curve, you have the guarantee that that every point on the curve satisfies these constraints. This will allow you to easily specify constraints on the derivatives\n",
    "\n",
    "If you choose to go that way, you can decide that your optimisation variables are the control points of your Bezier curve; you can then linearly define the value of the trajectory at each time step as a linear combination of these control points and write your cost function as a function of these points.\n",
    "\n",
    "\n",
    "In this approach, the idea is not to explicitely address the collision constraints. You will assume that if you track your path well enough you will avoid collisions. This is commonly done in robotics.\n",
    "\n",
    "**Optional: the ndcurves library**\n",
    "\n",
    "If you are interested in trajectory optimisation, you could check out [this tutorial I wrote for the ndcurves library](https://github.com/loco-3d/ndcurves/blob/master/python/test/sandbox/test.ipynb). While the API is different the concepts described are the same. **Note that I m not necessarily suggesting to use ndcurves, simply to look at the tutorials to understand the ideas and replicate this.** Still, if you want to you can decide to directly use ndcurves. It has an api a bit more complex that the simple Bezier class I provided as it is more powerful, so it is for you to decide, there is no good or bad solution here.\n",
    "\n",
    "\n",
    "\n",
    "#### Time parametrisation:\n",
    "If you follow the guidelines from the tutorial, the optimisation will give you a smooth trajectory, but by default it will have a duration of 1. You can easily add velocity constraints and include time as a variable in your problem . You could also add acceleration constraints and ignore velocities by using $t^2$ as a variable instead of $t$. We can discuss this on Piazza. You can't consider both velocity and acceleration constraints otherwise your problem will become non-linear (do you see why ?)\n",
    "\n",
    "#### Handling grasping constraints:\n",
    "If you are planning a trajectory just for the effectors then it is really easy to handle these constraints: you can just plan for the cube and then deduce the effector placements at each step.\n",
    "It might be harder to do it for a configuration space trajectory, but it is not necessarily required:\n",
    "I believe that if the tracked trajectory is good enough you should not need to handle the grasping constraints as accurately as this will be fixed by the control law. \n",
    "If you want to handle these constraints, there are a variety of post processes that you can choose to implement this, but though the problem becomes non-linear again... I suggest to avoid this unless it really proves useful (again I think not, or at least it was not necessary for me).\n",
    "\n",
    "\n",
    "\n",
    "### 3. level 2: Handling collision constraints\n",
    "First of all, don't go there in the first instance. Wait until you are done with the complete lab to decide whether you are interested in doing this. You can try to handle collisions in a variety of ways that we can discuss on Piazza. The straightforward approach is to write a non linear program that will handle this as we have done before. Because you have a reference motion that is collision free, you may get away with a NLP to refine the trajectory obtained when solving level 1 so as to avoid collisions."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b5edb285",
   "metadata": {},
   "source": [
    "## II. Implementing a torque control law <a class=\"anchor\" id=\"control\"></a>\n",
    "Ok now, you have everything that you need to grasp that box and move it to its target.\n",
    "\n",
    "Again, we are going to use pybullet to control our robot. Pybullet is a dynamics simulator which is used by roboticists to test the behaviours of their controllers before deploying them on their robots. It was originally developed for video games, and depending on your application it is not always considered as very accurate. However for manipulators it works well.\n",
    "\n",
    "### Interaction with Pybullet \n",
    "\n",
    "The philosophy I propose to interact with Pybullet is implemented in the [control.py main function](https://github.com/ediaro23/lab/blob/9a2b0c74b4ba1702da8f86fc1fa845e513ab4cfd/control.py#L61):\n",
    "\n",
    "+ Set the initial configuration of your robot in pybullet (method setqsim)\n",
    "\n",
    "+ At each frame:\n",
    "\n",
    "    - get the current state of the robot in the simulator (method getpybulletstate)\n",
    "    \n",
    "    - work out all the computations in pinocchio\n",
    "    \n",
    "    - send a torque command to the simulator to update the state of the robot (method step)\n",
    "\n",
    "Pybullet is configured here to work as a step-by-step simulation, as opposed to real time. Everytime you call the step method, the simulation integrates DT seconds of time, with DT defined in config.py.\n",
    "\n",
    "In tools.py, I have added a method [rununtil](https://github.com/ediaro23/lab/blob/9a2b0c74b4ba1702da8f86fc1fa845e513ab4cfd/tools.py#L102C5-L102C10) that you can use to update the simulation at a frequency that corresponds to a realtime mode.\n",
    "\n",
    "If you run control.py, assuming the initial state is the robot.q0 configuration, the first thing you will see is a very chaotic behaviour from the robot: indeed, as it starts in collision, the simulator will apply really high forces to compense the penetration violation.\n",
    "\n",
    "If you initialise correctly the inital state, in the absence of torque command the robot arms will fall under the effect of gravity.\n",
    "\n",
    "The main methods needed to work with pybullet are defined in the Simulation class defined in [setup_pybullet.py](https://github.com/ediaro23/lab/blob/9a2b0c74b4ba1702da8f86fc1fa845e513ab4cfd/setup_pybullet.py). A few others are defined in the base class Simulation_base but I don't expect you will need them.\n",
    "\n",
    "The helper method to load the robot in both pinocchio and pybullet environments is setupwithpybullet. If you still want to work with meshcast in parallel (for some strange reason that I can't explain I found it convenient) you can instead call setupwithpybulletandmeshcat.\n",
    "\n",
    "\n",
    "### Optional task 0: Control without the cube\n",
    "To test your controller, it might be a good idea to first control the motion of the effectors without grasping the cube. Generate a simple trajectory that brings the effectors above the starting position and implement the control law to achieve the motion. Regarding the gains, you definitely have the options to tune them individually for each joint (and you would have do this on the actual robot). When I programmed the lab I just used Kp = 300 and $Kd = 2 \\sqrt(Kd)$ for all joints and it worked fine. Once you ll have verified that you obtain a satisfying behaviour, you'll be ready to move to the actual task.\n",
    "\n",
    "### The actual task\n",
    "Proceed as you wish to have the robot grasp the cube and then bring it to the target location. It does not matter to me how this is achieved in terms of the motion I see, as long as some grasping (ie the robot is holding the cube above the table for some relevant period of time) happens. You can push the cube to align it in the end if you need. \n",
    "\n",
    "To achieve the behaviour, you will need to apply a control law. Everything here is set for you to control the robot in torque, which I do believe is the easy way to go. If you want to try controling this using position or velocity control, this is also an option (actually the real Nextage robot is only position controlled).\n",
    "\n",
    "The recommended way, in my opinion, is to apply inverse dynamics control to track the trajectory while applying a linear force to the cube with both hands.\n",
    "\n",
    "This force control law should naturally cope with the alignment errors and have the effect of attracting the effectors to the cube \n",
    "\n"
   ]
  }
 ],
 "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}