Center of pressure residual function. More...
#include <crocoddyl/multibody/residuals/contact-cop-position.hpp>
Public Types | |
typedef ResidualModelAbstractTpl< Scalar > | Base |
typedef CoPSupportTpl< Scalar > | CoPSupport |
typedef ResidualDataContactCoPPositionTpl< Scalar > | Data |
typedef DataCollectorAbstractTpl< Scalar > | DataCollectorAbstract |
typedef MathBaseTpl< Scalar > | MathBase |
typedef MathBase::Matrix46s | Matrix46 |
typedef MathBase::MatrixXs | MatrixXs |
typedef ResidualDataAbstractTpl< Scalar > | ResidualDataAbstract |
typedef StateMultibodyTpl< Scalar > | StateMultibody |
typedef MathBase::VectorXs | VectorXs |
![]() | |
typedef DataCollectorAbstractTpl< Scalar > | DataCollectorAbstract |
typedef MathBaseTpl< Scalar > | MathBase |
typedef MathBase::MatrixXs | MatrixXs |
typedef ResidualDataAbstractTpl< Scalar > | ResidualDataAbstract |
typedef StateAbstractTpl< Scalar > | StateAbstract |
typedef MathBase::VectorXs | VectorXs |
Public Member Functions | |
ResidualModelContactCoPPositionTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const CoPSupport &cref, const std::size_t nu) | |
Initialize the contact CoP residual model. More... | |
ResidualModelContactCoPPositionTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const CoPSupport &cref) | |
Initialize the contact CoP residual model. More... | |
virtual void | calc (const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) |
Compute the contact CoP residual. More... | |
virtual void | calcDiff (const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) |
Compute the Jacobians of the contact CoP residual. More... | |
virtual boost::shared_ptr< ResidualDataAbstract > | createData (DataCollectorAbstract *const data) |
Create the contact CoP residual data. More... | |
pinocchio::FrameIndex | get_id () const |
Return the reference frame id. | |
const CoPSupport & | get_reference () const |
Return the reference support region of CoP. | |
virtual void | print (std::ostream &os) const |
Print relevant information of the cop-position residual. More... | |
void | set_id (pinocchio::FrameIndex id) |
Modify the reference frame id. | |
void | set_reference (const CoPSupport &reference) |
Modify the reference support region of CoP. | |
![]() | |
ResidualModelAbstractTpl (boost::shared_ptr< StateAbstract > state, const std::size_t nr, const std::size_t nu, const bool q_dependent=true, const bool v_dependent=true, const bool u_dependent=true) | |
Initialize the residual model. More... | |
ResidualModelAbstractTpl (boost::shared_ptr< StateAbstract > state, const std::size_t nr, const bool q_dependent=true, const bool v_dependent=true, const bool u_dependent=true) | |
void | calc (const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
void | calcDiff (const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
std::size_t | get_nr () const |
Return the dimension of the residual vector. | |
std::size_t | get_nu () const |
Return the dimension of the control input. | |
bool | get_q_dependent () const |
Return true if the residual function depends on q. | |
const boost::shared_ptr< StateAbstract > & | get_state () const |
Return the state. | |
bool | get_u_dependent () const |
Return true if the residual function depends on u. | |
bool | get_v_dependent () const |
Return true if the residual function depends on v. | |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
![]() | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
Additional Inherited Members | |
![]() | |
std::size_t | nr_ |
Residual vector dimension. | |
std::size_t | nu_ |
Control dimension. | |
bool | q_dependent_ |
Label that indicates if the residual function depends on q. | |
boost::shared_ptr< StateAbstract > | state_ |
State description. | |
bool | u_dependent_ |
Label that indicates if the residual function depends on u. | |
VectorXs | unone_ |
No control vector. | |
bool | v_dependent_ |
Label that indicates if the residual function depends on v. | |
Center of pressure residual function.
It builds a residual function that bounds the center of pressure (CoP) for one contact surface to lie inside a certain geometric area defined around the reference contact frame. The residual residual vector is described as \(\mathbf{r}=\mathbf{A}\boldsymbol{\lambda}\geq\mathbf{0}\), where
\[ \mathbf{A}= \begin{bmatrix} 0 & 0 & X/2 & 0 & -1 & 0 \\ 0 & 0 & X/2 & 0 & 1 & 0 \\ 0 & 0 & Y/2 & 1 & 0 & 0 \\ 0 & 0 & Y/2 & -1 & 0 & 0 \end{bmatrix} \]
is the inequality matrix and \(\boldsymbol{\lambda}\) is the reference spatial contact force in the frame coordinate. The CoP lies inside the convex hull of the foot, see eq.(18-19) of https://hal.archives-ouvertes.fr/hal-02108449/document is we have:
\[ \begin{align}\begin{split}\tau^x &\leq Yf^z \\-\tau^x &\leq Yf^z \\\tau^y &\leq Yf^z \\-\tau^y &\leq Yf^z \end{split}\end{align} \]
with \(\boldsymbol{\lambda}= \begin{bmatrix}f^x & f^y & f^z & \tau^x & \tau^y & \tau^z \end{bmatrix}^T\).
The residual is computed, from the residual vector \(\mathbf{r}\), through an user defined activation model. Additionally, the contact frame id, the desired support region for the CoP and the inequality matrix are handled within CoPSupportTpl
. The force vector \(\boldsymbol{\lambda}\) and its derivatives are computed by DifferentialActionModelContactFwdDynamicsTpl
or ActionModelImpulseFwdDynamicTpl
. These values are stored in a shared data (i.e., DataCollectorContactTpl
or DataCollectorImpulseTpl
). Note that this residual function cannot be used with other action models.
ResidualModelAbstractTpl
, calc()
, calcDiff()
, createData()
, DifferentialActionModelContactFwdDynamicsTpl
, ActionModelImpulseFwdDynamicTpl
, DataCollectorForceTpl
ResidualModelContactCoPPositionTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const pinocchio::FrameIndex | id, | ||
const CoPSupport & | cref, | ||
const std::size_t | nu | ||
) |
Initialize the contact CoP residual model.
[in] | state | State of the multibody system |
[in] | id | Reference frame id |
[in] | cref | Reference support region of the CoP |
[in] | nu | Dimension of control vector |
ResidualModelContactCoPPositionTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const pinocchio::FrameIndex | id, | ||
const CoPSupport & | cref | ||
) |
Initialize the contact CoP residual model.
The default nu
value is obtained from StateAbstractTpl::get_nv()
.
[in] | state | State of the multibody system |
[in] | id | Reference frame id |
[in] | cref | Reference support region of the CoP |
|
virtual |
Compute the contact CoP residual.
The CoP residual is computed based on the \(\mathbf{A}\) matrix, the force vector is computed by DifferentialActionModelContactFwdDynamicsTpl
or ActionModelImpulseFwdDynamicTpl
which is stored in DataCollectorContactTpl
or DataCollectorImpulseTpl
.
[in] | data | Contact CoP data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
[in] | u | Control input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
Reimplemented from ResidualModelAbstractTpl< _Scalar >.
|
virtual |
Compute the Jacobians of the contact CoP residual.
The CoP residual is computed based on the \(\mathbf{A}\) matrix, the force vector is computed by DifferentialActionModelContactFwdDynamicsTpl
or ActionModelImpulseFwdDynamicTpl
which is stored in DataCollectorContactTpl
or DataCollectorImpulseTpl
.
[in] | data | Contact CoP data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
[in] | u | Control input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
Reimplemented from ResidualModelAbstractTpl< _Scalar >.
|
virtual |
Create the contact CoP residual data.
Each residual model has its own data that needs to be allocated. This function returns the allocated data for a predefined residual.
[in] | data | Shared data (it should be of type DataCollectorContactTpl ) |
Reimplemented from ResidualModelAbstractTpl< _Scalar >.
|
virtual |
Print relevant information of the cop-position residual.
[out] | os | Output stream object |
Reimplemented from ResidualModelAbstractTpl< _Scalar >.