Crocoddyl
 
Loading...
Searching...
No Matches
cost-sum.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5// Heriot-Watt University
6// Copyright note valid unless otherwise stated in individual files.
7// All rights reserved.
9
10#ifndef CROCODDYL_CORE_COSTS_COST_SUM_HPP_
11#define CROCODDYL_CORE_COSTS_COST_SUM_HPP_
12
13#include "crocoddyl/core/cost-base.hpp"
14#include "crocoddyl/core/fwd.hpp"
15
16namespace crocoddyl {
17
18template <typename _Scalar>
20 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
21
22 typedef _Scalar Scalar;
24
25 CostItemTpl() {}
26 CostItemTpl(const std::string& name, std::shared_ptr<CostModelAbstract> cost,
27 const Scalar weight, const bool active = true)
28 : name(name), cost(cost), weight(weight), active(active) {}
29
30 template <typename NewScalar>
31 CostItemTpl<NewScalar> cast() const {
32 typedef CostItemTpl<NewScalar> ReturnType;
33 ReturnType ret(name, cost->template cast<NewScalar>(),
34 scalar_cast<NewScalar>(weight), active);
35 return ret;
36 }
37
41 friend std::ostream& operator<<(std::ostream& os,
42 const CostItemTpl<Scalar>& model) {
43 os << "{w=" << model.weight << ", " << *model.cost << "}";
44 return os;
45 }
46
47 std::string name;
48 std::shared_ptr<CostModelAbstract> cost;
49 Scalar weight;
50 bool active;
51};
52
74template <typename _Scalar>
76 public:
77 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
78
79 typedef _Scalar Scalar;
87 typedef typename MathBase::VectorXs VectorXs;
88 typedef typename MathBase::MatrixXs MatrixXs;
89
90 typedef std::map<std::string, std::shared_ptr<CostItem> > CostModelContainer;
91 typedef std::map<std::string, std::shared_ptr<CostDataAbstract> >
92 CostDataContainer;
93
100 CostModelSumTpl(std::shared_ptr<StateAbstract> state, const std::size_t nu);
101
109 explicit CostModelSumTpl(std::shared_ptr<StateAbstract> state);
111
120 void addCost(const std::string& name, std::shared_ptr<CostModelAbstract> cost,
121 const Scalar weight, const bool active = true);
122
128 void addCost(const std::shared_ptr<CostItem>& cost_item);
129
135 void removeCost(const std::string& name);
136
143 void changeCostStatus(const std::string& name, const bool active);
144
152 void calc(const std::shared_ptr<CostDataSum>& data,
153 const Eigen::Ref<const VectorXs>& x,
154 const Eigen::Ref<const VectorXs>& u);
155
166 void calc(const std::shared_ptr<CostDataSum>& data,
167 const Eigen::Ref<const VectorXs>& x);
168
176 void calcDiff(const std::shared_ptr<CostDataSum>& data,
177 const Eigen::Ref<const VectorXs>& x,
178 const Eigen::Ref<const VectorXs>& u);
179
192 void calcDiff(const std::shared_ptr<CostDataSum>& data,
193 const Eigen::Ref<const VectorXs>& x);
194
206 std::shared_ptr<CostDataSum> createData(DataCollectorAbstract* const data);
207
217 template <typename NewScalar>
219
223 const std::shared_ptr<StateAbstract>& get_state() const;
224
228 const CostModelContainer& get_costs() const;
229
233 std::size_t get_nu() const;
234
238 std::size_t get_nr() const;
239
243 std::size_t get_nr_total() const;
244
248 const std::set<std::string>& get_active_set() const;
249
253 const std::set<std::string>& get_inactive_set() const;
254
255 DEPRECATED(
256 "get_active() is deprecated and will be replaced with get_active_set()",
257 const std::vector<std::string>& get_active() {
258 active_.clear();
259 active_.reserve(active_set_.size());
260 for (const auto& contact : active_set_) {
261 active_.push_back(contact);
262 }
263 return active_;
264 };)
265
266 DEPRECATED(
267 "get_inactive() is deprecated and will be replaced with "
268 "get_inactive_set()",
269 const std::vector<std::string>& get_inactive() {
270 inactive_.clear();
271 inactive_.reserve(inactive_set_.size());
272 for (const auto& contact : inactive_set_) {
273 inactive_.push_back(contact);
274 }
275 return inactive_;
276 };)
277
283 bool getCostStatus(const std::string& name) const;
284
288 template <class Scalar>
289 friend std::ostream& operator<<(std::ostream& os,
290 const CostModelSumTpl<Scalar>& model);
291
292 private:
293 std::shared_ptr<StateAbstract> state_;
294 CostModelContainer costs_;
295 std::size_t nu_;
296 std::size_t nr_;
297 std::size_t nr_total_;
298 std::set<std::string> active_set_;
299 std::set<std::string>
300 inactive_set_;
301
302 // Vector variants. These are to maintain the API compatibility for the
303 // deprecated syntax. These will be removed in future versions along with
304 // get_active() / get_inactive()
305 std::vector<std::string> active_;
306 std::vector<std::string> inactive_;
307};
308
309template <typename _Scalar>
311 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
312
313 typedef _Scalar Scalar;
317 typedef typename MathBase::VectorXs VectorXs;
318 typedef typename MathBase::MatrixXs MatrixXs;
319
320 template <template <typename Scalar> class Model>
321 CostDataSumTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
322 : Lx_internal(model->get_state()->get_ndx()),
323 Lu_internal(model->get_nu()),
324 Lxx_internal(model->get_state()->get_ndx(),
325 model->get_state()->get_ndx()),
326 Lxu_internal(model->get_state()->get_ndx(), model->get_nu()),
327 Luu_internal(model->get_nu(), model->get_nu()),
328 shared(data),
329 cost(Scalar(0.)),
330 Lx(Lx_internal.data(), model->get_state()->get_ndx()),
331 Lu(Lu_internal.data(), model->get_nu()),
332 Lxx(Lxx_internal.data(), model->get_state()->get_ndx(),
333 model->get_state()->get_ndx()),
334 Lxu(Lxu_internal.data(), model->get_state()->get_ndx(),
335 model->get_nu()),
336 Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) {
337 Lx.setZero();
338 Lu.setZero();
339 Lxx.setZero();
340 Lxu.setZero();
341 Luu.setZero();
343 it = model->get_costs().begin();
344 it != model->get_costs().end(); ++it) {
345 const std::shared_ptr<CostItem>& item = it->second;
346 costs.insert(std::make_pair(item->name, item->cost->createData(data)));
347 }
348 }
349
350 template <class ActionData>
351 void shareMemory(ActionData* const data) {
352 // Save memory by setting the internal variables with null dimension
353 Lx_internal.resize(0);
354 Lu_internal.resize(0);
355 Lxx_internal.resize(0, 0);
356 Lxu_internal.resize(0, 0);
357 Luu_internal.resize(0, 0);
358 // Share memory with the differential action data
359 new (&Lx) Eigen::Map<VectorXs>(data->Lx.data(), data->Lx.size());
360 new (&Lu) Eigen::Map<VectorXs>(data->Lu.data(), data->Lu.size());
361 new (&Lxx) Eigen::Map<MatrixXs>(data->Lxx.data(), data->Lxx.rows(),
362 data->Lxx.cols());
363 new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(),
364 data->Lxu.cols());
365 new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(),
366 data->Luu.cols());
367 }
368
369 VectorXs get_Lx() const { return Lx; }
370 VectorXs get_Lu() const { return Lu; }
371 MatrixXs get_Lxx() const { return Lxx; }
372 MatrixXs get_Lxu() const { return Lxu; }
373 MatrixXs get_Luu() const { return Luu; }
374
375 void set_Lx(const VectorXs& _Lx) {
376 if (Lx.size() != _Lx.size()) {
377 throw_pretty(
378 "Invalid argument: " << "Lx has wrong dimension (it should be " +
379 std::to_string(Lx.size()) + ")");
380 }
381 Lx = _Lx;
382 }
383 void set_Lu(const VectorXs& _Lu) {
384 if (Lu.size() != _Lu.size()) {
385 throw_pretty(
386 "Invalid argument: " << "Lu has wrong dimension (it should be " +
387 std::to_string(Lu.size()) + ")");
388 }
389 Lu = _Lu;
390 }
391 void set_Lxx(const MatrixXs& _Lxx) {
392 if (Lxx.rows() != _Lxx.rows() || Lxx.cols() != _Lxx.cols()) {
393 throw_pretty(
394 "Invalid argument: " << "Lxx has wrong dimension (it should be " +
395 std::to_string(Lxx.rows()) + ", " +
396 std::to_string(Lxx.cols()) + ")");
397 }
398 Lxx = _Lxx;
399 }
400 void set_Lxu(const MatrixXs& _Lxu) {
401 if (Lxu.rows() != _Lxu.rows() || Lxu.cols() != _Lxu.cols()) {
402 throw_pretty(
403 "Invalid argument: " << "Lxu has wrong dimension (it should be " +
404 std::to_string(Lxu.rows()) + ", " +
405 std::to_string(Lxu.cols()) + ")");
406 }
407 Lxu = _Lxu;
408 }
409 void set_Luu(const MatrixXs& _Luu) {
410 if (Luu.rows() != _Luu.rows() || Luu.cols() != _Luu.cols()) {
411 throw_pretty(
412 "Invalid argument: " << "Luu has wrong dimension (it should be " +
413 std::to_string(Luu.rows()) + ", " +
414 std::to_string(Luu.cols()) + ")");
415 }
416 Luu = _Luu;
417 }
418
419 // Creates internal data in case we don't share it externally
420 VectorXs Lx_internal;
421 VectorXs Lu_internal;
422 MatrixXs Lxx_internal;
423 MatrixXs Lxu_internal;
424 MatrixXs Luu_internal;
425
426 typename CostModelSumTpl<Scalar>::CostDataContainer costs;
427 DataCollectorAbstract* shared;
428 Scalar cost;
429 Eigen::Map<VectorXs> Lx;
430 Eigen::Map<VectorXs> Lu;
431 Eigen::Map<MatrixXs> Lxx;
432 Eigen::Map<MatrixXs> Lxu;
433 Eigen::Map<MatrixXs> Luu;
434};
435
436} // namespace crocoddyl
437
438/* --- Details -------------------------------------------------------------- */
439/* --- Details -------------------------------------------------------------- */
440/* --- Details -------------------------------------------------------------- */
441#include "crocoddyl/core/costs/cost-sum.hxx"
442
443CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::CostItemTpl)
444CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::CostModelSumTpl)
445CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::CostDataSumTpl)
446
447#endif // CROCODDYL_CORE_COSTS_COST_SUM_HPP_
Abstract class for cost models.
Definition cost-base.hpp:64
Summation of individual cost models.
Definition cost-sum.hpp:75
void calcDiff(const std::shared_ptr< CostDataSum > &data, const Eigen::Ref< const VectorXs > &x)
Compute the Jacobian and Hessian of the total cost for nodes that depends on the state only.
void removeCost(const std::string &name)
Remove a cost item.
void addCost(const std::string &name, std::shared_ptr< CostModelAbstract > cost, const Scalar weight, const bool active=true)
Add a cost item.
const std::set< std::string > & get_active_set() const
Return the names of the set of active costs.
const std::set< std::string > & get_inactive_set() const
Return the names of the set of inactive costs.
void calc(const std::shared_ptr< CostDataSum > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total cost value for nodes that depends only on the state.
void calc(const std::shared_ptr< CostDataSum > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the total cost value.
void changeCostStatus(const std::string &name, const bool active)
Change the cost status.
void addCost(const std::shared_ptr< CostItem > &cost_item)
Add a cost item.
CostModelSumTpl(std::shared_ptr< StateAbstract > state)
Initialize the cost-sum model.
CostModelSumTpl(std::shared_ptr< StateAbstract > state, const std::size_t nu)
Initialize the cost-sum model.
void calcDiff(const std::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.
std::shared_ptr< CostDataSum > createData(DataCollectorAbstract *const data)
Create the cost data.
const CostModelContainer & get_costs() const
Return the stack of cost models.
std::size_t get_nr_total() const
Return the dimension of the total residual vector.
const std::shared_ptr< StateAbstract > & get_state() const
Return the state.
CostModelSumTpl< NewScalar > cast() const
Cast the cost-sum model to a different scalar type.
std::size_t get_nr() const
Return the dimension of the active residual vector.
bool getCostStatus(const std::string &name) const
Return the status of a given cost name.
friend std::ostream & operator<<(std::ostream &os, const CostModelSumTpl< Scalar > &model)
Print information on the stack of costs.
std::size_t get_nu() const
Return the dimension of the control input.
Abstract class for the state representation.
friend std::ostream & operator<<(std::ostream &os, const CostItemTpl< Scalar > &model)
Print information on the cost item.
Definition cost-sum.hpp:41