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 gepetuto.magic"
   ]
  },
  {
   "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. "
   ]
  },
  {
   "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)\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 = .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",
    "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": [
    "# %load tp5/generated/unicycle_solutions_hyperparams\n",
    "### HYPER PARAMS: horizon and initial state\n",
    "T  = 100\n",
    "x0 = np.array([-1,-1,1])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "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.array([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 plotUnicycleSolution\n",
    "plotUnicycleSolution(xs)\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": [
    "plotUnicycleSolution(ddp.xs)\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?\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": [
    "%do_not_load_snippet 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": [
    "plotUnicycleSolution(ddp.xs)\n",
    "plt.axis([-2,2.,-2.,2.]);"
   ]
  }
 ],
 "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.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}