cost-sum.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/cost-sum.hpp"
11 
12 namespace crocoddyl {
13 
14 template <typename Scalar>
15 CostModelSumTpl<Scalar>::CostModelSumTpl(boost::shared_ptr<StateMultibody> state, const std::size_t& nu)
16  : state_(state), nu_(nu), nr_(0), nr_total_(0) {}
17 
18 template <typename Scalar>
19 CostModelSumTpl<Scalar>::CostModelSumTpl(boost::shared_ptr<StateMultibody> state)
20  : state_(state), nu_(state->get_nv()), nr_(0), nr_total_(0) {}
21 
22 template <typename Scalar>
23 CostModelSumTpl<Scalar>::~CostModelSumTpl() {}
24 
25 template <typename Scalar>
26 void CostModelSumTpl<Scalar>::addCost(const std::string& name, boost::shared_ptr<CostModelAbstract> cost,
27  const Scalar& weight, bool active) {
28  if (cost->get_nu() != nu_) {
29  throw_pretty("Cost item doesn't have the same control dimension (it should be " + std::to_string(nu_) + ")");
30  }
31  std::pair<typename CostModelContainer::iterator, bool> ret =
32  costs_.insert(std::make_pair(name, boost::make_shared<CostItem>(name, cost, weight, active)));
33  if (ret.second == false) {
34  std::cout << "Warning: this cost item already existed, we cannot add it" << std::endl;
35  } else if (active) {
36  nr_ += cost->get_activation()->get_nr();
37  nr_total_ += cost->get_activation()->get_nr();
38  } else if (!active) {
39  nr_total_ += cost->get_activation()->get_nr();
40  }
41 }
42 
43 template <typename Scalar>
44 void CostModelSumTpl<Scalar>::removeCost(const std::string& name) {
45  typename CostModelContainer::iterator it = costs_.find(name);
46  if (it != costs_.end()) {
47  nr_ -= it->second->cost->get_activation()->get_nr();
48  nr_total_ -= it->second->cost->get_activation()->get_nr();
49  costs_.erase(it);
50  } else {
51  std::cout << "Warning: this cost item doesn't exist, we cannot remove it" << std::endl;
52  }
53 }
54 
55 template <typename Scalar>
56 void CostModelSumTpl<Scalar>::changeCostStatus(const std::string& name, bool active) {
57  typename CostModelContainer::iterator it = costs_.find(name);
58  if (it != costs_.end()) {
59  if (active && !it->second->active) {
60  nr_ += it->second->cost->get_activation()->get_nr();
61  } else if (!active && it->second->active) {
62  nr_ -= it->second->cost->get_activation()->get_nr();
63  }
64  it->second->active = active;
65  } else {
66  std::cout << "Warning: this cost item doesn't exist, we cannot change its status" << std::endl;
67  }
68 }
69 
70 template <typename Scalar>
71 void CostModelSumTpl<Scalar>::calc(const boost::shared_ptr<CostDataSumTpl<Scalar> >& data,
72  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
73  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
74  throw_pretty("Invalid argument: "
75  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
76  }
77  if (static_cast<std::size_t>(u.size()) != nu_) {
78  throw_pretty("Invalid argument: "
79  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
80  }
81  if (data->costs.size() != costs_.size()) {
82  throw_pretty("Invalid argument: "
83  << "it doesn't match the number of cost datas and models");
84  }
85  data->cost = 0.;
86 
87  typename CostModelContainer::iterator it_m, end_m;
88  typename CostDataContainer::iterator it_d, end_d;
89  for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(), end_d = data->costs.end();
90  it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
91  const boost::shared_ptr<CostItem>& m_i = it_m->second;
92  if (m_i->active) {
93  const boost::shared_ptr<CostDataAbstract>& d_i = it_d->second;
94  assert_pretty(it_m->first == it_d->first, "it doesn't match the cost name between data and model");
95 
96  m_i->cost->calc(d_i, x, u);
97  data->cost += m_i->weight * d_i->cost;
98  }
99  }
100 }
101 
102 template <typename Scalar>
103 void CostModelSumTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataSumTpl<Scalar> >& data,
104  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
105  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
106  throw_pretty("Invalid argument: "
107  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
108  }
109  if (static_cast<std::size_t>(u.size()) != nu_) {
110  throw_pretty("Invalid argument: "
111  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
112  }
113  if (data->costs.size() != costs_.size()) {
114  throw_pretty("Invalid argument: "
115  << "it doesn't match the number of cost datas and models");
116  }
117  data->Lx.setZero();
118  data->Lu.setZero();
119  data->Lxx.setZero();
120  data->Lxu.setZero();
121  data->Luu.setZero();
122 
123  typename CostModelContainer::iterator it_m, end_m;
124  typename CostDataContainer::iterator it_d, end_d;
125  for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(), end_d = data->costs.end();
126  it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
127  const boost::shared_ptr<CostItem>& m_i = it_m->second;
128  if (m_i->active) {
129  const boost::shared_ptr<CostDataAbstract>& d_i = it_d->second;
130  assert_pretty(it_m->first == it_d->first, "it doesn't match the cost name between data and model");
131 
132  m_i->cost->calcDiff(d_i, x, u);
133  data->Lx += m_i->weight * d_i->Lx;
134  data->Lu += m_i->weight * d_i->Lu;
135  data->Lxx += m_i->weight * d_i->Lxx;
136  data->Lxu += m_i->weight * d_i->Lxu;
137  data->Luu += m_i->weight * d_i->Luu;
138  }
139  }
140 }
141 
142 template <typename Scalar>
143 boost::shared_ptr<CostDataSumTpl<Scalar> > CostModelSumTpl<Scalar>::createData(DataCollectorAbstract* const data) {
144  return boost::make_shared<CostDataSumTpl<Scalar> >(this, data);
145 }
146 
147 template <typename Scalar>
148 void CostModelSumTpl<Scalar>::calc(const boost::shared_ptr<CostDataSumTpl<Scalar> >& data,
149  const Eigen::Ref<const VectorXs>& x) {
150  calc(data, x, unone_);
151 }
152 
153 template <typename Scalar>
154 void CostModelSumTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataSumTpl<Scalar> >& data,
155  const Eigen::Ref<const VectorXs>& x) {
156  calcDiff(data, x, unone_);
157 }
158 
159 template <typename Scalar>
160 const boost::shared_ptr<StateMultibodyTpl<Scalar> >& CostModelSumTpl<Scalar>::get_state() const {
161  return state_;
162 }
163 
164 template <typename Scalar>
165 const typename CostModelSumTpl<Scalar>::CostModelContainer& CostModelSumTpl<Scalar>::get_costs() const {
166  return costs_;
167 }
168 
169 template <typename Scalar>
170 const std::size_t& CostModelSumTpl<Scalar>::get_nu() const {
171  return nu_;
172 }
173 
174 template <typename Scalar>
175 const std::size_t& CostModelSumTpl<Scalar>::get_nr() const {
176  return nr_;
177 }
178 
179 template <typename Scalar>
180 const std::size_t& CostModelSumTpl<Scalar>::get_nr_total() const {
181  return nr_total_;
182 }
183 
184 } // namespace crocoddyl
Definition: action-base.hxx:11