# Optimal Control and Parameter Identification of Dynamcal Systems with Direct Collocation and SymPy¶

Jason K. Moore
July 8, 2015
SciPy 2015, Austin, Texas, USA

# Previous Use Cases: Optimal Gait on Earth, Moon, Mars¶

Ackermann, M. and van den Bogert, A. J. Predictive simulation of gait at low gravity reveals skipping as the preferred locomotion strategy. Journal of Biomechanics, 45, 2012

 Earth: g=9.8 m/s-2 Moon: g=1.6 m/s-2

# Trajectory Optimization Example¶

## 1-DoF, 1 parameter pendulum equations of motion¶

$\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}$

## Objective: Minimize "effort" and bound the states at¶

$J(\mathbf{T}) = \min\limits_{\mathbf{T}} \int_{t_0}^{t_f} \mathbf{T}^2 dt$

## Boundary conditions¶

• $\mathbf{\theta}(t_0) = 0$
• $\mathbf{\theta}(t_f) = \pi$

Pendulum figure: https://commons.wikimedia.org/wiki/File:Pendulum_gravity.svg, Creative Commons Attribution-Share Alike 3.0 Unported

# Parameter Identification¶

Find the parameters $\mathbf{p}$ such that the difference between the model simulation, $\mathbf{y}$, and measurements, $\mathbf{y}_m$ is minimized.

### Dynamic system¶

• Equations of Motion: $\dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, \mathbf{p})$
• Measurement variables: $\mathbf{y} = \mathbf{g}(\mathbf{x}, \mathbf{p})$

### Objective¶

$$\min\limits_\mathbf{p} J(\mathbf{p})$$ where $$J(\mathbf{p}) = \int [\mathbf{y}_m - \mathbf{y}(\mathbf{p})]^2 dt$$

# Optimal Control By Shooting¶

• Repeated simulations are computationally costly (i.e. stiff systems are especially slow)
• Maybe limited to parameterized functions (e.g. polynomials, piecewise constants)
• Too many free variables
• Systems may be unstable and thus have an ill-defined objective
• Local minima are inevitable
• May requires a superb guess
• May need time intensive global optimization methods

# Local Minima Example: Simple pendulum¶

Vyasarayani, Chandrika P., Thomas Uchida, Ashwin Carvalho, and John McPhee. "Parameter Identification in Dynamic Systems Using the Homotopy Optimization Approach". Multibody System Dynamics 26, no. 4 (2011): 411-24.

## 1-DoF, 1 parameter pendulum equations of motion¶

$\dot{\mathbf{x}} = \begin{bmatrix} \dot{\theta}(t) \\ \dot{\omega}(t) \end{bmatrix} = \begin{bmatrix} \omega(t) \\ -p \sin{\theta}(t) \end{bmatrix}$

## Objective: Minimize least squares¶

$J(p) = \min\limits_{p} \int_{t_0}^{t_f} [\theta_m(t) - \theta(\mathbf{x}, p, t)]^2 dt$

# Direct Collocation¶

## Benefits¶

• Fast computation times
• Handles unstable systems with ease
• Less susceptible to local minima

• Accurate solution requires large number of nodes
• Memory management for large sparse matrices and operations
• Tedious and error prone to form gradients, Jacobians, and Hessians

# Our Direct Collocation Implementation¶

### Implicit Continous Equations of Motion¶

No need to solve for $\dot{\mathbf{x}}$.

$$\mathbf{f}(\dot{\mathbf{x}}, \mathbf{x}, \mathbf{r}, \mathbf{p}, t) = 0$$

• $\mathbf{x}, \dot{\mathbf{x}}$: states and their derivatives
• $\mathbf{r}$: exogenous inputs
• $\mathbf{p}$: constant parameters

# Discretization¶

### First order discrete integration options:¶

Backward Euler:

$\mathbf{f}(\frac{\mathbf{x}_i - \mathbf{x}_{i-1}}{h} , \mathbf{x}_i, \mathbf{r}_i, \mathbf{p}, t_i) = 0$

Midpoint Rule:

$\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$

### Nonlinear Programming Formulation¶

$$\min\limits_{\theta} J(\theta)$$

$$\textrm{where } \mathbf{\theta} = [\mathbf{x}_i, \dots, \mathbf{x}_N, \mathbf{r}_i, \ldots, \mathbf{r}_N, \mathbf{p}]$$

