In this last notebook, we will create a full state feedback controller that will allow the human model to balance. There isn't time to give a good overview of controls, but we will use a very common optimal control method to quickly create a controller for our system.
Bring in the results from the previous notebooks.
from solution.visualization import *
We will be working with numerical matrices so we will need several functions from NumPy.
from numpy import array, zeros, eye, asarray, dot, rad2deg
from numpy.linalg import inv
We will be simulating the model again with the controller in place and will make some plots of the state trajectories as before so bring in some useful functions from matplotlib. First enable inline plotting:
%matplotlib inline
from matplotlib.pyplot import plot, xlabel, ylabel, legend, rcParams
rcParams['figure.figsize'] = (14, 8)
Bring in some SymPy functions for simpilfication and printing:
from sympy import simplify
from sympy.physics.vector import init_vprinting, vlatex
init_vprinting()
We will be displaying numerical matrices and the IPython magic %precision
can be used to adjust how many decimal places are shown for nice compact printing.
%precision 3
We current have symbolic equations of motion in this form:
$$ \mathbf{M}(\mathbf{x}) \dot{\mathbf{x}} = \mathbf{f}(\mathbf{x}, \mathbf{u}, t) $$where $\mathbf{M}$ is the mass matrix, $\mathbf{x}$ is the state vector made up of the generalized coordinates and speeds, and $\mathbf{f}(\mathbf{x}, \mathbf{u})$ is a vector of non-linear functions of the states, $\mathbf{x}$, and the specified inputs, $\mathbf{u}$.
This model is relatively easy to linearize and we will use a quick and dirty method to do so. In general, the non-linear equations of motion (for an unconstrained system) can be transformed into a linear form by expanding the Taylor series about small perturbations and truncating any non-linear terms. For this problem, this amounts to computing the Jacobian of the forcing vector, $\mathbf{f}$. The linear equations take this form:
$$ \mathbf{M} \Delta \dot{\mathbf{x}} = \mathbf{f}_A \Delta \mathbf{x} + \mathbf{f}_B \Delta \mathbf{u} $$where $\mathbf{f}_A$ and $\mathbf{f}_B$ are the Jacobians of $\mathbf{f}$ with respect to the states and specified inputs, respectively:
$$ \mathbf{f}_A = \left[ \frac{\delta \mathbf{f} }{ \delta x_1}, \ldots, \frac{\delta \mathbf{f} }{ \delta x_n} \right] $$$$ \mathbf{f}_B = \left[ \frac{\delta \mathbf{f} }{ \delta u_1}, \ldots, \frac{\delta \mathbf{f} }{ \delta u_n} \right] $$and $\Delta \mathbf{x}$ and $\Delta \mathbf{u}$ are differential changes in $\mathbf{x}$ and $\mathbf{u}$ respectively.
Once $\mathbf{f}_A$ and $\mathbf{f}_B$ are found, then the standard state space form of the linear equations can be formed by transforming the linear equations.
$$ \Delta \dot{x} = \mathbf{M}^{-1} \mathbf{f}_A \Delta \mathbf{x} + \mathbf{M}^{-1} \mathbf{f}_B \Delta \mathbf{u} $$which can be rewritten as
$$ \Delta \dot{x} = \mathbf{A} \Delta \mathbf{x} + \mathbf{B} \Delta \mathbf{u} $$where $\mathbf{A}$ is the state matrix and $\mathbf{B}$ is the input matrix.
The first step is to specify the equilbrium point about which to linearize. Our system has two unique equilibrium points, one when the person is upright, $\mathbf{x} = [0, \ldots, 0]$, and one when the person is upside down, $\mathbf{x} = [\pi, \ldots, \pi]$ (180 degrees). We are only concerned with the former because we want the model to "stand up".
equilibrium_point = zeros(len(coordinates + speeds))
equilibrium_point
We will use SymPy's sub()
method to convert the linear equations of motion to numerical arrays, so we create two dictionaries containing the numerical values of the equilibrium point and the constants.
equilibrium_dict = dict(zip(coordinates + speeds, equilibrium_point))
equilibrium_dict
As an exercise, create a parameter_dict
that maps the symbolic constants to the numerical constants. We have variable names for each already available from the previous notebooks (constants
and numerical_constants
).
## Solution
%load exercise_solutions/n09_control_parameter-dict.py
Now we can compute the Jacobian of the forcing vector with respect to the states and the inputs using the jacobian()
method.
F_A = forcing_vector.jacobian(coordinates + speeds)
First substitute in the equilibrium point values:
F_A = simplify(F_A.subs(equilibrium_dict))
F_A
Now let's substitute in the numerical values for the constants and convert to a floating point NumPy array:
F_A = F_A.subs(parameter_dict)
F_A = array(F_A.tolist(), dtype=float)
print(F_A)
Linearize the forcing vector with respect to the specified inputs by computing the correct Jacobian and creating a numerical array for $\mathbf{f}_B$.
## Solution
%load exercise_solutions/n09_control_linearize-forcing-vector.py
We also need the mass matrix evaluated at the equilibrium and with the numerical values of the constants.
M = mass_matrix.subs(equilibrium_dict)
simplify(M)
M = M.subs(parameter_dict)
M = array(M.tolist(), dtype=float)
print(M)
Now we can use the NumPy matrix multiplication function dot()
and the matrix inverse function inv()
to compute the state $\mathbf{A}$ and input $\mathbf{B}$ matrices we need to create the controller.
A = dot(inv(M), F_A)
print(A)
Create the input matrix, $\mathbf{B}$ using dot()
and inv()
.
## Solution
%load exercise_solutions/n09_control_input-matrix.py
print(B)
Before we try to create a controller, we need to check to see if it is actually controllable. You can import a function from the utils
module included with these notebooks to check.
from utils import controllable
controllable(A, B)
So yes, the system is controllable. Now we will use optimal control theory to create a controller that compute the necessary joint torques at each time step to balance the person.
It turns out that if you setup a cost function in which you'd like to minimize the deviation from the desired state, 0, and minimize the effort in the joint torques, there is an elegant solution to find a matrix $\mathbf{K}$ which can be used to compute the joint torques given the current state value.
Given the cost function:
$$J = \frac{1}{2} \int_0^\infty\left[\mathbf{x}^T \mathbf{Q} \mathbf{x} + \mathbf{u}^T \mathbf{R} \mathbf{u}\right]dt$$There is a matrix $\mathbf{K}$ that computes the inputs $\mathbf{u}(t)$ given the states $\mathbf{x}$:
$$\mathbf{u}(t) = -\mathbf{K} \mathbf{x}(t)$$where $\mathbf{K}$ can be found by the solution to the algebraic Riccati Equation such that:
$$\mathbf{K} = \mathbf{R}^{-1} \mathbf{B}^T \mathbf{S}$$and the Riccati equation is:
$$\textbf{0} = -\textbf{S}\textbf{A}-\textbf{A}^{\text{T}}\textbf{S}+\textbf{S}\textbf{B}\textbf{R}^{-1}\textbf{B}^{\text{T}}\textbf{S}-\textbf{Q}$$SciPy provides a function that can compute the solution, $\mathbf{S}$, of the continous algebraic Riccati Equation.
from scipy.linalg import solve_continuous_are
The weighting matrices, $\mathbf{Q}$ and $\mathbf{R}$, allow you to control how much weight is applied to minimizing the error in the states and the effort in the inputs. A typical starting point for these matrices is to set them equal to the identity matrix, which can be created with the NumPy eye()
function.
Q = eye(6)
Q
R = eye(3)
R
$\mathbf{K}$ can now be computed.
S = solve_continuous_are(A, B, Q, R)
K = dot(dot(inv(R), B.T), S)
K
Remember that the right_hand_side
function we generated earlier can take a function for the specified values.
help(right_hand_side)
Create a function named controller
that computes the specified inputs given the current state and time.
def controller():
"""Returns the output of the controller, i.e. the joint torques, given the current state."""
return
## Solution
%load exercise_solutions/n09_control_controller-function.py
Now we can integrate the equations of motion again, but this time with the controller in place.
args['specified'] = controller
Integrate the equations of motion and plot the coordinates and speeds as we did in the simulation notebook and see if the system behaves as expected.
## Solution
%load exercise_solutions/n09_control_integrate-eom.py
Use the same plot functions as were used in the simulation notebook to inspect the state trajectories.
# Plot the angles
## Solution
%load exercise_solutions/n09_control_plot-angles.py
# Plot the angular velocities
## Solution
%load exercise_solutions/n09_control_plot-velocities.py
What do these plots tell you about the controller? Does it work?
Finally, regenerate the visualization to see the 3D animation of the system under the influence of the controller.
scene.generate_visualization_json(coordinates + speeds, constants, y, numerical_constants)
scene.display()
Now experiment with several of the parameters of our human model. Below is a taller and much heavier version of our model. See how the controller can make this person balance.
numerical_constants = array([0.711, # lower_leg_length [m]
0.437, # lower_leg_com_length [m]
6.769, # lower_leg_mass [kg]
0.101, # lower_leg_inertia [kg*m^2]
0.524, # upper_leg_length [m]
0.243, # upper_leg_com_length
17.01, # upper_leg_mass [kg]
0.282, # upper_leg_inertia [kg*m^2]
10.355, # torso_com_length [m]
150.00, # torso_mass [kg]
1.485, # torso_inertia [kg*m^2]
9.81], # acceleration due to gravity [m/s^2]
)
For convienence the early portions of the notebook repeated in this script. Just run the following cell after modifying the previous cell.
%run -i exercise_solutions/n09_control_snippit.py
Create plots here to inspect the state trajectories
Now experiment with the $\mathbf{Q}$ and $\mathbf{R}$ weighting matricies to see if you can come up with a better controller for this new model.
Q = array([[ .1, 0., 0., 0., 0., 0.],
[ 0., 1., 0., 0., 0., 0.],
[ 0., 0., .1, 0., 0., 0.],
[ 0., 0., 0., 1., 0., 0.],
[ 0., 0., 0., 0., 1., 0.],
[ 0., 0., 0., 0., 0., .1]])
R = array([[ 20., 0., 0.],
[ 0., 1., 0.],
[ 0., 0., 1.]])
%run -i exercise_solutions/n09_control_snippit.py
Create plots here to inspect the state trajectories.
If you would like, regenerate the 3D visualization.
scene.generate_visualization_json(coordinates + speeds, constants, y, numerical_constants)
scene.display()