centroidal-momentum.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, 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 "crocoddyl/multibody/costs/centroidal-momentum.hpp"
11 
12 #include <pinocchio/algorithm/centroidal-derivatives.hpp>
13 
14 namespace crocoddyl {
15 
16 template <typename Scalar>
17 CostModelCentroidalMomentumTpl<Scalar>::CostModelCentroidalMomentumTpl(
18  boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
19  const Vector6s& href, const std::size_t& nu)
20  : Base(state, activation, nu), href_(href) {
21  if (activation_->get_nr() != 6) {
22  throw_pretty("Invalid argument: "
23  << "nr is equals to 6");
24  }
25 }
26 
27 template <typename Scalar>
28 CostModelCentroidalMomentumTpl<Scalar>::CostModelCentroidalMomentumTpl(
29  boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActivationModelAbstract> activation,
30  const Vector6s& href)
31  : Base(state, activation), href_(href) {
32  if (activation_->get_nr() != 6) {
33  throw_pretty("Invalid argument: "
34  << "nr is equals to 6");
35  }
36 }
37 
38 template <typename Scalar>
39 CostModelCentroidalMomentumTpl<Scalar>::CostModelCentroidalMomentumTpl(boost::shared_ptr<StateMultibody> state,
40  const Vector6s& href, const std::size_t& nu)
41  : Base(state, 6, nu), href_(href) {}
42 
43 template <typename Scalar>
44 CostModelCentroidalMomentumTpl<Scalar>::CostModelCentroidalMomentumTpl(boost::shared_ptr<StateMultibody> state,
45  const Vector6s& href)
46  : Base(state, 6), href_(href) {}
47 
48 template <typename Scalar>
49 CostModelCentroidalMomentumTpl<Scalar>::~CostModelCentroidalMomentumTpl() {}
50 
51 template <typename Scalar>
52 void CostModelCentroidalMomentumTpl<Scalar>::calc(const boost::shared_ptr<CostDataAbstract>& data,
53  const Eigen::Ref<const VectorXs>&,
54  const Eigen::Ref<const VectorXs>&) {
55  // Compute the cost residual give the reference CentroidalMomentum
56  CostDataCentroidalMomentumTpl<Scalar>* d = static_cast<CostDataCentroidalMomentumTpl<Scalar>*>(data.get());
57  data->r = d->pinocchio->hg.toVector() - href_;
58 
59  activation_->calc(data->activation, data->r);
60  data->cost = data->activation->a_value;
61 }
62 
63 template <typename Scalar>
64 void CostModelCentroidalMomentumTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
65  const Eigen::Ref<const VectorXs>&,
66  const Eigen::Ref<const VectorXs>&) {
67  CostDataCentroidalMomentumTpl<Scalar>* d = static_cast<CostDataCentroidalMomentumTpl<Scalar>*>(data.get());
68  const std::size_t& nv = state_->get_nv();
69  Eigen::Ref<Matrix6xs> Rq = data->Rx.leftCols(nv);
70  Eigen::Ref<Matrix6xs> Rv = data->Rx.rightCols(nv);
71 
72  activation_->calcDiff(data->activation, data->r);
73  pinocchio::getCentroidalDynamicsDerivatives(*state_->get_pinocchio().get(), *d->pinocchio, Rq, d->dhd_dq, d->dhd_dv,
74  Rv);
75 
76  // The derivative computation in pinocchio does not take the frame of reference into
77  // account. So we need to update the com frame as well.
78  for (int i = 0; i < d->pinocchio->Jcom.cols(); ++i) {
79  data->Rx.template block<3, 1>(3, i) -= d->pinocchio->Jcom.col(i).cross(d->pinocchio->hg.linear());
80  }
81 
82  d->Arr_Rx.noalias() = data->activation->Arr * data->Rx;
83  data->Lx.noalias() = data->Rx.transpose() * data->activation->Ar;
84  data->Lxx.noalias() = data->Rx.transpose() * d->Arr_Rx;
85 }
86 
87 template <typename Scalar>
88 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelCentroidalMomentumTpl<Scalar>::createData(
89  DataCollectorAbstract* const data) {
90  return boost::make_shared<CostDataCentroidalMomentumTpl<Scalar> >(this, data);
91 }
92 
93 template <typename Scalar>
94 void CostModelCentroidalMomentumTpl<Scalar>::set_referenceImpl(const std::type_info& ti, const void* pv) {
95  if (ti == typeid(Vector6s)) {
96  href_ = *static_cast<const Vector6s*>(pv);
97  } else {
98  throw_pretty("Invalid argument: incorrect type (it should be Vector6s)");
99  }
100 }
101 
102 template <typename Scalar>
103 void CostModelCentroidalMomentumTpl<Scalar>::get_referenceImpl(const std::type_info& ti, void* pv) {
104  if (ti == typeid(Vector6s)) {
105  Eigen::Map<Vector6s> ref_map(static_cast<Vector6s*>(pv)->data());
106  ref_map[0] = href_[0];
107  ref_map[1] = href_[1];
108  ref_map[2] = href_[2];
109  ref_map[3] = href_[3];
110  ref_map[4] = href_[4];
111  ref_map[5] = href_[5];
112  } else {
113  throw_pretty("Invalid argument: incorrect type (it should be Vector6s)");
114  }
115 }
116 
117 template <typename Scalar>
118 const typename MathBaseTpl<Scalar>::Vector6s& CostModelCentroidalMomentumTpl<Scalar>::get_href() const {
119  return href_;
120 }
121 
122 template <typename Scalar>
123 void CostModelCentroidalMomentumTpl<Scalar>::set_href(const Vector6s& href_in) {
124  href_ = href_in;
125 }
126 
127 } // namespace crocoddyl
Definition: action-base.hxx:11