9 #include <crocoddyl/core/utils/exception.hpp>
10 #include <pinocchio/algorithm/center-of-mass-derivatives.hpp>
11 #include <pinocchio/algorithm/center-of-mass.hpp>
16 using namespace crocoddyl;
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>
MathBase::Vector3s Vector3s
Definition: residual-com-velocity.hpp:84
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
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
const Vector3s & get_reference() const
Return the CoM velocity reference.
Definition: residual-com-velocity.hxx:78
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
virtual ~ResidualModelCoMVelocityTpl()
Definition: residual-com-velocity.hxx:33
ResidualModelAbstractTpl< Scalar > Base
Definition: residual-com-velocity.hpp:79
DataCollectorAbstractTpl< Scalar > DataCollectorAbstract
Definition: residual-com-velocity.hpp:83
virtual boost::shared_ptr< ResidualDataAbstract > createData(DataCollectorAbstract *const data)
Definition: residual-com-velocity.hxx:70
void set_reference(const Vector3s &vref)
Modify the CoM velocity reference.
Definition: residual-com-velocity.hxx:83
Definition: activation-quad-ref.hpp:19
Definition: residual-com-velocity.hpp:23
Matrix3xs dvcom_dq
Definition: residual-com-velocity.hpp:49
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: residual-com-velocity.hpp:50