control.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/control.hpp"
11 
12 namespace crocoddyl {
13 template <typename Scalar>
14 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state,
15  boost::shared_ptr<ActivationModelAbstract> activation,
16  const VectorXs& uref)
17  : Base(state, activation, static_cast<std::size_t>(uref.size())), uref_(uref) {
18  if (activation_->get_nr() != nu_) {
19  throw_pretty("Invalid argument: "
20  << "nr is equals to " + std::to_string(nu_));
21  }
22 }
23 
24 template <typename Scalar>
25 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state,
26  boost::shared_ptr<ActivationModelAbstract> activation)
27  : Base(state, activation), uref_(VectorXs::Zero(activation->get_nr())) {}
28 
29 template <typename Scalar>
30 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state,
31  boost::shared_ptr<ActivationModelAbstract> activation,
32  const std::size_t& nu)
33  : Base(state, activation, nu), uref_(VectorXs::Zero(nu)) {
34  if (activation_->get_nr() != nu_) {
35  throw_pretty("Invalid argument: "
36  << "nr is equals to " + std::to_string(nu_));
37  }
38 }
39 
40 template <typename Scalar>
41 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state, const VectorXs& uref)
42  : Base(state, static_cast<std::size_t>(uref.size()), static_cast<std::size_t>(uref.size())), uref_(uref) {}
43 
44 template <typename Scalar>
45 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state)
46  : Base(state, state->get_nv()), uref_(VectorXs::Zero(state->get_nv())) {}
47 
48 template <typename Scalar>
49 CostModelControlTpl<Scalar>::CostModelControlTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& nu)
50  : Base(state, nu, nu), uref_(VectorXs::Zero(nu)) {}
51 
52 template <typename Scalar>
53 CostModelControlTpl<Scalar>::~CostModelControlTpl() {}
54 
55 template <typename Scalar>
56 void CostModelControlTpl<Scalar>::calc(const boost::shared_ptr<CostDataAbstract>& data,
57  const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>& u) {
58  if (nu_ == 0) {
59  throw_pretty("Invalid argument: "
60  << "it seems to be an autonomous system, if so, don't add this cost function");
61  }
62  if (static_cast<std::size_t>(u.size()) != nu_) {
63  throw_pretty("Invalid argument: "
64  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
65  }
66 
67  data->r = u - uref_;
68  activation_->calc(data->activation, data->r);
69  data->cost = data->activation->a_value;
70 }
71 
72 template <typename Scalar>
73 void CostModelControlTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
74  const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>& u) {
75  if (nu_ == 0) {
76  throw_pretty("Invalid argument: "
77  << "it seems to be an autonomous system, if so, don't add this cost function");
78  }
79  if (static_cast<std::size_t>(u.size()) != nu_) {
80  throw_pretty("Invalid argument: "
81  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
82  }
83 
84  activation_->calcDiff(data->activation, data->r);
85  data->Lu = data->activation->Ar;
86  data->Luu.diagonal() = data->activation->Arr.diagonal();
87 }
88 
89 template <typename Scalar>
90 void CostModelControlTpl<Scalar>::set_referenceImpl(const std::type_info& ti, const void* pv) {
91  if (ti == typeid(VectorXs)) {
92  if (static_cast<std::size_t>(static_cast<const VectorXs*>(pv)->size()) != nu_) {
93  throw_pretty("Invalid argument: "
94  << "reference has wrong dimension (it should be " + std::to_string(nu_) + ")");
95  }
96  uref_ = *static_cast<const VectorXs*>(pv);
97  } else {
98  throw_pretty("Invalid argument: incorrect type (it should be VectorXs)");
99  }
100 }
101 
102 template <typename Scalar>
103 void CostModelControlTpl<Scalar>::get_referenceImpl(const std::type_info& ti, void* pv) {
104  if (ti == typeid(VectorXs)) {
105  Eigen::VectorXd& tmp = *static_cast<VectorXs*>(pv);
106  tmp.resize(nu_);
107  Eigen::Map<VectorXs> ref_map(static_cast<VectorXs*>(pv)->data(), nu_);
108  for (std::size_t i = 0; i < nu_; ++i) {
109  ref_map[i] = uref_[i];
110  }
111  } else {
112  throw_pretty("Invalid argument: incorrect type (it should be VectorXs)");
113  }
114 }
115 
116 template <typename Scalar>
117 const typename MathBaseTpl<Scalar>::VectorXs& CostModelControlTpl<Scalar>::get_uref() const {
118  return uref_;
119 }
120 
121 template <typename Scalar>
122 void CostModelControlTpl<Scalar>::set_uref(const VectorXs& uref_in) {
123  if (static_cast<std::size_t>(uref_in.size()) != nu_) {
124  throw_pretty("Invalid argument: "
125  << "uref has wrong dimension (it should be " + std::to_string(nu_) + ")");
126  }
127  uref_ = uref_in;
128 }
129 
130 } // namespace crocoddyl
Definition: action-base.hxx:11