crocoddyl  1.9.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;
27  typedef CostModelAbstractTpl<Scalar> CostModelAbstract;
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  friend std::ostream& operator<<(std::ostream& os, const CostItemTpl<Scalar>& model) {
38  os << "{w=" << model.weight << ", " << *model.cost << "}";
39  return os;
40  }
41 
42  std::string name;
43  boost::shared_ptr<CostModelAbstract> cost;
44  Scalar weight;
45  bool active;
46 };
47 
64 template <typename _Scalar>
65 class CostModelSumTpl {
66  public:
67  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
68 
69  typedef _Scalar Scalar;
70  typedef MathBaseTpl<Scalar> MathBase;
71  typedef CostDataSumTpl<Scalar> CostDataSum;
72  typedef StateAbstractTpl<Scalar> StateAbstract;
73  typedef CostModelAbstractTpl<Scalar> CostModelAbstract;
74  typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
75  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
76  typedef CostItemTpl<Scalar> CostItem;
77  typedef typename MathBase::VectorXs VectorXs;
78  typedef typename MathBase::MatrixXs MatrixXs;
79 
80  typedef std::map<std::string, boost::shared_ptr<CostItem> > CostModelContainer;
81  typedef std::map<std::string, boost::shared_ptr<CostDataAbstract> > CostDataContainer;
82 
89  CostModelSumTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nu);
90 
98  explicit CostModelSumTpl(boost::shared_ptr<StateAbstract> state);
99  ~CostModelSumTpl();
100 
109  void addCost(const std::string& name, boost::shared_ptr<CostModelAbstract> cost, const Scalar weight,
110  const bool active = true);
111 
117  void removeCost(const std::string& name);
118 
125  void changeCostStatus(const std::string& name, const bool active);
126 
134  void calc(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x,
135  const Eigen::Ref<const VectorXs>& u);
136 
146  void calc(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x);
147 
155  void calcDiff(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x,
156  const Eigen::Ref<const VectorXs>& u);
157 
168  void calcDiff(const boost::shared_ptr<CostDataSum>& data, const Eigen::Ref<const VectorXs>& x);
169 
180  boost::shared_ptr<CostDataSum> createData(DataCollectorAbstract* const data);
181 
185  const boost::shared_ptr<StateAbstract>& get_state() const;
186 
190  const CostModelContainer& get_costs() const;
191 
195  std::size_t get_nu() const;
196 
200  std::size_t get_nr() const;
201 
205  std::size_t get_nr_total() const;
206 
210  const std::vector<std::string>& get_active() const;
211 
215  const std::vector<std::string>& get_inactive() const;
216 
222  bool getCostStatus(const std::string& name) const;
223 
227  template <class Scalar>
228  friend std::ostream& operator<<(std::ostream& os, const CostModelSumTpl<Scalar>& model);
229 
230  private:
231  boost::shared_ptr<StateAbstract> state_;
232  CostModelContainer costs_;
233  std::size_t nu_;
234  std::size_t nr_;
235  std::size_t nr_total_;
236  std::vector<std::string> active_;
237  std::vector<std::string> inactive_;
238 };
239 
240 template <typename _Scalar>
241 struct CostDataSumTpl {
242  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
243 
244  typedef _Scalar Scalar;
245  typedef MathBaseTpl<Scalar> MathBase;
246  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
247  typedef CostItemTpl<Scalar> CostItem;
248  typedef typename MathBase::VectorXs VectorXs;
249  typedef typename MathBase::MatrixXs MatrixXs;
250 
251  template <template <typename Scalar> class Model>
252  CostDataSumTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
253  : Lx_internal(model->get_state()->get_ndx()),
254  Lu_internal(model->get_nu()),
255  Lxx_internal(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
256  Lxu_internal(model->get_state()->get_ndx(), model->get_nu()),
257  Luu_internal(model->get_nu(), model->get_nu()),
258  shared(data),
259  cost(Scalar(0.)),
260  Lx(Lx_internal.data(), model->get_state()->get_ndx()),
261  Lu(Lu_internal.data(), model->get_nu()),
262  Lxx(Lxx_internal.data(), model->get_state()->get_ndx(), model->get_state()->get_ndx()),
263  Lxu(Lxu_internal.data(), model->get_state()->get_ndx(), model->get_nu()),
264  Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) {
265  Lx.setZero();
266  Lu.setZero();
267  Lxx.setZero();
268  Lxu.setZero();
269  Luu.setZero();
270  for (typename CostModelSumTpl<Scalar>::CostModelContainer::const_iterator it = model->get_costs().begin();
271  it != model->get_costs().end(); ++it) {
272  const boost::shared_ptr<CostItem>& item = it->second;
273  costs.insert(std::make_pair(item->name, item->cost->createData(data)));
274  }
275  }
276 
277  template <class ActionData>
278  void shareMemory(ActionData* const data) {
279  // Share memory with the differential action data
280  new (&Lx) Eigen::Map<VectorXs>(data->Lx.data(), data->Lx.size());
281  new (&Lu) Eigen::Map<VectorXs>(data->Lu.data(), data->Lu.size());
282  new (&Lxx) Eigen::Map<MatrixXs>(data->Lxx.data(), data->Lxx.rows(), data->Lxx.cols());
283  new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(), data->Lxu.cols());
284  new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(), data->Luu.cols());
285  }
286 
287  VectorXs get_Lx() const { return Lx; }
288  VectorXs get_Lu() const { return Lu; }
289  MatrixXs get_Lxx() const { return Lxx; }
290  MatrixXs get_Lxu() const { return Lxu; }
291  MatrixXs get_Luu() const { return Luu; }
292 
293  void set_Lx(const VectorXs& _Lx) {
294  if (Lx.size() != _Lx.size()) {
295  throw_pretty("Invalid argument: "
296  << "Lx has wrong dimension (it should be " + std::to_string(Lx.size()) + ")");
297  }
298  Lx = _Lx;
299  }
300  void set_Lu(const VectorXs& _Lu) {
301  if (Lu.size() != _Lu.size()) {
302  throw_pretty("Invalid argument: "
303  << "Lu has wrong dimension (it should be " + std::to_string(Lu.size()) + ")");
304  }
305  Lu = _Lu;
306  }
307  void set_Lxx(const MatrixXs& _Lxx) {
308  if (Lxx.rows() != _Lxx.rows() || Lxx.cols() != _Lxx.cols()) {
309  throw_pretty("Invalid argument: "
310  << "Lxx has wrong dimension (it should be " + std::to_string(Lxx.rows()) + ", " +
311  std::to_string(Lxx.cols()) + ")");
312  }
313  Lxx = _Lxx;
314  }
315  void set_Lxu(const MatrixXs& _Lxu) {
316  if (Lxu.rows() != _Lxu.rows() || Lxu.cols() != _Lxu.cols()) {
317  throw_pretty("Invalid argument: "
318  << "Lxu has wrong dimension (it should be " + std::to_string(Lxu.rows()) + ", " +
319  std::to_string(Lxu.cols()) + ")");
320  }
321  Lxu = _Lxu;
322  }
323  void set_Luu(const MatrixXs& _Luu) {
324  if (Luu.rows() != _Luu.rows() || Luu.cols() != _Luu.cols()) {
325  throw_pretty("Invalid argument: "
326  << "Luu has wrong dimension (it should be " + std::to_string(Luu.rows()) + ", " +
327  std::to_string(Luu.cols()) + ")");
328  }
329  Luu = _Luu;
330  }
331 
332  // Creates internal data in case we don't share it externally
333  VectorXs Lx_internal;
334  VectorXs Lu_internal;
335  MatrixXs Lxx_internal;
336  MatrixXs Lxu_internal;
337  MatrixXs Luu_internal;
338 
339  typename CostModelSumTpl<Scalar>::CostDataContainer costs;
340  DataCollectorAbstract* shared;
341  Scalar cost;
342  Eigen::Map<VectorXs> Lx;
343  Eigen::Map<VectorXs> Lu;
344  Eigen::Map<MatrixXs> Lxx;
345  Eigen::Map<MatrixXs> Lxu;
346  Eigen::Map<MatrixXs> Luu;
347 };
348 
349 } // namespace crocoddyl
350 
351 /* --- Details -------------------------------------------------------------- */
352 /* --- Details -------------------------------------------------------------- */
353 /* --- Details -------------------------------------------------------------- */
354 #include "crocoddyl/core/costs/cost-sum.hxx"
355 
356 #endif // CROCODDYL_CORE_COSTS_COST_SUM_HPP_
crocoddyl::DataCollectorAbstractTpl
Definition: data-collector-base.hpp:18
crocoddyl::CostItemTpl::operator<<
friend std::ostream & operator<<(std::ostream &os, const CostItemTpl< Scalar > &model)
Print information on the cost item.
Definition: cost-sum.hpp:37
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::CostDataSumTpl
Definition: cost-sum.hpp:241
crocoddyl::CostModelSumTpl::get_nr
std::size_t get_nr() const
Return the dimension of the active residual vector.
crocoddyl::CostModelAbstractTpl
Abstract class for cost models.
Definition: cost-base.hpp:49
crocoddyl::CostModelSumTpl::getCostStatus
bool getCostStatus(const std::string &name) const
Return the status of a given cost name.
crocoddyl::CostModelSumTpl
Summation of individual cost models.
Definition: cost-sum.hpp:65
crocoddyl::CostModelSumTpl::get_nu
std::size_t get_nu() const
Return the dimension of the control input.
crocoddyl::CostItemTpl
Definition: cost-sum.hpp:23
crocoddyl::CostModelSumTpl::addCost
void addCost(const std::string &name, boost::shared_ptr< CostModelAbstract > cost, const Scalar weight, const bool active=true)
Add a cost item.
crocoddyl::CostModelSumTpl::get_active
const std::vector< std::string > & get_active() const
Return the names of the active costs.
crocoddyl::CostModelSumTpl::get_costs
const CostModelContainer & get_costs() const
Return the stack of cost models.
crocoddyl::CostModelSumTpl::removeCost
void removeCost(const std::string &name)
Remove a cost item.
crocoddyl::CostModelSumTpl::operator<<
friend std::ostream & operator<<(std::ostream &os, const CostModelSumTpl< Scalar > &model)
Print information on the stack of costs.
crocoddyl::CostDataAbstractTpl
Definition: cost-base.hpp:248
crocoddyl::CostModelSumTpl::get_nr_total
std::size_t get_nr_total() const
Return the dimension of the total residual vector.
crocoddyl::CostModelSumTpl::get_inactive
const std::vector< std::string > & get_inactive() const
Return the names of the inactive costs.
crocoddyl::CostModelSumTpl::CostModelSumTpl
CostModelSumTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nu)
Initialize the cost-sum model.
crocoddyl::CostModelSumTpl::calc
void calc(const boost::shared_ptr< CostDataSum > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the total cost value.
crocoddyl::CostModelSumTpl::calcDiff
void calcDiff(const boost::shared_ptr< CostDataSum > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the Jacobian and Hessian of the total cost.
crocoddyl::CostModelSumTpl::changeCostStatus
void changeCostStatus(const std::string &name, const bool active)
Change the cost status.
crocoddyl::StateAbstractTpl
Abstract class for the state representation.
Definition: fwd.hpp:131
crocoddyl::CostModelSumTpl::createData
boost::shared_ptr< CostDataSum > createData(DataCollectorAbstract *const data)
Create the cost data.
crocoddyl::CostModelSumTpl::get_state
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.