Frame velocity cost. More...
#include <crocoddyl/multibody/costs/frame-velocity.hpp>
Public Member Functions | |
CostModelFrameVelocityTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameMotion &Fref) | |
Initialize the frame velocity cost model. More... | |
CostModelFrameVelocityTpl (boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActivationModelAbstract > activation, const FrameMotion &Fref, const std::size_t nu) | |
Initialize the frame velocity cost model. More... | |
CostModelFrameVelocityTpl (boost::shared_ptr< StateMultibody > state, const FrameMotion &Fref) | |
Initialize the frame velocity cost model. More... | |
CostModelFrameVelocityTpl (boost::shared_ptr< StateMultibody > state, const FrameMotion &Fref, const std::size_t nu) | |
Initialize the frame velocity cost model. More... | |
![]() | |
CostModelResidualTpl (boost::shared_ptr< typename Base::StateAbstract > state, boost::shared_ptr< ActivationModelAbstract > activation, boost::shared_ptr< ResidualModelAbstract > residual) | |
Initialize the residual cost model. More... | |
CostModelResidualTpl (boost::shared_ptr< typename Base::StateAbstract > state, boost::shared_ptr< ResidualModelAbstract > residual) | |
Initialize the residual cost model. More... | |
virtual void | calc (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
Compute the residual cost based on state only. More... | |
virtual void | calc (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) |
Compute the residual cost. More... | |
virtual void | calcDiff (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
Compute the derivatives of the residual cost with respect to the state only. More... | |
virtual void | calcDiff (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) |
Compute the derivatives of the residual cost. More... | |
virtual boost::shared_ptr< CostDataAbstract > | createData (DataCollectorAbstract *const data) |
Create the residual cost data. More... | |
virtual void | print (std::ostream &os) const |
Print relevant information of the cost-residual model. More... | |
![]() | |
CostModelAbstractTpl (boost::shared_ptr< StateAbstract > state, boost::shared_ptr< ActivationModelAbstract > activation) | |
CostModelAbstractTpl (boost::shared_ptr< StateAbstract > state, boost::shared_ptr< ActivationModelAbstract > activation, boost::shared_ptr< ResidualModelAbstract > residual) | |
Initialize the cost model. More... | |
CostModelAbstractTpl (boost::shared_ptr< StateAbstract > state, boost::shared_ptr< ActivationModelAbstract > activation, const std::size_t nu) | |
Initialize the cost model. More... | |
CostModelAbstractTpl (boost::shared_ptr< StateAbstract > state, boost::shared_ptr< ResidualModelAbstract > residual) | |
CostModelAbstractTpl (boost::shared_ptr< StateAbstract > state, const std::size_t nr) | |
CostModelAbstractTpl (boost::shared_ptr< StateAbstract > state, const std::size_t nr, const std::size_t nu) | |
virtual void | calc (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
Compute the total cost value for nodes that depends only on the state. More... | |
virtual void | calc (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0 |
Compute the cost value and its residual vector. More... | |
virtual void | calcDiff (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) |
Compute the Jacobian and Hessian of the cost functions with respect to the state only. More... | |
virtual void | calcDiff (const boost::shared_ptr< CostDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0 |
Compute the Jacobian and Hessian of cost and its residual vector. More... | |
virtual boost::shared_ptr< CostDataAbstract > | createData (DataCollectorAbstract *const data) |
Create the cost data. More... | |
const boost::shared_ptr< ActivationModelAbstract > & | get_activation () const |
Return the activation model. | |
std::size_t | get_nu () const |
Return the dimension of the control input. | |
template<class ReferenceType > | |
ReferenceType | get_reference () |
Return the cost reference. | |
const boost::shared_ptr< ResidualModelAbstract > & | get_residual () const |
Return the residual model. | |
const boost::shared_ptr< StateAbstract > & | get_state () const |
Return the state. | |
virtual void | print (std::ostream &os) const |
Print relevant information of the cost model. More... | |
template<class ReferenceType > | |
void | set_reference (ReferenceType ref) |
Modify the cost reference. | |
Public Attributes | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
![]() | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
![]() | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar | Scalar |
Protected Member Functions | |
virtual void | get_referenceImpl (const std::type_info &ti, void *pv) |
Return the frame velocity reference. More... | |
virtual void | set_referenceImpl (const std::type_info &ti, const void *pv) |
Modify the frame velocity reference. More... | |
virtual void | get_referenceImpl (const std::type_info &, void *) |
Return the cost reference. More... | |
virtual void | set_referenceImpl (const std::type_info &, const void *) |
Modify the cost reference. More... | |
Protected Attributes | |
boost::shared_ptr< ActivationModelAbstract > | activation_ |
Activation model. More... | |
std::size_t | nu_ |
Control dimension. More... | |
boost::shared_ptr< ResidualModelAbstract > | residual_ |
Residual model. More... | |
boost::shared_ptr< StateAbstract > | state_ |
State description. More... | |
VectorXs | unone_ |
No control vector. More... | |
![]() | |
boost::shared_ptr< ActivationModelAbstract > | activation_ |
Activation model. More... | |
std::size_t | nu_ |
Control dimension. More... | |
boost::shared_ptr< ResidualModelAbstract > | residual_ |
Residual model. More... | |
boost::shared_ptr< StateAbstract > | state_ |
State description. More... | |
VectorXs | unone_ |
No control vector. More... | |
![]() | |
boost::shared_ptr< ActivationModelAbstract > | activation_ |
Activation model. More... | |
std::size_t | nu_ |
Control dimension. More... | |
boost::shared_ptr< ResidualModelAbstract > | residual_ |
Residual model. More... | |
boost::shared_ptr< StateAbstract > | state_ |
State description. More... | |
VectorXs | unone_ |
No control vector. More... | |
Frame velocity cost.
This cost function defines a residual vector 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.
Both cost and residual derivatives are computed analytically. For the computation of the cost Hessian, we use the Gauss-Newton approximation, e.g. \(\mathbf{l_{xu}} = \mathbf{l_{x}}^T \mathbf{l_{u}} \).
As described in CostModelResidualTpl
, the cost value and its derivatives are calculated by calc
and calcDiff
, respectively.
CostModelResidualTpl
, calc()
, calcDiff()
, createData()
Definition at line 43 of file frame-velocity.hpp.
typedef MathBaseTpl<Scalar> MathBase |
Definition at line 48 of file frame-velocity.hpp.
typedef CostModelResidualTpl<Scalar> Base |
Definition at line 49 of file frame-velocity.hpp.
typedef CostDataFrameVelocityTpl<Scalar> Data |
Definition at line 50 of file frame-velocity.hpp.
typedef StateMultibodyTpl<Scalar> StateMultibody |
Definition at line 51 of file frame-velocity.hpp.
typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract |
Definition at line 52 of file frame-velocity.hpp.
typedef ResidualModelFrameVelocityTpl<Scalar> ResidualModelFrameVelocity |
Definition at line 53 of file frame-velocity.hpp.
typedef FrameMotionTpl<Scalar> FrameMotion |
Definition at line 54 of file frame-velocity.hpp.
typedef MathBase::VectorXs VectorXs |
Definition at line 55 of file frame-velocity.hpp.
CostModelFrameVelocityTpl | ( | boost::shared_ptr< StateMultibody > | state, |
boost::shared_ptr< ActivationModelAbstract > | activation, | ||
const FrameMotion & | Fref, | ||
const std::size_t | nu | ||
) |
Initialize the frame velocity cost model.
[in] | state | State of the multibody system |
[in] | activation | Activation model |
[in] | Fref | Reference frame velocity |
[in] | nu | Dimension of the control vector |
CostModelFrameVelocityTpl | ( | boost::shared_ptr< StateMultibody > | state, |
boost::shared_ptr< ActivationModelAbstract > | activation, | ||
const FrameMotion & | Fref | ||
) |
Initialize the frame velocity cost model.
The default nu
value is obtained from StateAbstractTpl::get_nv()
.
[in] | state | State of the multibody system |
[in] | activation | Activation model |
[in] | Fref | Reference frame velocity |
CostModelFrameVelocityTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const FrameMotion & | Fref, | ||
const std::size_t | nu | ||
) |
Initialize the frame velocity cost model.
We use ActivationModelQuadTpl
as a default activation model (i.e. \(a=\frac{1}{2}\|\mathbf{r}\|^2\)).
[in] | state | State of the multibody system |
[in] | Fref | Reference frame velocity |
[in] | nu | Dimension of the control vector |
CostModelFrameVelocityTpl | ( | boost::shared_ptr< StateMultibody > | state, |
const FrameMotion & | Fref | ||
) |
Initialize the frame velocity cost model.
We use ActivationModelQuadTpl
as a default activation model (i.e. \(a=\frac{1}{2}\|\mathbf{r}\|^2\)). Furthermore, the default nu
value is obtained from StateAbstractTpl::get_nv()
[in] | state | State of the multibody system |
[in] | Fref | Reference frame velocity |
|
protectedvirtual |
Return the frame velocity reference.
Reimplemented from CostModelAbstractTpl< _Scalar >.
|
protectedvirtual |
Modify the frame velocity reference.
Reimplemented from CostModelAbstractTpl< _Scalar >.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar |
Definition at line 47 of file frame-velocity.hpp.
|
protected |
Activation model.
Definition at line 128 of file cost-base.hpp.
|
protected |
Control dimension.
Definition at line 129 of file cost-base.hpp.
|
protected |
Residual model.
Definition at line 130 of file cost-base.hpp.
|
protected |
State description.
Definition at line 131 of file cost-base.hpp.
|
protected |
No control vector.
Definition at line 132 of file cost-base.hpp.