{ "cells": [ { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Optimal Control and Parameter Identification of Dynamcal Systems with Direct Collocation and SymPy\n", "\n", "Jason K. Moore \n", "July 8, 2015 \n", "SciPy 2015, Austin, Texas, USA\n", "\n", "\n", "\n", "\n", "\n", "\n", "\n", "
\n", "\n", "\n", "\n", "\n", "\n", "
" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Motivation: Identification of Gait Control\n", "\n", "" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Previous Use Cases: Optimal Gait on Earth, Moon, Mars\n", "\n", "> Ackermann, M. and van den Bogert, A. J. Predictive simulation of gait at low gravity reveals skipping as the preferred\n", "locomotion strategy. Journal of Biomechanics, 45, 2012\n", "\n", "\n", " \n", " \n", " \n", " \n", "
\n", " \n", "

Earth: g=9.8 m/s-2

\n", " 		\n", " \n", "

Moon: g=1.6 m/s-2

\n", "
" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Trajectory Optimization Example\n", "\n", "\n", "\n", "## 1-DoF, 1 parameter pendulum equations of motion\n", "\n", "$\\dot{\\mathbf{x}} = \\begin{bmatrix} \\dot{\\theta}(t) \\\\ \\dot{\\omega}(t) \\end{bmatrix} = \\begin{bmatrix} \\omega(t) \\\\ \\frac{g}{l} \\sin{\\theta}(t) + \\mathbf{T} \\end{bmatrix}$\n", "\n", "## Objective: Minimize \"effort\" and bound the states at\n", "\n", "$J(\\mathbf{T}) = \\min\\limits_{\\mathbf{T}} \\int_{t_0}^{t_f} \\mathbf{T}^2 dt$\n", "\n", "## Boundary conditions\n", "\n", "- $\\mathbf{\\theta}(t_0) = 0$\n", "- $\\mathbf{\\theta}(t_f) = \\pi$\n", "\n", "> Pendulum figure: https://commons.wikimedia.org/wiki/File:Pendulum_gravity.svg, Creative Commons Attribution-Share Alike 3.0 Unported" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Parameter Identification\n", "\n", "Find the parameters $\\mathbf{p}$ such that the difference between the model simulation, $\\mathbf{y}$, and measurements, $\\mathbf{y}_m$ is minimized.\n", "\n", "### Dynamic system\n", "\n", "- Equations of Motion: $\\dot{\\mathbf{x}} = \\mathbf{f}(\\mathbf{x}, \\mathbf{p})$\n", "- Measurement variables: $\\mathbf{y} = \\mathbf{g}(\\mathbf{x}, \\mathbf{p})$\n", "\n", "### Objective\n", "\n", "$$\\min\\limits_\\mathbf{p} J(\\mathbf{p})$$\n", "where\n", "$$J(\\mathbf{p}) = \\int [\\mathbf{y}_m - \\mathbf{y}(\\mathbf{p})]^2 dt$$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Optimal Control By Shooting\n", "\n", "- Repeated simulations are computationally costly (i.e. stiff systems are especially slow)\n", "- Maybe limited to parameterized functions (e.g. polynomials, piecewise constants)\n", "- Too many free variables\n", "- Systems may be unstable and thus have an ill-defined objective\n", "- Local minima are inevitable\n", " - May requires a superb guess\n", " - May need time intensive global optimization methods" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Local Minima Example: Simple pendulum\n", "\n", "> Vyasarayani, Chandrika P., Thomas Uchida, Ashwin Carvalho, and John McPhee.\n", "\"Parameter Identification in Dynamic Systems Using the Homotopy Optimization\n", "Approach\". Multibody System Dynamics 26, no. 4 (2011): 411-24.\n", "\n", "\n", "\n", "## 1-DoF, 1 parameter pendulum equations of motion\n", "\n", "$\\dot{\\mathbf{x}} = \\begin{bmatrix} \\dot{\\theta}(t) \\\\ \\dot{\\omega}(t) \\end{bmatrix} = \\begin{bmatrix} \\omega(t) \\\\ -p \\sin{\\theta}(t) \\end{bmatrix}$\n", "\n", "## Objective: Minimize least squares\n", "\n", "$J(p) = \\min\\limits_{p} \\int_{t_0}^{t_f} [\\theta_m(t) - \\theta(\\mathbf{x}, p, t)]^2 dt$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Direct Collocation\n", "\n", "\n", "## Benefits\n", "\n", "\n", "\n", "## Disadvantages\n", "\n", "" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Our Direct Collocation Implementation\n", "\n", "### Implicit Continous Equations of Motion\n", "\n", "No need to solve for $\\dot{\\mathbf{x}}$.\n", "\n", "$$\\mathbf{f}(\\dot{\\mathbf{x}}, \\mathbf{x}, \\mathbf{r}, \\mathbf{p}, t) = 0$$\n", "\n", "- $\\mathbf{x}, \\dot{\\mathbf{x}}$: states and their derivatives\n", "- $\\mathbf{r}$: exogenous inputs\n", "- $\\mathbf{p}$: constant parameters" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Discretization\n", "\n", "### First order discrete integration options:\n", "\n", "Backward Euler:\n", "\n", "$\\mathbf{f}(\\frac{\\mathbf{x}_i - \\mathbf{x}_{i-1}}{h} , \\mathbf{x}_i, \\mathbf{r}_i, \\mathbf{p}, t_i) = 0$\n", "\n", "Midpoint Rule:\n", "\n", "$\\mathbf{f}(\\frac{\\mathbf{x}_{i + 1} - \\mathbf{x}_i}{h}, \\frac{\\mathbf{x}_i + \\mathbf{x}_{i + 1}}{2}, \\frac{\\mathbf{r}_i + \\mathbf{r}_{i + 1}}{2}, \\mathbf{p}, t_i) = 0$\n", "\n", "### Nonlinear Programming Formulation\n", " \n", "$$\\min\\limits_{\\theta} J(\\theta)$$\n", "\n", "$$\\textrm{where } \\mathbf{\\theta} = [\\mathbf{x}_i, \\dots, \\mathbf{x}_N, \\mathbf{r}_i, \\ldots, \\mathbf{r}_N, \\mathbf{p}]$$\n", "\n", "$$\\textrm{subject to } \\mathbf{f}(\\mathbf{x}_i, \\mathbf{x}_{i+1}, \\mathbf{r}_i, \\mathbf{r}_{i+1}, \\mathbf{p}, t_i) = 0 \\textrm{ and } \\theta_L \\leq \\theta \\leq \\theta_U$$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Software Tool: opty\n", "\n", "## http://github.com/csu-hmc/opty\n", "\n", "- User specifies continous symbolic:\n", " - objective\n", " - equations of motion (explicit or implicit)\n", " - additional constraints\n", " - bounds on free variables: $\\mathbf{x}, \\mathbf{r}, \\mathbf{p}$\n", "- EoMs can be generated with PyDy (http://pydy.org)\n", "- Effficient just-in-time compiled C code is generated for functions that evaluate:\n", " - objective and its gradient\n", " - constraints and its Jacobian\n", "- NLP problem automatically formed for IPOPT\n", "- Open source: BSD license\n", "- Written in Python" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Example Code: Specify Equations of Motion\n", "\n", "
\n", "\n", "$$\\dot{\\mathbf{x}} = \\begin{bmatrix} \\dot{\\theta}(t) \\\\ \\dot{\\omega}(t) \\end{bmatrix} = \\begin{bmatrix} \\omega(t) \\\\ -p \\sin{\\theta}(t) + T(t) \\end{bmatrix}$$\n", "\n", "### Symbolic EoM\n", "\n", "```python\n", "# Specify symbols for the parameters\n", "p, t = symbols('p, t')\n", "\n", "# Specify the functions of time\n", "theta, omega, theta_m, T = symbols('theta, omega, theta_m, T', cls=Function)\n", "\n", "# Specify the symbolic equations of motion\n", "eom = Matrix([theta(t), omega(t)]).diff(t) - Matrix([omega(t), -p * sin(theta(t) + T(t))])\n", "```\n", "\n", "### Discretization\n", "\n", "```python\n", "# Choose discretization values\n", "num_nodes = 1000\n", "interval = 0.01 # seconds\n", "```" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Example Code: Trajectory Optimization\n", "\n", "### Objective\n", "\n", "```python\n", "obj = Integral(T(t)**2, t)\n", "```\n", "\n", "### Boundary Contraints\n", "```python\n", "boundary_constraints = (theta(0.0), theta(duration) - pi / 2, omega(0.0), omega(duration))\n", "```\n", "\n", "### Define Problem\n", "\n", "```python\n", "prob = Problem(obj, # symbolic objective\n", " eom, # symbolic equations of motion\n", " (theta(t), omega(t)), # system symbolic states\n", " num_nodes,\n", " interval,\n", " known_parameter_map={p: 9.81},\n", " instance_constraints=boundary_constraints)\n", "```" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Example Code: Single Parameter Identification\n", "\n", "### Objective\n", "\n", "```python\n", "obj = Integral((theta_m(t) - theta(t))**2, t)\n", "```\n", "\n", "### Form Problem\n", "\n", "```python\n", "prob = Problem(obj,\n", " eom,\n", " (theta(t), omega(t)),\n", " num_nodes,\n", " interval,\n", " known_trajectory_map={T(t): 0.0, y1_m(t): measured_data},\n", " integration_method='midpoint')\n", "```" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Example Code: Solve\n", "```python\n", "# Set an initial guess\n", "initial_guess = random(prob.num_free)\n", "\n", "# Solve the system\n", "solution, info = prob.solve(initial_guess)\n", "```" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# The Problem Class: Objective\n", "\n", "1. Converts objective integral to discrete form\n", "2. Computes analytic gradient of objective wrt to discrete variables\n", "3. Generates efficient NumPy functions for objective and gradient\n", "\n", "*Still in development" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# The Problem Class: Constraints\n", "\n", "1. Converts continous EoM to discrete versions\n", "2. Computes the analytic Jacobian wrt to discrete variables\n", "3. Generates efficient C code to \"ufuncify\" discrete EoM and Jacobian evaluations\n", "4. Generates Cython wrapper for C code\n", "5. Compiles Cython wrapper\n", "6. NumPy functions are generated for instance constraints" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "### The Problem Class: Solve\n", "\n", "1. IPOPT data structure is formed with cyipopt\n", "2. Bounds on variables are set\n", "3. IPOPT settings are set\n", "4. Solve is called!" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Computational Speed\n", "\n", "### Example Larger System\n", "\n", "" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Computational Speed\n", "\n", "### Discretization Variables\n", "\n", "- 10,000 collocation nodes\n", "- 219,978 constraints\n", "- 14,518,548 nonzero entries in the Jacobian\n", "- 220,022 free variables\n", "\n", "### Timings\n", "\n", "- Integrating with ODEPACK lsoda: **5.6 s**\n", "- Constraint evaluation: **33 ms (0.033 s)**\n", "- Jacobian evaluation: **128 ms (0.128 s)**" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Case Study: Minamal Effort Double Pendulum Swing Up\n", "\n", "\n", "\n", "\n", "" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Trajectory Opimization Problem Specification\n", "\n", "Given the boundary conditions can we find the optimal input trajectory such that the \"effort\" is minimized?\n", "\n", "$$\\min\\limits_\\theta J(\\mathbf{\\theta}), \\quad\n", "J(\\mathbf{\\theta})= \\sum_{i=1}^N h [\\mathbf{T}_i]^2$$\n", "\n", "where\n", "\n", "$$ \\mathbf{\\theta} = [\\mathbf{x}_1, \\ldots, \\mathbf{x}_N, \\mathbf{T}_i, \\ldots \\mathbf{T}_N] $$\n", "\n", "Subject to the constraints:\n", "\n", "$$\\mathbf{f}(\\mathbf{x}_i, \\mathbf{T}_i)=0, \\quad i=1 \\ldots N$$\n", "\n", "And the initial guess:\n", "\n", "$$\\mathbf{\\theta}_0 = [\\mathbf{0}]$$\n", "\n", "For, $N$ = 500:\n", "\n", "- 24008 free variables\n", "- Jacobian matrix with 38933 non-zero entries" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Demo\n", "\n", "" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Case Study: Human Control Parameter Identification\n", "\n", "
\n", "

Plant

\n", " \n", " \n", "

Open Loop Equations of Motion

\n", "

$$\\dot{\\mathbf{x}} = \\mathbf{f}_o(\\mathbf{x}, \\mathbf{r}_c, \\mathbf{r}_k, \\mathbf{p}_k, t)$$

\n", "
" ] }, { "cell_type": "markdown", "metadata": { "collapsed": true, "slideshow": { "slide_type": "slide" } }, "source": [ "# Lumped Passive+Active Controller\n", "\n", "- True human controller is practically impossible to isolate and identify\n", "- Identify a controller for a similar system that causes the same behavior as the real system\n", "\n", "## Simple State Feedback\n", "\n", "$$\\mathbf{r}_c(t) = -\\mathbf{K}\\mathbf{x}(t)$$\n", "\n", "## Unknown Parameters\n", "\n", "$$\\mathbf{p}_u = \\mathrm{vec}(\\mathbf{K})$$\n", "\n", "## Closed Loop Equations of Motion\n", " \n", "$$\\dot{\\mathbf{x}} = \\mathbf{f}_c(\\mathbf{x}, \\mathbf{r}_k, \\mathbf{p}_k, \\mathbf{p}_u, t)$$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Generate Data\n", "\n", "## Specify the psuedo-random platform acceleration\n", " \n", "$$a(t)=\\sum_{i=1}^{12} A_i\\sin(\\omega_i t) \\quad \\mathrm{where,} \\quad 0.15 \\mathrm{rad/s} < \\omega_i < 15.0 \\mathrm{rad/s}$$\n", "\n", "### Choose a stable controller\n", "\n", "$$\n", "\\mathbf{K} =\n", "\\begin{bmatrix}\n", " 950 & 175 & 185 & 50 \\\\\n", " 45 & 290 & 60 & 26\n", "\\end{bmatrix}\n", "$$" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Generate Data\n", "\n", "### Simulate closed loop system under the influence of perturbations for 60 seconds, sampled at 100 Hz\n", " \n", "$$ \\dot{\\mathbf{x}} = \\mathbf{f}_c(\\mathbf{x}, \\mathbf{r}_k, \\mathbf{p}_k, \\mathbf{p}_u, t) $$\n", "\n", "### Add Gaussian measurement noise\n", "\n", "$$\\mathbf{x}_m(t) = \\mathbf{x}(t) + \\mathbf{v}_x(t) \\\\ a_m(t) = a(t) + v_a(t)$$\n", "\n", "- $\\sigma_\\theta$ = 0.3 deg\n", "- $\\sigma_\\omega$ = 4 deg/s\n", "- $\\sigma_a$ = 0.42 ms-2" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Parameter Identification Problem Specification\n", "\n", "Given noisy measurements of the states, $\\mathbf{x}_m$, and the platform acceleration, $a_m$, can we identify the controller parameters $\\mathbf{K}$?\n", "\n", "$$\\min\\limits_\\theta J(\\mathbf{\\theta}), \\quad\n", "J(\\mathbf{\\theta})= \\sum_{i=1}^N h [\\mathbf{x}_{mi} - \\mathbf{x}_i]^2$$\n", "\n", "where\n", "\n", "$$ \\mathbf{\\theta} = [\\mathbf{x}_1, \\ldots, \\mathbf{x}_N, \\mathbf{p}_u] $$\n", "\n", "Subject to the constraints:\n", "\n", "$$\\mathbf{f}_{ci}(\\mathbf{x}_i, a_{mi}, \\mathbf{p}_u)=0, \\quad i=1 \\ldots N$$\n", "\n", "And the initial guess:\n", "\n", "$$\\mathbf{\\theta}_0 = [\\mathbf{x}_{m1}, \\ldots, \\mathbf{x}_{mN}, \\mathbf{0}]$$\n", "\n", "For, $N$ = 6000:\n", "\n", "- 24008 free variables\n", "- 23996 x 24008 Jacobian matrix with 384000 non-zero entries" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Demo\n", "\n", "" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Results\n", "\n", "Converges in **11 iterations** in **2.8 seconds** of computation time.\n", "\n", "\n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", " \n", "
KnownIdentifiedError
$k_{00}$950946-0.4%
$k_{01}$1751771.4%
$k_{02}$185185-0.2%
$k_{03}$50559.4%
$k_{10}$45451.1%
$k_{11}$290289-0.3%
$k_{12}$6059-2.1%
$k_{13}$26274.2%
\n" ] }, { "cell_type": "markdown", "metadata": { "collapsed": true, "slideshow": { "slide_type": "slide" } }, "source": [ "# Identified State Trajectories\n", "\n", "![](trajectory-comparison.png)" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Conclusion\n", "\n", "- Direct collocation is suitable for biomechanical parameter identification and trajectory optimization\n", "- Computation speeds are orders of magnitude faster than shooting\n", "- Parameter identification accuracy improves with # nodes\n", "- Complex problems can be solved with few lines of code and high level mathematical abstractions" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Questions?\n", "\n", "- Social media: moorepants\n", "- Personal website: http://moorepants.info\n", "- SymPy: http://sympy.org\n", "- PyDy: http://pydy.org\n", "- opty: http://github.com/csu-hmc/opty\n", "- Human Motion and Control Lab: http://hmc.csuohio.edu" ] } ], "metadata": { "celltoolbar": "Slideshow", "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.10" } }, "nbformat": 4, "nbformat_minor": 0 }