Frame rotation residual. More...
#include <crocoddyl/multibody/residuals/frame-rotation.hpp>
Public Types | |
typedef ResidualModelAbstractTpl< Scalar > | Base |
typedef ResidualDataFrameRotationTpl< Scalar > | Data |
typedef DataCollectorAbstractTpl< Scalar > | DataCollectorAbstract |
typedef MathBaseTpl< Scalar > | MathBase |
typedef MathBase::Matrix3s | Matrix3s |
typedef ResidualDataAbstractTpl< Scalar > | ResidualDataAbstract |
typedef StateMultibodyTpl< Scalar > | StateMultibody |
typedef MathBase::VectorXs | VectorXs |
Public Member Functions | |
ResidualModelFrameRotationTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Matrix3s &Rref) | |
Initialize the frame rotation residual model. More... | |
ResidualModelFrameRotationTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Matrix3s &Rref, const std::size_t nu) | |
Initialize the frame rotation residual model. More... | |
virtual void | calc (const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) |
Compute the frame rotation residual. 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 frame rotation residual. More... | |
virtual boost::shared_ptr< ResidualDataAbstract > | createData (DataCollectorAbstract *const data) |
Create the frame rotation residual data. | |
pinocchio::FrameIndex | get_id () const |
Return the reference frame id. | |
const Matrix3s & | get_reference () const |
Return the reference frame rotation. | |
virtual void | print (std::ostream &os) const |
Print relevant information of the frame-rotation residual. More... | |
void | set_id (const pinocchio::FrameIndex id) |
Modify the reference frame id. | |
void | set_reference (const Matrix3s &reference) |
Modify the reference frame rotation. | |
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. | |
bool | u_dependent_ |
Label that indicates if the residual function depends on u. | |
VectorXs | unone_ |
No control vector. | |
bool | v_dependent_ |
Label that indicates if the residual function depends on v. | |
Frame rotation residual.
This residual function is defined as \(\mathbf{r}=\mathbf{R}\ominus\mathbf{R}^*\), where \(\mathbf{R},\mathbf{R}^*\in~\mathbb{SO(3)}\) are the current and reference frame rotations, respectively. Note that the dimension of the residual vector is 3. Furthermore, the Jacobians of the residual function are computed analytically.
As described in ResidualModelAbstractTpl()
, the residual value and its Jacobians are calculated by calc
and calcDiff
, respectively.
ResidualModelAbstractTpl
, calc()
, calcDiff()
, createData()
ResidualModelFrameRotationTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const pinocchio::FrameIndex | id, | ||
const Matrix3s & | Rref, | ||
const std::size_t | nu | ||
) |
Initialize the frame rotation residual model.
[in] | state | State of the multibody system |
[in] | id | Reference frame id |
[in] | Rref | Reference frame rotation |
[in] | nu | Dimension of the control vector |
ResidualModelFrameRotationTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const pinocchio::FrameIndex | id, | ||
const Matrix3s & | Rref | ||
) |
Initialize the frame rotation residual model.
The default nu
is equals to StateAbstractTpl::get_nv().
[in] | state | State of the multibody system |
[in] | id | Reference frame id |
[in] | Rref | Reference frame rotation |
|
virtual |
Compute the frame rotation residual.
[in] | data | Frame rotation residual data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
[in] | u | Control input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
|
virtual |
Compute the derivatives of the frame rotation residual.
[in] | data | Frame rotation residual data |
[in] | x | State point \(\mathbf{x}\in\mathbb{R}^{ndx}\) |
[in] | u | Control input \(\mathbf{u}\in\mathbb{R}^{nu}\) |
|
virtual |
Print relevant information of the frame-rotation residual.
[out] | os | Output stream object |