$$\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$$

# Software Tool: opty¶

## http://github.com/csu-hmc/opty¶

• User specifies continous symbolic:
• objective
• equations of motion (explicit or implicit)
• bounds on free variables: $\mathbf{x}, \mathbf{r}, \mathbf{p}$
• EoMs can be generated with PyDy (http://pydy.org)
• Effficient just-in-time compiled C code is generated for functions that evaluate:
• constraints and its Jacobian
• NLP problem automatically formed for IPOPT
• Written in Python

# Example Code: Specify Equations of Motion¶

$$\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}$$

### Symbolic EoM¶

# Specify symbols for the parameters
p, t = symbols('p, t')

# Specify the functions of time
theta, omega, theta_m, T = symbols('theta, omega, theta_m, T', cls=Function)

# Specify the symbolic equations of motion
eom = Matrix([theta(t), omega(t)]).diff(t) - Matrix([omega(t), -p * sin(theta(t) + T(t))])


### Discretization¶

# Choose discretization values
num_nodes = 1000
interval = 0.01  # seconds


# Example Code: Trajectory Optimization¶

### Objective¶

obj = Integral(T(t)**2, t)


### Boundary Contraints¶

boundary_constraints = (theta(0.0), theta(duration) - pi / 2, omega(0.0), omega(duration))


### Define Problem¶

prob = Problem(obj,  # symbolic objective
eom,  # symbolic equations of motion
(theta(t), omega(t)),  # system symbolic states
num_nodes,
interval,
known_parameter_map={p: 9.81},
instance_constraints=boundary_constraints)


# Example Code: Single Parameter Identification¶

### Objective¶

obj = Integral((theta_m(t) - theta(t))**2, t)


### Form Problem¶

prob = Problem(obj,
eom,
(theta(t), omega(t)),
num_nodes,
interval,
known_trajectory_map={T(t): 0.0, y1_m(t): measured_data},
integration_method='midpoint')


# Example Code: Solve¶

# Set an initial guess
initial_guess = random(prob.num_free)

# Solve the system
solution, info = prob.solve(initial_guess)


# The Problem Class: Objective¶

1. Converts objective integral to discrete form
2. Computes analytic gradient of objective wrt to discrete variables
3. Generates efficient NumPy functions for objective and gradient

*Still in development

# The Problem Class: Constraints¶

1. Converts continous EoM to discrete versions
2. Computes the analytic Jacobian wrt to discrete variables
3. Generates efficient C code to "ufuncify" discrete EoM and Jacobian evaluations
4. Generates Cython wrapper for C code
5. Compiles Cython wrapper
6. NumPy functions are generated for instance constraints

### The Problem Class: Solve¶

1. IPOPT data structure is formed with cyipopt
2. Bounds on variables are set
3. IPOPT settings are set
4. Solve is called!

# Computational Speed¶

### Example Larger System¶

• 10 link pendulum on sliding cart (not stiff)
• 11 DoF, 22 states, 22 parameters
• 12800 mathematical operations in constraint expressions
• 100 s sampled @ 100 hz

# Computational Speed¶

### Discretization Variables¶

• 10,000 collocation nodes
• 219,978 constraints
• 14,518,548 nonzero entries in the Jacobian
• 220,022 free variables

### Timings¶

• Integrating with ODEPACK lsoda: 5.6 s
• Constraint evaluation: 33 ms (0.033 s)
• Jacobian evaluation: 128 ms (0.128 s)

# Case Study: Minamal Effort Double Pendulum Swing Up¶

• Two-link pendulum with lateral force applied to base
• States: $\mathbf{x}=[q_0 \quad q_1 \quad q_2 \quad u_0 \quad u_1 \quad u_2]^T$
• Unknown exogoneous input: $\mathbf{F}$
• Known parameters: $[m_0, m_1, m_2, l_0, l_1]$
• Boundary conditions:
• $q_0(t_0)=0$
• $q_1(t_0),q_1(t_0)=-\frac{\pi}{2}$
• $q_1(t_f),q_1(t_f)=\frac{\pi}{2}$
• $u_0(t_0), u_2(t_0), u_2(t_0), u_0(t_f), u_1(t_f), u_2(t_f)=0$

