centroidal_control_collection
Classes | Public Types | Public Member Functions | Protected Member Functions | Friends | List of all members
CCC::PreviewControlZmp1d Class Reference

Preview control for one-dimensional CoM-ZMP model. More...

#include <PreviewControlZmp.h>

Inheritance diagram for CCC::PreviewControlZmp1d:
Inheritance graph
[legend]
Collaboration diagram for CCC::PreviewControlZmp1d:
Collaboration graph
[legend]

Classes

struct  WeightParam
 Weight parameter. More...
 

Public Types

using InitialParam = PreviewControl< 3, 1, 1 >::StateDimVector
 Initial parameter. More...
 
- Public Types inherited from CCC::PreviewControl< 3, 1, 1 >
using StateDimVector = Eigen::Matrix< double, StateDim, 1 >
 Type of state vector. More...
 
using InputDimVector = Eigen::Matrix< double, InputDim, 1 >
 Type of input vector. More...
 
using OutputDimVector = Eigen::Matrix< double, OutputDim, 1 >
 Type of output vector. More...
 
using _StateSpaceModel = StateSpaceModel< StateDim, InputDim, OutputDim >
 Type of state-space model. More...
 

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW PreviewControlZmp1d (double com_height, double horizon_duration, double horizon_dt, const WeightParam &weight_param=WeightParam())
 Constructor. More...
 
double planOnce (const std::function< double(double)> &ref_zmp_func, const InitialParam &initial_param, double current_time, double control_dt=-1) const
 Plan one step. More...
 
- Public Member Functions inherited from CCC::PreviewControl< 3, 1, 1 >
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PreviewControl (const std::shared_ptr< _StateSpaceModel > &model, double horizon_duration, double horizon_dt, const WeightParam &weight_param)
 Constructor. More...
 
InputDimVector calcOptimalInput (const StateDimVector &x, const Eigen::VectorXd &ref_output_seq) const
 Calculate optimal input. More...
 

Protected Member Functions

double procOnce (const Eigen::VectorXd &ref_zmp_seq, const InitialParam &initial_param, double current_time, double control_dt) const
 Process one step. More...
 
- Protected Member Functions inherited from CCC::PreviewControl< 3, 1, 1 >
void calcGain (const WeightParam &weight_param)
 Calculate the gain of the preview control. More...
 

Friends

class PreviewControlZmp
 

Additional Inherited Members

- Public Attributes inherited from CCC::PreviewControl< 3, 1, 1 >
std::shared_ptr< _StateSpaceModelmodel_
 State-space model. More...
 
double horizon_dt_
 Discretization timestep in horizon [sec]. More...
 
int horizon_steps_
 Number of steps in horizon. More...
 
double riccati_error_
 Error of algebraic Riccati equation. More...
 
- Protected Attributes inherited from CCC::PreviewControl< 3, 1, 1 >
Eigen::Matrix< double, StateDim, StateDim > P_
 Solution of the algebraic Riccati equation. More...
 
Eigen::Matrix< double, InputDim, StateDim > K_
 Feedback gain. More...
 
Eigen::Matrix< double, InputDim, Eigen::Dynamic > F_
 Preview gain. More...
 

Detailed Description

Preview control for one-dimensional CoM-ZMP model.

See the following for a detailed formulation.

Definition at line 16 of file PreviewControlZmp.h.

Member Typedef Documentation

◆ InitialParam

Initial parameter.

First element is CoM position, second element is CoM velocity, and third element is CoM acceleration.

Definition at line 25 of file PreviewControlZmp.h.

Constructor & Destructor Documentation

◆ PreviewControlZmp1d()

EIGEN_MAKE_ALIGNED_OPERATOR_NEW CCC::PreviewControlZmp1d::PreviewControlZmp1d ( double  com_height,
double  horizon_duration,
double  horizon_dt,
const WeightParam weight_param = WeightParam() 
)
inline

Constructor.

Parameters
com_heightheight of robot CoM [m]
horizon_durationhorizon duration [sec]
horizon_dtdiscretization timestep in horizon [sec]
weight_paramobjective weight parameter

Definition at line 55 of file PreviewControlZmp.h.

Member Function Documentation

◆ planOnce()

double CCC::PreviewControlZmp1d::planOnce ( const std::function< double(double)> &  ref_zmp_func,
const InitialParam initial_param,
double  current_time,
double  control_dt = -1 
) const

Plan one step.

Parameters
ref_zmp_funcfunction of reference ZMP [m]
initial_paraminitial parameter (CoM position, velocity, and acceleration)
current_timecurrent time (i.e., start time of horizon) [sec]
control_dtcontrol timestep used to calculate ZMP (if omitted, horizon_dt is used)
Returns
planned ZMP

◆ procOnce()

double CCC::PreviewControlZmp1d::procOnce ( const Eigen::VectorXd &  ref_zmp_seq,
const InitialParam initial_param,
double  current_time,
double  control_dt 
) const
protected

Process one step.

Friends And Related Function Documentation

◆ PreviewControlZmp

friend class PreviewControlZmp
friend

Definition at line 18 of file PreviewControlZmp.h.


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