Contact friction cone residual. More...
#include <crocoddyl/multibody/residuals/contact-friction-cone.hpp>
Public Types | |
typedef ResidualModelAbstractTpl< Scalar > | Base |
typedef ResidualDataContactFrictionConeTpl< Scalar > | Data |
typedef DataCollectorAbstractTpl< Scalar > | DataCollectorAbstract |
typedef FrictionConeTpl< Scalar > | FrictionCone |
typedef MathBaseTpl< Scalar > | MathBase |
typedef MathBase::MatrixX3s | MatrixX3s |
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 | |
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. More... | |
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) | |
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... | |
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 residual vector. 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 Jacobian of the residual vector. More... | |
virtual boost::shared_ptr< ResidualDataAbstract > | createData (DataCollectorAbstract *const data) |
Create the residual data. 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. | |
virtual void | print (std::ostream &os) const |
Print relevant information of the residual model. More... | |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
![]() | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
Protected Attributes | |
std::size_t | nu_ |
Control dimension. More... | |
boost::shared_ptr< StateAbstract > | state_ |
State description. More... | |
VectorXs | unone_ |
No control vector. More... | |
![]() | |
std::size_t | nr_ |
Residual vector dimension. More... | |
std::size_t | nu_ |
Control dimension. More... | |
bool | q_dependent_ |
Label that indicates if the residual function depends on q. More... | |
boost::shared_ptr< StateAbstract > | state_ |
State description. More... | |
bool | u_dependent_ |
Label that indicates if the residual function depends on u. More... | |
VectorXs | unone_ |
No control vector. More... | |
bool | v_dependent_ |
Label that indicates if the residual function depends on v. More... | |
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.
ResidualModelAbstractTpl
, calc()
, calcDiff()
, createData()
, DifferentialActionModelContactFwdDynamicsTpl
, ActionModelImpulseFwdDynamicTpl
, DataCollectorForceTpl
Definition at line 54 of file contact-friction-cone.hpp.
typedef MathBaseTpl<Scalar> MathBase |
Definition at line 59 of file contact-friction-cone.hpp.
typedef ResidualModelAbstractTpl<Scalar> Base |
Definition at line 60 of file contact-friction-cone.hpp.
typedef ResidualDataContactFrictionConeTpl<Scalar> Data |
Definition at line 61 of file contact-friction-cone.hpp.
typedef StateMultibodyTpl<Scalar> StateMultibody |
Definition at line 62 of file contact-friction-cone.hpp.
typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract |
Definition at line 63 of file contact-friction-cone.hpp.
typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract |
Definition at line 64 of file contact-friction-cone.hpp.
typedef FrictionConeTpl<Scalar> FrictionCone |
Definition at line 65 of file contact-friction-cone.hpp.
typedef MathBase::VectorXs VectorXs |
Definition at line 66 of file contact-friction-cone.hpp.
typedef MathBase::MatrixXs MatrixXs |
Definition at line 67 of file contact-friction-cone.hpp.
typedef MathBase::MatrixX3s MatrixX3s |
Definition at line 68 of file contact-friction-cone.hpp.
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.
[in] | state | State of the multibody system |
[in] | id | Reference frame id |
[in] | fref | Reference friction cone |
[in] | nu | Dimension of the control vector |
ResidualModelContactFrictionConeTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const pinocchio::FrameIndex | id, | ||
const FrictionCone & | fref | ||
) |
Initialize the contact friction cone 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] | fref | Reference friction cone |
|
virtual |
Compute the contact friction cone residual.
[in] | data | Contact friction cone residual 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 residual vector for nodes that depends only on the state.
It updates the residual vector based on the state only (i.e., it ignores the contact forces). This function is used in the terminal nodes of an optimal control problem.
[in] | data | Residual data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
Reimplemented from ResidualModelAbstractTpl< _Scalar >.
|
virtual |
Compute the Jacobians of the contact friction cone residual.
[in] | data | Contact friction cone residual 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 Jacobian of the residual functions with respect to the state only.
It updates the Jacobian of the residual function based on the state only (i.e., it ignores the contact forces). This function is used in the terminal nodes of an optimal control problem.
[in] | data | Residual data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
Reimplemented from ResidualModelAbstractTpl< _Scalar >.
|
virtual |
Create the contact friction cone residual data.
Reimplemented from ResidualModelAbstractTpl< _Scalar >.
|
virtual |
Print relevant information of the contact-friction-cone residual.
[out] | os | Output stream object |
Reimplemented from ResidualModelAbstractTpl< _Scalar >.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar |
Definition at line 58 of file contact-friction-cone.hpp.
|
protected |
Control dimension.
Definition at line 181 of file residual-base.hpp.
|
protected |
State description.
Definition at line 179 of file residual-base.hpp.
|
protected |
No control vector.
Definition at line 182 of file residual-base.hpp.