|
| ResidualModelContactFrictionConeTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const FrictionCone &fref) |
| Initialize the contact friction cone residual model. More...
|
|
| ResidualModelContactFrictionConeTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const FrictionCone &fref, const std::size_t nu) |
| Initialize the contact friction cone residual model. More...
|
|
virtual void | calc (const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
| Compute the residual vector for nodes that depends only on the state. 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 friction cone residual. More...
|
|
virtual void | calcDiff (const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
| Compute the Jacobian of the residual functions with respect to the state only. 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 friction cone residual. More...
|
|
virtual boost::shared_ptr< ResidualDataAbstract > | createData (DataCollectorAbstract *const data) |
| Create the contact friction cone residual data.
|
|
pinocchio::FrameIndex | get_id () const |
| Return the reference frame id.
|
|
const FrictionCone & | get_reference () const |
| Return the reference contact friction cone.
|
|
virtual void | print (std::ostream &os) const |
| Print relevant information of the contact-friction-cone residual. More...
|
|
void | set_id (const pinocchio::FrameIndex id) |
| Modify the reference frame id.
|
|
void | set_reference (const FrictionCone &reference) |
| Modify the reference contact friction cone.
|
|
| 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) |
| Initialize the residual model. More...
|
|
| 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...
|
|
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.
|
|
template<typename _Scalar>
class crocoddyl::ResidualModelContactFrictionConeTpl< _Scalar >
Contact friction cone residual.
This residual function is defined as \(\mathbf{r}=\mathbf{A}\boldsymbol{\lambda}\), where \(\mathbf{A}\in~\mathbb{R}^{nr\times nc}\) describes the linearized friction cone, \(\boldsymbol{\lambda}\in~\mathbb{R}^{nc}\) is the spatial contact forces computed by DifferentialActionModelContactFwdDynamicsTpl
, and nr
, nc
are the number of cone facets and dimension of the contact, respectively.
Both residual and residual Jacobians are computed analytically, where th force vector \(\boldsymbol{\lambda}\) and its Jacobians \(\left(\frac{\partial\boldsymbol{\lambda}}{\partial\mathbf{x}}, \frac{\partial\boldsymbol{\lambda}}{\partial\mathbf{u}}\right)\) 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.
As described in ResidualModelAbstractTpl()
, the residual value and its derivatives are calculated by calc
and calcDiff
, respectively.
- See also
ResidualModelAbstractTpl
, calc()
, calcDiff()
, createData()
, DifferentialActionModelContactFwdDynamicsTpl
, ActionModelImpulseFwdDynamicTpl
, DataCollectorForceTpl
Definition at line 54 of file contact-friction-cone.hpp.