crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
cost-sum.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_COSTS_COST_SUM_HPP_
10 #define CROCODDYL_CORE_COSTS_COST_SUM_HPP_
11 
12 #include <string>
13 #include <map>
14 #include <utility>
15 
16 #include "crocoddyl/core/fwd.hpp"
17 #include "crocoddyl/core/cost-base.hpp"
18 #include "crocoddyl/core/utils/exception.hpp"
19 
20 namespace crocoddyl {
21 
22 template <typename _Scalar>
23 struct CostItemTpl {
24  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 
26  typedef _Scalar Scalar;
28 
29  CostItemTpl() {}
30  CostItemTpl(const std::string& name, boost::shared_ptr<CostModelAbstract> cost, const Scalar weight,
31  const bool active = true)
32  : name(name), cost(cost), weight(weight), active(active) {}
33 
37  template <class Scalar>
38  friend std::ostream& operator<<(std::ostream& os, const CostItemTpl<Scalar>& model) {
39  os << "{w=" << model.weight << ", " << *model.cost << "}";
40  return os;
41  }
42 
43  std::string name;
44  boost::shared_ptr<CostModelAbstract> cost;
45  Scalar weight;
46  bool active;
47 };
48 
65 template <typename _Scalar>
67  public:
68  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69 
70  typedef _Scalar Scalar;
78  typedef typename MathBase::VectorXs VectorXs;
79  typedef typename MathBase::MatrixXs MatrixXs;
80 
81  typedef std::map<std::string, boost::shared_ptr<CostItem> > CostModelContainer;
82  typedef std::map<std::string, boost::shared_ptr<CostDataAbstract> > CostDataContainer;
83 
90  CostModelSumTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nu);
91 
99  explicit CostModelSumTpl(boost::shared_ptr<StateAbstract> state);
100  ~CostModelSumTpl();
101 
110  void addCost(const std::string& name, boost::shared_ptr<CostModelAbstract> cost, const Scalar weight,
111  const bool active = true);
112 
118  void removeCost(const std::string& name);
119 
126  void changeCostStatus(const std::string& name, const bool active);
127 
135  void calc(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x,
136  const Eigen::Ref<const VectorXs>& u);
137 
145  void calcDiff(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x,
146  const Eigen::Ref<const VectorXs>& u);
147 
158  boost::shared_ptr<CostDataSum> createData(DataCollectorAbstract* const data);
159 
166  void calc(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x);
167 
174  void calcDiff(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x);
175 
179  const boost::shared_ptr<StateAbstract>& get_state() const;
180 
184  const CostModelContainer& get_costs() const;
185 
189  std::size_t get_nu() const;
190 
194  std::size_t get_nr() const;
195 
199  std::size_t get_nr_total() const;
200 
204  const std::vector<std::string>& get_active() const;
205 
209  const std::vector<std::string>& get_inactive() const;
210 
216  bool getCostStatus(const std::string& name) const;
217 
221  template <class Scalar>
222  friend std::ostream& operator<<(std::ostream& os, const CostModelSumTpl<Scalar>& model);
223 
224  private:
225  boost::shared_ptr<StateAbstract> state_;
226  CostModelContainer costs_;
227  std::size_t nu_;
228  std::size_t nr_;
229  std::size_t nr_total_;
230  std::vector<std::string> active_;
231  std::vector<std::string> inactive_;
232  VectorXs unone_;
233 };
234 
235 template <typename _Scalar>
237  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
238 
239  typedef _Scalar Scalar;
243  typedef typename MathBase::VectorXs VectorXs;
244  typedef typename MathBase::MatrixXs MatrixXs;
245 
246  template <template <typename Scalar> class Model>
247  CostDataSumTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
248  : Lx_internal(model->get_state()->get_ndx()),
249  Lu_internal(model->get_nu()),
250  Lxx_internal(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
251  Lxu_internal(model->get_state()->get_ndx(), model->get_nu()),
252  Luu_internal(model->get_nu(), model->get_nu()),
253  shared(data),
254  cost(Scalar(0.)),
255  Lx(Lx_internal.data(), model->get_state()->get_ndx()),
256  Lu(Lu_internal.data(), model->get_nu()),
257  Lxx(Lxx_internal.data(), model->get_state()->get_ndx(), model->get_state()->get_ndx()),
258  Lxu(Lxu_internal.data(), model->get_state()->get_ndx(), model->get_nu()),
259  Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) {
260  Lx.setZero();
261  Lu.setZero();
262  Lxx.setZero();
263  Lxu.setZero();
264  Luu.setZero();
266  it != model->get_costs().end(); ++it) {
267  const boost::shared_ptr<CostItem>& item = it->second;
268  costs.insert(std::make_pair(item->name, item->cost->createData(data)));
269  }
270  }
271 
272  template <class ActionData>
273  void shareMemory(ActionData* const data) {
274  // Share memory with the differential action data
275  new (&Lx) Eigen::Map<VectorXs>(data->Lx.data(), data->Lx.size());
276  new (&Lu) Eigen::Map<VectorXs>(data->Lu.data(), data->Lu.size());
277  new (&Lxx) Eigen::Map<MatrixXs>(data->Lxx.data(), data->Lxx.rows(), data->Lxx.cols());
278  new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(), data->Lxu.cols());
279  new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(), data->Luu.cols());
280  }
281 
282  VectorXs get_Lx() const { return Lx; }
283  VectorXs get_Lu() const { return Lu; }
284  MatrixXs get_Lxx() const { return Lxx; }
285  MatrixXs get_Lxu() const { return Lxu; }
286  MatrixXs get_Luu() const { return Luu; }
287 
288  void set_Lx(const VectorXs& _Lx) {
289  if (Lx.size() != _Lx.size()) {
290  throw_pretty("Invalid argument: "
291  << "Lx has wrong dimension (it should be " + std::to_string(Lx.size()) + ")");
292  }
293  Lx = _Lx;
294  }
295  void set_Lu(const VectorXs& _Lu) {
296  if (Lu.size() != _Lu.size()) {
297  throw_pretty("Invalid argument: "
298  << "Lu has wrong dimension (it should be " + std::to_string(Lu.size()) + ")");
299  }
300  Lu = _Lu;
301  }
302  void set_Lxx(const MatrixXs& _Lxx) {
303  if (Lxx.rows() != _Lxx.rows() || Lxx.cols() != _Lxx.cols()) {
304  throw_pretty("Invalid argument: "
305  << "Lxx has wrong dimension (it should be " + std::to_string(Lxx.rows()) + ", " +
306  std::to_string(Lxx.cols()) + ")");
307  }
308  Lxx = _Lxx;
309  }
310  void set_Lxu(const MatrixXs& _Lxu) {
311  if (Lxu.rows() != _Lxu.rows() || Lxu.cols() != _Lxu.cols()) {
312  throw_pretty("Invalid argument: "
313  << "Lxu has wrong dimension (it should be " + std::to_string(Lxu.rows()) + ", " +
314  std::to_string(Lxu.cols()) + ")");
315  }
316  Lxu = _Lxu;
317  }
318  void set_Luu(const MatrixXs& _Luu) {
319  if (Luu.rows() != _Luu.rows() || Luu.cols() != _Luu.cols()) {
320  throw_pretty("Invalid argument: "
321  << "Luu has wrong dimension (it should be " + std::to_string(Luu.rows()) + ", " +
322  std::to_string(Luu.cols()) + ")");
323  }
324  Luu = _Luu;
325  }
326 
327  // Creates internal data in case we don't share it externally
328  VectorXs Lx_internal;
329  VectorXs Lu_internal;
330  MatrixXs Lxx_internal;
331  MatrixXs Lxu_internal;
332  MatrixXs Luu_internal;
333 
335  DataCollectorAbstract* shared;
336  Scalar cost;
337  Eigen::Map<VectorXs> Lx;
338  Eigen::Map<VectorXs> Lu;
339  Eigen::Map<MatrixXs> Lxx;
340  Eigen::Map<MatrixXs> Lxu;
341  Eigen::Map<MatrixXs> Luu;
342 };
343 
344 } // namespace crocoddyl
345 
346 /* --- Details -------------------------------------------------------------- */
347 /* --- Details -------------------------------------------------------------- */
348 /* --- Details -------------------------------------------------------------- */
349 #include "crocoddyl/core/costs/cost-sum.hxx"
350 
351 #endif // CROCODDYL_CORE_COSTS_COST_SUM_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:49
Abstract class for the state representation.
Definition: fwd.hpp:112
const CostModelContainer & get_costs() const
Return the stack of cost models.
Summation of individual cost models.
Definition: cost-sum.hpp:66