 |
ocra-wbi-plugins
Doxygen documentation for the ocra-wbi-plugins repository
|
|
|
ocra-wbi-plugins
Doxygen documentation for the ocra-wbi-plugins repository
|
|
Builds base of support and corresponding constraints based on the robot's feet location. More...
#include <BaseOfSupport.h>
Public Member Functions | |
| BaseOfSupport (std::shared_ptr< StepController > stepController, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &T, MIQPParameters miqpParams) | |
| virtual | ~BaseOfSupport () |
| bool | update (const Eigen::VectorXd &xi_k) |
| void | computeBoundingBox (const Eigen::MatrixXd &feetCorners, Eigen::Matrix2d &minMaxBoundingBox) |
| void | getA (Eigen::MatrixXd &A) |
| void | getrhs (Eigen::VectorXd &rhs) |
Protected Member Functions | |
| void | buildAb () |
| void | buildb (const Eigen::Matrix2d &minMaxBoundingBox) |
| void | buildCp (double cz, double g) |
| void | buildCi (const Eigen::MatrixXd &Ab, const Eigen::MatrixXd &Cp) |
| void | buildf () |
| void | buildfbar (const Eigen::VectorXd &f) |
| void | buildB (const Eigen::MatrixXd &Ci, const Eigen::MatrixXd &Q) |
| void | buildA (const Eigen::MatrixXd &Ci, const Eigen::MatrixXd &Q, const Eigen::MatrixXd &T) |
Private Attributes | |
| Eigen::MatrixXd | _Ab |
| Eigen::VectorXd | _b |
| Eigen::MatrixXd | _Cp |
| Eigen::MatrixXd | _Ci |
| Eigen::VectorXd | _f |
| Eigen::MatrixXd | _A |
| Eigen::VectorXd | _fbar |
| Eigen::MatrixXd | _B |
| Eigen::VectorXd | _rhs |
| Polygon | _poly |
| Box | _bbox |
| std::shared_ptr< StepController > | _stepController |
| Eigen::MatrixXd | _Q |
| Eigen::MatrixXd | _T |
| MIQPParameters | _miqpParams |
Builds base of support and corresponding constraints based on the robot's feet location.
This class uses the Geometry Boost libraries in order to find the vertices of the bounding box defined by the location of the feet of the robot. Afterwards, it will build the inequality constraints in a preview window of size \(N\) delimiting the area of this polygon where the Center of Pressure is constrained to lie.
The bounding box of the Base of Support as the ones shown in the figure above writes as a set of intequality constraints, where \(\mathbf{p}\) is the horizonal Center of Pressure (CoP).\[ \left[\begin{array}{cc} -1 & 0\\ 1 & 0\\ 0 & -1\\ 0 & 1 \end{array}\right] \mathbf{p} \leq \left[\begin{array}{c} -x_{\text{min}}\\ x_{\text{max}}\\ -y_{\text{min}}\\ y_{\text{max}} \end{array}\right] \]
or,
\[ \mathbf{A}_b\mathbf{p} \leq \mathbf{b} \]
Which in terms of the system state \(\xi\) writes:
\[ \left[\begin{array}{cc} \mathbf{0}_{10\times10} & \\ & \mathbf{A}_b \mathbf{C}_p \end{array}\right] \mathbf{\xi} \leq \left[\begin{array}{c} \mathbf{0} \\ \mathbf{b} \end{array}\right] \]
or:
\[ \mathbf{C}_i \mathbf{\xi} \leq \mathbf{f}_c \]
Where \(\mathbf{C}_p\) expresses the relationship between CoM and CoP, see buildCp() for more details.
For this type of constraint, in a preview window of size \(N\) we have:
\[ \left[\begin{array}{cccc} \mathbf{C}_i\mathbf{T} & 0 & \cdots & 0 \\ \mathbf{C}_i\mathbf{Q}\mathbf{T} & \mathbf{C}_i\mathbf{T} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{C}_i\mathbf{Q}^{N-1}\mathbf{T} & \mathbf{C}_i\mathbf{Q}^{N-2}\mathbf{T} & \cdots & \mathbf{C}_i\mathbf{T} \end{array}\right] \mathbf{\mathcal{X}}_{N,k} \leq \left[\begin{array}{c} \mathbf{f}_c\\ \mathbf{f}_c\\ \vdots\\ \mathbf{f}_c \end{array}\right] - \left[\begin{array}{c} \mathbf{C}_i\mathbf{Q}\\ \mathbf{C}_i\mathbf{Q}^2\\ \vdots\\ \mathbf{C}_i\mathbf{Q}^N \end{array}\right] \mathbf{\xi}_k \]
or:
\[ \mathbf{A} \mathbf{\mathcal{X}}_{N,k} \leq \bar{\mathbf{f}} - \mathbf{B}\xi_k \]
| BaseOfSupport::BaseOfSupport | ( | std::shared_ptr< StepController > | stepController, |
| const Eigen::MatrixXd & | Q, | ||
| const Eigen::MatrixXd & | T, | ||
| MIQPParameters | miqpParams | ||
| ) |
Constructor.
| [in] | stepController | Pointer to the stepController object instantiated by `walking-client` |
| BaseOfSupport::~BaseOfSupport | ( | ) | [virtual] |
Destructor
| void BaseOfSupport::buildA | ( | const Eigen::MatrixXd & | Ci, |
| const Eigen::MatrixXd & | Q, | ||
| const Eigen::MatrixXd & | T | ||
| ) | [protected] |
| void BaseOfSupport::buildAb | ( | ) | [protected] |
Builds \(\mathbf{A}_b\).
| void BaseOfSupport::buildb | ( | const Eigen::Matrix2d & | minMaxBoundingBox | ) | [protected] |
Builds \(\mathbf{b}\).
| [in] | minMaxBoundingBox | Matrix containing minimum and maximum points of the current bounding box. |
| void BaseOfSupport::buildB | ( | const Eigen::MatrixXd & | Ci, |
| const Eigen::MatrixXd & | Q | ||
| ) | [protected] |
| void BaseOfSupport::buildCi | ( | const Eigen::MatrixXd & | Ab, |
| const Eigen::MatrixXd & | Cp | ||
| ) | [protected] |
| void BaseOfSupport::buildCp | ( | double | cz, |
| double | g | ||
| ) | [protected] |
Builds matrix \(\mathbf{C}_p\).
| [in] | cz | Constant CoM height. |
| [in] | g | Gravity acceleration [m/s^2]. |
| void BaseOfSupport::buildf | ( | ) | [protected] |
Builds vector \(\mathbf{f}\).
| void BaseOfSupport::buildfbar | ( | const Eigen::VectorXd & | f | ) | [protected] |
| void BaseOfSupport::computeBoundingBox | ( | const Eigen::MatrixXd & | feetCorners, |
| Eigen::Matrix2d & | minMaxBoundingBox | ||
| ) |
Computes the bounding box points (minimum and maximum points of the box of the current support configuration) given the corners of the robot's feet in #feetCorners.
| [in] | feetCorners | Matrix of 2D coordinates of the feet corners (or contact points). |
| [out] | minMaxBoundingBox | Two row-wise points corresponding to the minimum and maximum points and in the resulting bounding box. |
| void BaseOfSupport::getA | ( | Eigen::MatrixXd & | A | ) |
Getter for the inequality matrix A
| [out] | Constraint | matrix \(\mathbf{A}\) in preview window. |
| void BaseOfSupport::getrhs | ( | Eigen::VectorXd & | rhs | ) |
| bool BaseOfSupport::update | ( | const Eigen::VectorXd & | xi_k | ) |
To be called at every update cycle of the hosting client.
| [in] | xi_k | Current system state. |
Eigen::MatrixXd BaseOfSupport::_A [private] |
Matrix \(\mathbf{A}\) in the bounding box constraints for a preview window of size \(N\)
\[ \mathbf{A} = \left[\begin{array}{cccc} \mathbf{C}_i\mathbf{T} & 0 & \cdots & 0 \\ \mathbf{C}_i\mathbf{Q}\mathbf{T} & \mathbf{C}_i\mathbf{T} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ \mathbf{C}_i\mathbf{Q}^{N-1}\mathbf{T} & \mathbf{C}_i\mathbf{Q}^{N-2}\mathbf{T} & \cdots & \mathbf{C}_i\mathbf{T} \end{array}\right] \]
Eigen::MatrixXd BaseOfSupport::_Ab [private] |
Inequality matrix \(\mathbf{A}_b\) expressing the bounding box for the current contact configuration of the robot's feet
\[ \mathbf{A}_b = \left[\begin{array}{cc} -1 & 0\\ 1 & 0\\ 0 & -1\\ 0 & 1 \end{array}\right] \]
Eigen::VectorXd BaseOfSupport::_b [private] |
Right-hand-side vector of the inequality expression the interior of the bounding box for the current contact configuration of the robot's feet. For the current bounding box defined by minimum and maximum points \(\mathbf{p}_{\text{min}}\) and \(\mathbf{p}_{\text{max}}\) we get:
\[ \mathbf{b} = \left[ \begin{array}{c} -x_{\text{min}}\\ x_{\text{max}}\\ -y_{\text{min}}\\ y_{\text{max}} \end{array} \right] \]
Eigen::MatrixXd BaseOfSupport::_B [private] |
Box BaseOfSupport::_bbox [private] |
Polygon object representing the bounding box (envelope) delimiting _poly.
Eigen::MatrixXd BaseOfSupport::_Ci [private] |
Matrix \(\mathbf{C}_i\) in the inequality expressing the CoP constraints of the system at the current time instant \(k\) and given by \(\mathbf{C}_i \xi_k \leq \mathbf{f}\). \(\mathbf{C}_i\) is a \(14 \times 16\) matrix, and \(\mathbf{f}_p\) a vector in \(\mathbb{R}^{n_p}\) or:
\[ \left[\begin{array}{cc} \mathbf{0}_{10\times10} & \\ & \mathbf{A}_b \mathbf{C}_p \end{array}\right] \mathbf{\xi} \leq \left[\begin{array}{c} \mathbf{0} \\ \mathbf{b} \end{array}\right] \]
Eigen::MatrixXd BaseOfSupport::_Cp [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] \]
Eigen::VectorXd BaseOfSupport::_f [private] |
Right-hand side vector \(\mathbf{f}_c\) of the inequalities expressing the bounding box, when the latter is expressed in terms of the sytem state \(\xi_k\) and equal to:
\[ \left[ \begin{array}{c} \mathbf{0}\\ \mathbf{b} \end{array}\right] \]
Eigen::VectorXd BaseOfSupport::_fbar [private] |
Vector \(\bar{\mathbf{f}}\) in the bounding box constraints for a preview window of size \(N\)
\[ \bar{\mathbf{f}} = \left[\begin{array}{c} \mathbf{f}_c\\ \mathbf{f}_c\\ \vdots\\ \mathbf{f}_c \end{array}\right] \]
MIQPParameters BaseOfSupport::_miqpParams [private] |
Copy of the parameters of the MIQP controller which instantiates MIQPLinearConstraints in which the BoundingBox object will be instantiated.
Polygon BaseOfSupport::_poly [private] |
Polygon object holding the current points of contact.
Eigen::MatrixXd BaseOfSupport::_Q [private] |
Matrix \(\mathbf{Q}\) in preview state model:
\[ \mathbf{\xi}_{k+1|k} = \mathbf{Q} \xi_{k|k} + \mathbf{T}\mathcal{X}_{k+1|k} \]
\[ \mathbf{Q} = \left[\begin{array}{cc} \mathbf{0}_{10\times10} & \mathbf{0}_{10\times6}\\ \mathbf{0}_{6\times10} & \mathbf{A_h}_{6\times6} \end{array}\right] \]
Size: \([16\times16]\)
Eigen::VectorXd BaseOfSupport::_rhs [private] |
std::shared_ptr<StepController> BaseOfSupport::_stepController [private] |
Pointer to the stepController object instantiated by the `walking-client`. Since it contains all the contact tasks, it was easier to interface to it to requests the current contact state of the robot, i.e. contact points locations on the ground.
Eigen::MatrixXd BaseOfSupport::_T [private] |
Matrix \(\mathbf{T}\) in preview state model:
\[ \mathbf{\xi}_{k+1|k} = \mathbf{Q} \xi_{k|k} + \mathbf{T}\mathcal{X}_{k+1|k} \]
\[ \mathbf{T} = \left[\begin{array}{cc} \mathbf{I}_{10\times10} & \mathbf{0}_{10\times2}\\ \mathbf{0}_{6\times10} & \mathbf{B_h}_{6\times2} \end{array}\right] \]
Size: \([16\times12]\)
1.7.6.1