examples/notebooks/introduction_to_crocoddyl.ipynb

{
  "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Crocoddyl: Contact RObot COntrol by Differential DYnamic programming Library\n",
    "\n",
    "\n",
    "## I. Welcome to crocoddyl\n",
    "Crocoddyl is an **optimal control library for robot control under contact sequence**. Its solver is based on an efficient Differential Dynamic Programming (DDP) algorithm. Crocoddyl computes optimal trajectories along with optimal feedback gains. It uses Pinocchio for fast computation of robot dynamics and its analytical derivatives. \n",
    "\n",
    "Crocoddyl is focused on multi-contact optimal control problem (MCOP) which as the form:\n",
    "\n",
    "$$\\mathbf{X}^*,\\mathbf{U}^*=\n",
    "\\begin{Bmatrix} \\mathbf{x}^*_0,\\cdots,\\mathbf{x}^*_N \\\\\n",
    "\t\t\t\t  \\mathbf{u}^*_0,\\cdots,\\mathbf{u}^*_N\n",
    "\\end{Bmatrix} =\n",
    "\\arg\\min_{\\mathbf{X},\\mathbf{U}} \\sum_{k=1}^N \\int_{t_k}^{t_k+\\Delta t} l(\\mathbf{x},\\mathbf{u})dt$$\n",
    "subject to\n",
    "$$ \\mathbf{\\dot{x}} = \\mathbf{f}(\\mathbf{x},\\mathbf{u}),$$\n",
    "$$ \\mathbf{x}\\in\\mathcal{X}, \\mathbf{u}\\in\\mathcal{U}, \\boldsymbol{\\lambda}\\in\\mathcal{K}.$$\n",
    "where\n",
    " - the state $\\mathbf{x}=(\\mathbf{q},\\mathbf{v})$ lies in a manifold, e.g. Lie manifold $\\mathbf{q}\\in SE(3)\\times \\mathbb{R}^{n_j}$, $n_j$ being the number of degrees of freedom of the robot.\n",
    " - the system has underactuacted dynamics, i.e. $\\mathbf{u}=(\\mathbf{0},\\boldsymbol{\\tau})$,\n",
    " - $\\mathcal{X}$, $\\mathcal{U}$ are the state and control admissible sets, and\n",
    " - $\\mathcal{K}$ represents the contact constraints.\n",
    " \n",
    " Note that $\\boldsymbol{\\lambda}=\\mathbf{g}(\\mathbf{x},\\mathbf{u})$ denotes the contact force, and is dependent on the state and control.\n",
    " \n",
    "Let's start by understanding the concept behind crocoddyl design."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# II. Action models\n",
    "\n",
    "In crocoddyl, an action model combines dynamics and cost models. Each node, in our optimal control problem, is described through an action model. In order to describe a problem, we need to provide ways of computing the dynamics, the cost functions and their derivatives. All these are described inside the action model.\n",
    "\n",
    "To understand the mathematical aspects behind an action model, let's first get a locally linearize version of our optimal control problem as:\n",
    "\n",
    "$$\\mathbf{X}^*(\\mathbf{x}_0),\\mathbf{U}^*(\\mathbf{x}_0)\n",
    "=\n",
    "\\arg\\max_{\\mathbf{X},\\mathbf{U}} = cost_T(\\delta\\mathbf{x}_N) + \\sum_{k=1}^N cost_t(\\delta\\mathbf{x}_k, \\delta\\mathbf{u}_k)$$\n",
    "subject to\n",
    "$$dynamics(\\delta\\mathbf{x}_{k+1},\\delta\\mathbf{x}_k,\\delta\\mathbf{u}_k)=\\mathbf{0},$$\n",
    "\n",
    "where\n",
    "$$cost_T(\\delta\\mathbf{x}) = \\frac{1}{2}\n",
    "\\begin{bmatrix} \n",
    "  1 \\\\ \\delta\\mathbf{x}\n",
    "\\end{bmatrix}^\\top\n",
    "\\begin{bmatrix}\n",
    "0 & \\mathbf{l_x}^\\top \\\\\n",
    "\\mathbf{l_x} & \\mathbf{l_{xx}}\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "  1 \\\\ \\delta\\mathbf{x}\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "$$cost_t(\\delta\\mathbf{x},\\delta\\mathbf{u}) = \\frac{1}{2}\n",
    "\\begin{bmatrix} \n",
    "  1 \\\\ \\delta\\mathbf{x} \\\\ \\delta\\mathbf{u}\n",
    "\\end{bmatrix}^\\top\n",
    "\\begin{bmatrix}\n",
    "0 & \\mathbf{l_x}^\\top & \\mathbf{l_u}^\\top\\\\\n",
    "\\mathbf{l_x} & \\mathbf{l_{xx}} & \\mathbf{l_{ux}}^\\top\\\\\n",
    "\\mathbf{l_u} & \\mathbf{l_{ux}} & \\mathbf{l_{uu}}\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "  1 \\\\ \\delta\\mathbf{x} \\\\ \\delta\\mathbf{u}\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "$$\n",
    "dynamics(\\delta\\mathbf{x}_{k+1},\\delta\\mathbf{x}_k,\\delta\\mathbf{u}_k) = \\delta\\mathbf{x}_{k+1} - (\\mathbf{f_x}\\delta\\mathbf{x}_k + \\mathbf{f_u}\\delta\\mathbf{u}_k)\n",
    "$$\n",
    "\n",
    "where an action model defines a time interval of this problem:\n",
    " - $actions = dynamics + cost$\n",
    "\n",
    "### Important notes:\n",
    " - An action model describes the dynamics and cost functions for a node in our optimal control problem.\n",
    " - Action models lie in the discrete time space.\n",
    " - For debugging and prototyping, we have also implemented numerical differentiation (NumDiff) abstractions. These computations depend only on the definition of the dynamics equation and cost functions. However to asses efficiency, crocoddyl uses **analytical derivatives** computed from Pinocchio.\n",
    "\n",
    "\n",
    "## II.a Differential and Integrated Action Models\n",
    "Optimal control solvers require the time-discrete model of the cost and the dynamics. However, it's often convenient to implement them in continuous time (e.g. to combine with abstract integration rules). In crocoddyl, this continuous-time action models are called \"Differential Action Model (DAM)\". And together with predefined \"Integrated Action Models (IAM)\", it possible to retrieve the time-discrete action model.\n",
    "\n",
    "At the moment, we have:\n",
    " - a simpletic Euler and\n",
    " - a Runge-Kutte 4 integration rules.\n",
    "\n",
    "An optimal control problem can be written from a set of DAMs as:\n",
    "$$\\mathbf{X}^*(\\mathbf{x}_0),\\mathbf{U}^*(\\mathbf{x}_0)\n",
    "=\n",
    "\\arg\\max_{\\mathbf{X},\\mathbf{U}} = cost_T(\\delta\\mathbf{x}_N) + \\sum_{k=1}^N \\int_{t_k}^{t_k+\\Delta t} cost_t(\\delta\\mathbf{x}_k, \\delta\\mathbf{u}_k) dt$$\n",
    "subject to\n",
    "$$dynamics(\\delta\\mathbf{x}_{k+1},\\delta\\mathbf{x}_k,\\delta\\mathbf{u}_k)=\\mathbf{0},$$\n",
    "\n",
    "where\n",
    "$$cost_T(\\delta\\mathbf{x}) = \\frac{1}{2}\n",
    "\\begin{bmatrix} \n",
    "  1 \\\\ \\delta\\mathbf{x}\n",
    "\\end{bmatrix}^\\top\n",
    "\\begin{bmatrix}\n",
    "0 & \\mathbf{l_x}^\\top \\\\\n",
    "\\mathbf{l_x} & \\mathbf{l_{xx}}\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "  1 \\\\ \\delta\\mathbf{x}\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "$$cost_t(\\delta\\mathbf{x},\\delta\\mathbf{u}) = \\frac{1}{2}\n",
    "\\begin{bmatrix} \n",
    "  1 \\\\ \\delta\\mathbf{x} \\\\ \\delta\\mathbf{u}\n",
    "\\end{bmatrix}^\\top\n",
    "\\begin{bmatrix}\n",
    "0 & \\mathbf{l_x}^\\top & \\mathbf{l_u}^\\top\\\\\n",
    "\\mathbf{l_x} & \\mathbf{l_{xx}} & \\mathbf{l_{ux}}^\\top\\\\\n",
    "\\mathbf{l_u} & \\mathbf{l_{ux}} & \\mathbf{l_{uu}}\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "  1 \\\\ \\delta\\mathbf{x} \\\\ \\delta\\mathbf{u}\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "$$\n",
    "dynamics(\\delta\\mathbf{\\dot{x}},\\delta\\mathbf{x},\\delta\\mathbf{u}) = \\delta\\mathbf{\\dot{x}} - (\\mathbf{f_x}\\delta\\mathbf{x} + \\mathbf{f_u}\\delta\\mathbf{u})\n",
    "$$"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Building a differential action model for robot forward dynamics\n",
    "#### Loading the robot\n",
    "\n",
    "Crocoddyl offers several robot models for benchmarking our optimal control solvers (e.g. manipulators, humanoids, quadrupeds, etc). The collection of Talos models can be downloaded in Ubuntu with the APT package *robotpkg-talos-data*.\n",
    "\n",
    "Let's load a single Talos arm (left one):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import crocoddyl\n",
    "import numpy as np\n",
    "import example_robot_data\n",
    "\n",
    "talos_arm = example_robot_data.load(\"talos_arm\")\n",
    "robot_model = talos_arm.model  # getting the Pinocchio model\n",
    "\n",
    "# Defining a initial state\n",
    "q0 = np.array([0.173046, 1.0, -0.52366, 0.0, 0.0, 0.1, -0.005])\n",
    "x0 = np.concatenate([q0, np.zeros(talos_arm.model.nv)])"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### calc and calcDiff\n",
    "Optimal control solvers often need to compute a quadratic approximation of the action model (as previously described); this provides a search direction (computeDirection). Then it's needed to try the step along this direction (tryStep).\n",
    "\n",
    "Typically calc and calcDiff do the precomputations that are required before computeDirection and tryStep respectively (inside the solver). These functions update the information of:\n",
    " - **calc**: update the next state and its cost value\n",
    " $$\\delta\\mathbf{\\dot{x}}_{k+1} = \\mathbf{f}(\\delta\\mathbf{x}_k,\\mathbf{u}_k)$$\n",
    " - **calcDiff**: update the derivatives of the dynamics and cost (quadratic approximation)\n",
    " $$\\mathbf{f_x}, \\mathbf{f_u} \\hspace{1em} (dynamics)$$\n",
    " $$\\mathbf{l_x}, \\mathbf{l_u}, \\mathbf{l_{xx}}, \\mathbf{l_{ux}}, \\mathbf{l_{uu}} \\hspace{1em} (cost)$$\n",
    " \n",
    " **Crocoddyl put all information inside data**, so avoiding dynamic reallocation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pinocchio\n",
    "\n",
    "\n",
    "class DifferentialFwdDynamics(crocoddyl.DifferentialActionModelAbstract):\n",
    "    def __init__(self, state, costModel):\n",
    "        crocoddyl.DifferentialActionModelAbstract.__init__(\n",
    "            self, state, state.nv, costModel.nr\n",
    "        )\n",
    "        self.costs = costModel\n",
    "        self.enable_force = True\n",
    "        self.armature = np.zeros(0)\n",
    "\n",
    "    def calc(self, data, x, u=None):\n",
    "        if u is None:\n",
    "            u = self.unone\n",
    "        q, v = x[: self.state.nq], x[-self.state.nv :]\n",
    "        # Computing the dynamics using ABA or manually for armature case\n",
    "        if self.enable_force:\n",
    "            data.xout = pinocchio.aba(self.state.pinocchio, data.pinocchio, q, v, u)\n",
    "        else:\n",
    "            pinocchio.computeAllTerms(self.state.pinocchio, data.pinocchio, q, v)\n",
    "            data.M = data.pinocchio.M\n",
    "            if self.armature.size == self.state.nv:\n",
    "                data.M[range(self.state.nv), range(self.state.nv)] += self.armature\n",
    "            data.Minv = np.linalg.inv(data.M)\n",
    "            data.xout = data.Minv * (u - data.pinocchio.nle)\n",
    "        # Computing the cost value and residuals\n",
    "        pinocchio.forwardKinematics(self.state.pinocchio, data.pinocchio, q, v)\n",
    "        pinocchio.updateFramePlacements(self.state.pinocchio, data.pinocchio)\n",
    "        self.costs.calc(data.costs, x, u)\n",
    "        data.cost = data.costs.cost\n",
    "\n",
    "    def calcDiff(self, data, x, u=None):\n",
    "        q, v = x[: self.state.nq], x[-self.state.nv :]\n",
    "        if u is None:\n",
    "            u = self.unone\n",
    "        if True:\n",
    "            self.calc(data, x, u)\n",
    "        # Computing the dynamics derivatives\n",
    "        if self.enable_force:\n",
    "            pinocchio.computeABADerivatives(\n",
    "                self.state.pinocchio, data.pinocchio, q, v, u\n",
    "            )\n",
    "            data.Fx = np.hstack([data.pinocchio.ddq_dq, data.pinocchio.ddq_dv])\n",
    "            data.Fu = data.pinocchio.Minv\n",
    "        else:\n",
    "            pinocchio.computeRNEADerivatives(\n",
    "                self.state.pinocchio, data.pinocchio, q, v, data.xout\n",
    "            )\n",
    "            data.Fx = -np.hstack(\n",
    "                [data.Minv * data.pinocchio.dtau_dq, data.Minv * data.pinocchio.dtau_dv]\n",
    "            )\n",
    "            data.Fu = data.Minv\n",
    "        # Computing the cost derivatives\n",
    "        self.costs.calcDiff(data.costs, x, u)\n",
    "\n",
    "    def set_armature(self, armature):\n",
    "        if armature.size is not self.state.nv:\n",
    "            print(\"The armature dimension is wrong, we cannot set it.\")\n",
    "        else:\n",
    "            self.enable_force = False\n",
    "            self.armature = armature.T\n",
    "\n",
    "    def createData(self):\n",
    "        data = crocoddyl.DifferentialActionModelAbstract.createData(self)\n",
    "        data.pinocchio = pinocchio.Data(self.state.pinocchio)\n",
    "        data.multibody = crocoddyl.DataCollectorMultibody(data.pinocchio)\n",
    "        data.costs = self.costs.createData(data.multibody)\n",
    "        data.costs.shareMemory(\n",
    "            data\n",
    "        )  # this allows us to share the memory of cost-terms of action model\n",
    "        return data"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## II.b State and its integrate and difference rules\n",
    "General speaking, the system's state can lie in a manifold $M$ where the state rate of change lies in its tangent space $T_\\mathbf{x}M$. There are few operators that needs to be defined for different rutines inside our solvers:\n",
    "  - $\\mathbf{x}_{k+1} = integrate(\\mathbf{x}_k,\\delta\\mathbf{x}_k) = \\mathbf{x}_k \\oplus \\delta\\mathbf{x}_k$\n",
    "  - $\\delta\\mathbf{x}_k = difference(\\mathbf{x}_{k+1},\\mathbf{x}_k) = \\mathbf{x}_{k+1} \\ominus \\mathbf{x}_k$\n",
    "\n",
    "where $\\mathbf{x}\\in M$ and $\\delta\\mathbf{x}\\in T_\\mathbf{x} M$.\n",
    " \n",
    "\n",
    "And we also need to defined the Jacobians of these operators with respect to the first and second arguments:\n",
    "  - $\\frac{\\partial \\mathbf{x}\\oplus\\delta\\mathbf{x}}{\\partial \\mathbf{x}}, \\frac{\\partial \\mathbf{x}\\oplus\\delta\\mathbf{x}}{\\partial\\delta\\mathbf{x}} =Jintegrante(\\mathbf{x},\\delta\\mathbf{x})$\n",
    "  - $\\frac{\\partial\\mathbf{x}_2\\ominus\\mathbf{x}_2}{\\partial \\mathbf{x}_1}, \\frac{\\partial \\mathbf{x}_2\\ominus\\mathbf{x}_1}{\\partial\\mathbf{x}_1} =Jdifference(\\mathbf{x}_2,\\mathbf{x}_1)$\n",
    "\n",
    "For instance, a state that lies in the Euclidean space will the typical operators:\n",
    "  - $integrate(\\mathbf{x},\\delta\\mathbf{x}) = \\mathbf{x} + \\delta\\mathbf{x}$\n",
    "  - $difference(\\mathbf{x}_2,\\mathbf{x}_1) = \\mathbf{x}_2 - \\mathbf{x}_1$\n",
    "  - $Jintegrate(\\cdot,\\cdot) = Jdifference(\\cdot,\\cdot) = \\mathbf{I}$\n",
    "  \n",
    "\n",
    "These defines inare encapsulate inside the State class. **For Pinocchio models, we have implemented the StatePinocchio class which can be used for any robot model**."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# III. Solving optimal control problems with DDP\n",
    "\n",
    "## III.a ABA dynamics for reaching a goal with Talos arm\n",
    "\n",
    "Our optimal control solver interacts with a defined ShootingProblem. A shooting problem represents a stack of action models in which an action model defines a specific node along the OC problem.\n",
    "\n",
    "First we need to create an action model from DifferentialFwdDynamics. We use it for building terminal and running action models. In this example, we employ an simpletic Euler integration rule.\n",
    "\n",
    "Next we define the set of cost functions for this problem. For this particular example, we formulate three running-cost functions:\n",
    "\n",
    "    goal-tracking cost, 𝑙𝑜𝑔(𝑓𝑋𝑑𝑜𝑜𝑋𝑓)\n",
    "\n",
    "state and control regularization; and ‖𝐱−𝐱𝑟𝑒𝑓‖,‖𝐮‖\n",
    "\n",
    "one terminal-cost:\n",
    "\n",
    "    goal cost. ‖𝐮𝑇‖\n",
    "\n",
    "First, let's create the common cost functions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create the cost functions\n",
    "target = np.array([0.4, 0.0, 0.4])\n",
    "state = crocoddyl.StateMultibody(robot_model)\n",
    "frameTranslationResidual = crocoddyl.ResidualModelFrameTranslation(\n",
    "    state, robot_model.getFrameId(\"gripper_left_joint\"), target\n",
    ")\n",
    "goalTrackingCost = crocoddyl.CostModelResidual(state, frameTranslationResidual)\n",
    "xRegCost = crocoddyl.CostModelResidual(state, crocoddyl.ResidualModelState(state))\n",
    "uRegCost = crocoddyl.CostModelResidual(state, crocoddyl.ResidualModelControl(state))\n",
    "\n",
    "# Create cost model per each action model\n",
    "runningCostModel = crocoddyl.CostModelSum(state)\n",
    "terminalCostModel = crocoddyl.CostModelSum(state)\n",
    "\n",
    "# Then let's added the running and terminal cost functions\n",
    "runningCostModel.addCost(\"gripperPose\", goalTrackingCost, 1e2)\n",
    "runningCostModel.addCost(\"stateReg\", xRegCost, 1e-4)\n",
    "runningCostModel.addCost(\"ctrlReg\", uRegCost, 1e-7)\n",
    "terminalCostModel.addCost(\"gripperPose\", goalTrackingCost, 1e5)\n",
    "terminalCostModel.addCost(\"stateReg\", xRegCost, 1e-4)\n",
    "terminalCostModel.addCost(\"ctrlReg\", uRegCost, 1e-7)\n",
    "\n",
    "# Running and terminal action models\n",
    "DT = 1e-3\n",
    "actuationModel = crocoddyl.ActuationModelFull(state)\n",
    "runningModel = crocoddyl.IntegratedActionModelEuler(\n",
    "    crocoddyl.DifferentialActionModelFreeFwdDynamics(\n",
    "        state, actuationModel, runningCostModel\n",
    "    ),\n",
    "    DT,\n",
    ")\n",
    "terminalModel = crocoddyl.IntegratedActionModelEuler(\n",
    "    crocoddyl.DifferentialActionModelFreeFwdDynamics(\n",
    "        state, actuationModel, terminalCostModel\n",
    "    ),\n",
    "    0.0,\n",
    ")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We create a trajectory with 250 knots"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "# For this optimal control problem, we define 250 knots (or running action\n",
    "# models) plus a terminal knot\n",
    "T = 250\n",
    "problem = crocoddyl.ShootingProblem(x0, [runningModel] * T, terminalModel)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Onces we have defined our shooting problem, we create a DDP solver object and pass some callback functions for analysing  its performance.\n",
    "\n",
    "Please note that:\n",
    "- CallbackDDPLogger: store the solution information.\n",
    "- CallbackDDPVerbose(level): printing message during the iterates.\n",
    "- CallbackDisplay(robot,rate): display the state trajectory using Gepetto viewer."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7001/static/\n"
     ]
    }
   ],
   "source": [
    "# Creating the DDP solver for this OC problem, defining a logger\n",
    "ddp = crocoddyl.SolverDDP(problem)\n",
    "log = crocoddyl.CallbackLogger()\n",
    "\n",
    "# Using the meshcat displayer, you could enable gepetto viewer for nicer view\n",
    "# display = crocoddyl.GepettoDisplay(talos_arm, 4, 4)\n",
    "display = crocoddyl.MeshcatDisplay(talos_arm, 4, 4, False)\n",
    "ddp.setCallbacks([log, crocoddyl.CallbackVerbose(), crocoddyl.CallbackDisplay(display)])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "<div style=\"height: 400px; width: 600px; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "<iframe src=\"http://127.0.0.1:7001/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "</div>\n"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Emdebbed meshcat in this cell\n",
    "display.robot.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "The reached pose by the wrist is\n",
      "  R =\n",
      "-0.227356  0.695555  0.681552\n",
      "-0.961861 -0.269711  -0.04561\n",
      " 0.152098 -0.665928  0.730347\n",
      "  p =   0.379547 -0.0466406   0.421906\n",
      "\n"
     ]
    }
   ],
   "source": [
    "# Solving it with the DDP algorithm\n",
    "ddp.solve()\n",
    "\n",
    "# Printing the reached position\n",
    "frame_idx = talos_arm.model.getFrameId(\"gripper_left_joint\")\n",
    "xT = ddp.xs[-1]\n",
    "qT = xT[:talos_arm.model.nq]\n",
    "print\n",
    "print \"The reached pose by the wrist is\"\n",
    "print talos_arm.framePlacement(qT, frame_idx)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's plot the results and display final trajectory"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "# # Plotting the solution and the DDP convergence\n",
    "crocoddyl.plotOCSolution(log.xs, log.us)\n",
    "crocoddyl.plotConvergence(\n",
    "    log.costs, log.pregs, log.dregs, log.grads, log.stops, log.steps\n",
    ")\n",
    "\n",
    "# Visualizing the solution in gepetto-viewer\n",
    "display.displayFromSolver(ddp)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## III.b Multi-Contact dynamics for biped walking (Talos legs)\n",
    "In crocoddyl, we can describe the multi-contact dynamics through holonomic constraints for the support legs. From the Gauss principle, we have derived the model as:\n",
    "$$\n",
    "\\left[\\begin{matrix}\n",
    " \\mathbf{M} & \\mathbf{J}^{\\top}_c \\\\\n",
    " {\\mathbf{J}_{c}} & \\mathbf{0} \\\\\n",
    "\\end{matrix}\\right]\n",
    "\\left[\\begin{matrix}\n",
    " \\dot{\\mathbf{v}} \\\\ -\\boldsymbol{\\lambda}\n",
    "\\end{matrix}\\right]\n",
    " = \n",
    "\\left[\\begin{matrix}\n",
    "  \\boldsymbol{\\tau} - \\mathbf{h} \\\\\n",
    "  -\\dot{\\mathbf{J}}_c \\mathbf{v} \\\\\n",
    "\\end{matrix}\\right]$$.\n",
    "\n",
    "This DAM is defined in \"DifferentialActionModelFloatingInContact\" class.\n",
    "\n",
    "Given a predefined contact sequence and timings, we build per each phase a specific multi-contact dynamics. Indeed we need to describe multi-phase optimal control problem. One can formulate the multi-contact optimal control problem (MCOP) as follows:\n",
    "\n",
    "\n",
    "$$\\mathbf{X}^*,\\mathbf{U}^*=\n",
    "\\begin{Bmatrix} \\mathbf{x}^*_0,\\cdots,\\mathbf{x}^*_N \\\\\n",
    "\t\t\t\t  \\mathbf{u}^*_0,\\cdots,\\mathbf{u}^*_N\n",
    "\\end{Bmatrix} =\n",
    "\\arg\\min_{\\mathbf{X},\\mathbf{U}} \\sum_{p=0}^P \\sum_{k=1}^{N(p)} \\int_{t_k}^{t_k+\\Delta t} l_p(\\mathbf{x},\\mathbf{u})dt$$\n",
    "subject to\n",
    "$$ \\mathbf{\\dot{x}} = \\mathbf{f}_p(\\mathbf{x},\\mathbf{u}), \\text{for } t \\in [\\tau_p,\\tau_{p+1}]$$\n",
    "\n",
    "$$ \\mathbf{g}(\\mathbf{v}^{p+1},\\mathbf{v}^p) = \\mathbf{0}$$\n",
    "\n",
    "$$ \\mathbf{x}\\in\\mathcal{X}_p, \\mathbf{u}\\in\\mathcal{U}_p, \\boldsymbol{\\lambda}\\in\\mathcal{K}_p.$$\n",
    "\n",
    "where $\\mathbf{g}(\\cdot,\\cdot,\\cdot)$ describes the contact dynamics, and they represents terminal constraints in each walking phase. In this example we use the following impact model:\n",
    "\n",
    "$$\\mathbf{M}(\\mathbf{v}_{next}-\\mathbf{v}) = \\mathbf{J}_{impulse}^T$$\n",
    "\n",
    "$$\\mathbf{J}_{impulse} \\mathbf{v}_{next} = \\mathbf{0}$$\n",
    "\n",
    "$$\\mathbf{J}_{c} \\mathbf{v}_{next} = \\mathbf{J}_{c} \\mathbf{v}$$\n",
    "\n",
    "### Note:\n",
    "You can find an example of such kind of problems in bipedal_walking_from_foot_traj.ipynb."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Reference\n",
    "\n",
    "The material presented in this Notebook was previously presented at the ICRA at 2020. For more information, please read the following paper:\n",
    "\n",
    "C. Mastalli et al. Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control, 2020"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.display import HTML\n",
    "\n",
    "# Youtube\n",
    "HTML(\n",
    "    '<iframe width=\"560\" height=\"315\" src=\"https://www.youtube.com/embed/nTeiHioEO4w\" frameborder=\"0\" allow=\"accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture\" allowfullscreen></iframe>'\n",
    ")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "\n",
    "![title](https://cmastalli.github.io/assets/img/publications/astronaut360_v2.png)"
   ]
  }
 ],
 "metadata": {
  "celltoolbar": "Attachments",
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.17"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}