com-position.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/com-position.hpp"
11 
12 namespace crocoddyl {
13 
14 template <typename Scalar>
15 CostModelCoMPositionTpl<Scalar>::CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state,
16  boost::shared_ptr<ActivationModelAbstract> activation,
17  const Vector3s& cref, const std::size_t& nu)
18  : Base(state, activation, nu), cref_(cref) {
19  if (activation_->get_nr() != 3) {
20  throw_pretty("Invalid argument: "
21  << "nr is equals to 3");
22  }
23 }
24 
25 template <typename Scalar>
26 CostModelCoMPositionTpl<Scalar>::CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state,
27  boost::shared_ptr<ActivationModelAbstract> activation,
28  const Vector3s& cref)
29  : Base(state, activation), cref_(cref) {
30  if (activation_->get_nr() != 3) {
31  throw_pretty("Invalid argument: "
32  << "nr is equals to 3");
33  }
34 }
35 
36 template <typename Scalar>
37 CostModelCoMPositionTpl<Scalar>::CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state, const Vector3s& cref,
38  const std::size_t& nu)
39  : Base(state, 3, nu), cref_(cref) {}
40 
41 template <typename Scalar>
42 CostModelCoMPositionTpl<Scalar>::CostModelCoMPositionTpl(boost::shared_ptr<StateMultibody> state, const Vector3s& cref)
43  : Base(state, 3), cref_(cref) {}
44 
45 template <typename Scalar>
46 CostModelCoMPositionTpl<Scalar>::~CostModelCoMPositionTpl() {}
47 
48 template <typename Scalar>
49 void CostModelCoMPositionTpl<Scalar>::calc(const boost::shared_ptr<CostDataAbstract>& data,
50  const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
51  // Compute the cost residual give the reference CoMPosition position
52  CostDataCoMPositionTpl<Scalar>* d = static_cast<CostDataCoMPositionTpl<Scalar>*>(data.get());
53  data->r = d->pinocchio->com[0] - cref_;
54 
55  // Compute the cost
56  activation_->calc(data->activation, data->r);
57  data->cost = data->activation->a_value;
58 }
59 
60 template <typename Scalar>
61 void CostModelCoMPositionTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
62  const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
63  CostDataCoMPositionTpl<Scalar>* d = static_cast<CostDataCoMPositionTpl<Scalar>*>(data.get());
64 
65  // Compute the derivatives of the frame placement
66  const std::size_t& nv = state_->get_nv();
67  activation_->calcDiff(data->activation, data->r);
68  data->Rx.leftCols(nv) = d->pinocchio->Jcom;
69  data->Lx.head(nv).noalias() = d->pinocchio->Jcom.transpose() * data->activation->Ar;
70  d->Arr_Jcom.noalias() = data->activation->Arr * d->pinocchio->Jcom;
71  data->Lxx.topLeftCorner(nv, nv).noalias() = d->pinocchio->Jcom.transpose() * d->Arr_Jcom;
72 }
73 
74 template <typename Scalar>
75 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelCoMPositionTpl<Scalar>::createData(
76  DataCollectorAbstract* const data) {
77  return boost::make_shared<CostDataCoMPositionTpl<Scalar> >(this, data);
78 }
79 
80 template <typename Scalar>
81 void CostModelCoMPositionTpl<Scalar>::set_referenceImpl(const std::type_info& ti, const void* pv) {
82  if (ti == typeid(Vector3s)) {
83  cref_ = *static_cast<const Vector3s*>(pv);
84  } else {
85  throw_pretty("Invalid argument: incorrect type (it should be Vector3s)");
86  }
87 }
88 
89 template <typename Scalar>
90 void CostModelCoMPositionTpl<Scalar>::get_referenceImpl(const std::type_info& ti, void* pv) {
91  if (ti == typeid(Vector3s)) {
92  Eigen::Map<Vector3s> ref_map(static_cast<Vector3s*>(pv)->data());
93  ref_map[0] = cref_[0];
94  ref_map[1] = cref_[1];
95  ref_map[2] = cref_[2];
96  } else {
97  throw_pretty("Invalid argument: incorrect type (it should be Vector3s)");
98  }
99 }
100 
101 template <typename Scalar>
102 const typename MathBaseTpl<Scalar>::Vector3s& CostModelCoMPositionTpl<Scalar>::get_cref() const {
103  return cref_;
104 }
105 
106 template <typename Scalar>
107 void CostModelCoMPositionTpl<Scalar>::set_cref(const Vector3s& cref_in) {
108  cref_ = cref_in;
109 }
110 
111 } // namespace crocoddyl
Definition: action-base.hxx:11