Define a contact force cost function. More...
#include <crocoddyl/multibody/costs/contact-force.hpp>
Public Types | |
typedef ActivationModelAbstractTpl< Scalar > | ActivationModelAbstract |
typedef ActivationModelQuadTpl< Scalar > | ActivationModelQuad |
typedef CostModelAbstractTpl< Scalar > | Base |
typedef CostDataAbstractTpl< Scalar > | CostDataAbstract |
typedef CostDataContactForceTpl< Scalar > | Data |
typedef DataCollectorAbstractTpl< Scalar > | DataCollectorAbstract |
typedef FrameForceTpl< Scalar > | FrameForce |
typedef MathBaseTpl< Scalar > | MathBase |
typedef MathBase::MatrixXs | MatrixXs |
typedef StateMultibodyTpl< Scalar > | StateMultibody |
typedef MathBase::Vector6s | Vector6s |
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 | |
CostModelContactForceTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameForce &fref, const std::size_t &nu) | |
Initialize the contact force cost model. More... | |
CostModelContactForceTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameForce &fref) | |
Initialize the contact force cost model. More... | |
CostModelContactForceTpl (boost::shared_ptr< StateMultibody > state, const FrameForce &fref, const std::size_t &nr, const std::size_t &nu) | |
Initialize the contact force cost model. More... | |
CostModelContactForceTpl (boost::shared_ptr< StateMultibody > state, const FrameForce &fref, const std::size_t &nr) | |
Initialize the contact force cost model. More... | |
CostModelContactForceTpl (boost::shared_ptr< StateMultibody > state, const FrameForce &fref) | |
Initialize the contact force 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 contact force 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 contact force cost. More... | |
virtual boost::shared_ptr< CostDataAbstract > | createData (DataCollectorAbstract *const data) |
Create the contact force cost data. More... | |
DEPRECATED ("Use set_reference<FrameForceTpl<Scalar> >()", void set_fref(const FrameForce &fref)) | |
DEPRECATED ("Use get_reference<FrameForceTpl<Scalar> >()", const FrameForce &get_fref() const) | |
![]() | |
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 reference spatial contact force in the contact coordinates \({}^o\underline{\boldsymbol{\lambda}}_c^{reference}\). | |
virtual void | set_referenceImpl (const std::type_info &ti, const void *pv) |
Return the reference spatial contact force in the contact coordinates \({}^o\underline{\boldsymbol{\lambda}}_c^{reference}\). | |
Protected Attributes | |
FrameForce | fref_ |
![]() | |
boost::shared_ptr< ActivationModelAbstract > | activation_ |
std::size_t | nu_ |
boost::shared_ptr< StateMultibody > | state_ |
VectorXs | unone_ |
Define a contact force cost function.
It builds a cost function that tracks a desired spatial force in the contact coordinates \({}^o\underline{\boldsymbol{\lambda}}_c\in\mathbb{R}^{nc}\), i.e. the cost residual vector is described as:
\begin{equation*} \mathbf{r} = {}^o\underline{\boldsymbol{\lambda}}_c - {}^o\underline{\boldsymbol{\lambda}}_c^{reference},\end{equation*}
where \({}^o\underline{\boldsymbol{\lambda}}_c^{reference}\) is the reference spatial contact force in the frame coordinate \(c\), and \(nc\) defines the dimension of constrained space \((nc < 6)\). The cost is computed, from the residual vector \(\mathbf{r}\in\mathbb{R}^{nc}\), through an user defined activation model. Additionally, the reference force vector is defined using FrameForceTpl even for cases where \(nc < 6\).
The force vector \({}^o\underline{\boldsymbol{\lambda}}_c\) and its derivatives \(\left(\frac{\partial{}^o\underline{\boldsymbol{\lambda}}_c}{\partial\mathbf{x}}, \frac{\partial{}^o\underline{\boldsymbol{\lambda}}_c}{\partial\mathbf{u}}\right)\) 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 45 of file contact-force.hpp.
CostModelContactForceTpl | ( | boost::shared_ptr< StateMultibody > | state, |
boost::shared_ptr< ActivationModelAbstract > | activation, | ||
const FrameForce & | fref, | ||
const std::size_t & | nu | ||
) |
Initialize the contact force cost model.
Note that the nr
, defined in the activation model, has to be lower / equals than 6.
[in] | state | Multibody state |
[in] | activation | Activation model |
[in] | fref | Reference spatial contact force in the contact coordinates \({}^o\underline{\boldsymbol{\lambda}}_c^{reference}\) |
[in] | nu | Dimension of control vector |
CostModelContactForceTpl | ( | boost::shared_ptr< StateMultibody > | state, |
boost::shared_ptr< ActivationModelAbstract > | activation, | ||
const FrameForce & | fref | ||
) |
Initialize the contact force cost model.
For this case the default nu is equals to state->get_nv()
. Note that the nr
, defined in the activation model, has to be lower / equals than 6.
[in] | state | Multibody state |
[in] | activation | Activation model |
[in] | fref | Reference spatial contact force in the contact coordinates \({}^o\underline{\boldsymbol{\lambda}}_c^{reference}\) |
CostModelContactForceTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const FrameForce & | fref, | ||
const std::size_t & | nr, | ||
const std::size_t & | nu | ||
) |
Initialize the contact force cost model.
For this case the default activation model is quadratic, i.e. ActivationModelQuadTpl(nr)
. Note that the nr
, defined in the activation model, has to be lower / equals than 6.
[in] | state | Multibody state |
[in] | fref | Reference spatial contact force in the contact coordinates \({}^o\underline{\boldsymbol{\lambda}}_c^{reference}\) |
[in] | nr | Dimension of residual vector |
[in] | nu | Dimension of control vector |
CostModelContactForceTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const FrameForce & | fref, | ||
const std::size_t & | nr | ||
) |
Initialize the contact force cost model.
For this case the default activation model is quadratic, i.e. ActivationModelQuadTpl(nr)
, and nu
is equals to state->get_nv()
. Note that the nr
, defined in the activation model, has to be lower / equals than 6.
[in] | state | Multibody state |
[in] | fref | Reference spatial contact force in the contact coordinates \({}^o\underline{\boldsymbol{\lambda}}_c^{reference}\) |
[in] | nr | Dimension of residual vector |
CostModelContactForceTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const FrameForce & | fref | ||
) |
Initialize the contact force cost model.
For this case the default activation model is quadratic, i.e. ActivationModelQuadTpl(nr)
, and nr
and nu
is equals to 6 and state->get_nv()
, respectively.
[in] | state | Multibody state |
[in] | fref | Reference spatial contact force in the contact coordinates \({}^o\underline{\boldsymbol{\lambda}}_c^{reference}\) |
|
virtual |
Compute the contact force cost.
The force vector is computed by DifferentialActionModelContactFwdDynamicsTpl and stored in DataCollectorContactTpl.
[in] | data | Contact force 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 contact force cost.
The force derivatives are computed by DifferentialActionModelContactFwdDynamicsTpl and stored in DataCollectorContactTpl.
[in] | data | Contact force data |
[in] | x | State vector \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
[in] | u | Control input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
|
virtual |
Create the contact force 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 >.
|
protected |
Reference spatial contact force in the contact coordinates \({}^o\underline{\boldsymbol{\lambda}}_c^{reference}\)
Definition at line 192 of file contact-force.hpp.