2_inv_geom.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Direct and inverse geometry of 3d robots\n",
    "This notebook introduces the kinematic tree of Pinocchio for a serial manipulator, explain how to compute the forward and inverse geometry (from configuration to end-effector placements, and inversely). The ideas are examplified with a simplified case-study taken from parallel robotics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "NB: as for all the tutorials, a magic command %do_not_load is introduced to hide the solutions to some questions. Change it for %load if you want to see (and execute) the solution.\n"
     ]
    }
   ],
   "source": [
    "import magic_donotload  # noqa: F401"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up\n",
    "We will need Pinocchio, Gepetto-Viewer, SciPy for the solvers"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "import math\n",
    "import numpy as np\n",
    "from numpy.linalg import norm\n",
    "import pinocchio as pin\n",
    "import example_robot_data as robex\n",
    "from scipy.optimize import fmin_bfgs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Kinematic tree in Pinocchio\n",
    "Let's now play with 3D robots. We will load the models from URDF files.\n",
    "\n",
    "*The robot UR5* is a low-cost manipulator robot with good performances. It is a fixed robot with one 6-DOF arms developed by the Danish company Universal Robot. All its 6 joints are revolute joints. Its configuration is in R^6 and is not subject to any constraint. The model of UR5 is described in a URDF file, with the visuals of the bodies of the robot being described as meshed (i.e. polygon soups) using the Collada format \".dae\". Both the URDF and the DAE files are available in the repository in the model directory. \n",
    "\n",
    "This robot model, as well as other models used in the notebooks, are installed from the apt paquet robotpkg-example-robot-data and stored in /opt/openrobots/share/example-robot-data.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = robex.load('ur5')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The kinematic tree is represented by two C++ objects called Model (which contains the model constants: lengths, masses, names, etc) and Data (which contains the working memory used by the model algorithms). Both C\\++ objects are contained in a unique Python class. The first class is called RobotWrapper and is generic."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Nb joints = 7 (nq=6,nv=6)\n",
      "  Joint 0 universe: parent=0\n",
      "  Joint 1 shoulder_pan_joint: parent=0\n",
      "  Joint 2 shoulder_lift_joint: parent=1\n",
      "  Joint 3 elbow_joint: parent=2\n",
      "  Joint 4 wrist_1_joint: parent=3\n",
      "  Joint 5 wrist_2_joint: parent=4\n",
      "  Joint 6 wrist_3_joint: parent=5\n",
      "\n"
     ]
    }
   ],
   "source": [
    "print(robot.model)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For the next steps, we are going to work with the RobotWrapper.\n",
    "\n",
    "Import the class RobotWrapper and create an instance of this class in the python terminal. At initialization, RobotWrapper will read the model description in the URDF file given as argument. In the following, we will use the model of the UR5 robot, available in the directory \"models\" of pinocchio (available in the homedir of the VBox). The code of the RobotWrapper class is in /opt/openrobots/lib/python2.7/site-packages/pinocchio/robot_wrapper.py . Do not hesitate to have a look at it and to take inspiration from the implementation of the class functions.\n",
    "\n",
    "Here are some import methods of the class.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.q0 contains a reference initial configuration of the robot (not a pretty good one for the UR-5)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.index('joint name') returns the index of the joint."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "7"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.index(' wrist_3_joint')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.model.names is a container (~list) that contains all the joint names"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0 universe\n",
      "1 shoulder_pan_joint\n",
      "2 shoulder_lift_joint\n",
      "3 elbow_joint\n",
      "4 wrist_1_joint\n",
      "5 wrist_2_joint\n",
      "6 wrist_3_joint\n"
     ]
    }
   ],
   "source": [
    "for i, n in enumerate(robot.model.names):\n",
    "    print(i, n)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* robot.model.frames contains all the import frames attached to the robot. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "universe attached to joint # 0\n",
      "root_joint attached to joint # 0\n",
      "world attached to joint # 0\n",
      "world_joint attached to joint # 0\n",
      "base_link attached to joint # 0\n",
      "base_link-base_fixed_joint attached to joint # 0\n",
      "base attached to joint # 0\n",
      "shoulder_pan_joint attached to joint # 1\n",
      "shoulder_link attached to joint # 1\n",
      "shoulder_lift_joint attached to joint # 2\n",
      "upper_arm_link attached to joint # 2\n",
      "elbow_joint attached to joint # 3\n",
      "forearm_link attached to joint # 3\n",
      "wrist_1_joint attached to joint # 4\n",
      "wrist_1_link attached to joint # 4\n",
      "wrist_2_joint attached to joint # 5\n",
      "wrist_2_link attached to joint # 5\n",
      "wrist_3_joint attached to joint # 6\n",
      "wrist_3_link attached to joint # 6\n",
      "ee_fixed_joint attached to joint # 6\n",
      "ee_link attached to joint # 6\n",
      "wrist_3_link-tool0_fixed_joint attached to joint # 6\n",
      "tool0 attached to joint # 6\n"
     ]
    }
   ],
   "source": [
    "for f in robot.model.frames:\n",
    "    print(f.name, 'attached to joint #', f.parent)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "robot.placement(idx) and robot.framePlacement(idx) returns the placement (i.e. translation+rotation of the joint / frame in argument."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "a = robot.placement(robot.q0, 6)  # Placement of the end effector joint.\n",
    "b = robot.framePlacement(robot.q0, 22)  # Placement of the end effector tip.\n",
    "\n",
    "tool_axis = b.rotation[:, 2]  # Axis of the tool"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The dimension of the configuration space (i.e. the number of joints) is given in:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "NQ = robot.model.nq\n",
    "NV = robot.model.nv  # for this simple robot, NV == NQ"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Display simple geometries\n",
    "The robot is displayed in the viewer. We are going to use Meshcat to visualize the 3d robot and scene. First open the viewer and load the robot geometries."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7003/static/\n"
     ]
    }
   ],
   "source": [
    "from utils.meshcat_viewer_wrapper import MeshcatVisualizer, colors  # noqa: E402\n",
    "\n",
    "viz = MeshcatVisualizer(robot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "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:7003/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A configuration *q* can be displayed in the viewer:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = np.array([-1., -1.5, 2.1, -.5, -.5, 0])\n",
    "\n",
    "viz.display(q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Other geometries (cubes, spheres, etc) can be displayed as well."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add a red box in the viewer\n",
    "ballID = \"world/ball\"\n",
    "radius = 0.1\n",
    "viz.addSphere(ballID, radius, colors.red)\n",
    "\n",
    "# Place the ball at the position ( 0.5, 0.1, 0.2 )\n",
    "# The viewer expect position and rotation, apppend the identity quaternion\n",
    "ball_position = [0.5, 0.1, 0.2]\n",
    "q_ball = ball_position +  [1, 0, 0, 0]\n",
    "viz.applyConfiguration(ballID, q_ball)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Pick and place 3D"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Inverse geometry in 3D for pick position...\n",
    "\n",
    "We will use inverse geometry method to find a configuration of the robot so that the end effector touches the ball. We will use an unconstrained optimization method.\n",
    "\n",
    "We can use the  the scipy solver [used in the previous notebook](1_geometry_2d.ipynb#section_optim), compute a configuration q where the end effector reaches p.\n",
    "\n",
    "For that, implement a cost function that takes a configuration as argument and returns the squared distance between the end effetor tip and the target accounting for the fact we want to touch the ball. Here you can ignore the collision."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "target = np.array(ball_position)  # x,y,z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "q_touch = robot.q0  # REPLACE WITH INVERSE GEOMETRY PROCEDURE"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is a solution if you need it"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Optimization terminated successfully.\n",
      "         Current function value: 0.000000\n",
      "         Iterations: 29\n",
      "         Function evaluations: 224\n",
      "         Gradient evaluations: 32\n"
     ]
    }
   ],
   "source": [
    "# %load tp2/generated/invgeom3d_1\n",
    "def cost(q):\n",
    "    '''Compute score from a configuration'''\n",
    "    m = robot.framePlacement(q, 22)\n",
    "    p = m.translation\n",
    "    offset = m.rotation[:, 2] * radius\n",
    "    return norm(p +  offset - target)**2\n",
    "\n",
    "\n",
    "def callback(q):\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)\n",
    "\n",
    "q_touch = fmin_bfgs(cost, robot.q0, callback=callback)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Take care to explicitely mention copy when you want a copy of array.\n",
    "q = q_touch.copy()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### ...and simulation of place movement"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At the reference position you built, the end effector placement can be obtained by calling "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 0.56595833, -0.00816157,  0.06891268])"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.placement(q, 6).translation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Only the translation part of the placement has been selected. The rotation is free.\n",
    "\n",
    "Now, choose any trajectory you want in the configuration space (it can be sinus-cosinus waves, polynomials, splines, straight lines). Make a for loop to display the robot at sampling positions along this trajectory. The function sleep can be used to slow down the loop."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "At each instant of your loop, recompute the position of the ball and display it so that it always \"sticks\" to the robot end effector, by modifying the template code below:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = q_touch.copy()\n",
    "vq = np.array([2., 0, 0, 4., 0, 0])\n",
    "idx = 6\n",
    "\n",
    "# TODO: Compute the placement of the ball in the end effector frame\n",
    "eff_eff_ball = np.zeros(3)  # Position of ball wrt eff in local coordinate\n",
    "\n",
    "for i in range(200):\n",
    "    # Chose new configuration of the robot\n",
    "    q += vq / 40\n",
    "    q[2] = 1.71 + math.sin(i * 0.05) / 2\n",
    "\n",
    "    # TODO: Compute the new placement of the ball\n",
    "    o_ball = np.zeros(3)\n",
    "\n",
    "    # Display new configuration for robot and ball\n",
    "    viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0])\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The solution is below, should you need it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp2/generated/simple_pick_and_place_4\n",
    "q = q_touch.copy()\n",
    "vq = np.array([2., 0, 0, 4., 0, 0])\n",
    "idx = 6\n",
    "\n",
    "p = robot.placement(q, idx)\n",
    "o_eff = p.translation  # Position of end-eff wrt world at current configuration\n",
    "o_ball = q_ball[:3]  # Position of ball wrt world\n",
    "o_eff_ball = o_ball - o_eff  # Position of ball wrt eff in world coordinate\n",
    "eff_eff_ball = p.rotation.T @ o_eff_ball  # Position of ball wrt eff in local coordinate\n",
    "\n",
    "for i in range(200):\n",
    "    # Chose new configuration of the robot\n",
    "    q += vq / 40\n",
    "    q[2] = 1.71 + math.sin(i * 0.05) / 2\n",
    "\n",
    "    # Gets the new position of the ball\n",
    "    o_ball = robot.placement(q, idx) * eff_eff_ball\n",
    "\n",
    "    # Display new configuration for robot and ball\n",
    "    viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0])\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Pick and place 6D\n",
    "\n",
    "Say now that the object is a rectangle and not a sphere. Pick the object at a reference position with the rotation that is imposed, so that the end effector is aligned with one of the faces of the rectangle.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Add a red box in the viewer\n",
    "boxID = \"world/box\"\n",
    "try:\n",
    "    viz.delete(ballID)\n",
    "except:\n",
    "    pass\n",
    "viz.addBox(boxID, [0.1, 0.2, 0.1], colors.magenta)\n",
    "\n",
    "# Place the box at the position (0.5, 0.1, 0.2)\n",
    "q_box = [0.5, 0.1, 0.2, 1, 0, 0, 0]\n",
    "viz.applyConfiguration(boxID, q_box)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Redo the same two questions as before, but now also choosing the orientation of the box. For that, at each robot configuration in your for-loop, compute the box placement wrt the world (let's denote it by oMbox) and display both the box and the robot configuration in the view."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Inverse geometry in 6D\n",
    "6D means: translation and rotation. Change the previous cost function for a cost measuring the difference between the current placement root.placement(q,6) and a reference placement oMdes. \n",
    "For that, you can use the SE(3) log function to score the distance between two placements. The log returns a 6D velocity, represented by a class Motion, that must be transformed to a vector of R^6 from which you can take the norm.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "oMtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([-0.5, 0.1, 0.2]))  # x,y,z\n",
    "Moffset = pin.XYZQUATToSE3([0., 0., 0.05, 1, 0, 0, 0])  # Offset to make the tip touch the edge\n",
    "oMbox = oMtarget * Moffset"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The robot finally reached effector placement at\n",
      "   R =\n",
      "          -1            0  9.79328e-12\n",
      "           0            1            0\n",
      "-9.79328e-12            0           -1\n",
      "  p =   0.81725   0.10915 -0.005491\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# TODO: place the box in target\n",
    "\n",
    "qopt = robot.q0  # Replace with optimization procedure\n",
    "\n",
    "print('The robot finally reached effector placement at\\n', robot.placement(qopt, 6))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The solution if you need it"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Warning: Desired error not necessarily achieved due to precision loss.\n",
      "         Current function value: 0.000000\n",
      "         Iterations: 61\n",
      "         Function evaluations: 831\n",
      "         Gradient evaluations: 117\n",
      "The robot finally reached effector placement at\n",
      "   R =\n",
      "           1 -1.72203e-09 -9.34074e-09\n",
      " -7.8247e-09    -0.706825    -0.707388\n",
      "-5.38413e-09     0.707388    -0.706825\n",
      "  p =     -0.5 0.158172 0.141782\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# %load tp2/generated/invgeom6d_1\n",
    "viz.applyConfiguration(boxID, oMbox)\n",
    "\n",
    "def cost(q):\n",
    "    '''Compute score from a configuration'''\n",
    "    M = robot.framePlacement(q, 22)\n",
    "    return norm(pin.log(M.inverse() * oMtarget).vector)\n",
    "\n",
    "\n",
    "def callback(q):\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)\n",
    "\n",
    "\n",
    "qopt = fmin_bfgs(cost, robot.q0, callback=callback)\n",
    "\n",
    "print('The robot finally reached effector placement at\\n', robot.placement(qopt, 6))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now move the box following the motion."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = qopt.copy()\n",
    "vq = np.array([2., 0, 0, 4., 0, 0])\n",
    "idx = 6\n",
    "\n",
    "for i in range(100):\n",
    "    # Chose new configuration of the robot\n",
    "    q += vq / 40\n",
    "    q[2] = 1.71 + math.sin(i * 0.05) / 2\n",
    "\n",
    "    # TODO: replace with good calculation\n",
    "    oMbox = oMbox\n",
    "\n",
    "    # Display new configuration for robot and box\n",
    "    viz.applyConfiguration(boxID, oMbox)\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load tp2/generated/simple_pick_and_place_7\n",
    "q = qopt.copy()\n",
    "vq = np.array([2., 0, 0, 4., 0, 0])\n",
    "idx = 6\n",
    "\n",
    "oMeff = robot.placement(q, idx)  # Placement of end-eff wrt world at current configuration\n",
    "effMbox = oMeff.inverse() * oMbox  # Placement of box     wrt eff\n",
    "\n",
    "for i in range(100):\n",
    "    # Chose new configuration of the robot\n",
    "    q += vq / 40\n",
    "    q[2] = 1.71 + math.sin(i * 0.05) / 2\n",
    "\n",
    "    # Gets the new position of the box\n",
    "    oMbox = robot.placement(q, idx) * effMbox\n",
    "\n",
    "    # Display new configuration for robot and box\n",
    "    viz.applyConfiguration(boxID, oMbox)\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## On inverse geometry while taking collisions into account"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As we have seen in the first TP, we can use constrain optimization solver and create a constraint related to the distance between object being positive. Under the hood, it use the distance computation and finite difference pattern to estimate the gradient and use it in the optimization procedure. If it worked well with the UR5 constrained to a plan with 2 degree of freedom, in the general case those approach get stuck. Recent advances on perturbed optimization allows to have better gradient for the collision function and nicely use it in optimization routine. [random smooth collision gradient](https://hal.archives-ouvertes.fr/hal-03780482/file/Differentiable_collision_detection.pdf)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Optimizing in the quaternion space\n",
    "\n",
    "Let's now work with a floating robot: the quadruped ANYmal. This robot has 12 joints, but Q-space of size 19 (robot.model.nq) and Q-tangent space of size 18 (robot.model.nv). This is because with need 7D vector to encode the robot placement in space, which indeed to only 6 DOF.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7004/static/\n"
     ]
    },
    {
     "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:7004/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 27,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot = robex.load('solo12')\n",
    "viz = MeshcatVisualizer(robot)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(robot.q0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "Run the following code. Can you explain what just happened? Then correct it to have a proper optimization of ANYmal configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Optimization terminated successfully.\n",
      "         Current function value: 0.000000\n",
      "         Iterations: 46\n",
      "         Function evaluations: 1060\n",
      "         Gradient evaluations: 53\n"
     ]
    }
   ],
   "source": [
    "\n",
    "robot.feetIndexes = [robot.model.getFrameId(frameName) for frameName in ['HR_FOOT', 'HL_FOOT', 'FR_FOOT', 'FL_FOOT']]\n",
    "\n",
    "# --- Add box to represent target\n",
    "colors = ['red', 'blue', 'green', 'magenta']\n",
    "for color in colors:\n",
    "    viz.addSphere(\"world/%s\" % color, .05, color)\n",
    "    viz.addSphere(\"world/%s_des\" % color, .05, color)\n",
    "\n",
    "#\n",
    "# OPTIM 6D #########################################################\n",
    "#\n",
    "\n",
    "targets = [\n",
    "    np.array([-0.7, -0.2, 1.2]),\n",
    "    np.array([-0.3, 0.5, 0.8]),\n",
    "    np.array([0.3, 0.1, -0.1]),\n",
    "    np.array([0.9, 0.9, 0.5])\n",
    "]\n",
    "for i in range(4):\n",
    "    targets[i][2] += 1\n",
    "\n",
    "\n",
    "def cost(q):\n",
    "    '''Compute score from a configuration'''\n",
    "    cost = 0.\n",
    "    for i in range(4):\n",
    "        p_i = robot.framePlacement(q, robot.feetIndexes[i]).translation\n",
    "        cost += norm(p_i - targets[i])**2\n",
    "    return cost\n",
    "\n",
    "\n",
    "def callback(q):\n",
    "    viz.applyConfiguration('world/box', Mtarget)\n",
    "\n",
    "    for i in range(4):\n",
    "        p_i = robot.framePlacement(q, robot.feetIndexes[i])\n",
    "        viz.applyConfiguration('world/%s' % colors[i], p_i)\n",
    "        viz.applyConfiguration('world/%s_des' % colors[i], list(targets[i]) + [1, 0, 0, 0])\n",
    "\n",
    "    viz.display(q)\n",
    "    time.sleep(1e-2)\n",
    "\n",
    "\n",
    "Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2]))  # x,y,z\n",
    "qopt = fmin_bfgs(cost, robot.q0, callback=callback)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Configuration of parallel robots\n",
    "A parallel robot is composed of several kinematic chains (called the robot legs) that are all attached to the same end effector. This imposes strict constraints in the configuration space of the robot: a configuration is valide iff all the legs meets the same end-effector placement. We consider here only the geometry aspect of parallel robots (additionnally, some joints are not actuated, which causes additional problems).\n",
    "\n",
    "The kinematic structure of a paralel robot indeed induces loops in the joint connection graph. In Pinocchio, we can only represents (one of) the underlying kinematic tree. The loop constraints have to be handled separately. An example that loads 4 manipulator arms is given below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "from utils. load_ur5_parallel import load_ur5_parallel  # noqa: E402\n",
    "\n",
    "robot = load_ur5_parallel()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7005/static/\n"
     ]
    },
    {
     "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:7005/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 31,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "viz = MeshcatVisualizer(robot)\n",
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(robot.q0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [],
   "source": [
    "[w, h, d] = [0.5, 0.5, 0.005]\n",
    "color = [red, green, blue, transparency] = [1, 1, 0.78, .8]\n",
    "viz.addBox('world/robot0/toolplate', [w, h, d], color)\n",
    "Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75]))\n",
    "viz.applyConfiguration('world/robot0/toolplate', Mtool)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The 4 legs of the robot are loaded in a single robot model. The 4 effector placements are computed by:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "  R =\n",
       "   -0.707107    -0.707107 -3.46245e-12\n",
       "    0.707107    -0.707107 -3.46236e-12\n",
       "-6.12323e-17 -4.89664e-12            1\n",
       "  p = 0.306122 0.160483 0.749342"
      ]
     },
     "execution_count": 34,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "effIdxs = [robot.model.getFrameId('tool0_#%d' % i) for i in range(4)]\n",
    "robot.framePlacement(robot.q0, effIdxs[0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The loop constraints are that the relative placement of every leg end-effector must stay the same that in the initial configuration given as example in with the configuration *robot.q0* and the plate placement *Mtool*. To be valid, a configuration *q* must satisfy these 4 relative placement constraints.\n",
    "\n",
    "Consider now that the orientation of the tool plate is given by the following quaternion, with the translation that you like (see [the notebook about rotations if you need more details](appendix1_quaternions.ipynb)): \n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.13978495 -0.8172043   0.55913978]\n",
      " [ 0.98924731  0.13978495 -0.04301075]\n",
      " [-0.04301075  0.55913978  0.82795699]]\n"
     ]
    }
   ],
   "source": [
    "quat = pin.Quaternion(0.7, 0.2, 0.2, 0.6).normalized()\n",
    "print(quat.matrix())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "- **Apply the rotation defined using the previous quaternion on the plate**\n",
    "- **Find using the above optimization routines the configuration of each robot leg so that the loop constraints are all met** for the new orientation of the plate"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [],
   "source": [
    "# YOUR CODE"
   ]
  }
 ],
 "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.15"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}