crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
cost-sum.hpp
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 #ifndef CROCODDYL_MULTIBODY_COSTS_COST_SUM_HPP_
10 #define CROCODDYL_MULTIBODY_COSTS_COST_SUM_HPP_
11 
12 #include <string>
13 #include <map>
14 #include <utility>
15 
16 #include "crocoddyl/multibody/fwd.hpp"
17 #include "crocoddyl/core/utils/exception.hpp"
18 #include "crocoddyl/multibody/cost-base.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 
40 template <typename _Scalar>
42  public:
43  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44 
45  typedef _Scalar Scalar;
53  typedef typename MathBase::VectorXs VectorXs;
54  typedef typename MathBase::MatrixXs MatrixXs;
55 
56  typedef std::map<std::string, boost::shared_ptr<CostItem> > CostModelContainer;
57  typedef std::map<std::string, boost::shared_ptr<CostDataAbstract> > CostDataContainer;
58 
59  CostModelSumTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& nu);
60  explicit CostModelSumTpl(boost::shared_ptr<StateMultibody> state);
61  ~CostModelSumTpl();
62 
63  void addCost(const std::string& name, boost::shared_ptr<CostModelAbstract> cost, const Scalar& weight,
64  bool active = true);
65  void removeCost(const std::string& name);
66  void changeCostStatus(const std::string& name, bool active);
67 
68  void calc(const boost::shared_ptr<CostDataSumTpl<Scalar> >& data, const Eigen::Ref<const VectorXs>& x,
69  const Eigen::Ref<const VectorXs>& u);
70  void calcDiff(const boost::shared_ptr<CostDataSumTpl<Scalar> >& data, const Eigen::Ref<const VectorXs>& x,
71  const Eigen::Ref<const VectorXs>& u);
72  boost::shared_ptr<CostDataSumTpl<Scalar> > createData(DataCollectorAbstract* const data);
73 
74  void calc(const boost::shared_ptr<CostDataSumTpl<Scalar> >& data, const Eigen::Ref<const VectorXs>& x);
75  void calcDiff(const boost::shared_ptr<CostDataSumTpl<Scalar> >& data, const Eigen::Ref<const VectorXs>& x);
76 
77  const boost::shared_ptr<StateMultibody>& get_state() const;
78  const CostModelContainer& get_costs() const;
79  const std::size_t& get_nu() const;
80  const std::size_t& get_nr() const;
81  const std::size_t& get_nr_total() const;
82  const std::vector<std::string>& get_active() const;
83  const std::vector<std::string>& get_inactive() const;
84  bool getCostStatus(const std::string& name) const;
85 
86  private:
87  boost::shared_ptr<StateMultibody> state_;
88  CostModelContainer costs_;
89  std::size_t nu_;
90  std::size_t nr_;
91  std::size_t nr_total_;
92  std::vector<std::string> active_;
93  std::vector<std::string> inactive_;
94  VectorXs unone_;
95 };
96 
97 template <typename _Scalar>
99  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
100 
101  typedef _Scalar Scalar;
105  typedef typename MathBase::VectorXs VectorXs;
106  typedef typename MathBase::MatrixXs MatrixXs;
107 
108  template <template <typename Scalar> class Model>
109  CostDataSumTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
110  : Lx_internal(model->get_state()->get_ndx()),
111  Lu_internal(model->get_nu()),
112  Lxx_internal(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
113  Lxu_internal(model->get_state()->get_ndx(), model->get_nu()),
114  Luu_internal(model->get_nu(), model->get_nu()),
115  shared(data),
116  cost(Scalar(0.)),
117  Lx(Lx_internal.data(), model->get_state()->get_ndx()),
118  Lu(Lu_internal.data(), model->get_nu()),
119  Lxx(Lxx_internal.data(), model->get_state()->get_ndx(), model->get_state()->get_ndx()),
120  Lxu(Lxu_internal.data(), model->get_state()->get_ndx(), model->get_nu()),
121  Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) {
122  Lx.setZero();
123  Lu.setZero();
124  Lxx.setZero();
125  Lxu.setZero();
126  Luu.setZero();
127  for (typename CostModelSumTpl<Scalar>::CostModelContainer::const_iterator it = model->get_costs().begin();
128  it != model->get_costs().end(); ++it) {
129  const boost::shared_ptr<CostItem>& item = it->second;
130  costs.insert(std::make_pair(item->name, item->cost->createData(data)));
131  }
132  }
133 
134  template <class ActionData>
135  void shareMemory(ActionData* const data) {
136  // Share memory with the differential action data
137  new (&Lx) Eigen::Map<VectorXs>(data->Lx.data(), data->Lx.size());
138  new (&Lu) Eigen::Map<VectorXs>(data->Lu.data(), data->Lu.size());
139  new (&Lxx) Eigen::Map<MatrixXs>(data->Lxx.data(), data->Lxx.rows(), data->Lxx.cols());
140  new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(), data->Lxu.cols());
141  new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(), data->Luu.cols());
142  }
143 
144  VectorXs get_Lx() const { return Lx; }
145  VectorXs get_Lu() const { return Lu; }
146  MatrixXs get_Lxx() const { return Lxx; }
147  MatrixXs get_Lxu() const { return Lxu; }
148  MatrixXs get_Luu() const { return Luu; }
149 
150  void set_Lx(const VectorXs& _Lx) {
151  if (Lx.size() != _Lx.size()) {
152  throw_pretty("Invalid argument: "
153  << "Lx has wrong dimension (it should be " + std::to_string(Lx.size()) + ")");
154  }
155  Lx = _Lx;
156  }
157  void set_Lu(const VectorXs& _Lu) {
158  if (Lu.size() != _Lu.size()) {
159  throw_pretty("Invalid argument: "
160  << "Lu has wrong dimension (it should be " + std::to_string(Lu.size()) + ")");
161  }
162  Lu = _Lu;
163  }
164  void set_Lxx(const MatrixXs& _Lxx) {
165  if (Lxx.rows() != _Lxx.rows() || Lxx.cols() != _Lxx.cols()) {
166  throw_pretty("Invalid argument: "
167  << "Lxx has wrong dimension (it should be " + std::to_string(Lxx.rows()) + ", " +
168  std::to_string(Lxx.cols()) + ")");
169  }
170  Lxx = _Lxx;
171  }
172  void set_Lxu(const MatrixXs& _Lxu) {
173  if (Lxu.rows() != _Lxu.rows() || Lxu.cols() != _Lxu.cols()) {
174  throw_pretty("Invalid argument: "
175  << "Lxu has wrong dimension (it should be " + std::to_string(Lxu.rows()) + ", " +
176  std::to_string(Lxu.cols()) + ")");
177  }
178  Lxu = _Lxu;
179  }
180  void set_Luu(const MatrixXs& _Luu) {
181  if (Luu.rows() != _Luu.rows() || Luu.cols() != _Luu.cols()) {
182  throw_pretty("Invalid argument: "
183  << "Luu has wrong dimension (it should be " + std::to_string(Luu.rows()) + ", " +
184  std::to_string(Luu.cols()) + ")");
185  }
186  Luu = _Luu;
187  }
188 
189  // Creates internal data in case we don't share it externally
190  VectorXs Lx_internal;
191  VectorXs Lu_internal;
192  MatrixXs Lxx_internal;
193  MatrixXs Lxu_internal;
194  MatrixXs Luu_internal;
195 
197  DataCollectorAbstract* shared;
198  Scalar cost;
199  Eigen::Map<VectorXs> Lx;
200  Eigen::Map<VectorXs> Lu;
201  Eigen::Map<MatrixXs> Lxx;
202  Eigen::Map<MatrixXs> Lxu;
203  Eigen::Map<MatrixXs> Luu;
204 };
205 
206 } // namespace crocoddyl
207 
208 /* --- Details -------------------------------------------------------------- */
209 /* --- Details -------------------------------------------------------------- */
210 /* --- Details -------------------------------------------------------------- */
211 #include "crocoddyl/multibody/costs/cost-sum.hxx"
212 
213 #endif // CROCODDYL_MULTIBODY_COSTS_COST_SUM_HPP_