Define a contact force residual function. More...
#include <crocoddyl/multibody/residuals/contact-force.hpp>
Public Types | |
typedef ResidualModelAbstractTpl< Scalar > | Base |
typedef ResidualDataContactForceTpl< Scalar > | Data |
typedef DataCollectorAbstractTpl< Scalar > | DataCollectorAbstract |
typedef pinocchio::ForceTpl< Scalar > | Force |
typedef MathBaseTpl< Scalar > | MathBase |
typedef MathBase::MatrixXs | MatrixXs |
typedef ResidualDataAbstractTpl< Scalar > | ResidualDataAbstract |
typedef StateMultibodyTpl< Scalar > | StateMultibody |
typedef MathBase::VectorXs | VectorXs |
Public Member Functions | |
ResidualModelContactForceTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Force &fref, const std::size_t nc) | |
Initialize the contact force residual model. More... | |
ResidualModelContactForceTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Force &fref, const std::size_t nc, const std::size_t nu) | |
Initialize the contact force 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 force 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 force residual. More... | |
virtual boost::shared_ptr< ResidualDataAbstract > | createData (DataCollectorAbstract *const data) |
Create the contact force residual data. More... | |
pinocchio::FrameIndex | get_id () const |
Return the reference frame id. | |
const Force & | get_reference () const |
Return the reference spatial contact force in the contact coordinates. | |
virtual void | print (std::ostream &os) const |
Print relevant information of the contact-force residual. More... | |
void | set_id (const pinocchio::FrameIndex id) |
Modify the reference frame id. | |
void | set_reference (const Force &reference) |
Modify the reference spatial contact force in the contact coordinates. | |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
Protected Attributes | |
std::size_t | nr_ |
Residual vector dimension. | |
std::size_t | nu_ |
Control dimension. | |
boost::shared_ptr< StateAbstract > | state_ |
State description. | |
VectorXs | unone_ |
No control vector. | |
Define a contact force residual function.
This residual function is defined as \(\mathbf{r}=\boldsymbol{\lambda}-\boldsymbol{\lambda}^*\), where \(\boldsymbol{\lambda}, \boldsymbol{\lambda}^*\) are the current and reference spatial forces, respectively. 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
, DataCollectorContactTpl
, DataCollectorImpulseTpl
ResidualModelContactForceTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const pinocchio::FrameIndex | id, | ||
const Force & | fref, | ||
const std::size_t | nc, | ||
const std::size_t | nu | ||
) |
Initialize the contact force residual model.
[in] | state | Multibody state |
[in] | id | Reference frame id |
[in] | fref | Reference spatial contact force in the contact coordinates |
[in] | nc | Dimension of the contact force (nc <= 6) |
[in] | nu | Dimension of control vector |
ResidualModelContactForceTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const pinocchio::FrameIndex | id, | ||
const Force & | fref, | ||
const std::size_t | nc | ||
) |
Initialize the contact force residual model.
The default nu
is obtained from StateAbstractTpl::get_nv()
.
[in] | state | Multibody state |
[in] | id | Reference frame id |
[in] | fref | Reference spatial contact force in the contact coordinates |
[in] | nc | Dimension of the contact force (nc <= 6) |
|
virtual |
Compute the contact force 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 force 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 force residual data.
[in] | data | shared data (it should be of type DataCollectorContactTpl) |
|
virtual |
Print relevant information of the contact-force residual.
[out] | os | Output stream object |