# Trajectory Opimization Problem Specification¶

Given the boundary conditions can we find the optimal input trajectory such that the "effort" is minimized?

$$\min\limits_\theta J(\mathbf{\theta}), \quad J(\mathbf{\theta})= \sum_{i=1}^N h [\mathbf{T}_i]^2$$

where

$$\mathbf{\theta} = [\mathbf{x}_1, \ldots, \mathbf{x}_N, \mathbf{T}_i, \ldots \mathbf{T}_N]$$

Subject to the constraints:

$$\mathbf{f}(\mathbf{x}_i, \mathbf{T}_i)=0, \quad i=1 \ldots N$$

And the initial guess:

$$\mathbf{\theta}_0 = [\mathbf{0}]$$

For, $N$ = 500:

• 24008 free variables
• Jacobian matrix with 38933 non-zero entries

# Case Study: Human Control Parameter Identification¶

## Plant

• Torque driven two-link inverted pendulum with an accelerating base.
• States: $\mathbf{x}=[\theta_a \quad \theta_h \quad \omega_a \quad \omega_h]^T$
• Exogoneous inputs:
• Controlled: $\mathbf{r}_c = [T_a \quad T_h]^T$
• Specified: $\mathbf{r}_k = [a]$
• Known parameters: $\mathbf{p}_k$

### Open Loop Equations of Motion

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

# Lumped Passive+Active Controller¶

• True human controller is practically impossible to isolate and identify
• Identify a controller for a similar system that causes the same behavior as the real system

## Simple State Feedback¶

$$\mathbf{r}_c(t) = -\mathbf{K}\mathbf{x}(t)$$

## Unknown Parameters¶

$$\mathbf{p}_u = \mathrm{vec}(\mathbf{K})$$

## Closed Loop Equations of Motion¶

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

# Generate Data¶

## Specify the psuedo-random platform acceleration¶

$$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}$$

### Choose a stable controller¶

$$\mathbf{K} = \begin{bmatrix} 950 & 175 & 185 & 50 \\ 45 & 290 & 60 & 26 \end{bmatrix}$$

# Generate Data¶

### Simulate closed loop system under the influence of perturbations for 60 seconds, sampled at 100 Hz¶

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

$$\mathbf{x}_m(t) = \mathbf{x}(t) + \mathbf{v}_x(t) \\ a_m(t) = a(t) + v_a(t)$$

• $\sigma_\theta$ = 0.3 deg
• $\sigma_\omega$ = 4 deg/s
• $\sigma_a$ = 0.42 ms-2

# Parameter Identification Problem Specification¶

Given noisy measurements of the states, $\mathbf{x}_m$, and the platform acceleration, $a_m$, can we identify the controller parameters $\mathbf{K}$?

$$\min\limits_\theta J(\mathbf{\theta}), \quad J(\mathbf{\theta})= \sum_{i=1}^N h [\mathbf{x}_{mi} - \mathbf{x}_i]^2$$

where

$$\mathbf{\theta} = [\mathbf{x}_1, \ldots, \mathbf{x}_N, \mathbf{p}_u]$$

Subject to the constraints:

$$\mathbf{f}_{ci}(\mathbf{x}_i, a_{mi}, \mathbf{p}_u)=0, \quad i=1 \ldots N$$

And the initial guess:

$$\mathbf{\theta}_0 = [\mathbf{x}_{m1}, \ldots, \mathbf{x}_{mN}, \mathbf{0}]$$

For, $N$ = 6000:

• 24008 free variables
• 23996 x 24008 Jacobian matrix with 384000 non-zero entries

# Results¶

Converges in 11 iterations in 2.8 seconds of computation time.

Known Identified Error
$k_{00}$ 950 946 -0.4%
$k_{01}$ 175 177 1.4%
$k_{02}$ 185 185 -0.2%
$k_{03}$ 50 55 9.4%
$k_{10}$ 45 45 1.1%
$k_{11}$ 290 289 -0.3%
$k_{12}$ 60 59 -2.1%
$k_{13}$ 26 27 4.2%

# Conclusion¶

• Direct collocation is suitable for biomechanical parameter identification and trajectory optimization
• Computation speeds are orders of magnitude faster than shooting
• Parameter identification accuracy improves with # nodes
• Complex problems can be solved with few lines of code and high level mathematical abstractions