residual-com-velocity.hxx
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include <crocoddyl/core/utils/exception.hpp>
10 #include <pinocchio/algorithm/center-of-mass-derivatives.hpp>
11 #include <pinocchio/algorithm/center-of-mass.hpp>
12 
13 #include "sobec/residual-com-velocity.hpp"
14 
15 namespace sobec {
16 using namespace crocoddyl;
17 template <typename Scalar>
18 ResidualModelCoMVelocityTpl<Scalar>::ResidualModelCoMVelocityTpl(
19  boost::shared_ptr<StateMultibody> state, const Vector3s& vref,
20  const std::size_t nu)
21  : Base(state, 3, nu, true, true, false),
22  vref_(vref),
23  pin_model_(*state->get_pinocchio()) {}
24 
25 template <typename Scalar>
26 ResidualModelCoMVelocityTpl<Scalar>::ResidualModelCoMVelocityTpl(
27  boost::shared_ptr<StateMultibody> state, const Vector3s& vref)
28  : Base(state, 3, true, true, false),
29  vref_(vref),
30  pin_model_(*state->get_pinocchio()) {}
31 
32 template <typename Scalar>
33 ResidualModelCoMVelocityTpl<Scalar>::~ResidualModelCoMVelocityTpl() {}
34 
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>&) {
39  // Compute the residual residual give the reference CoM velocity
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());
45 
46  pinocchio::centerOfMass(pin_model_, *d->pinocchio, q, v);
47  data->r = d->pinocchio->vcom[0] - vref_;
48 }
49 
50 template <typename Scalar>
51 void ResidualModelCoMVelocityTpl<Scalar>::calcDiff(
52  const boost::shared_ptr<ResidualDataAbstract>& data,
53  const Eigen::Ref<const VectorXs>& /*x*/,
54  const Eigen::Ref<const VectorXs>&) {
55  Data* d = static_cast<Data*>(data.get());
56  // const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic>
57  // q = x.head(state_->get_nq()); const Eigen::VectorBlock<const
58  // Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(state_->get_nv());
59 
60  const std::size_t nv = state_->get_nv();
61 
62  pinocchio::getCenterOfMassVelocityDerivatives(pin_model_, *d->pinocchio,
63  d->dvcom_dq);
64  data->Rx.leftCols(nv) = d->dvcom_dq;
65  data->Rx.rightCols(nv) = d->pinocchio->Jcom;
66 }
67 
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,
73  data);
74 }
75 
76 template <typename Scalar>
77 const typename MathBaseTpl<Scalar>::Vector3s&
78 ResidualModelCoMVelocityTpl<Scalar>::get_reference() const {
79  return vref_;
80 }
81 
82 template <typename Scalar>
83 void ResidualModelCoMVelocityTpl<Scalar>::set_reference(const Vector3s& vref) {
84  vref_ = vref;
85 }
86 
87 } // namespace sobec
sobec
Definition: contact-force.hxx:11