|
lipm_walking_controller
1.6.0
|
This page is the unfinished draft of accompanying notes for a tutorial I gave in December 2019.
Walking pattern generation produces reference trajectories that the Stabilizer will then try to track at best. These reference trajectories consist of positions for the CoM and swing foot, but also forces such as the ZMP that parameterizes centroidal momentum. In this controller, we generate the CoM trajectory by solving linear model predictive control (LMPC) problems, a specific kind of quadratic programs (QP). In what follows, we outline how to update ModelPredictiveControl to switch the formulation of this problem from a CoM jerk input (as in Wieber (2006)) to a ZMP velocity input (as in Scianca et al. (2019)).
We use the copra library to build and solve our model predictive control problems. See this post for an introduction to the discretization of continuous-time state-space models and a corresponding example with copra. In the initial ModelPredictiveControl, our discrete-time state-space model is a triple integrator with the CoM jerk \(u_k\) as input:
\[ \begin{align} x_k & = \begin{bmatrix} {c}_{k} \\ \dot{c}_{k} \\ \ddot{c}_{k} \end{bmatrix} & x_{k+1} = \begin{bmatrix} I & T I & T^2 / 2 I \\ 0 & I & T I \\ 0 & 0 & I \end{bmatrix} x_k + \begin{bmatrix} T^3 / 6 I \\ T^2 / 2 I \\ T I \end{bmatrix} u_k \end{align} \]
Here our vector \(c_k\) of horizontal CoM coordinates has dimension two (x and y axes of the inertial frame), so that \(I\) is the \(2 \times 2\) identity matrix. The sampling period \(T\) is set to 100 ms with \(N = 16\) discretization steps that give us a receding horizon of 1.6 s. (These are standard values from the literature, go ahead and try some others!) We define this preview system in the constructor:
Let's check out what happens when the MPC input is the ZMP position \(z\) (second-order) rather than the CoM jerk (third-order). The discretization of the input function in the MPC problem formulation implies that our ZMP trajectory \(z(t)\) will be piecewise-constant. From the linear inverted pendulum model, the acceleration of the center of mass in the horizontal plane (we drop the z coordinate for simplicity):
\[ \ddot{c} = \omega^2 (c - z) \]
This is a second-order non-homogeneous linear differential equation with constant coefficients. We can solve it analytically:
\[ \begin{align} c(t) & = c(0) \cosh(\omega t) + \dot{c}(0) \frac{\sinh(\omega t)}{\omega} + z(0) (1 - \cosh(\omega t)) \\ \dot{c}(t) & = c(0) \omega \sinh(\omega t) + \dot{c}(0) \cosh(\omega t) - z(0) \omega \sinh(\omega t) \end{align} \]
You can check out this summary about the capture point to learn more about the structure of this solution. Having the analytical solution gives us a straightforward discretization our state-space model:
\[ \begin{align} x_k & = \begin{bmatrix} c_{k} \\ \dot{c}_{k} \end{bmatrix} & x_{k+1} = \begin{bmatrix} \cosh(\omega t) I & {\sinh(\omega t)} I / \omega \\ \omega \sinh(\omega t) I & \cosh(\omega t) I \end{bmatrix} x_k + \begin{bmatrix} (1 - \cosh(\omega t)) I \\ -\omega \sinh(\omega t) I \end{bmatrix} u_k \end{align} \]
Yet, piecewise-constant ZMP trajectories are not nice in practice. With such an implementation, the ModelPredictiveControl trajectory generator would provide discontinuous force targets to the Stabilizer, each jump being tracked with delay coming from admittance control and mechanical flexibilities. To avoid this, we want to generate trajectories that are as smooth as possible. This is possible, for instance, by using the ZMP velocity as control input.
Try putting the following in the "jvrc1" entry of your configuration "plans":
1.8.11 
