 |
ocra-wbi-plugins
Doxygen documentation for the ocra-wbi-plugins repository
|
|
|
ocra-wbi-plugins
Doxygen documentation for the ocra-wbi-plugins repository
|
|
Implementes an extended ZMP preview controller as an unconstrained QP problem. More...
#include <ZmpPreviewController.h>
Public Member Functions | |
| ZmpPreviewController (const double period, std::shared_ptr< ZmpPreviewParams > parameters) | |
| virtual | ~ZmpPreviewController () |
| bool | initialize () |
| template<typename Derived > | |
| void | computeOptimalInput (const Eigen::MatrixBase< Derived > &zmpRef, const Eigen::MatrixBase< Derived > &comVelRef, const Eigen::MatrixBase< Derived > &hk, Eigen::MatrixBase< Derived > &optimalU) |
| void | integrateCom (Eigen::VectorXd comJerk, Eigen::VectorXd hk, Eigen::VectorXd &hkk) |
| void | tableCartModel (Eigen::Vector2d hk, Eigen::VectorXd ddhk, Eigen::Vector2d &p) |
| void | tableCartModel (Eigen::VectorXd hkk, Eigen::Vector2d &p) |
| Eigen::MatrixXd | buildAh (const double dt) |
| Eigen::MatrixXd | buildBh (const double dt) |
| Eigen::MatrixXd | buildCp (const double cz, const double g) |
| Eigen::MatrixXd | buildGp (Eigen::MatrixXd Cp, Eigen::MatrixXd Ah, const int Np) |
| Eigen::MatrixXd | buildHp (Eigen::MatrixXd Cp, Eigen::MatrixXd Bh, Eigen::MatrixXd Ah, const int Nc, const int Np) |
| Eigen::MatrixXd | buildCh () |
| Eigen::MatrixXd | buildGh (Eigen::MatrixXd Ch, Eigen::MatrixXd Ah, const int Np) |
| Eigen::MatrixXd | buildHh (Eigen::MatrixXd Ch, Eigen::MatrixXd Bh, Eigen::MatrixXd Ah, const int Nc, const int Np) |
| Eigen::MatrixXd | buildNu (const double nu, const int Nc) |
| Eigen::MatrixXd | buildNw (const double nw, const int Np) |
| Eigen::MatrixXd | buildNb (const double nb, const int Np) |
| bool | computeFootZMP (FOOT whichFoot, Eigen::VectorXd wrench, Eigen::Vector2d &footZMP, Eigen::VectorXd &wrenchInWorldRef, ocra::Model::Ptr model, const double tolerance=1e-3) |
| bool | computeGlobalZMPFromSensors (Eigen::VectorXd rawLeftFootWrench, Eigen::VectorXd rawRightFootWrench, ocra::Model::Ptr model, Eigen::Vector2d &globalZMP) |
| void | getFTSensorAdjointMatrix (FOOT whichFoot, Eigen::MatrixXd &T, Eigen::Vector3d &sensorPosition, ocra::Model::Ptr model) |
Private Attributes | |
| const double | cz |
| const double | g = 9.8 |
| const double | dt |
| const int | Nc |
| const int | Np |
| const double | nu |
| const double | nw |
| const double | nb |
| const Eigen::MatrixXd | Nu |
| const Eigen::MatrixXd | Nw |
| const Eigen::MatrixXd | Nb |
| const Eigen::MatrixXd | Ah |
| const Eigen::MatrixXd | Bh |
| const Eigen::MatrixXd | Cp |
| const Eigen::MatrixXd | Gp |
| const Eigen::MatrixXd | Hp |
| const Eigen::MatrixXd | Ch |
| const Eigen::MatrixXd | Gh |
| const Eigen::MatrixXd | Hh |
| Eigen::MatrixXd | AOptimal |
| Eigen::MatrixXd | bOptimal |
Implementes an extended ZMP preview controller as an unconstrained QP problem.
Given a rough ZMP trajectory and a corresponding consistent CoM horizontal velocity, this class computes at a fast rate optimal CoM jerks over a refined preview horizon. The ZMP preview control formulation in (reference goes here) is extended to account for a CoM velocity tracking objective in order to minimize the error over a preview horizon to reference CoM velocities. This preview controller solves the following problem in a time window of size \(N_c\)
\begin{align*} \mathcal{U}_{k+N_c|k}^* = \underset{\mathcal{U}_{k+N_c|k}}{\text{arg min}} \; \sum_{j=1}^{N} & \eta_b || \mathbf{p}_{k+j|k} - \mathbf{r}_{k+j|k}^r ||^2 + \eta_w ||\mathbf{\dot{h}}_{k+j|k} - \dot{\mathbf{h}}^r_{k+j|k} ||^2 + \eta_u || \mathbf{u} ||^2 \\ \text{such that}&\\ \mathbf{p} &= \mathbf{h} - \mathbf{c}\mathbf{e}_2 \mathbf{\ddot{h}} \\ \mathbf{\hat{h}}_{k+j+1|k} &= \mathbf{A}_h \hat{\mathbf{h}}_{k+j|k} + \mathbf{B}_h \mathbf{u}_{k+j+1|k} \\ \mathbf{h}_{k+j|k} &= \mathbf{h}_k\\ \mathbf{h} &= \mathbf{c} - (\mathbf{c}\cdot\mathbf{e}_2)\mathbf{e}_2 \end{align*}
Where \({U}_{k+N_c|k}^*\) is the optimal horizon of inputs, \(\mathbf{p}\) is the horizontal ZMP coordinate, \( \mathbf{r}^r \) ZMP references (in `walking-client` it's the interpolated ZMP reference from the walking MIQP problem) \( \mathbf{\dot{h}} \) the horizontal CoM velocity, \( \mathbf{\dot{h}}^r \) CoM horizontal velocity reference (in walking-client it's the interpolated CoM velocity reference from the walking MIQP problem). \(\mathbf{u}\) is the input CoM jerk, \(\mathbf{c}\) the three-dimensional CoM position. The horizontal CoM state is denoted \(\hat{\mathbf{h}} = (\mathbf{h}, \mathbf{\dot{h}}, \mathbf{\ddot{h}}) \) (CoM horizontal position, velocity and acceleration respectively) while \(\mathbf{e}_2\) is the vertical unit vector \( [0\;0\;1]^T\). \(\mathbf{A}_h\) and \(\mathbf{B}_h\) are integration matrices.
The previously described problem is an unconstrained QP problem which has a closed-form solution. The same can be expressed in matrix form as:
\[ \underset{\mathbf{U}}{\text{min}} \; (\mathbf{P}_r - \mathbf{P})^T \mathbf{N}_b (\mathbf{P}_r - \mathbf{P}) + (\tilde{\mathbf{H}}_r - \mathbf{H})^T\mathbf{N}_w(\tilde{\mathbf{H}}_r - \mathbf{H}) + \mathbf{U}^T\mathbf{N}_u \mathbf{U} \]
Where:
\begin{align*} \mathbf{U}^T &= \left[ \mathbf{u}^T_{k+1|k}, \cdots, \mathbf{u}^T_{k+N_c|k} \right]^T \\ \mathbf{P}^T &= \left[ \mathbf{p}^T_{k+1|k}, \cdots, \mathbf{p}^T_{k+N_p|k} \right]^T \\ \mathbf{P}^T_r &= \left[ \mathbf{r}^T_{k+1|k}, \cdots, \mathbf{r}^T_{k+N_p|k} \right]^T \\ \mathbf{\tilde{H}}^T &= \left[ \dot{\mathbf{h}}^T_{k+1|k}, \cdots, \dot{\mathbf{h}}^T_{k+N_p|k} \right]^T \\ \mathbf{\tilde{H}}^T_r &= \left[ \dot{\mathbf{h}^r}^T_{k+1|k}, \cdots, \dot{\mathbf{h}^r}^T_{k+N_p|k} \right]^T \\ \mathbf{N}_b &= \eta_b\left[\begin{array}{ccc} 1 & \cdots & 0 \\ \vdots & \ddots & \vdots\\ 0 & \cdots & 1 \end{array}\right] \end{align*}
\(\mathbf{N}_w\) and \(\mathbf{N}_u\) have a similar structure to \(\mathbf{N}_b\).
A closed-form solution can thus be found and is equal to:
\[ \mathbf{U}_{k+N_c|k} = (\mathbf{H}_p^T \mathbf{N}_b \mathbf{H}_p + \mathbf{N}_u + \mathbf{H}_h^T \mathbf{N}_w \mathbf{H}_h)^{-1} \left(\mathbf{H}^T_p \mathbf{N}_b (\mathbf{P}_r - \mathbf{G}_p \hat{\mathbf{h}}_k) + \mathbf{H}^T_h\mathbf{N}_w(\tilde{\mathbf{H}}_r - \mathbf{G}_h \hat{\mathbf{h}}_k)\right) \]
| ZmpPreviewController::ZmpPreviewController | ( | const double | period, |
| std::shared_ptr< ZmpPreviewParams > | parameters | ||
| ) |
Class constructor. Builds all the time-invariant matrices used to compute the optimal output of the preview controller.
| period | Period (in ms) of the thread in which an object of this class is created. |
| parameters | An object containing the main parameters. |
| ZmpPreviewController::~ZmpPreviewController | ( | ) | [virtual] |
| Eigen::MatrixXd ZmpPreviewController::buildAh | ( | const double | dt | ) |
Builds \(A_h\). Called during the member list initialization of the constructor of this class.
| dt | Thread period in which this classed in instantiated. |
| Eigen::MatrixXd ZmpPreviewController::buildBh | ( | const double | dt | ) |
Builds \(B_h\). Called during the member list initialization of the constructor of this class.
| dt | Thread period in which this classed in instantiated. |
| Eigen::MatrixXd ZmpPreviewController::buildCh | ( | ) |
Builds \(C_h\). Called during the member list initialization of the constructor of this class
| Eigen::MatrixXd ZmpPreviewController::buildCp | ( | const double | cz, |
| const double | g | ||
| ) |
Builds \(C_p\). Called during the member list initialization of the constructor of this class
| cz | Constant CoMheight. |
| g | Constant gravity acceleration (m^2/s) |
| Eigen::MatrixXd ZmpPreviewController::buildGh | ( | Eigen::MatrixXd | Ch, |
| Eigen::MatrixXd | Ah, | ||
| const int | Np | ||
| ) |
Builds \(G_h\). Called during the member list initialization of the constructor of this class
| Ch | \(C_h\) |
| Ah | \(A_h\) |
| Np | \(N_p\) |
| Eigen::MatrixXd ZmpPreviewController::buildGp | ( | Eigen::MatrixXd | Cp, |
| Eigen::MatrixXd | Ah, | ||
| const int | Np | ||
| ) |
Builds \(G_p\). Called during the member list initialization of the constructor of this class
| Cp | \(C_p\) |
| Ah | \(A_h\) |
| Nc | \(N_p\) |
| Eigen::MatrixXd ZmpPreviewController::buildHh | ( | Eigen::MatrixXd | Ch, |
| Eigen::MatrixXd | Bh, | ||
| Eigen::MatrixXd | Ah, | ||
| const int | Nc, | ||
| const int | Np | ||
| ) |
Builds \(H_h\). Called during the member list initialization of the constructor of this class
| Ch | \(C_h\) |
| Bh | \(B_h\) |
| Ah | \(A_h\) |
| Hh | \(N_c\) |
| Nc | \(N_p\) |
| Eigen::MatrixXd ZmpPreviewController::buildHp | ( | Eigen::MatrixXd | Cp, |
| Eigen::MatrixXd | Bh, | ||
| Eigen::MatrixXd | Ah, | ||
| const int | Nc, | ||
| const int | Np | ||
| ) |
Builds \(H_p\). Called during the member list initialization of the constructor of this class
| Cp | \(C_p\) |
| Bh | \(B_h\) |
| Ah | \(A_h\) |
| Nc | \(N_c\) |
| Np | \(N_p\) |
| Eigen::MatrixXd ZmpPreviewController::buildNb | ( | const double | nb, |
| const int | Np | ||
| ) |
Builds \(N_b\). Called during the member list initialization of the constructor of this class
| nb | \(\eta_b\) |
| Np | \(N_p\) |
| Eigen::MatrixXd ZmpPreviewController::buildNu | ( | const double | nu, |
| const int | Nc | ||
| ) |
Builds \(N_u\). Called during the member list initialization of the constructor of this class
| nu | \(\eta_u\) |
| Nc | \(N_c\) |
| Eigen::MatrixXd ZmpPreviewController::buildNw | ( | const double | nw, |
| const int | Np | ||
| ) |
Builds \(N_w\). Called during the member list initialization of the constructor of this class
| nw | \(\eta_w\) |
| Np | \(Nc\) |
| bool ZmpPreviewController::computeFootZMP | ( | FOOT | whichFoot, |
| Eigen::VectorXd | wrench, | ||
| Eigen::Vector2d & | footZMP, | ||
| Eigen::VectorXd & | wrenchInWorldRef, | ||
| ocra::Model::Ptr | model, | ||
| const double | tolerance = 1e-3 |
||
| ) |
Computes the ZMP for a single foot in world reference frame.
Assuming that \(\mathbf{p}\) is the position of the ZMP for a single foot, \(\mathbf{p}_s\) the position of a force torque (F/T) sensor at the foot, the ZMP position can be computed as:
\[ \left[\begin{array}{c}p_x \\ p_y \end{array}\right] = \frac{1}{f_z} \left[\begin{array}{cccccc} -p_{s_z} & 0 & p_{s_x} & 0 & -1 & 0 \\ 0 & -p_{s_z} & p_{s_y} & 1 & 0 & 0 \end{array}\right] \left[\begin{array}{c} \mathbf{f}\\ \mathbf{\tau} \end{array}\right] \]
| whichFoot | LEFT_FOOT or RIGHT_FOOT. | |
| wrench | External wrench on the foot as read by the F/T sensors. | |
| [out] | footZMP | Foot ZMP in world reference frame. |
| [out] | wrenchInWorldRef | Transformed wrench in world reference frame. |
| tolerance | Tolerance value below which the ZMP is considered null. Kajita2014Intro |
| bool ZmpPreviewController::computeGlobalZMPFromSensors | ( | Eigen::VectorXd | rawLeftFootWrench, |
| Eigen::VectorXd | rawRightFootWrench, | ||
| ocra::Model::Ptr | model, | ||
| Eigen::Vector2d & | globalZMP | ||
| ) |
Computes the global ZMP for two feet in contact.
After obtaining the ZMP position for both feet \(\mathbf{p}_R\) and \(\mathbf{p}_L\) independently and expressed in the world reference frame, in the case where both feet are in contact with the ground (or just one), the global expression of the ZMP \(\mathbf{p}\) expressed in the world reference frame is:
\[ \left[\begin{array}{c} p_x\\ p_y \end{array}\right] = \frac{1}{f_{R_z} + f_{L_z}} \left[\begin{array}{cc} \mathbf{p}_R & \mathbf{p}_L \end{array}\right] \left[\begin{array}{c} f_{R_z}\\ f_{L_z} \end{array}\right] \]
| rawLeftFootWrench | Raw left foot wrench as read from the sensors [force | torque] |
| rawRightFootWrench | Raw right foot wrench as read from the sensors. |
| globalZMP | Global zmp in world reference frame considering both feet. @ Kajita2014Intro |
| void ZmpPreviewController::computeOptimalInput | ( | const Eigen::MatrixBase< Derived > & | zmpRef, |
| const Eigen::MatrixBase< Derived > & | comVelRef, | ||
| const Eigen::MatrixBase< Derived > & | hk, | ||
| Eigen::MatrixBase< Derived > & | optimalU | ||
| ) | [inline] |
Computes a horizon of optimal inputs (CoM jerks) to be applied to the system, i.e.:
\begin{align*} \mathcal{U}_{k+N|k} &= (\mathbf{H}_p^T \mathbf{N}_b \mathbf{H}_p + \mathbf{N}_u + \mathbf{H}_h^T \mathbf{N}_w \mathbf{H}_h)^{-1} \left(\mathbf{H}^T_p \mathbf{N}_b (\mathbf{P}_r - \mathbf{G}_p \hat{\mathbf{h}}_k) + \mathbf{H}^T_h\mathbf{N}_w(\tilde{\mathbf{H}}_r - \mathbf{G}_h \hat{\mathbf{h}}_k)\right)\\ \mathcal{U}_{k+N|k} &= \mathbf{A}_{\text{opt}}^{-1} \mathbf{b}_{\text{opt}} \end{align*}
This solution is computed through standard Cholesky decomposition. See Eigen::LLT for more details. Matrix ZmpPreviewController::AOptimal is precomputed by the constructor for efficiency reasons.
| zmpRef | \(\mathbf{P}_r\) Horizon of \(N_c\) ZMP references. |
| comVelRef | \(\tilde{\mathbf{H}}_r\) Horizon of \(N_c\) CoM velocities. |
| hk | Current measured CoM state at time \(k\), i.e. \(\hat{\mathbf{h}}_k\) |
| optimalU | Closed-form solution to the unconstrained QP problem, \(\mathcal{U}_{k+N_c|k}\) |
| void ZmpPreviewController::getFTSensorAdjointMatrix | ( | FOOT | whichFoot, |
| Eigen::MatrixXd & | T, | ||
| Eigen::Vector3d & | sensorPosition, | ||
| ocra::Model::Ptr | model | ||
| ) |
Retrieves the FT sensor adjoint matrix expressed in the world reference frame which multiplied by the local measurement of the sensor gives you the measurement in the world reference.
| whichFoot | LEFT_FOOT or RIGHT_FOOT | |
| [out] | T | Adjoint matrix. |
| [out] | sensorPosition | 3D Position of the F/T sensor corresponding to whichFoot. |
| model | Pointer to the model that must have been initialized by the calling thread. |
| bool ZmpPreviewController::initialize | ( | ) |
Initializes the matrices and parameters used by the controller.
Currently we are not initializing through this method, but through the constructor.
| void ZmpPreviewController::integrateCom | ( | Eigen::VectorXd | comJerk, |
| Eigen::VectorXd | hk, | ||
| Eigen::VectorXd & | hkk | ||
| ) |
Integrates the CoM for a given CoM jerk input. Integration matrices \(\mathbf{A}_h\) and \(\mathbf{b}_h\) are precomputed by the constructor of this class.
\[ \mathbf{h}_{k+1} = \mathbf{A}_h \mathbf{h}_k + \mathbf{B}_h \mathbf{u}_k \]
| comJerk | Input com jerk \(\mathbf{u}_k\) | |
| hk | Current COM state \(\mathbf{h}_k\) | |
| [out] | hkk | Integrated COM state \(\mathbf{h}_{k+1}\) |
| void ZmpPreviewController::tableCartModel | ( | Eigen::Vector2d | hk, |
| Eigen::VectorXd | ddhk, | ||
| Eigen::Vector2d & | p | ||
| ) |
Computes a simplified model-based ZMP.
Computes the zero-moment point given the robot's horizontal CoM acceleration and position, besides a constant CoM height and gravity acceleration.
| hk | Horizontal CoM position. | |
| ddhk | Horizontal CoM acceleration. | |
| [out] | p | Computed ZMP. |
| void ZmpPreviewController::tableCartModel | ( | Eigen::VectorXd | hkk, |
| Eigen::Vector2d & | p | ||
| ) |
Computes a simplified model-based ZMP.
Computes the zero-moment point given the robot's horizontal CoM acceleration and position, besides a constant CoM height and gravity acceleration.
\[ \mathbf{p}_{k+1} = \mathbf{C}_p \mathbf{h}_{k+1} \]
| hkk | Horizontal CoM state. | |
| [out] | p | Computed ZMP. |
const Eigen::MatrixXd ZmpPreviewController::Ah [private] |
State matrix \(\mathbf{A}_h\) from the linear state process of the CoMstate \(\hat{\mathbf{h}}\). It is a constant matrix of size \(6\times6\) equal to:
\[ \mathbf{A_h} = \left[ \begin{array}{ccc} \mathbf{I}_2 & \delta t \mathbf{I}_2 & \frac{\delta t^2}{2} \mathbf{I}_2 \\ 0 & \mathbf{I}_2 & \delta t \\ 0 & 0 & \mathbf{I}_2 \end{array} \right] \]
Eigen::MatrixXd ZmpPreviewController::AOptimal [private] |
Matrix \(\mathbf{A}_{\text{opt}}\) in computeOptimalInput().
const Eigen::MatrixXd ZmpPreviewController::Bh [private] |
Input matrix \(\mathbf{B}_h\) from the linear state process of the CoMstate \(\hat{\mathbf{h}}\). It is constant of size \(6\times2\) and equal to:
\[ \mathbf{B_h} = \left[ \begin{array}{c} \frac{\delta^3}{6}\mathbf{I}_2 \\ \frac{\delta t^2}{2} \mathbf{I}_2 \\ \delta t \mathbf{I}_2 \end{array} \right] \]
Eigen::MatrixXd ZmpPreviewController::bOptimal [private] |
Vector \(\mathbf{b}_{\text{opt}}\) in computeOptimalInput().
const Eigen::MatrixXd ZmpPreviewController::Ch [private] |
Output matrix of the CoMlinear process state transition where the CoMhorizontal velocity is the output. It is of size \(2\times6\) and equal to:
\[ \mathbf{C}_h = \left[\begin{array}{ccc} \mathbf{0}_2 & \mathbf{I}_2 & \mathbf{0}_2 \\ \end{array}\right] \]
const Eigen::MatrixXd ZmpPreviewController::Cp [private] |
Output matrix \(C_p\) from the linear state process relating ZMP to the CoMdynamics \(\hat{\mathbf{h}}\). It is time-invariant of size \(2\times6\) and equal to:
\[ \mathbf{C}_p = \left[\begin{array}{ccc} \mathbf{I}_2 & \mathbf{0}_2 & -\frac{c_z}{g}\mathbf{I}_2 \\ \end{array}\right] \]
const double ZmpPreviewController::cz [private] |
CoMconstant height. It is not hardcoded but corresponds to the vertical coordinate (height) of the CoMat the beginning of execution, i.e. \( c_z \).
const double ZmpPreviewController::dt [private] |
Rate of the thread where this ZmpPreviewController is executed.
const double ZmpPreviewController::g = 9.8 [private] |
Gravity constant value in m/s^2, \( g \).
const Eigen::MatrixXd ZmpPreviewController::Gh [private] |
State matrix \(\mathbf{G}_h\) of size \(2N_p \times 6\) from the preview horizon of CoMvelocities. It is constant and equal to:
\[ \mathbf{G}_h = \left[\begin{array}{c} \mathbf{C}_h\mathbf{A}_h \\ \vdots \\ \mathbf{C}_h\mathbf{A}^{N_p}_h \end{array}\right] \]
const Eigen::MatrixXd ZmpPreviewController::Gp [private] |
State matrix \(\mathbf{G}_p\) from the preview horizon of ZMP outputs \(\mathbf{P}\). It is of size \(2N_p \times 6\) and equal to:
\[ \mathbf{G}_p = \left[\begin{array}{c} \mathbf{C}_p\mathbf{A}_h \\ \vdots \\ \mathbf{C}_p\mathbf{A}^{N_p}_h \end{array}\right] \]
const Eigen::MatrixXd ZmpPreviewController::Hh [private] |
Input matrix \(\mathbf{H}_h\) from the preview horizon of CoMvelocities \(\tilde{\mathbf{H}}\), of size \(2N_p \times 2N_c\) and equal to:
\[ \mathbf{H}_h = \left[\begin{array}{cccc} \mathbf{C}_h\mathbf{B}_h & 0 & \cdots & 0 \\ \mathbf{C}_h\mathbf{A}_h\mathbf{B}_h & \mathbf{C}_h\mathbf{B}_h & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{C}_h\mathbf{A}^{N_p-1}_h\mathbf{B}_h & \mathbf{C}_h\mathbf{A}^{N_p-2}_h\mathbf{B}_h & \cdots & \mathbf{C}_h\mathbf{A}^{N_p - N_c}\mathbf{B}_h \end{array}\right] \]
const Eigen::MatrixXd ZmpPreviewController::Hp [private] |
Input matrix \(\mathbf{H}_p\) from the preview horizon of ZMP outputs \(\mathbf{P}\). It is of size \(2N_p \times 2N_c\) and equal to:
\[ \mathbf{H}_p = \left[\begin{array}{cccc} \mathbf{C}_p\mathbf{B}_h & 0 & \cdots & 0 \\ \mathbf{C}_p\mathbf{A}_h\mathbf{B}_h & \mathbf{C}_p\mathbf{B}_h & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{C}_p\mathbf{A}^{N_p-1}_h\mathbf{B}_h & \mathbf{C}_p\mathbf{A}^{N_p-2}_h\mathbf{B}_h & \cdots & \mathbf{C}_p\mathbf{A}_p^{N_p - N_c}\mathbf{B}_h \end{array}\right] \]
const double ZmpPreviewController::nb [private] |
Weight of the balancing cost function \( \eta_b \)
const Eigen::MatrixXd ZmpPreviewController::Nb [private] |
Diagonal matrix \(N_b = \eta_b\mathbf{I}_{2N_p}\) of size \([2N_p \times 2N_p]\) weighting the balancing cost function in its matrix form.
const int ZmpPreviewController::Nc [private] |
Size of control window \( N_c \). It should less or equal to \(Np\).
const int ZmpPreviewController::Np [private] |
Size of preview window \( N_p \). It should be greater or equal to \(N_c\).
const double ZmpPreviewController::nu [private] |
Weight of the input regularization term in the cost function \(\eta_u\)
const Eigen::MatrixXd ZmpPreviewController::Nu [private] |
Diagonal matrix \(N_u = \eta_u\mathbf{I}_{2N_c}\) of size \([2N_c \times 2N_c]\) weighting the input regularization term in the matricial expression of the controller's cost function.
const double ZmpPreviewController::nw [private] |
Weight of the walking cost function \( \eta_w \)
const Eigen::MatrixXd ZmpPreviewController::Nw [private] |
Diagonal matrix \(N_w\ = \eta_w\mathbf{I}_{2N_p}\) of size \([2N_p \times 2N_p]\) weighting the walking cost function in its matrix form.
1.7.6.1