Template Class ResidualModelContactFrictionConeTpl
Defined in File contact-friction-cone.hpp
Inheritance Relationships
Base Type
public crocoddyl::ResidualModelAbstractTpl< _Scalar >(Template Class ResidualModelAbstractTpl)
Class Documentation
-
template<typename _Scalar>
class ResidualModelContactFrictionConeTpl : public crocoddyl::ResidualModelAbstractTpl<_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, andnr,ncare 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
DifferentialActionModelContactFwdDynamicsTplorActionModelImpulseFwdDynamicTpl. These values are stored in a shared data (i.e.DataCollectorContactTplorDataCollectorImpulseTpl). Note that this residual function cannot be used with other action models.As described in
ResidualModelAbstractTpl(), the residual value and its derivatives are calculated bycalcandcalcDiff, respectively.See also
ResidualModelAbstractTpl,calc(),calcDiff(),createData(),DifferentialActionModelContactFwdDynamicsTpl,ActionModelImpulseFwdDynamicTpl,DataCollectorForceTplPublic Types
-
typedef MathBaseTpl<Scalar> MathBase
-
typedef ResidualModelAbstractTpl<Scalar> Base
-
typedef ResidualDataContactFrictionConeTpl<Scalar> Data
-
typedef StateMultibodyTpl<Scalar> StateMultibody
-
typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract
-
typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract
-
typedef FrictionConeTpl<Scalar> FrictionCone
Public Functions
Initialize the contact friction cone residual model.
Note that for the inverse-dynamic cases, the control vector contains the generalized accelerations, torques, and all the contact forces.
- Parameters:
state – [in] State of the multibody system
id – [in] Reference frame id
fref – [in] Reference friction cone
nu – [in] Dimension of the control vector
fwddyn – [in] Indicates that we have a forward dynamics problem (true) or inverse dynamics (false)
Initialize the contact friction cone residual model.
The default
nuvalue is obtained fromStateAbstractTpl::get_nv(). Note that this constructor can be used for forward-dynamics cases only.- Parameters:
state – [in] State of the multibody system
id – [in] Reference frame id
fref – [in] Reference friction cone
-
virtual ~ResidualModelContactFrictionConeTpl() = default
Compute the contact friction cone residual.
- Parameters:
data – [in] Contact friction cone residual data
x – [in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
u – [in] Control input \(\mathbf{u}\in\mathbb{R}^{nu}\)
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.
- Parameters:
data – [in] Residual data
x – [in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
Compute the Jacobians of the contact friction cone residual.
- Parameters:
data – [in] Contact friction cone residual data
x – [in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
u – [in] Control input \(\mathbf{u}\in\mathbb{R}^{nu}\)
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.
- Parameters:
data – [in] Residual data
x – [in] State point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
-
virtual std::shared_ptr<ResidualDataAbstract> createData(DataCollectorAbstract *const data) override
Create the contact friction cone residual data.
Update the Jacobians of the contact friction cone residual.
- Parameters:
data – [in] Contact friction cone residual data
-
template<typename NewScalar>
ResidualModelContactFrictionConeTpl<NewScalar> cast() const Cast the contact-friction-cone residual model to a different scalar type.
It is useful for operations requiring different precision or scalar types.
- Template Parameters:
NewScalar – The new scalar type to cast to.
- Returns:
ResidualModelContactFrictionConeTpl<NewScalar> A residual model with the new scalar type.
-
bool is_fwddyn() const
Indicates if we are using the forward-dynamics (true) or inverse-dynamics (false)
-
pinocchio::FrameIndex get_id() const
Return the reference frame id.
-
const FrictionCone &get_reference() const
Return the reference contact friction cone.
- DEPRECATED ("Do not use set_id, instead create a new model", void set_id(const pinocchio::FrameIndex id);) void set_reference(const FrictionCone &reference)
Modify the reference frame id.
Modify the reference contact friction cone
-
virtual void print(std::ostream &os) const override
Print relevant information of the contact-friction-cone residual.
- Parameters:
os – [out] Output stream object
Public Members
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Protected Attributes
-
std::size_t nu_
Control dimension.
-
std::shared_ptr<StateAbstract> state_
State description.
-
typedef MathBaseTpl<Scalar> MathBase