examples/notebooks/unicycle_towards_origin.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",
    "\n",
    "**Issue on Ubuntu 16.04**\n",
    "\n",
    "You need to upgrate the matplotlib library, please do:\n",
    " - pip install --upgrade --user pip\n",
    " - pip install --upgrade --user matplotlib\n",
    "\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",
    "import matplotlib.pylab as plt\n",
    "\n",
    "x = np.random.rand(3)\n",
    "u = np.random.rand(2)\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])\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 = 0.5 * sum(costResiduals**2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For this basic example, the unicycle model is coded in the library. 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": [
    "We create such a model with the following lines:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import crocoddyl\n",
    "\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.matrix([1, 1]).T  # state weight  # control weight"
   ]
  },
  {
   "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.0, -1.0, 1.0]).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.0, 1.0]).T for _ 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",
    "from unicycle_utils import plotUnicycle\n",
    "\n",
    "for x in xs:\n",
    "    plotUnicycle(x)\n",
    "plt.axis([-2, 2.0, -2.0, 2.0])"
   ]
  },
  {
   "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:\n",
    "    plotUnicycle(x)\n",
    "plt.axis([-2, 2, -2, 2])"
   ]
  },
  {
   "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?\n",
    "\n",
    "Question 3: by changing the cost parameters, the time horizon and the initial position, can you trigger a maneuver?"
   ]
  }
 ],
 "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.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}