5_1_controlopt_intro.ipynb

{
 "cells": [
  {
   "attachments": {
    "image.png": {
     "image/png": "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"
    }
   },
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Starting example: the unicycle\n",
    "\n",
    "![image.png](attachment:image.png)\n",
    "\n",
    "An unicycle represents a kinematic model of a car where it's only possible to move in two directions, i.e. it drives forward and turns on the spot. Its dynamics has nonholonomic constraints because it cannot move sideways. Remember that nonholonomic constraints are nonintegral and has the form $\\mathbf{f(q,\\dot{q})=0}$.\n",
    "\n",
    "In this example, we define an optimal-control problem for the classical unicycle problem. Our goal is to drive the unicycle towards the origin but at the same time not too fast. For that, the cost function is described as the sum between the distance to the origin and the system speed.\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Setup"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import magic_donotload"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "An optimal control problem is:\n",
    "$$\\min_u \\int_0^T L(x(t),u(t))dt + \\Phi(x(T))$$\n",
    "$$\\dot{x} = f(x,u)$$\n",
    "where $x$ is the state variable, $u$ the control. For contact and collision, we could add continuous equality and/or equality constraint.\n",
    "\n",
    "Control in acceleration through as with the dynamic like in the previous TP is achieved thanks to using $\\tilde{x} = \\left(\\begin{array}{c}\n",
    "  x \\\\\n",
    "  \\dot{x}\n",
    "\\end{array}\\right)\n",
    "$, $u=\\tau$ and $f(\\tilde{x},u) = \\left(\\begin{array}{c}\n",
    "  \\dot{x} \\\\\n",
    "  dynamic(x, \\dot{x}, \\tau)\n",
    "\\end{array}\\right)\n",
    "$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Crocoddyl\n",
    "Crocoddyl stands for Contact robot control with differential dynamic programming library. We will need it for formulating and solving the optimal control problems. \n",
    "\n",
    "DDP is a second order method to solve the direct discretized version of the previous problem in a simple shooting flavor. As for previous problematic in robotic, recent research propose variation of DDP usefull in the case of robotic. [proxDDP](https://hal.archives-ouvertes.fr/hal-03597630/document)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Introducing Crocoddyl\n",
    "Basically, our optimal control problem has the following dynamical model and cost function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "x = np.random.rand(3)  # x, y, t\n",
    "u = np.random.rand(2)  # v, w\n",
    "\n",
    "# Unicycle dynamical model\n",
    "v,w = u\n",
    "c,s = np.cos(x[2]),np.sin(x[2])\n",
    "dt = 1e-2\n",
    "dx = np.array([ v*c, v*s, w ])  # In world ref\n",
    "xnext = x + dx*dt\n",
    "\n",
    "# Cost function: driving to origin (state) and reducing speed (control)\n",
    "stateWeight = 1\n",
    "ctrlWeight = 1\n",
    "costResiduals = np.concatenate([stateWeight*x,ctrlWeight*u])\n",
    "cost = .5* sum(costResiduals**2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For this basic example, the unicycle model is coded in the library [Crocoddyl](https://gepettoweb.laas.fr/doc/loco-3d/crocoddyl/master/doxygen-html/). We will just load it and use it. If you are very curious, have a look! It is in crocoddyl/unicycle.py."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here the cost is $L(x,u) = \\frac{1}{2}(A_s \\|x\\|^2 + A_u \\|u\\|^2)$, it encode the goal of driving the car to the origin, while using as less trust as possible. $A_s$ and $A_u$ tune the tradeoff between control use and importance of reaching the target soon enough.."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We create such a model with the following lines:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import crocoddyl\n",
    "model = crocoddyl.ActionModelUnicycle()\n",
    "data  = model.createData()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The action model contains ... well ... the description of the dynamics and cost function. There you find also the action model parameters (here the time step and the cost weights). On the other hand, the data has the buffers where the results of the calculus are stored.\n",
    "\n",
    "We decided for this separation for an obvious reason that is given just below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "model.costWeights = np.array([\n",
    "    1,   # state weight\n",
    "    1  # control weight\n",
    "])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**You can further understand the mathematical definition of action models see introduction_to_crocoddyl.ipynb**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## I. Defining the shooting problem\n",
    "A shooting problem is defined by the initial state from which computing the rollout and a sequence of action models.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x0 = np.matrix([ -1., -1., 1. ]).T #x,y,theta\n",
    "T  = 20\n",
    "problem = crocoddyl.ShootingProblem(x0, [ model ] * T, model)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here we define a problem starting from $\\mathbf{x}_0$ with 20 timesteps (of 0.1 sec by default implementation of unicycle). The terminal action model is defined using the running action model.\n",
    "\n",
    "This defines the model, not any algorithm to solve it. The only computation that the problem can provide is to integrate the system for a given sequence of controls."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "us = [ np.matrix([1., 1.]).T for t in range(T)]\n",
    "xs = problem.rollout(us)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The plotUnicycle function plots the system as two arrows that represent the wheels"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import matplotlib.pylab as plt\n",
    "from tp5.unicycle_utils import plotUnicycle\n",
    "for x in xs: plotUnicycle(x)\n",
    "plt.axis([-2,2.,-2.,2.])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## II. Solve the OCP\n",
    "The main solver is named SolverDDP. It is initialized from the problem object and mostly contains the ddp.solve method. We can warm start it and tune the parameters, but for the simple unicycle, let's just solve it!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ddp = crocoddyl.SolverDDP(problem)\n",
    "done = ddp.solve()\n",
    "assert(done)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.clf()\n",
    "for x in ddp.xs: plotUnicycle(x)\n",
    "plt.axis([-2,2,-2,2])\n",
    "print(ddp.xs[-1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and the final state is:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(ddp.xs[-1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Well, the terminal state is not so nicely in the origin.\n",
    "\n",
    "Question 1: why?\n",
    "\n",
    "Question 2: How can you change this? Create a new DDP problem\n",
    "\n",
    "Question 3: by changing the cost parameters, the time horizon and the initial position, can you trigger a maneuver?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Your code"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp5/generated/unicycle_solutions_termmodel"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "ddp.solve()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for x in ddp.xs: plotUnicycle(x)\n",
    "plt.axis([-2,2.,-2.,2.])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(ddp.xs[-1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# How to use DDP for real robot control\n",
    "In its natural form DDP tackle the problem of the whole trajectory for a problem. In a a sence it is a motion planning method, however in contract with RRT and/or PRM it is not a global space trajectory. DDP can be use to generate a reference trajectory for a task, and PD controller can be use to follow it with a robot. An other approach callled Modele Predictive Control (MPC) is to solve the DDP on some given horizon and take the first fex step of the control solution as an actual controller for the robot. However despite MPC being of a great appeal, there is a lot of computationnal challenges to be able to control robot at 100Hz."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}