crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
cost-sum.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, 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  bool active = true)
32  : name(name), cost(cost), weight(weight), active(active) {}
33 
34  std::string name;
35  boost::shared_ptr<CostModelAbstract> cost;
36  Scalar weight;
37  bool active;
38 };
39 
56 template <typename _Scalar>
58  public:
59  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60 
61  typedef _Scalar Scalar;
69  typedef typename MathBase::VectorXs VectorXs;
70  typedef typename MathBase::MatrixXs MatrixXs;
71 
72  typedef std::map<std::string, boost::shared_ptr<CostItem> > CostModelContainer;
73  typedef std::map<std::string, boost::shared_ptr<CostDataAbstract> > CostDataContainer;
74 
81  CostModelSumTpl(boost::shared_ptr<StateAbstract> state, const std::size_t& nu);
82 
90  explicit CostModelSumTpl(boost::shared_ptr<StateAbstract> state);
91  ~CostModelSumTpl();
92 
101  void addCost(const std::string& name, boost::shared_ptr<CostModelAbstract> cost, const Scalar& weight,
102  bool active = true);
103 
109  void removeCost(const std::string& name);
110 
117  void changeCostStatus(const std::string& name, bool active);
118 
126  void calc(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x,
127  const Eigen::Ref<const VectorXs>& u);
128 
136  void calcDiff(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x,
137  const Eigen::Ref<const VectorXs>& u);
138 
149  boost::shared_ptr<CostDataSum> createData(DataCollectorAbstract* const data);
150 
157  void calc(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x);
158 
165  void calcDiff(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x);
166 
170  const boost::shared_ptr<StateAbstract>& get_state() const;
171 
175  const CostModelContainer& get_costs() const;
176 
180  const std::size_t& get_nu() const;
181 
185  const std::size_t& get_nr() const;
186 
190  const std::size_t& get_nr_total() const;
191 
195  const std::vector<std::string>& get_active() const;
196 
200  const std::vector<std::string>& get_inactive() const;
201 
207  bool getCostStatus(const std::string& name) const;
208 
209  private:
210  boost::shared_ptr<StateAbstract> state_;
211  CostModelContainer costs_;
212  std::size_t nu_;
213  std::size_t nr_;
214  std::size_t nr_total_;
215  std::vector<std::string> active_;
216  std::vector<std::string> inactive_;
217  VectorXs unone_;
218 };
219 
220 template <typename _Scalar>
222  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
223 
224  typedef _Scalar Scalar;
228  typedef typename MathBase::VectorXs VectorXs;
229  typedef typename MathBase::MatrixXs MatrixXs;
230 
231  template <template <typename Scalar> class Model>
232  CostDataSumTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
233  : Lx_internal(model->get_state()->get_ndx()),
234  Lu_internal(model->get_nu()),
235  Lxx_internal(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
236  Lxu_internal(model->get_state()->get_ndx(), model->get_nu()),
237  Luu_internal(model->get_nu(), model->get_nu()),
238  shared(data),
239  cost(Scalar(0.)),
240  Lx(Lx_internal.data(), model->get_state()->get_ndx()),
241  Lu(Lu_internal.data(), model->get_nu()),
242  Lxx(Lxx_internal.data(), model->get_state()->get_ndx(), model->get_state()->get_ndx()),
243  Lxu(Lxu_internal.data(), model->get_state()->get_ndx(), model->get_nu()),
244  Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) {
245  Lx.setZero();
246  Lu.setZero();
247  Lxx.setZero();
248  Lxu.setZero();
249  Luu.setZero();
251  it != model->get_costs().end(); ++it) {
252  const boost::shared_ptr<CostItem>& item = it->second;
253  costs.insert(std::make_pair(item->name, item->cost->createData(data)));
254  }
255  }
256 
257  template <class ActionData>
258  void shareMemory(ActionData* const data) {
259  // Share memory with the differential action data
260  new (&Lx) Eigen::Map<VectorXs>(data->Lx.data(), data->Lx.size());
261  new (&Lu) Eigen::Map<VectorXs>(data->Lu.data(), data->Lu.size());
262  new (&Lxx) Eigen::Map<MatrixXs>(data->Lxx.data(), data->Lxx.rows(), data->Lxx.cols());
263  new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(), data->Lxu.cols());
264  new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(), data->Luu.cols());
265  }
266 
267  VectorXs get_Lx() const { return Lx; }
268  VectorXs get_Lu() const { return Lu; }
269  MatrixXs get_Lxx() const { return Lxx; }
270  MatrixXs get_Lxu() const { return Lxu; }
271  MatrixXs get_Luu() const { return Luu; }
272 
273  void set_Lx(const VectorXs& _Lx) {
274  if (Lx.size() != _Lx.size()) {
275  throw_pretty("Invalid argument: "
276  << "Lx has wrong dimension (it should be " + std::to_string(Lx.size()) + ")");
277  }
278  Lx = _Lx;
279  }
280  void set_Lu(const VectorXs& _Lu) {
281  if (Lu.size() != _Lu.size()) {
282  throw_pretty("Invalid argument: "
283  << "Lu has wrong dimension (it should be " + std::to_string(Lu.size()) + ")");
284  }
285  Lu = _Lu;
286  }
287  void set_Lxx(const MatrixXs& _Lxx) {
288  if (Lxx.rows() != _Lxx.rows() || Lxx.cols() != _Lxx.cols()) {
289  throw_pretty("Invalid argument: "
290  << "Lxx has wrong dimension (it should be " + std::to_string(Lxx.rows()) + ", " +
291  std::to_string(Lxx.cols()) + ")");
292  }
293  Lxx = _Lxx;
294  }
295  void set_Lxu(const MatrixXs& _Lxu) {
296  if (Lxu.rows() != _Lxu.rows() || Lxu.cols() != _Lxu.cols()) {
297  throw_pretty("Invalid argument: "
298  << "Lxu has wrong dimension (it should be " + std::to_string(Lxu.rows()) + ", " +
299  std::to_string(Lxu.cols()) + ")");
300  }
301  Lxu = _Lxu;
302  }
303  void set_Luu(const MatrixXs& _Luu) {
304  if (Luu.rows() != _Luu.rows() || Luu.cols() != _Luu.cols()) {
305  throw_pretty("Invalid argument: "
306  << "Luu has wrong dimension (it should be " + std::to_string(Luu.rows()) + ", " +
307  std::to_string(Luu.cols()) + ")");
308  }
309  Luu = _Luu;
310  }
311 
312  // Creates internal data in case we don't share it externally
313  VectorXs Lx_internal;
314  VectorXs Lu_internal;
315  MatrixXs Lxx_internal;
316  MatrixXs Lxu_internal;
317  MatrixXs Luu_internal;
318 
320  DataCollectorAbstract* shared;
321  Scalar cost;
322  Eigen::Map<VectorXs> Lx;
323  Eigen::Map<VectorXs> Lu;
324  Eigen::Map<MatrixXs> Lxx;
325  Eigen::Map<MatrixXs> Lxu;
326  Eigen::Map<MatrixXs> Luu;
327 };
328 
329 } // namespace crocoddyl
330 
331 /* --- Details -------------------------------------------------------------- */
332 /* --- Details -------------------------------------------------------------- */
333 /* --- Details -------------------------------------------------------------- */
334 #include "crocoddyl/core/costs/cost-sum.hxx"
335 
336 #endif // CROCODDYL_CORE_COSTS_COST_SUM_HPP_
Abstract class for cost models.
Definition: cost-base.hpp:47
Abstract class for the state representation.
Definition: fwd.hpp:106
const CostModelContainer & get_costs() const
Return the stack of cost models.
Summation of individual cost models.
Definition: cost-sum.hpp:57