simulation/sim1_rigid_body/1_geometry_and_dynamics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![](./logo-pinocchio.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Pinocchio: rigib-body algorithms\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import magic_donotload"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up\n",
    "We will need Pinocchio, the robot models stored in the package `example-robot-data`, a viewer (either GepettoViewer or MeshCat), some basic linear-algebra operators and the SciPy optimizers."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pinocchio as pin\n",
    "import example_robot_data as robex\n",
    "import numpy as np\n",
    "from numpy.linalg import inv, pinv, eig, norm, svd, det\n",
    "from scipy.optimize import fmin_bfgs\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1. Load and display the robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Pinocchio is a library to compute different quantities related to the robot model, like body positions, inertias, gravity or dynamic effects, joint jacobians, etc. For that, we need first to define the robot model. The easiest solution is to load it from a [URDF model](https://ocw.tudelft.nl/course-lectures/2-2-1-introduction-to-urdf/). This can be done with the function `buildModelFromUrdf`.\n",
    "\n",
    "The package `example-robot-data` proposes to directly load several URDF models with a single line of code. This is what we are going to use for the tutorial. Look at the code inside the package if you want to adapt it to the URDF model of your favorite robot.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = robex.load(\"talos_arm\")  # Load a 6-dof manipulator arm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The robot can be visualized in a viewer. Two viewers are proposed in this tutorial: Gepetto-Viewer and MeshCat. Both can display the robot model in an external windows managed by another processus.\n",
    "- MeshCat is browser based, which makes it very convenient in the jupyter context, as it can also be embedded inside the notebook.\n",
    "- Gepetto-Viewer is the historical viewer for Gepetto, and is more powerful. But you need to first run the command `gepetto-gui` in a terminal, before starting your python command."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "Viewer = pin.visualize.MeshcatVisualizer\n",
    "\n",
    "viz = Viewer(robot.model, robot.collision_model, robot.visual_model)\n",
    "viz.initViewer(loadModel=True)\n",
    "viz.display(robot.q0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If chosing MeshCat, you can open the display directly inside the notebook."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "hasattr(viz.viewer, 'jupyter_cell') and viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In both viewers, additional simple geometries can be added to enhance the visualization, although the syntax of the two viewers is not unified. For this tutorial, we provide a simple wrap-up for 3 methods to display a sphere, a box and place them in the scene. Check out the `vizutils.py` script if you are curious of the syntax, and would like to add more fancy primitives to your scene."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import vizutils\n",
    "vizutils.addViewerBox(viz, 'world/box', .05, .1, .2, [1., .2, .2, .5])\n",
    "vizutils.addViewerSphere(viz,'world/ball', .05, [.2, .2, 1., .5])\n",
    "vizutils.applyViewerConfiguration(viz, 'world/box', [0.5, -.2, .2, 1, 0, 0, 0])\n",
    "vizutils.applyViewerConfiguration(viz, 'world/ball', [0.5, .2, .2, 1, 0, 0, 0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-block alert-info\">\n",
    "    <img src=\"recap.png\" title=\"Recap\"/>\n",
    "    <h3>Recap of the main syntax elements exposed in this section</h3>\n",
    "    <ul>\n",
    "        <li><code>robex.loadXXX</code> loads the model of the robot XXX\n",
    "        <li><code>vizutils.applyViewerConfiguration(name, xyzquat_placement)</code> change the placement of the geometry named <code>name</code> in the viewer.\n",
    "     </ul>\n",
    "\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Pinocchio's philosophy (model, data and algorithms)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Model vs data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Pinocchio is not extensively using the object-oriented design pattern. We rather regroup the informations and buffers in some important data structure, then access to these structures using algorithms written in static functions.\n",
    "\n",
    "The two main data structures are `robot.model` and `robot.data`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rmodel = robot.model\n",
    "rdata = rmodel.createData()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The `robot.model` structure contains all the *constant* information that the algorithms need to process. It is typically loaded from a file describing the model (URDF). The `robot.model` is typically constant and is not modified by algorithms.\n",
    "\n",
    "The `robot.data` structure contains the memory buffers that the algorithms needs to store intermediary values or the final results to return to the user. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Joints names and indexes"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can get the list of the joint names with the following:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for n in rmodel.names:\n",
    "    print(n)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In what follows, we will specifically use the joint named `gripper_left_joint` (nothing specific, you can change it if you like). Its index can be obtained with:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "joint_index = rmodel.getJointId(\"gripper_left_joint\") # joint_index of the end effector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### A first algorithm: random configuration"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's take a first example of algorithm. You can pick a random configuration by calling the algorithm `randomConfiguration`. This algorithm just needs the `robot.model` (as no intermediary buffer are needed)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = pin.randomConfiguration(rmodel)\n",
    "print(\"q: \",q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### A second algorithm: forward kinematics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Another example is the algorithm to compute the foward kinematics. It recursively computes the placement of all the joint frames of the kinematic tree, and stores the results in `robot.data.oMj`, which is an array indexed by the joint indexes."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pin.forwardKinematics(rmodel, rdata, q)\n",
    "rdata.oMi[joint_index]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-block alert-info\">\n",
    "    <img src=\"recap.png\" title=\"Recap\"/>\n",
    "    <h3>Recap of the main syntax elements exposed in this section</h3>\n",
    "    <ul>\n",
    "        <li><code>robot.model</code> and <code>robot.data</code> structures\n",
    "        <li><code>pin.randomConfiguration(robot.model)</code> to sample a random configuration\n",
    "        <li><code>pin.forwardKinematics(robot.model, robot.data, q)</code> to compute joint frame placements.\n",
    "        <li><code>robot.data.oMi[jointIdex]</code> to access a particular joint frame.\n",
    "     </ul>\n",
    "\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. 3d cost: optimizing the end effector position"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will now define a first cost function, that penalizes the distance between the robot gripper and a target.\n",
    "For that, let's define a target position `ptarget` of dimension 3. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ptarget = np.array([.5, .1, .3])\n",
    "print(\"ptarget: \", ptarget)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Joints and frames\n",
    "As explained upper, the position of the joint `joint_index` is stored in `robot.data.oMi[joint_index].translation`, and is recomputed by `pin.forwardKinematics`. \n",
    "\n",
    "In Pinocchio, each joint is defined by its joint frame, whose **placement** (*i.e.* position & orientation) is stored in `robot.data.oMi`. \n",
    "\n",
    "We also defined in addition other *operational* frames. They are defined by a fixed placement with respect to their parent joint frame. Denoting by `oMi` the placement of the parent joint frame (function of `q`), and by `iMf` the fixed placement of the operational frame with respect to the parent joint frame, the placement of the operational frame with respect to the world is easily computed by `oMf(q) = oMi(q) * iMf`. \n",
    "\n",
    "A complete list of available frames is stored in `robot.frames`. \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for k,frame in enumerate(robot.model.frames):\n",
    "    print(\"frame {}: {}\".format(k,frame.name))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "All frame placements are computed directly by calling `pin.framesForwardKinematics` (or alternatively by calling `pin.updateFramePlacement` or `pin.updateFramePlacements` after a call to `pin.forwardKinematics`)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pin.framesForwardKinematics(rmodel, rdata, q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For the tutorial, we will use the frame `gripper_left_fingertip_1_link`:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "frame_index = rmodel.getFrameId('gripper_left_fingertip_1_link')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Cost-function template\n",
    "In this tutorial, the cost functions will all be defined following the same template:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Cost:\n",
    "    def __init__(self, rmodel, rdata, viz=None):  # add any other arguments you like\n",
    "        self.rmodel = rmodel\n",
    "        self.rdata  = rdata\n",
    "        self.viz = viz\n",
    "        \n",
    "    def calc(self, q):\n",
    "        ### Add the code to recompute your cost here\n",
    "        cost = 0\n",
    "        return cost\n",
    "    \n",
    "    def callback(self, q):\n",
    "        if viz is None: \n",
    "            return\n",
    "        # Display something in viz ..."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will see later that the callback can be used to display or print data during the optimization. For example, we may want to display the robot configuration, and visualize the target position with a simple sphere added to the viewer."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![Question](question.png) \n",
    "Implement a `Cost` class computing the quadratic cost `p(q) - ptarget`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load -r 11-36 costs.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-block alert-info\">\n",
    "    <img src=\"recap.png\" title=\"Recap\"/>\n",
    "    <h3>Recap of the main syntax elements exposed in this section</h3>\n",
    "    <ul>\n",
    "        <li> Frame placements are computed by <code>pin.framesForwardKinematics(rmodel, rdata, q)</code>, and accessed by <code>rdata.oMf[frameIndex]</code>.\n",
    "        <li> Frame translation is a 3d array stored in <code>rdata.oMf[frameIndex].translation</code>.            </ul>\n",
    "\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. 6d cost: optimizing the end effector placement (position+orientation)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will now define a cost function that penalizes the placement, i.e. rotation+translation between an operational frame and a target frame."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Spatial algebra in Pinocchio\n",
    "Most of the physical quantifies stored in Pinocchio are 6D quantities, that are elements of the so-called *Spatial Algebra*, following Roy Featherstone definition and namings. Featherstone's work, and in particular [his excellent 2008 book](https://link.springer.com/content/pdf/10.1007%2F978-1-4899-7560-7.pdf) is the basis for all the algorithms in Pinocchio.\n",
    "\n",
    "Frame placement, formally elements of the Lie group SE(3), are represented in Pinocchio by an object of class `pin.SE3` containing a 3x3 rotation array and 3 translation vector. Placements can by multiplied and inverted."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "M1 = pin.SE3.Identity()\n",
    "M2 = pin.SE3.Random()\n",
    "print(M2, M2.rotation, M2.translation)\n",
    "M3 = M1 * M2\n",
    "M4 = M2.inverse()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "SE(3) comes with `log` and `exp` operations. The `log` maps a SE3 placement into the 6D velocity that should be applied during 1 second to obtain this placement, starting from identity. In Pinocchio, 6D velocity are represented as `pin.Motion` object, and can be mapped to 6D array."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nu = pin.log(M1)\n",
    "print(nu.vector)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will not need much of spatial algebra in this tutorial. See the class `pin.Motion`, `pin.Force`, `pin.Inertia` for more details. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Distances between frames\n",
    "The `log` operator can be used to define a distance between placements. The norm of the log is evidently positive and null only if the placement is identity. Consequently, `log(M1.inverse() * M2)` is a positive scalar that is null only if `M1 == M2`."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![Question](question.png)\n",
    "Following the same previous pattern, define a Cost6d class penalizing the distance between a frame attached to the robot and a reference fixed frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load -r 47-72 costs.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-block alert-info\">\n",
    "    <img src=\"recap.png\" title=\"Recap\"/>\n",
    "    <h3>Recap of the main syntax elements exposed in this section</h3>\n",
    "    <ul>\n",
    "        <li>SE3 logarithm is implemented by <code>pin.log</code>, returns a <code>pin.Motion</code> class that can be converted into a vector with <code>pin.log(M).vector</code>.\n",
    "     </ul>\n",
    "\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. Redundancy and introduction of a posture cost"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will now run the optimizer with the two cost functions defined above, and add a posture cost to regularize the rank-deficient hessian."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Running the optimizer\n",
    "We will use the optimizer BFGS from SciPy. It is quasi-newton algorithm, which means that it only needs the first-order derivatives (while having super-linear convergence). Even better, it automatically approximates the derivatives by finite differences if you don't provide them. \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cost = Cost3d(rmodel, rdata, viz=viz)\n",
    "qguess = robot.q0.copy()\n",
    "qopt = fmin_bfgs(cost.calc, qguess, callback=cost.callback)\n",
    "viz.display(qopt)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Redundancy\n",
    "\n",
    "The arm of the robot Talos, that we are useing by default in the notebook, as 6 degrees of freedom (plus one for the gripper). If using the 3D cost function, there is a continuum of solutions, as the kinematic is redundant for achieve a pointing tasks. You can obtain different optimum by changing the initial guess. Each new run with a random initial guess gives you a new optimum."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "qguess = pin.randomConfiguration(rmodel)\n",
    "qopt = fmin_bfgs(cost.calc, qguess, callback=cost.callback)\n",
    "viz.display(qopt)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will now add a small regularization to the cost, by optimizing a full-rank term on the robot posture, to make the solution unique indenpendantly from the robot kinematics and the considered task cost."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![Question](question.png)\n",
    "Introduce a new cost function for penalizing the distance of the robot posture to a reference posture, for example `robot.q0`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load -r 82-95 costs.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Optimize a sum of cost"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can define an ad-hoc cost that makes a sum of both costs."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class SumOfCost:\n",
    "    def __init__(self, costs, weights):\n",
    "        self.costs = costs\n",
    "        self.weights = np.array(weights)\n",
    "    def calc(self, q):\n",
    "        return sum(self.weights * [cost.calc(q) for cost in self.costs])\n",
    "    \n",
    "mycost = SumOfCost([Cost3d(rmodel, rdata), CostPosture(rmodel, rdata)], [1, 1e-3])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And we optimize this new cost."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "qopt = fmin_bfgs(mycost.calc, qguess)\n",
    "viz.display(qopt)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-block alert-info\">\n",
    "    <img src=\"recap.png\" title=\"Recap\"/>\n",
    "    <h3>Recap of the main syntax elements exposed in this section</h3>\n",
    "    <ul>\n",
    "        <li>The BFGS solver is called by <code>fmin_bfgs(cost.calc, qguess, cost.callback)</code>.\n",
    "     </ul>\n",
    "\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 6. Gravity cost: introducing the dynamic model"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The posture cost is a nice regularization but requires a reference posture that may be hard to select. We will now define a cost to minimize the gravity torque, as an alternative regularization."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Dynamics in Pinocchio\n",
    "\n",
    "The whole-body dynamics equation can be written as:\n",
    "$$M(q) a_q + b(q,v_q) = \\tau_q   $$\n",
    "where $q,v_q,a_q$ are the position, velocity and acceleration in the configuration space, $M(q)$ is the configuration-space inertia matrix, of size `robot.model.nv`x`robot.model.nv`, $b(q,v_q)$ gathers all the drift terms (Coriolis, centrifugal, gravity) and $\\tau_q$ are the joint torques. We write $v_q$ and $a_q$ because in general $q$ (of size `robot.model.nq`) does not have the same size as its derivatives, although they corresponds to $\\dot{q}$ and $\\ddot{q}$ for now."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Computing the gravity term\n",
    "This equation corresponds to the inverse dynamics. We can evaluate parts of it or the entire equation, as we will see next. Let's start with a simple case. \n",
    "\n",
    "The gravity term corresponds to the torques when the robot has no velocity nor acceleration:\n",
    "$$g(q) = b(q,v_q=0) = dyninv(q,v_q=0,a_q=0)$$\n",
    "In Pinocchio, it can be directly computed with:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "g = pin.computeGeneralizedGravity(rmodel, rdata, q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![question](question.png)\n",
    "Define a new cost function that compute the squared norm of the gravity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load -r 107-117 costs.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-block alert-info\">\n",
    "    <img src=\"recap.png\" title=\"Recap\"/>\n",
    "    <h3>Recap of the main syntax elements exposed in this section</h3>\n",
    "    <ul>\n",
    "        <li> - The gravity torque can be computed by <code>pin.computeGeneralizedGravity(rmodel, rdata, q)</code>.\n",
    "     </ul>\n",
    "\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 7. Weighted gravity cost: RNEA and ABA recursive algorithms"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Minimizing the gravity cost is often obtained by bringing the robot to singular configurations. A better behavior might be obtained by a variation of this cost implying the generalized inertia matrix."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Recursive algorithms in Pinocchio\n",
    "The 3 most efficient algorithms to evaluate the dynamic equations are implemented in Pinocchio:\n",
    "- the recursive Newton-Euler algorithm (RNEA) computes the inverse dynamics $\\tau_q = invdyn(q,v_q,a_q)$\n",
    "- the articulated rigid body algorithm (ABA) computes the direct dynamics $a_q = dirdyn(q,v_q,_tau_q)$\n",
    "- the composite rigid body algorithm (CRBA) computes the mass matrix.\n",
    "The 3 algorithms can be directly called by their names in Pinocchio."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "vq = np.random.rand(rmodel.nv) * 2 - 1\n",
    "aq = np.random.rand(rmodel.nv) * 2 - 1\n",
    "tauq = pin.rnea(rmodel, rdata, q, vq, aq)\n",
    "aq2 = pin.aba(rmodel, rdata, q, vq, tauq)\n",
    "assert norm(aq - aq2) < 1e-6\n",
    "M = pin.crba(rmodel, rdata, q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The gravity torque $g(q)=b(q,0)$ and the dynamic drift $b(q,vq)$ can be also computed by a truncated RNEA implementation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "b = pin.nle(rmodel, rdata, q, vq)\n",
    "assert norm(M @ aq + b - tauq) < 1e-6"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Weighted gravity norm\n",
    "A better posture regularization can be obtained by taking the norm of the gravity weighted by the inertia:\n",
    "$$ l(q) = g(q)^T M(q)^{-1} g(q)$$\n",
    "\n",
    "We can directly evaluate this expression by compute and inverting $M$. Yet it is more efficient to recognize that $M(q)^{-1} g(q) = -dirdyn(q,0,0)$ and can be evaluated by:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "aq0 = -pin.aba(rmodel, rdata, q, np.zeros(rmodel.nv), np.zeros(rmodel.nv))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![question](question.png)\n",
    "Implement a new cost function for this weighted gravity, and run it as the regularization of a reaching task."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load -r 125-138 costs.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-block alert-info\">\n",
    "    <img src=\"recap.png\" title=\"Recap\"/>\n",
    "    <h3>Recap of the main syntax elements exposed in this section</h3>\n",
    "    <ul>\n",
    "        <li> Inverse dynamics: <code>tauq = pin.rnea(rmodel, rdata, q, vq, aq)</code>\n",
    "        <li>Direct dynamcis: <code>aq = pin.aba(rmodel, rdata, q, vq, tauq)</code>\n",
    "        <li> Inertia matrix: <code>M = pin.crba(rmodel, rdata, q)</code>\n",
    "        <li> Dynamics drift: <code>b = pin.nle(rmodel, rdata, q, vq)</code>\n",
    "     </ul>\n",
    "\n",
    "</div>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 8. Working with a free floating robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now apply the previous mathematical program to a more complex robot with a floatin basis."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Loading Solo or Talos\n",
    "You might want to use either the quadruped robot Solo or the humanoid robot Talos for this one."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# robot = robex.load(\"talos\")\n",
    "robot = robex.load(\"solo12\")\n",
    "viz = Viewer(robot.model, robot.collision_model, robot.visual_model)\n",
    "viz.initViewer(loadModel=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "hasattr(viz.viewer, 'jupyter_cell') and viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "viz.display(robot.q0)\n",
    "vizutils.addViewerBox(viz, 'world/box', .02, .0324, .0648, [1., .2, .2, .5])\n",
    "vizutils.addViewerSphere(viz, 'world/ball', .04, [.2, .2, 1., .5])\n",
    "vizutils.applyViewerConfiguration(viz, 'world/box', [0.5, -.2, .2, 1, 0, 0, 0])\n",
    "vizutils.applyViewerConfiguration(viz, 'world/ball', [0.5, .2, .2, 1, 0, 0, 0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rmodel = robot.model\n",
    "rdata = rmodel.createData()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Oh! no, my configuration space is now a Lie group!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In Pinocchio, the floating basis is represented by default as a free-flyer joint. Free-flyer joints have 6 degrees of freedom, but are represented by 7 scalars: the position of the basis center in the world frame, and the orientation of the basis in the world frame stored as a quaternion. This implies that the size of the configuration vector $q$ is one more than the size of the configuration velocity, acceleration or torque: `robot.model.nv`==`robot.model.nv`+1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(rmodel.joints[1])\n",
    "assert rmodel.nq == rmodel.nv + 1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Formally, we say that the configuration $q$ is now living in a Lie group $\\mathcal{Q}$, while $v_q$ is in the tangent of this group, the Lie algebra $T_q\\mathcal{Q}$. \n",
    "In practice, it means that sampling a random $q$ has it was a vector would not work: you must either use the `pin.randomConfiguration` introduced earlier, or normalize the configuration using `pin.normalize`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = np.random.rand(rmodel.nq)  # Not a valid configuration, because the quaternion is not normalized.\n",
    "q = pin.normalize(rmodel, q)  # Now, it is normalize\n",
    "# Or better, call directly randomConfiguration\n",
    "q = pin.randomConfiguration(rmodel)  # q is normalized\n",
    "viz.display(q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Similarly, you cannot directly sum a configuration with a velocity: they don't have the same size. Rather, you should integrate the velocity using `pin.integrate`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "vq = np.random.rand(rmodel.nv) * 2 - 1  # sample a random velocity between -1 and 1.\n",
    "try:\n",
    "    q += vq\n",
    "except:\n",
    "    print('!!! ERROR')\n",
    "    print('As expected, this raises an error because q and vq do not have the same dimension.')\n",
    "\n",
    "q = pin.integrate(rmodel, q, vq)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The reciprocal operation is `pin.difference(rmodel, q1, q2)`: it returns the velocity `vq` so that $q_1 \\oplus v_q = q_2$ (with $\\oplus$ the integration in $\\mathcal{Q}$)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q1 = pin.randomConfiguration(rmodel)\n",
    "q2 = pin.randomConfiguration(rmodel)\n",
    "vq = pin.difference(rmodel, q1, q2)\n",
    "print(q2 - pin.integrate(rmodel, q1, vq))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Depending on the random samples `q1`,`q2`, the print might be 0, or something different on the quaternion part ... again, because making the vector operation `qa - qb` is not valid in $\\mathcal{Q}$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Consequence on optimization\n",
    "So, can we directly use the mathematical program that we defined above with our free-basis robot? Yes and no. \n",
    "\n",
    "Let's see that in practice before answering in details.  Just optimize your latest mathematical program with the new robot. If you want the normalization to significantly impact the display, choose a target that is far away from the robot.\n",
    "\n",
    "**Before runing the next cell**, try to guess how the result will be displayed.\n",
    "![Think before executing](confused.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "mycost = SumOfCost([Cost3d(rmodel, rdata, ptarget = np.array([2, 2, -1]), viz=viz), CostPosture(rmodel, rdata)], [1, 1e-3])\n",
    "qopt = fmin_bfgs(mycost.calc, robot.q0, callback=mycost.costs[0].callback)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So, what happened?\n",
    "\n",
    "All the cost functions that we defined are valid when `q` is a proper normalized configuration. **BUT** `fmin_bfgs` is not aware of the normalization constraint. The solver modifies the initial `q` guess into a new `q` vector that is not normalized, hence not a valid configuration.\n",
    "\n",
    "To prevent that, two solutions are possible:\n",
    "1. a cost can be added to penalize the solver when choosing a new `q` that is not normalized: `cost(q) = (1 - q[3:7] ** 2) ** 2` (or more generic, `cost(q) = sum((q - pin.normalize(rmodel, q)) ** 2)`.\n",
    "2. or the vector `q` chosen by the solver should be first normalized when entering `cost.call`, so that the cost is evaluated on a proper configuration."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![Question](question.png)\n",
    "Redefine your sum-of-cost class with either adding an extra term in the sum to penalize the normalization, or by first normalizing the decision vector `q`.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load -r 2-23 solutions.py"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div class=\"alert alert-block alert-info\">\n",
    "    <img src=\"recap.png\" title=\"Recap\"/>\n",
    "    <h3>Recap of the main syntax elements exposed in this section</h3>\n",
    "    <ul>\n",
    "        <li><code>pin.integrate</code> adds a configuration with a velocity.</li>\n",
    "        <li><code>pin.difference</code> makes the difference between two configurations.</li>\n",
    "        <li><code>pin.normalize</code> project any vector of size NQ into a properly normalized configuration.</li>\n",
    "     </ul>\n",
    "\n",
    "</div>"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.18"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}