{ "cells": [ { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Human Standing Control Parameter Identification with Direct Collocation\n", "\n", "Jason K. Moore \n", "Ton van den Bogert \n", " \n", "\n", "\n", "July 10, 2015 \n", "ISB TGCS 2015, Edinburgh, UK" ] }, { "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": [ "# Parameter Identification By Shooting\n", "\n", "- Repeated simulations are computationally costly\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", "> Betts, J., and W. Huffman. “Large Scale Parameter Estimation Using Sparse Nonlinear Programming Methods.” SIAM Journal on Optimization 14, no. 1 (January 1, 2003): 223–44. doi:10.1137/S1052623401399216.\n", "\n", "## Benefits\n", "\n", "- Fast computation times\n", "- Handles unstable systems with ease\n", "- Less susceptible to local minima\n", "\n", "## Disadvantages\n", "\n", "- Accurate solution requires large number of nodes\n", "- Memory management for large sparse matrices and operations\n", "- Tedious and error prone to form gradients, Jacobians, and Hessians" ] }, { "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", "- 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\n", "- http://github.com/csu-hmc/opty\n" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Example Code: 1 DoF, 1 Parameter Pendulum\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) \\end{bmatrix}$$\n", "\n", "Paraphrased from https://github.com/csu-hmc/opty/blob/master/examples/vyasarayani2011.py\n", "\n", "### Symbolics\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 = symbols('theta, omega, theta_m', cls=Function)\n", "\n", "# Specify the symbolic equations of motion\n", "eom = (Matrix([theta(t), omega(t)]).diff(t) - \n", " Matrix([omega(t), -p * sin(theta(t))]))\n", "\n", "# Specify the symbolic objective function\n", "obj = Integral((theta_m(t) - theta(t))**2, t)\n", "" ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Example Code: 1 DoF, 1 Parameter Pendulum\n", "\n", "### Numerics\n", "\n", "python\n", "# Choose discritzation values\n", "num_nodes = 1000\n", "interval = 0.01 # seconds\n", "\n", "# Form the problem\n", "prob = Problem(obj, eom, (theta(t), omega(t)), num_nodes, interval,\n", " known_trajectory_map={y1_m(t): measured_data},\n", " integration_method='midpoint')\n", "\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": [ "# Computational Speed\n", "\n", "### Example Larger System\n", " \n", "
\n", "
• 10 link pendulum on sliding cart (not stiff)
• \n", "
• 11 DoF, 22 states, 22 parameters
• \n", "
• 12800 mathematical operations in constraint expressions
• \n", "
• 100 s sampled @ 100 hz
• \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: Human Control Parameter Identification\n", "\n", "
\n", "

## Plant

\n", " \n", "
\n", "
• Torque driven two-link inverted pendulum with an accelerating base.
• \n", "
• States: $\\mathbf{x}=[\\theta_a \\quad \\theta_h \\quad \\omega_a \\quad \\omega_h]^T$
• \n", "
• Exogoneous inputs:\n", "
\n", "
• Controlled: $\\mathbf{r}_c = [T_a \\quad T_h]^T$
• \n", "
• Specified: $\\mathbf{r}_k = [a]$
• \n", "
\n", "
• \n", "
• Known parameters: $\\mathbf{p}_k$
• \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}_{c}(\\mathbf{x}_i, \\mathbf{x}_{i+1}, a_{mi}, a_{mi+1}, \\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": [ "# 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", " " ] }, { "cell_type": "markdown", "metadata": { "slideshow": { "slide_type": "slide" } }, "source": [ "# Conclusion\n", "\n", "- Direct collocation is suitable for biomechanical parameter identification\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" ] } ], "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 }