Drake
ZMPPlanner Class Reference

Given a desired two dimensional (X and Y) zero-moment point (ZMP) trajectory parametrized as a piecewise polynomial, an optimal center of mass (CoM) trajectory is planned using a linear inverted pendulum model (LIPM). More...

#include <systems/controllers/zmp_planner.h>

Public Member Functions

 ZMPPlanner ()
 
void Plan (const 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=Eigen::Matrix2d::Zero())
 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 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
 
const PiecewisePolynomial< double > & get_desired_zmp () const
 Returns the desired ZMP trajectory. More...
 
const ExponentialPlusPiecewisePolynomial< double > & get_nominal_com () const
 Returns the nominal CoM trajectory. More...
 
const ExponentialPlusPiecewisePolynomial< double > & get_nominal_comd () const
 Returns the nominal CoM velocity trajectory. More...
 
const 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 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. More...
 
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
 ZMPPlanner (const ZMPPlanner &)=default
 
ZMPPlanneroperator= (const ZMPPlanner &)=default
 
 ZMPPlanner (ZMPPlanner &&)=default
 
ZMPPlanneroperator= (ZMPPlanner &&)=default
 

Detailed Description

Given a desired two dimensional (X and Y) zero-moment point (ZMP) trajectory parametrized 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 along 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 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.

Constructor & Destructor Documentation

ZMPPlanner ( const ZMPPlanner )
default
ZMPPlanner ( ZMPPlanner &&  )
default
ZMPPlanner ( )
inline

Here is the call graph for this function:

Member Function Documentation

Eigen::Vector2d comdd_to_cop ( const Eigen::Vector4d &  x,
const Eigen::Vector2d u 
) const
inline

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.

Parameters
x,CoMposition and velocity
u,CoMacceleration
Returns
center of pressure (CoP)

Here is the caller graph for this function:

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.

Should only be called after Plan is called.

Parameters
time,Currenttime.
x,Currentstate.
Returns
Optimal CoMdd.

Here is the call graph for this function:

Here is the caller graph for this function:

const Eigen::Matrix<double, 4, 4>& get_A ( ) const
inline

Getter for A matrix.

const Eigen::Matrix<double, 4, 2>& get_B ( ) const
inline

Getter for B matrix.

const Eigen::Matrix<double, 2, 4>& get_C ( ) const
inline

Getter for C matrix.

const Eigen::Matrix<double, 2, 2>& get_D ( ) const
inline

Getter for D matrix.

Eigen::Vector2d get_desired_zmp ( double  time) const
inline

Returns the desired ZMP evaluated at time.

Here is the call graph for this function:

Here is the caller graph for this function:

const PiecewisePolynomial<double>& get_desired_zmp ( ) const
inline

Returns the desired ZMP trajectory.

Eigen::Vector2d get_final_desired_zmp ( ) const
inline

Here is the call graph for this function:

Eigen::Vector2d get_nominal_com ( double  time) const
inline

Returns the nominal CoM evaluated at time.

Here is the call graph for this function:

Here is the caller graph for this function:

const ExponentialPlusPiecewisePolynomial<double>& get_nominal_com ( ) const
inline

Returns the nominal CoM trajectory.

Eigen::Vector2d get_nominal_comd ( double  time) const
inline

Returns the nominal CoM velocity evaluated at time.

Here is the call graph for this function:

Here is the caller graph for this function:

const ExponentialPlusPiecewisePolynomial<double>& get_nominal_comd ( ) const
inline

Returns the nominal CoM velocity trajectory.

Eigen::Vector2d get_nominal_comdd ( double  time) const
inline

Returns the nominal CoM acceleration evaluated at time.

Here is the call graph for this function:

Here is the caller graph for this function:

const ExponentialPlusPiecewisePolynomial<double>& get_nominal_comdd ( ) const
inline

Returns the nominal CoM acceleration trajectory.

const Eigen::Matrix<double, 2, 2>& get_Qy ( ) const
inline

Getter for Qy matrix.

const Eigen::Matrix<double, 2, 2>& get_R ( ) const
inline

Getter for R matrix.

const ExponentialPlusPiecewisePolynomial<double>& get_Vx ( ) const
inline

Returns the time varying first order term (s2 in [1]) of the value function.

const Eigen::Vector4d get_Vx ( double  time) const
inline

Returns the time varying first order term (s2 in [1]) of the value function.

Here is the call graph for this function:

const Eigen::Matrix<double, 4, 4>& get_Vxx ( ) const
inline

Returns the time invariant second order term (S1 in [1]) of the value function.

bool has_planned ( ) const
inline

Returns true if Plan has been called.

Here is the call graph for this function:

ZMPPlanner& operator= ( const ZMPPlanner )
default
ZMPPlanner& operator= ( ZMPPlanner &&  )
default
void Plan ( const 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 = Eigen::Matrix2d::Zero() 
)

Implements the algorithm in [1] that computes a nominal CoM trajectory, and the corresponding second order value function and linear policy.

None of the other public methods should be called until Plan is called.

It is allowed to pass in a zmp_d with a non-stationary end point, but the user should treat the result with caution, since the resulting nominal CoM trajectory diverges exponentially fast past the end point.

Parameters
zmp_d,Desiredtwo dimensional ZMP trajectory.
x0,InitialCoM state.
height,CoMheight from the ground.
gravity,Gravityconstant, defaults to 9.81
Qy,Quadraticcost term on ZMP deviation from the desired, defaults to identity.
R,Quadraticcost term on CoM acceleration, defaults to zero.

switch to zbar coord

Here is the call graph for this function:

Here is the caller graph for this function:


The documentation for this class was generated from the following files: