9 #ifndef CROCODDYL_CORE_COSTS_COST_SUM_HPP_ 10 #define CROCODDYL_CORE_COSTS_COST_SUM_HPP_ 16 #include "crocoddyl/core/fwd.hpp" 17 #include "crocoddyl/core/cost-base.hpp" 18 #include "crocoddyl/core/utils/exception.hpp" 22 template <
typename _Scalar>
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 typedef _Scalar Scalar;
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) {}
37 template <
class Scalar>
38 friend std::ostream& operator<<(std::ostream& os, const CostItemTpl<Scalar>& model) {
39 os <<
"{w=" << model.weight <<
", " << *model.cost <<
"}";
44 boost::shared_ptr<CostModelAbstract> cost;
65 template <
typename _Scalar>
68 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70 typedef _Scalar Scalar;
78 typedef typename MathBase::VectorXs VectorXs;
79 typedef typename MathBase::MatrixXs MatrixXs;
81 typedef std::map<std::string, boost::shared_ptr<CostItem> > CostModelContainer;
82 typedef std::map<std::string, boost::shared_ptr<CostDataAbstract> > CostDataContainer;
90 CostModelSumTpl(boost::shared_ptr<StateAbstract> state,
const std::size_t nu);
110 void addCost(
const std::string& name, boost::shared_ptr<CostModelAbstract> cost,
const Scalar weight,
111 const bool active =
true);
118 void removeCost(
const std::string& name);
126 void changeCostStatus(
const std::string& name,
const bool active);
135 void calc(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x,
136 const Eigen::Ref<const VectorXs>& u);
145 void calcDiff(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x,
146 const Eigen::Ref<const VectorXs>& u);
158 boost::shared_ptr<CostDataSum> createData(DataCollectorAbstract*
const data);
166 void calc(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x);
174 void calcDiff(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x);
179 const boost::shared_ptr<StateAbstract>& get_state()
const;
184 const CostModelContainer& get_costs()
const;
189 std::size_t get_nu()
const;
194 std::size_t get_nr()
const;
199 std::size_t get_nr_total()
const;
204 const std::vector<std::string>& get_active()
const;
209 const std::vector<std::string>& get_inactive()
const;
216 bool getCostStatus(
const std::string& name)
const;
221 template <
class Scalar>
222 friend std::ostream& operator<<(std::ostream& os, const CostModelSumTpl<Scalar>& model);
225 boost::shared_ptr<StateAbstract> state_;
226 CostModelContainer costs_;
229 std::size_t nr_total_;
230 std::vector<std::string> active_;
231 std::vector<std::string> inactive_;
235 template <
typename _Scalar>
237 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
239 typedef _Scalar Scalar;
243 typedef typename MathBase::VectorXs VectorXs;
244 typedef typename MathBase::MatrixXs MatrixXs;
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()),
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()) {
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)));
272 template <
class ActionData>
273 void shareMemory(ActionData*
const 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());
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; }
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()) +
")");
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()) +
")");
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()) +
")");
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()) +
")");
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()) +
")");
328 VectorXs Lx_internal;
329 VectorXs Lu_internal;
330 MatrixXs Lxx_internal;
331 MatrixXs Lxu_internal;
332 MatrixXs Luu_internal;
335 DataCollectorAbstract* shared;
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;
349 #include "crocoddyl/core/costs/cost-sum.hxx" 351 #endif // CROCODDYL_CORE_COSTS_COST_SUM_HPP_
Abstract class for cost models.
Abstract class for the state representation.
const CostModelContainer & get_costs() const
Return the stack of cost models.
Summation of individual cost models.