Define a center of pressure cost function. More...
#include <crocoddyl/multibody/costs/contact-cop-position.hpp>
Public Types | |
typedef ActivationBoundsTpl< Scalar > | ActivationBounds |
typedef ActivationModelAbstractTpl< Scalar > | ActivationModelAbstract |
typedef ActivationModelQuadraticBarrierTpl< Scalar > | ActivationModelQuadraticBarrier |
typedef CostModelAbstractTpl< Scalar > | Base |
typedef CostDataAbstractTpl< Scalar > | CostDataAbstract |
typedef CostDataContactCoPPositionTpl< Scalar > | Data |
typedef DataCollectorAbstractTpl< Scalar > | DataCollectorAbstract |
typedef FrameCoPSupportTpl< Scalar > | FrameCoPSupport |
typedef MathBaseTpl< Scalar > | MathBase |
typedef Eigen::Matrix< Scalar, 4, 6 > | Matrix46 |
typedef MathBase::MatrixX3s | MatrixX3s |
typedef MathBase::MatrixXs | MatrixXs |
typedef StateMultibodyTpl< Scalar > | StateMultibody |
typedef MathBase::Vector2s | Vector2s |
typedef MathBase::Vector3s | Vector3s |
typedef MathBase::VectorXs | VectorXs |
![]() | |
typedef ActivationModelAbstractTpl< Scalar > | ActivationModelAbstract |
typedef ActivationModelQuadTpl< Scalar > | ActivationModelQuad |
typedef CostDataAbstractTpl< Scalar > | CostDataAbstract |
typedef DataCollectorAbstractTpl< Scalar > | DataCollectorAbstract |
typedef MathBaseTpl< Scalar > | MathBase |
typedef MathBase::MatrixXs | MatrixXs |
typedef StateMultibodyTpl< Scalar > | StateMultibody |
typedef MathBase::VectorXs | VectorXs |
Public Member Functions | |
CostModelContactCoPPositionTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameCoPSupport &cop_support, const std::size_t &nu) | |
Initialize the cop cost model. More... | |
CostModelContactCoPPositionTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameCoPSupport &cop_support) | |
Initialize the cop cost model. More... | |
CostModelContactCoPPositionTpl (boost::shared_ptr< StateMultibody > state, const FrameCoPSupport &cop_support, const std::size_t &nu) | |
Initialize the cop cost model. More... | |
CostModelContactCoPPositionTpl (boost::shared_ptr< StateMultibody > state, const FrameCoPSupport &cop_support) | |
Initialize the cop cost model. More... | |
virtual void | calc (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) |
Compute the cop cost. More... | |
virtual void | calcDiff (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) |
Compute the derivatives of the cop cost. More... | |
virtual boost::shared_ptr< CostDataAbstract > | createData (DataCollectorAbstract *const data) |
Create the cop cost data. More... | |
![]() | |
CostModelAbstractTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const std::size_t &nu) | |
CostModelAbstractTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation) | |
CostModelAbstractTpl (boost::shared_ptr< StateMultibody > state, const std::size_t &nr, const std::size_t &nu) | |
CostModelAbstractTpl (boost::shared_ptr< StateMultibody > state, const std::size_t &nr) | |
virtual void | calc (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0 |
void | calc (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
virtual void | calcDiff (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0 |
void | calcDiff (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
const boost::shared_ptr< ActivationModelAbstract > & | get_activation () const |
const std::size_t & | get_nu () const |
template<class ReferenceType > | |
ReferenceType | get_reference () const |
const boost::shared_ptr< StateMultibody > & | get_state () const |
template<class ReferenceType > | |
void | set_reference (ReferenceType ref) |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
![]() | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
Protected Member Functions | |
virtual void | get_referenceImpl (const std::type_info &ti, void *pv) const |
Modify the cop. | |
virtual void | set_referenceImpl (const std::type_info &ti, const void *pv) |
Return the cop. | |
Additional Inherited Members | |
![]() | |
boost::shared_ptr< ActivationModelAbstract > | activation_ |
std::size_t | nu_ |
boost::shared_ptr< StateMultibody > | state_ |
VectorXs | unone_ |
Define a center of pressure cost function.
It builds a cost 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 cost residual vector is described as {r} = {A} {f} {0}, where {A}= {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 {bmatrix} is the inequality matrix and {f} is the reference spatial contact force in the frame coordinate. The constraints for the cop to lie inside the convex hull of the foot, see eq.(18-19) of https://hal.archives-ouvertes.fr/hal-02108449/document can be written as: {align}{split}^x & Yf^z \-^x & Yf^z \^y & Yf^z \-^y & Yf^z {split}{align}$` The cost is computed, from the residual vector {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 FrameCoPSupportTpl. The force vector {f} and its derivatives are computed by DifferentialActionModelContactFwdDynamicsTpl. These values are stored in a shared data (i.e. DataCollectorContactTpl). Note that this cost function cannot be used with other action models.
Definition at line 46 of file contact-cop-position.hpp.
CostModelContactCoPPositionTpl | ( | boost::shared_ptr< StateMultibody > | state, |
boost::shared_ptr< ActivationModelAbstract > | activation, | ||
const FrameCoPSupport & | cop_support, | ||
const std::size_t & | nu | ||
) |
Initialize the cop cost model.
[in] | state | Multibody state |
[in] | activation | Activation model |
[in] | cop_support | ID of contact frame and support region of the cop |
[in] | nu | Dimension of control vector |
CostModelContactCoPPositionTpl | ( | boost::shared_ptr< StateMultibody > | state, |
boost::shared_ptr< ActivationModelAbstract > | activation, | ||
const FrameCoPSupport & | cop_support | ||
) |
Initialize the cop cost model.
For this case the default nu is equal to state->get_nv()
.
[in] | state | Multibody state |
[in] | activation | Activation model |
[in] | cop_support | ID of contact frame and support region of the cop |
CostModelContactCoPPositionTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const FrameCoPSupport & | cop_support, | ||
const std::size_t & | nu | ||
) |
Initialize the cop cost model.
For this case the default activation model is quadratic barrier, i.e. ActivationModelQuadraticBarrierTpl(ActivationBounds(0, inf))
.
[in] | state | Multibody state |
[in] | cop_support | ID of contact frame and support region of the cop |
[in] | nu | Dimension of control vector |
CostModelContactCoPPositionTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const FrameCoPSupport & | cop_support | ||
) |
Initialize the cop cost model.
For this case the default activation model is quadratic barrier, i.e. ActivationModelQuadraticBarrierTpl(ActivationBounds(0, inf))
and is equal to `state->get_nv().
[in] | state | Multibody state |
[in] | cop_support | ID of contact frame and support region of the cop |
|
virtual |
Compute the cop cost.
The cop residual is computed based on the A matrix, the force vector is computed by DifferentialActionModelContactFwdDynamicsTpl and the results are stored in DataCollectorContactTpl.
[in] | data | cop data |
[in] | x | State vector \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
[in] | u | Control input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
|
virtual |
Compute the derivatives of the cop cost.
The cop derivatives are based on the force derivatives computed by DifferentialActionModelContactFwdDynamicsTpl and stored in DataCollectorContactTpl.
[in] | data | cop data |
[in] | x | State vector \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
[in] | u | Control input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
|
virtual |
Create the cop cost data.
Each cost model has its own data that needs to be allocated. This function returns the allocated data for a predefined cost.
[in] | data | shared data (it should be of type DataCollectorContactTpl) |
Reimplemented from CostModelAbstractTpl< _Scalar >.