9 #include <crocoddyl/core/utils/exception.hpp> 10 #include <pinocchio/algorithm/center-of-mass-derivatives.hpp> 11 #include <pinocchio/algorithm/center-of-mass.hpp> 17 template <
typename Scalar>
19 boost::shared_ptr<StateMultibody> state,
const Vector3s& vref,
21 :
Base(state, 3, nu, true, true, false),
23 pin_model_(*state->get_pinocchio()) {}
25 template <
typename Scalar>
27 boost::shared_ptr<StateMultibody> state,
const Vector3s& vref)
28 :
Base(state, 3, true, true, false),
30 pin_model_(*state->get_pinocchio()) {}
32 template <
typename Scalar>
35 template <
typename Scalar>
37 const boost::shared_ptr<ResidualDataAbstract>& data,
38 const Eigen::Ref<const VectorXs>&
x,
const Eigen::Ref<const VectorXs>&) {
40 Data* d =
static_cast<Data*
>(data.get());
41 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
42 x.head(state_->get_nq());
43 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
44 x.tail(state_->get_nv());
46 pinocchio::centerOfMass(pin_model_, *d->
pinocchio, q, v);
50 template <
typename Scalar>
52 const boost::shared_ptr<ResidualDataAbstract>& data,
53 const Eigen::Ref<const VectorXs>& ,
54 const Eigen::Ref<const VectorXs>&) {
55 Data* d =
static_cast<Data*
>(data.get());
60 const std::size_t nv = state_->get_nv();
62 pinocchio::getCenterOfMassVelocityDerivatives(pin_model_, *d->
pinocchio,
65 data->Rx.rightCols(nv) = d->
pinocchio->Jcom;
68 template <
typename Scalar>
69 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
72 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
76 template <
typename Scalar>
77 const typename MathBaseTpl<Scalar>::Vector3s&
82 template <
typename Scalar>
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-com-velocity.hpp:83
const Vector3s & get_reference() const
Return the CoM velocity reference.
Definition: residual-com-velocity.hxx:78
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Definition: residual-com-velocity.hxx:70
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: residual-com-velocity.hpp:50
void set_reference(const Vector3s &vref)
Modify the CoM velocity reference.
Definition: residual-com-velocity.hxx:83
ResidualModelCoMVelocityTpl(boost::shared_ptr< StateMultibody > state, const Vector3s &vref, const std::size_t nu)
Initialize the CoM velocity residual model.
Definition: residual-com-velocity.hxx:18
Definition: activation-quad-ref.hpp:19
virtual ~ResidualModelCoMVelocityTpl()
Definition: residual-com-velocity.hxx:33
Matrix3xs dvcom_dq
Definition: residual-com-velocity.hpp:49
ResidualModelAbstractTpl< Scalar > Base
Definition: residual-com-velocity.hpp:79
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 CoM velocity residual.
Definition: residual-com-velocity.hxx:51
MathBase::Vector3s Vector3s
Definition: residual-com-velocity.hpp:84
Definition: residual-com-velocity.hpp:23
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the CoM velocity residual.
Definition: residual-com-velocity.hxx:36