9 #include <crocoddyl/core/utils/exception.hpp>
10 #include <pinocchio/algorithm/center-of-mass-derivatives.hpp>
11 #include <pinocchio/algorithm/center-of-mass.hpp>
13 #include "sobec/residual-com-velocity.hpp"
16 using namespace crocoddyl;
17 template <
typename Scalar>
18 ResidualModelCoMVelocityTpl<Scalar>::ResidualModelCoMVelocityTpl(
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>
26 ResidualModelCoMVelocityTpl<Scalar>::ResidualModelCoMVelocityTpl(
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>
33 ResidualModelCoMVelocityTpl<Scalar>::~ResidualModelCoMVelocityTpl() {}
35 template <
typename Scalar>
36 void ResidualModelCoMVelocityTpl<Scalar>::calc(
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);
47 data->r = d->pinocchio->vcom[0] - vref_;
50 template <
typename Scalar>
51 void ResidualModelCoMVelocityTpl<Scalar>::calcDiff(
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,
64 data->Rx.leftCols(nv) = d->dvcom_dq;
65 data->Rx.rightCols(nv) = d->pinocchio->Jcom;
68 template <
typename Scalar>
69 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
70 ResidualModelCoMVelocityTpl<Scalar>::createData(
71 DataCollectorAbstract*
const data) {
72 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this,
76 template <
typename Scalar>
77 const typename MathBaseTpl<Scalar>::Vector3s&
78 ResidualModelCoMVelocityTpl<Scalar>::get_reference()
const {
82 template <
typename Scalar>
83 void ResidualModelCoMVelocityTpl<Scalar>::set_reference(
const Vector3s& vref) {
Definition: contact-force.hxx:11