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,CoM position and velocity u,CoM acceleration
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,Current time. x,Current state.
Returns
Optimal CoMdd.

Here is the call graph for this function:

Here is the caller graph for this function:

 const Eigen::Matrix& get_A ( ) const
inline

Getter for A matrix.

 const Eigen::Matrix& get_B ( ) const
inline

Getter for B matrix.

 const Eigen::Matrix& get_C ( ) const
inline

Getter for C matrix.

 const Eigen::Matrix& 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& 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& 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& 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& get_nominal_comdd ( ) const
inline

Returns the nominal CoM acceleration trajectory.

 const Eigen::Matrix& get_Qy ( ) const
inline

Getter for Qy matrix.

 const Eigen::Matrix& get_R ( ) const
inline

Getter for R matrix.

 const ExponentialPlusPiecewisePolynomial& 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& 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,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, defaults to identity. R,Quadratic cost 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: