5_2_controlopt.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Reaching multiple targets with a manipulator\n",
    "The objective of this exercise is to reach multiple targets with a manipulator.\n",
    "\n",
    "We provide a basic example for reaching one point, and you have to modify it for sequence of multiple targets. Below it is the basic example, there we'll guide you to the final result.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up"
   ]
  },
  {
   "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"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will need crocoddyl as in the previous notebook, with the model of the arm of the humanoid robot Talos, a 7-dof arm. It can be found in example robot data."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import crocoddyl; crocoddyl.switchToNumpyMatrix()\n",
    "import pinocchio\n",
    "import numpy as np\n",
    "import example_robot_data"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## The optimal-control program"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load -r 17-93 tp5/arm_example.py\n",
    "# First, let's load the Pinocchio model for the Talos arm.\n",
    "robotWrapper = example_robot_data.loadTalosArm()\n",
    "robotWrapper.initViewer(loadModel=True)\n",
    "viewer = robotWrapper.viewer.gui\n",
    "\n",
    "# Set robot model\n",
    "robot_model = robotWrapper.model\n",
    "robot_model.armature =np.matrix([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.]).T*5\n",
    "robot_model.q0 = np.matrix([3.5,2,2,0,0,0,0]).T\n",
    "robot_model.x0 = np.concatenate([robot_model.q0, pinocchio.utils.zero(robot_model.nv)])\n",
    "robot_model.gravity *= 0\n",
    "\n",
    "# Configure task\n",
    "#FRAME_TIP = robot_model.getFrameId(\"gripper_left_joint\")\n",
    "FRAME_TIP = robot_model.getFrameId(\"gripper_left_fingertip_3_link\")\n",
    "goal = np.matrix([.2,0.5,.5]).T\n",
    "\n",
    "# Configure viewer\n",
    "viewer.addXYZaxis('world/frame',[1.,0,0,1],.03,.1)\n",
    "#viewer.addSphere('world/goal',.1,[1.,0,0,1])\n",
    "viewer.addXYZaxis('world/goal',[0,1,0,1],.03,.1)\n",
    "viewer.applyConfiguration('world/goal',[0.2,0.5,.5,0,0,0,1])\n",
    "viewer.refresh()           \n",
    "\n",
    "# Create a cost model per the running and terminal action model.\n",
    "state = crocoddyl.StateMultibody(robot_model)\n",
    "runningCostModel = crocoddyl.CostModelSum(state)\n",
    "terminalCostModel = crocoddyl.CostModelSum(state)\n",
    "\n",
    "# Note that we need to include a cost model (i.e. set of cost functions) in\n",
    "# order to fully define the action model for our optimal control problem.\n",
    "# For this particular example, we formulate three running-cost functions:\n",
    "# goal-tracking cost, state and control regularization; and one terminal-cost:\n",
    "# goal cost. First, let's create the common cost functions.\n",
    "Mref = crocoddyl.FramePlacement(FRAME_TIP,pinocchio.SE3(np.eye(3), goal))\n",
    "#goalTrackingCost = crocoddyl.CostModelFramePlacement(state, Mref)\n",
    "pref = crocoddyl.FrameTranslation(FRAME_TIP,goal)\n",
    "goalTrackingCost = crocoddyl.CostModelFrameTranslation(state, pref)\n",
    "weights=crocoddyl.ActivationModelWeightedQuad(np.array([1,1,1,1,1,1,1, 1,1,1,1,2,2,2.]))\n",
    "xRegCost = crocoddyl.CostModelState(state,weights,robot_model.x0)\n",
    "uRegCost = crocoddyl.CostModelControl(state)\n",
    "weightsT=crocoddyl.ActivationModelWeightedQuad(np.array([.01,.01,.01,.01,.01,.01,.01, 1,1,1,1,2,2,2.]))\n",
    "xRegCostT = crocoddyl.CostModelState(state,weights,robot_model.x0)\n",
    "\n",
    "# Then let's added the running and terminal cost functions\n",
    "runningCostModel.addCost(\"gripperPose\", goalTrackingCost, .001)\n",
    "runningCostModel.addCost(\"xReg\", xRegCost, 1e-3)\n",
    "runningCostModel.addCost(\"uReg\", uRegCost, 1e-6)\n",
    "terminalCostModel.addCost(\"gripperPose\", goalTrackingCost, 10)\n",
    "terminalCostModel.addCost(\"xReg\", xRegCostT, .01)\n",
    "\n",
    "# Next, we need to create an action model for running and terminal knots. The\n",
    "# forward dynamics (computed using ABA) are implemented\n",
    "# inside DifferentialActionModelFullyActuated.\n",
    "actuationModel = crocoddyl.ActuationModelFull(state)\n",
    "dt = 1e-2\n",
    "runningModel = crocoddyl.IntegratedActionModelEuler(\n",
    "    crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, runningCostModel), dt)\n",
    "runningModel.differential.armature = robot_model.armature\n",
    "terminalModel = crocoddyl.IntegratedActionModelEuler(\n",
    "    crocoddyl.DifferentialActionModelFreeFwdDynamics(state, actuationModel, terminalCostModel), 0.)\n",
    "terminalModel.differential.armature = robot_model.armature\n",
    "\n",
    "# For this optimal control problem, we define 250 knots (or running action\n",
    "# models) plus a terminal knot\n",
    "T = 100\n",
    "problem = crocoddyl.ShootingProblem(robot_model.x0, [runningModel] * T, terminalModel)\n",
    "\n",
    "# Creating the DDP solver for this OC problem, defining a logger\n",
    "ddp = crocoddyl.SolverDDP(problem)\n",
    "ddp.setCallbacks([\n",
    "    crocoddyl.CallbackLogger(),\n",
    "    crocoddyl.CallbackVerbose(),\n",
    "])\n",
    "\n",
    "# Solving it with the DDP algorithm\n",
    "ddp.solve([],[],1000)  # xs_init,us_init,maxiter"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can now look at the results, either in Gepetto-viewer by running the trajectory, or by plotting it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load -r 103-104 tp5/arm_example.py\n",
    "import tp5.croco_utils as crocutils\n",
    "crocutils.displayTrajectory(robotWrapper,ddp.xs)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And plotting..."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %load -r 97-99 tp5/arm_example.py\n",
    "log = ddp.getCallbacks()[0]\n",
    "crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=False)\n",
    "crocoddyl.plotConvergence(log.costs, log.control_regs, log.state_regs, log.gm_stops, log.th_stops, log.steps, figIndex=2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## I. DifferentialActionModel for Pinocchio ABA\n",
    "This scenario uses an action model that computes 2nd order differential dynamics with Pinocchio. Note that it can accept several cost models. This action model is tailored for robot applications, and at the same time, it's modular since:\n",
    " - you can modify the robot dynamics by changing Pinocchio model, and\n",
    " - you can formulate any cost function by simply adding running a terminal costs.\n",
    "\n",
    "## II. Cost models\n",
    "\n",
    "A cost model computes a scalar cost value and its gradient and Hessian. All the models implemented are computing a cost residual and are computing the Hessian with the Gauss approximation.\n",
    "\n",
    "We implemented reusable cost models for controlling \n",
    " - a frame placement (translation or velocity),\n",
    " - the center of mass position, and \n",
    " - state  and control spaces.\n",
    "\n",
    "In the example above, we used the CostModelFrameTranslation which defines a 3d position task, and the state and control regularizers.\n",
    "\n",
    "As for any cost model in crocoddyl, if you write your own cost model you need to create a data class for your cost function. The cost data must be created from a pinocchio data (the rational is that the pinocchio data used to compute the dynamics should be re-used to compute the cost).\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "trackData = goalTrackingCost.createData(robot.data)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### II.a Frame position cost\n",
    "\n",
    "You define a frame ID and the reference position as a 3D array. The cost is the distance between the frame and the target. This cost depends on $\\mathbf{x}$ (specifically the configuration $\\mathbf{q}$). You can double check the 0s in its gradient."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pinocchio.updateFramePlacements(robot.model, robot.data)\n",
    "pinocchio.computeJointJacobians(robot.model, robot.data, xT[:state.nq])\n",
    "goalTrackingCost.calcDiff(trackData, x0)\n",
    "print(trackData.Lx, trackData.Lu)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### II.b State cost\n",
    "In this part of the tutorial you must define a State model. It defines \n",
    " - the dimension of the state and its tangent, and\n",
    " - the exponential/integrate and difference/log operators.\n",
    "The operators can described using Pinocchio functions. And the exercite consists on adding them into your State class. Please note crocoddyl has abstract functions for this.\n",
    "\n",
    "The state cost uses a reference in state space (State.zero() by default). The cost is the distance, computed with state.difference between the current state and the reference. Hence, with this cost, we regularize both position and velocity.\n",
    "\n",
    "### II.c Control cost\n",
    "\n",
    "The control cost uses a control reference as in the state cost. The cost is the distance the current control and the reference. Hence the cost regularizes torque commands."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### II.d Add cost models to the differential action model\n",
    "Each time we want to include a new cost function, we use addCost function inside our DAM. In this function you're also able its weight."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## III. Create the problem with integrated action model\n",
    "Differential action models describe cost and dynamics in continuous-time, however our optimal control solvers work in discrete-time. We have created the integrated action model in order to deal with this.\n",
    "\n",
    "In the previous code, we have used an abstract class that uses simpletic Euler rules. In the cartpole exercise you have learnt how to use integrated action models for your problem."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## IV. Callbacks\n",
    "\n",
    "Callback functions are needed for analysing and debugging the performance of the solver for your specific problem.\n",
    "For problems defined with Pinocchio, you can display the robot trajectory per each iterate by including CallbackDisplay. With this callback, you can display robot motions with different rates. Additionally, CallbackVerbose prints a message that allows us to understand the behaviour of the solver.\n",
    "\n",
    "Generally speaking, an user is able to describe any callback function. This function will be run once per iterate and it has access to all data."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## VI. Modifying the example\n",
    "\n",
    "Start by defining several targets (let's say 4 targets, all at x=0.4, and at y and z being either 0 or 0.4), and display then in the viewer.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The shooting problem will be composed of 4 sequences of action models. Each sequence consists on T shooting \"running\" nodes and 1 terminal node. The running nodes mostly have regularization terms, while the terminal nodes have a strong cost toward the respective target.\n",
    "\n",
    "[ R1,R1,R1 ... R1,T1, R2,R2 .... R2, T2, R3 ... R3, T3, R4 ... R4 ] , T4\n",
    "\n",
    "First create 4 running models and 4 terminal models."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then you need to add a position cost, and state and control regularization to each running action model. Please  note that for terminal action model is only needed the position cost. Additionally, in the running models, the position cost should be low, and it should be high in the terminal models."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now create a shooting problem."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "seq0 = [runningModels[0]]*T + [terminalModels[0]]\n",
    "seq1 = [runningModels[1]]*T + [terminalModels[1]]\n",
    "seq2 = [runningModels[2]]*T + [terminalModels[2]]\n",
    "seq3 = [runningModels[3]]*T \n",
    "problem = crocoddyl.ShootingProblem(x0,seq0+seq1+seq2+seq3,terminalmodel[3])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Create a DDP solver for this problem and run it. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ddp = crocoddyl.SolverDDP(problem)\n",
    "ddp.solve()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Well, it should not work, at least no on the first shot. The DDP solver is likely not strong enough to accept the random weights that you have selected. \n",
    "\n",
    "If it is working nicely from the first shot, display it in the viewer and go take a coffee. But you will likely have to tweak the gains to make it work.\n",
    "\n",
    "**It is suggested to first optimize only sequence 1. When you are happy with it, add sequence 2 and optimize again, etc.**\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## V. Penalty\n",
    "The solver works with double precisions, so it is quite robust to high weight. 10000 is likely to be accepted for example. But if you make the problem too difficult, the solver will break. \n",
    "\n",
    "In that case, you can implement a simple penalty solver by setting the weight to be 10**i, and creating a for loop to explore i from 0 to 5. At each iteration of the loop, run the solver from the previous solution and for few iterations only."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for i in range(1,6):\n",
    "    for m in terminalModels:\n",
    "        m.costs.costs['gripperPose'].weight = 10**i\n",
    "    ddp.solve(ddp.xs, ddp.us, 10)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "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.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}