state.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/state.hpp"
11 
12 namespace crocoddyl {
13 
14 template <typename Scalar>
15 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state,
16  boost::shared_ptr<ActivationModelAbstract> activation,
17  const VectorXs& xref, const std::size_t& nu)
18  : Base(state, activation, nu), xref_(xref) {
19  if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
20  throw_pretty("Invalid argument: "
21  << "xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
22  }
23  if (activation_->get_nr() != state_->get_ndx()) {
24  throw_pretty("Invalid argument: "
25  << "nr is equals to " + std::to_string(state_->get_ndx()));
26  }
27 }
28 
29 template <typename Scalar>
30 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state,
31  boost::shared_ptr<ActivationModelAbstract> activation,
32  const VectorXs& xref)
33  : Base(state, activation), xref_(xref) {
34  if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
35  throw_pretty("Invalid argument: "
36  << "xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
37  }
38  if (activation_->get_nr() != state_->get_ndx()) {
39  throw_pretty("Invalid argument: "
40  << "nr is equals to " + std::to_string(state_->get_ndx()));
41  }
42 }
43 
44 template <typename Scalar>
45 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state, const VectorXs& xref,
46  const std::size_t& nu)
47  : Base(state, state->get_ndx(), nu), xref_(xref) {
48  if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
49  throw_pretty("Invalid argument: "
50  << "xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
51  }
52  if (activation_->get_nr() != state_->get_ndx()) {
53  throw_pretty("Invalid argument: "
54  << "nr is equals to " + std::to_string(state_->get_ndx()));
55  }
56 }
57 
58 template <typename Scalar>
59 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state, const VectorXs& xref)
60  : Base(state, state->get_ndx()), xref_(xref) {
61  if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
62  throw_pretty("Invalid argument: "
63  << "xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
64  }
65  if (activation_->get_nr() != state_->get_ndx()) {
66  throw_pretty("Invalid argument: "
67  << "nr is equals to " + std::to_string(state_->get_ndx()));
68  }
69 }
70 
71 template <typename Scalar>
72 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state,
73  boost::shared_ptr<ActivationModelAbstract> activation,
74  const std::size_t& nu)
75  : Base(state, activation, nu), xref_(state->zero()) {
76  if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
77  throw_pretty("Invalid argument: "
78  << "xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
79  }
80  if (activation_->get_nr() != state_->get_ndx()) {
81  throw_pretty("Invalid argument: "
82  << "nr is equals to " + std::to_string(state_->get_ndx()));
83  }
84 }
85 
86 template <typename Scalar>
87 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& nu)
88  : Base(state, state->get_ndx(), nu), xref_(state->zero()) {
89  if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
90  throw_pretty("Invalid argument: "
91  << "xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
92  }
93  if (activation_->get_nr() != state_->get_ndx()) {
94  throw_pretty("Invalid argument: "
95  << "nr is equals to " + std::to_string(state_->get_ndx()));
96  }
97 }
98 
99 template <typename Scalar>
100 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state,
101  boost::shared_ptr<ActivationModelAbstract> activation)
102  : Base(state, activation), xref_(state->zero()) {
103  if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
104  throw_pretty("Invalid argument: "
105  << "xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
106  }
107  if (activation_->get_nr() != state_->get_ndx()) {
108  throw_pretty("Invalid argument: "
109  << "nr is equals to " + std::to_string(state_->get_ndx()));
110  }
111 }
112 
113 template <typename Scalar>
114 CostModelStateTpl<Scalar>::CostModelStateTpl(boost::shared_ptr<StateMultibody> state)
115  : Base(state, state->get_ndx()), xref_(state->zero()) {
116  if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
117  throw_pretty("Invalid argument: "
118  << "xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
119  }
120  if (activation_->get_nr() != state_->get_ndx()) {
121  throw_pretty("Invalid argument: "
122  << "nr is equals to " + std::to_string(state_->get_ndx()));
123  }
124 }
125 
126 template <typename Scalar>
127 CostModelStateTpl<Scalar>::~CostModelStateTpl() {}
128 
129 template <typename Scalar>
130 void CostModelStateTpl<Scalar>::calc(const boost::shared_ptr<CostDataAbstract>& data,
131  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
132  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
133  throw_pretty("Invalid argument: "
134  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
135  }
136 
137  state_->diff(xref_, x, data->r);
138  activation_->calc(data->activation, data->r);
139  data->cost = data->activation->a_value;
140 }
141 
142 template <typename Scalar>
143 void CostModelStateTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
144  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
145  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
146  throw_pretty("Invalid argument: "
147  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
148  }
149 
150  CostDataStateTpl<Scalar>* d = static_cast<CostDataStateTpl<Scalar>*>(data.get());
151  state_->Jdiff(xref_, x, data->Rx, data->Rx, second);
152  activation_->calcDiff(data->activation, data->r);
153  data->Lx.noalias() = data->Rx.transpose() * data->activation->Ar;
154  d->Arr_Rx.noalias() = data->activation->Arr * data->Rx;
155  data->Lxx.noalias() = data->Rx.transpose() * d->Arr_Rx;
156 }
157 
158 template <typename Scalar>
159 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelStateTpl<Scalar>::createData(
160  DataCollectorAbstract* const data) {
161  return boost::make_shared<CostDataStateTpl<Scalar> >(this, data);
162 }
163 
164 template <typename Scalar>
165 void CostModelStateTpl<Scalar>::set_referenceImpl(const std::type_info& ti, const void* pv) {
166  if (ti == typeid(VectorXs)) {
167  if (static_cast<std::size_t>(static_cast<const VectorXs*>(pv)->size()) != state_->get_nx()) {
168  throw_pretty("Invalid argument: "
169  << "reference has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
170  }
171  xref_ = *static_cast<const VectorXs*>(pv);
172  } else {
173  throw_pretty("Invalid argument: incorrect type (it should be VectorXs)");
174  }
175 }
176 
177 template <typename Scalar>
178 void CostModelStateTpl<Scalar>::get_referenceImpl(const std::type_info& ti, void* pv) {
179  if (ti == typeid(VectorXs)) {
180  Eigen::VectorXd& tmp = *static_cast<VectorXs*>(pv);
181  tmp.resize(state_->get_nx());
182  Eigen::Map<VectorXs> ref_map(static_cast<VectorXs*>(pv)->data(), state_->get_nx());
183  for (std::size_t i = 0; i < state_->get_nx(); ++i) {
184  ref_map[i] = xref_[i];
185  }
186  } else {
187  throw_pretty("Invalid argument: incorrect type (it should be VectorXs)");
188  }
189 }
190 
191 template <typename Scalar>
192 const typename MathBaseTpl<Scalar>::VectorXs& CostModelStateTpl<Scalar>::get_xref() const {
193  return xref_;
194 }
195 
196 template <typename Scalar>
197 void CostModelStateTpl<Scalar>::set_xref(const VectorXs& xref_in) {
198  if (static_cast<std::size_t>(xref_in.size()) != state_->get_nx()) {
199  throw_pretty("Invalid argument: "
200  << "xref has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
201  }
202  xref_ = xref_in;
203 }
204 
205 } // namespace crocoddyl
Definition: action-base.hxx:11