Contact wrench cone residual function. More...
#include <crocoddyl/multibody/residuals/contact-wrench-cone.hpp>
Public Types | |
typedef ResidualModelAbstractTpl< Scalar > | Base |
typedef ResidualDataContactWrenchConeTpl< Scalar > | Data |
typedef DataCollectorAbstractTpl< Scalar > | DataCollectorAbstract |
typedef MathBaseTpl< Scalar > | MathBase |
typedef MathBase::MatrixX6s | MatrixX6s |
typedef MathBase::MatrixXs | MatrixXs |
typedef ResidualDataAbstractTpl< Scalar > | ResidualDataAbstract |
typedef StateMultibodyTpl< Scalar > | StateMultibody |
typedef MathBase::VectorXs | VectorXs |
typedef WrenchConeTpl< Scalar > | WrenchCone |
Public Member Functions | |
ResidualModelContactWrenchConeTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const WrenchCone &fref) | |
Initialize the contact wrench cone residual model. More... | |
ResidualModelContactWrenchConeTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const WrenchCone &fref, const std::size_t nu) | |
Initialize the contact wrench 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 wrench 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 derivatives of the contact wrench cone residual. More... | |
virtual boost::shared_ptr< ResidualDataAbstract > | createData (DataCollectorAbstract *const data) |
Create the contact wrench cone residual data. More... | |
pinocchio::FrameIndex | get_id () const |
Return the reference frame id. | |
const WrenchCone & | get_reference () const |
Return the reference contact wrench cone. | |
virtual void | print (std::ostream &os) const |
Print relevant information of the contact-wrench-cone residual. More... | |
void | set_id (const pinocchio::FrameIndex id) |
Modify the reference frame id. | |
void | set_reference (const WrenchCone &reference) |
Modify the reference contact wrench cone. | |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
Protected Attributes | |
std::size_t | nu_ |
Control dimension. | |
boost::shared_ptr< StateAbstract > | state_ |
State description. | |
VectorXs | unone_ |
No control vector. | |
Contact wrench cone residual function.
This residual function is defined as \(\mathbf{r}=\mathbf{A}\boldsymbol{\lambda}\), where \(\mathbf{A}\) is the inequality matrix defined by the contact wrench cone, and \(\boldsymbol{\lambda}\) is the current spatial forces. The current spatial forces \(\boldsymbol{\lambda}\in\mathbb{R}^{nc}\) is computed by DifferentialActionModelContactFwdDynamicsTpl
or ActionModelImpulseFwdDynamicTpl
, with nc
as the dimension of the contact.
Both residual and residual Jacobians are computed analytically, where the 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.
ResidualModelAbstractTpl
, calc()
, calcDiff()
, createData()
, DifferentialActionModelContactFwdDynamicsTpl
, ActionModelImpulseFwdDynamicTpl
, DataCollectorForceTpl
ResidualModelContactWrenchConeTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const pinocchio::FrameIndex | id, | ||
const WrenchCone & | fref, | ||
const std::size_t | nu | ||
) |
Initialize the contact wrench cone residual model.
[in] | state | Multibody state |
[in] | id | Reference frame id |
[in] | fref | Reference contact wrench cone |
[in] | nu | Dimension of control vector |
ResidualModelContactWrenchConeTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const pinocchio::FrameIndex | id, | ||
const WrenchCone & | fref | ||
) |
Initialize the contact wrench cone residual model.
The default nu
is obtained from StateAbstractTpl::get_nv()
.
[in] | state | Multibody state |
[in] | id | Reference frame id |
[in] | fref | Reference contact wrench cone |
|
virtual |
Compute the contact wrench cone residual.
The CoP residual is computed based on the \(\mathbf{A}\) matrix, the force vector is computed by DifferentialActionModelContactFwdDynamicsTpl
or ActionModelImpulseFwdDynamicTpl
which is stored in DataCollectorContactTpl
or DataCollectorImpulseTpl
.
[in] | data | Contact force data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
[in] | u | Control input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
|
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}\) |
|
virtual |
Compute the derivatives of the contact wrench cone residual.
The CoP residual is computed based on the \(\mathbf{A}\) matrix, the force vector is computed by DifferentialActionModelContactFwdDynamicsTpl
or ActionModelImpulseFwdDynamicTpl
which is stored in DataCollectorContactTpl
or DataCollectorImpulseTpl
.
[in] | data | Contact force data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
[in] | u | Control input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
|
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}\) |
|
virtual |
Create the contact wrench cone residual data.
[in] | data | shared data (it should be of type DataCollectorContactTpl) |
|
virtual |
Print relevant information of the contact-wrench-cone residual.
[out] | os | Output stream object |