|
Drake
|
|
Drake
|
Given a desired two dimensional (X and Y) zero-moment point (ZMP) trajectory parameterized as a piecewise polynomial, an optimal center of mass (CoM) trajectory is planned using a linear inverted pendulum model (LIPM).
A second order value function (optimal cost-to-go) and a linear policy are also computed alongside the optimal trajectory. The system dynamics for the X and Y directions are decoupled, however, we plan the XY motion together for convenience.
Let \( c \) be the CoM position, the state of the system, \( x \), is \( [c; \dot{c}] \), the control, \( u = \ddot{c} \), and \( y \) represents the center of pressure (CoP). For the X direction, the LIPM dynamics is:
\[ y = c - \frac{z}{g} * u, \]
where \( g \) is the gravity constant and \( z \) is the CoM height. \( z \) is assumed to be constant in LIPM. The full dynamics can also be written in the matrix form as:
\[ \dot{x} = A x + B u \\ y = C x + D u \]
The one step cost function \( L \) is defined as:
\[ L(y, u, t) = (y - y_d(t))^T Q_y (y - y_d(t)) + u^T R u, \]
where \( Q_y \) and \( R \) are weighting matrices, and \( y_d(t) \) is the desired ZMP trajectory at time \( t \).
The value function is defined as
\[ V(x, t) = \min_{u[t:t_f]} \bar{x}(t_f)^T S \bar{x}(t_f) + \int_{t}^{t_f} L(y, u, \tau) d\tau, \]
subject to the dynamics, and \( t_f \) is the last time in the desired ZMP trajectory, \( \bar{x} = [c - y_d(t_f); \dot{c}] \), \( S \) is the quadratic term from the infinite horizon continuous time LQR solution solved with the same dynamics and one step cost function.
For this problem, \( V \) is known to have a quadratic form of:
\[ V(x, t) = \bar{x}^T V_{xx} \bar{x} + \bar{x}^T V_x(t) + V_0(t), \]
and the corresponding optimal control policy, \( u^* \), is linear w.r.t. to \( x \):
\[ u^*(x, t) = K \bar{x} + u_0(t). \]
See the following reference for more details about the algorithm:
[1] R. Tedrake, S. Kuindersma, R. Deits and K. Miura, "A closed-form solution for real-time ZMP gait generation and feedback stabilization," 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids), Seoul, 2015, pp. 936-940.
#include <drake/planning/locomotion/zmp_planner.h>
Public Member Functions | |
| ZmpPlanner ()=default | |
| void | Plan (const trajectories::PiecewisePolynomial< double > &zmp_d, const Eigen::Vector4d &x0, double height, double gravity=9.81, const Eigen::Matrix2d &Qy=Eigen::Matrix2d::Identity(), const Eigen::Matrix2d &R=0.1 *Eigen::Matrix2d::Identity()) |
| Implements the algorithm in [1] that computes a nominal CoM trajectory, and the corresponding second order value function and linear policy. More... | |
| bool | has_planned () const |
| Returns true if Plan() has been called. More... | |
| Eigen::Vector2d | ComputeOptimalCoMdd (double time, const Eigen::Vector4d &x) const |
Computes the optimal control (CoM acceleration) at time given CoM state x using the linear policy. More... | |
| Eigen::Vector2d | comdd_to_cop (const Eigen::Vector4d &x, const Eigen::Vector2d &u) const |
| Converts CoM acceleration to center of pressure (CoP) using cop = C * x + D * u, which is equivalent to cop = com - z / g * comdd Should only be called after Plan is called. More... | |
| const Eigen::Matrix< double, 4, 4 > & | get_A () const |
| Getter for A matrix. More... | |
| const Eigen::Matrix< double, 4, 2 > & | get_B () const |
| Getter for B matrix. More... | |
| const Eigen::Matrix< double, 2, 4 > & | get_C () const |
| Getter for C matrix. More... | |
| const Eigen::Matrix< double, 2, 2 > & | get_D () const |
| Getter for D matrix. More... | |
| const Eigen::Matrix< double, 2, 2 > & | get_Qy () const |
| Getter for Qy matrix. More... | |
| const Eigen::Matrix< double, 2, 2 > & | get_R () const |
| Getter for R matrix. More... | |
| Eigen::Vector2d | get_desired_zmp (double time) const |
Returns the desired ZMP evaluated at time. More... | |
| Eigen::Vector2d | get_nominal_com (double time) const |
Returns the nominal CoM position evaluated at time. More... | |
| Eigen::Vector2d | get_nominal_comd (double time) const |
Returns the nominal CoM velocity evaluated at time. More... | |
| Eigen::Vector2d | get_nominal_comdd (double time) const |
Returns the nominal CoM acceleration evaluated at time. More... | |
| Eigen::Vector2d | get_final_desired_zmp () const |
| Returns the position of the ZMP at the end of the desired trajectory. More... | |
| const trajectories::PiecewisePolynomial< double > & | get_desired_zmp () const |
| Returns the desired ZMP trajectory. More... | |
| const trajectories::ExponentialPlusPiecewisePolynomial< double > & | get_nominal_com () const |
| Returns the nominal CoM trajectory. More... | |
| const trajectories::ExponentialPlusPiecewisePolynomial< double > & | get_nominal_comd () const |
| Returns the nominal CoM velocity trajectory. More... | |
| const trajectories::ExponentialPlusPiecewisePolynomial< double > & | get_nominal_comdd () const |
| Returns the nominal CoM acceleration trajectory. More... | |
| const Eigen::Matrix< double, 4, 4 > & | get_Vxx () const |
| Returns the time invariant second order term (S1 in [1]) of the value function. More... | |
| const trajectories::ExponentialPlusPiecewisePolynomial< double > & | get_Vx () const |
| Returns the time varying first order term (s2 in [1]) of the value function. More... | |
| const Eigen::Vector4d | get_Vx (double time) const |
Returns the time varying first order term (s2 in [1]) of the value function, evaluated at the given time. More... | |
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable | |
| ZmpPlanner (const ZmpPlanner &)=default | |
| ZmpPlanner & | operator= (const ZmpPlanner &)=default |
| ZmpPlanner (ZmpPlanner &&)=default | |
| ZmpPlanner & | operator= (ZmpPlanner &&)=default |
|
default |
|
default |
|
default |
| Eigen::Vector2d comdd_to_cop | ( | const Eigen::Vector4d & | x, |
| const Eigen::Vector2d & | u | ||
| ) | const |
Converts CoM acceleration to center of pressure (CoP) using cop = C * x + D * u, which is equivalent to cop = com - z / g * comdd Should only be called after Plan is called.
| x | CoM position and velocity |
| u | CoM acceleration |
| Eigen::Vector2d ComputeOptimalCoMdd | ( | double | time, |
| const Eigen::Vector4d & | x | ||
| ) | const |
Computes the optimal control (CoM acceleration) at time given CoM state x using the linear policy.
| time | Current time. |
| x | Current state. |
| const Eigen::Matrix<double, 4, 4>& get_A | ( | ) | const |
Getter for A matrix.
| const Eigen::Matrix<double, 4, 2>& get_B | ( | ) | const |
Getter for B matrix.
| const Eigen::Matrix<double, 2, 4>& get_C | ( | ) | const |
Getter for C matrix.
| const Eigen::Matrix<double, 2, 2>& get_D | ( | ) | const |
Getter for D matrix.
| Eigen::Vector2d get_desired_zmp | ( | double | time | ) | const |
Returns the desired ZMP evaluated at time.
| const trajectories::PiecewisePolynomial<double>& get_desired_zmp | ( | ) | const |
Returns the desired ZMP trajectory.
| Eigen::Vector2d get_final_desired_zmp | ( | ) | const |
Returns the position of the ZMP at the end of the desired trajectory.
| Eigen::Vector2d get_nominal_com | ( | double | time | ) | const |
Returns the nominal CoM position evaluated at time.
| const trajectories::ExponentialPlusPiecewisePolynomial<double>& get_nominal_com | ( | ) | const |
Returns the nominal CoM trajectory.
| Eigen::Vector2d get_nominal_comd | ( | double | time | ) | const |
Returns the nominal CoM velocity evaluated at time.
| const trajectories::ExponentialPlusPiecewisePolynomial<double>& get_nominal_comd | ( | ) | const |
Returns the nominal CoM velocity trajectory.
| Eigen::Vector2d get_nominal_comdd | ( | double | time | ) | const |
Returns the nominal CoM acceleration evaluated at time.
| const trajectories::ExponentialPlusPiecewisePolynomial<double>& get_nominal_comdd | ( | ) | const |
Returns the nominal CoM acceleration trajectory.
| const Eigen::Matrix<double, 2, 2>& get_Qy | ( | ) | const |
Getter for Qy matrix.
| const Eigen::Matrix<double, 2, 2>& get_R | ( | ) | const |
Getter for R matrix.
| const trajectories::ExponentialPlusPiecewisePolynomial<double>& get_Vx | ( | ) | const |
Returns the time varying first order term (s2 in [1]) of the value function.
| const Eigen::Vector4d get_Vx | ( | double | time | ) | const |
Returns the time varying first order term (s2 in [1]) of the value function, evaluated at the given time.
| const Eigen::Matrix<double, 4, 4>& get_Vxx | ( | ) | const |
Returns the time invariant second order term (S1 in [1]) of the value function.
| bool has_planned | ( | ) | const |
Returns true if Plan() has been called.
|
default |
|
default |
| void Plan | ( | const trajectories::PiecewisePolynomial< double > & | zmp_d, |
| const Eigen::Vector4d & | x0, | ||
| double | height, | ||
| double | gravity = 9.81, |
||
| const Eigen::Matrix2d & | Qy = Eigen::Matrix2d::Identity(), |
||
| const Eigen::Matrix2d & | R = 0.1 *Eigen::Matrix2d::Identity() |
||
| ) |
Implements the algorithm in [1] that computes a nominal CoM trajectory, and the corresponding second order value function and linear policy.
No other public method should be called until Plan() has been called.
The velocity of the ZMP at the end of zmp_d does not need to be zero, but the user should treat the result with caution, since the resulting nominal CoM trajectory diverges exponentially quickly beyond the end of zmp_d.
| zmp_d | Desired two dimensional ZMP trajectory. |
| x0 | Initial CoM state. |
| height | CoM height from the ground. |
| gravity | Gravity constant, defaults to 9.81 |
| Qy | Quadratic cost term on ZMP deviation from the desired. |
| R | Quadratic cost term on CoM acceleration. |