crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
ResidualModelFrameRotationTpl< _Scalar > Class Template Reference

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< ResidualDataAbstractcreateData (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< StateAbstractstate_
 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.
 

Detailed Description

template<typename _Scalar>
class crocoddyl::ResidualModelFrameRotationTpl< _Scalar >

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.

See also
ResidualModelAbstractTpl, calc(), calcDiff(), createData()

Definition at line 137 of file fwd.hpp.

Constructor & Destructor Documentation

◆ ResidualModelFrameRotationTpl() [1/2]

ResidualModelFrameRotationTpl ( boost::shared_ptr< StateMultibody state,
const pinocchio::FrameIndex  id,
const Matrix3s &  Rref,
const std::size_t  nu 
)

Initialize the frame rotation residual model.

Parameters
[in]stateState of the multibody system
[in]idReference frame id
[in]RrefReference frame rotation
[in]nuDimension of the control vector

◆ ResidualModelFrameRotationTpl() [2/2]

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().

Parameters
[in]stateState of the multibody system
[in]idReference frame id
[in]RrefReference frame rotation

Member Function Documentation

◆ calc()

virtual void calc ( const boost::shared_ptr< ResidualDataAbstract > &  data,
const Eigen::Ref< const VectorXs > &  x,
const Eigen::Ref< const VectorXs > &  u 
)
virtual

Compute the frame rotation residual.

Parameters
[in]dataFrame rotation residual data
[in]xState point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
[in]uControl input \(\mathbf{u}\in\mathbb{R}^{nu}\)

◆ calcDiff()

virtual void calcDiff ( const boost::shared_ptr< ResidualDataAbstract > &  data,
const Eigen::Ref< const VectorXs > &  x,
const Eigen::Ref< const VectorXs > &  u 
)
virtual

Compute the derivatives of the frame rotation residual.

Parameters
[in]dataFrame rotation residual data
[in]xState point \(\mathbf{x}\in\mathbb{R}^{ndx}\)
[in]uControl input \(\mathbf{u}\in\mathbb{R}^{nu}\)

◆ print()

virtual void print ( std::ostream &  os) const
virtual

Print relevant information of the frame-rotation residual.

Parameters
[out]osOutput stream object

The documentation for this class was generated from the following files: