|
| ResidualModelFrameVelocityTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Motion &velocity, const pinocchio::ReferenceFrame type) |
| Initialize the frame velocity residual model. More...
|
|
| ResidualModelFrameVelocityTpl (boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Motion &velocity, const pinocchio::ReferenceFrame type, const std::size_t nu) |
| Initialize the frame velocity 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 velocity residual vector. More...
|
|
virtual void | calcDiff (const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) |
| Compute the Jacobians of the frame velocity residual. More...
|
|
virtual boost::shared_ptr< ResidualDataAbstract > | createData (DataCollectorAbstract *const data) |
| Create the frame velocity residual data.
|
|
pinocchio::FrameIndex | get_id () const |
| Modify the reference frame id.
|
|
const Motion & | get_reference () const |
| Modify the reference velocity.
|
|
pinocchio::ReferenceFrame | get_type () const |
| Modify the reference type of velocity.
|
|
virtual void | print (std::ostream &os) const |
| Print relevant information of the frame-velocity residual. More...
|
|
void | set_id (const pinocchio::FrameIndex id) |
| Return reference frame id.
|
|
void | set_reference (const Motion &velocity) |
| Return reference velocity.
|
|
void | set_type (const pinocchio::ReferenceFrame type) |
| Return reference type of velocity.
|
|
template<typename _Scalar>
class crocoddyl::ResidualModelFrameVelocityTpl< _Scalar >
Frame velocity residual.
This residual function defines a tracking of frame velocity as \(\mathbf{r}=\mathbf{v}-\mathbf{v}^*\), where \(\mathbf{v},\mathbf{v}^*\in~T_{\mathbf{p}}~\mathbb{SE(3)}\) are the current and reference frame velocities, respectively. Note that the tangent vector is described by the frame placement \(\mathbf{p}\), and the dimension of the residual vector is 6. Furthermore, the Jacobians of the residual function are computed analytically.
As described in ResidualModelAbstractTpl
, the residual vector and its Jacobians are calculated by calc
and calcDiff
, respectively.
- See also
ResidualModelAbstractTpl
, calc()
, calcDiff()
, createData()
Definition at line 147 of file fwd.hpp.