# Optimal Control

Optimal control theory aims to find the control inputs required for a system to perform a task optimally with respect to a predefined objective. It requires the definition of optimization variables, a model of the system dynamics, constraints to define the task, and the objective. In the MaD lab, optimal control theory is applied to solve trajectory optimization problems of human motion. We use the fact that humans minimize energy expenditure in movements to find the optimal trajectory to perform a motion. This trajectory is defined by the initial state, x(0), and the set of control inputs, u(t), usually joint torques or muscle stimulations, to perform the desired task optimally. The resulting simulations with state x(t) are used to reconstruct and predict human movements, specifically gait.

To find human movements, typically an objective combining effort minimization and tracking error minimization is used. The problem can be formulated as follows:


\begin{aligned}
\underset{\mathbf{x}(t), \mathbf{u}(t), T}{\text{min}} ~~~~
& \frac{1}{T} \int\limits_{0}^{T}
\frac{W_{tr}}{N_{tr}} \sum\limits_{j=1}^{N_{tr}} w_j \left(\frac{y_{sim,j}(t) - y_{meas,j}(t)}{\sigma_{y,meas,j}(t)} \right)^2 +
\frac{W_{eff}}{N_u} \sum\limits_{i=1}^{N_u} w_i u_i^{e_i} \,dt \\ \\ \\
\text{subject to} ~~
& \mathbf{f}(\mathbf{x}(t),\mathbf{{\dot{x}}}(t),\mathbf{u}(t)) = \mathbf{0} && \hspace{-5.5cm} \text{(Dynamics)}\\
& \mathbf{x}(T) = \mathbf{R}_{per} \ \mathbf{x}(0) + \mathbf{t_{per}} && \hspace{-5.5cm} \text{(Task)}\\
& \mathbf{u}(T) = \mathbf{u}(0) && \hspace{-5.5cm} \text{(Task)}\\
& \mathbf{x}_{L} \le \mathbf{x} \le \mathbf{x}_{U} && \hspace{-5.5cm} \text{(Bounds)}\\
& \mathbf{u}_{L} \le \mathbf{u} \le \mathbf{u}_{U} && \hspace{-5.5cm} \text{(Bounds)}\\
\end{aligned}

A periodicity constraint is used to simulate gait. This means that a dynamics constraint is also added between the first and last time node, together with a displacement, such that the inputs and the internal states are the same at the beginning and end of the trajectory, while a certain horizontal displacement has been achieved.

We solve these problems using direct collocation. This means that the trajectory is discretized into time nodes. For each two consecutive time nodes, a dynamics constraint is added, such that the state and input at the time node yield the state at the next time point. This allows for an analytical derivation of the dynamics and their derivatives, such that the problem can be solved efficiently using a large-scale nonlinear optimization algorithm, such as IPOPT or SNOPT.