centroidal_control_collection
|
Preview control for one-dimensional CoM-ZMP model. More...
#include <PreviewControlZmp.h>
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< _StateSpaceModel > | model_ |
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... | |
Preview control for one-dimensional CoM-ZMP model.
See the following for a detailed formulation.
Definition at line 16 of file PreviewControlZmp.h.
using CCC::PreviewControlZmp1d::InitialParam = PreviewControl<3, 1, 1>::StateDimVector |
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.
|
inline |
Constructor.
com_height | height of robot CoM [m] |
horizon_duration | horizon duration [sec] |
horizon_dt | discretization timestep in horizon [sec] |
weight_param | objective weight parameter |
Definition at line 55 of file PreviewControlZmp.h.
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.
ref_zmp_func | function of reference ZMP [m] |
initial_param | initial parameter (CoM position, velocity, and acceleration) |
current_time | current time (i.e., start time of horizon) [sec] |
control_dt | control timestep used to calculate ZMP (if omitted, horizon_dt is used) |
|
protected |
Process one step.
|
friend |
Definition at line 18 of file PreviewControlZmp.h.