crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
cost-sum.hpp
1
2// 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
20namespace crocoddyl {
21
22template <typename _Scalar>
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 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
64template <typename _Scalar>
66 public:
67 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
68
69 typedef _Scalar Scalar;
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);
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
240template <typename _Scalar>
242 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
243
244 typedef _Scalar Scalar;
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();
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_
Abstract class for cost models.
Definition: cost-base.hpp:49
Summation of individual cost models.
Definition: cost-sum.hpp:65
void addCost(const std::string &name, boost::shared_ptr< CostModelAbstract > cost, const Scalar weight, const bool active=true)
Add a cost item.
void calcDiff(const boost::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.
CostModelSumTpl(boost::shared_ptr< StateAbstract > state)
Initialize the cost-sum model.
void changeCostStatus(const std::string &name, const bool active)
Change the cost status.
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.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
const std::vector< std::string > & get_inactive() const
Return the names of the inactive costs.
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.
void calc(const boost::shared_ptr< CostDataSum > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total cost value for nodes that depends only on the state.
const std::vector< std::string > & get_active() const
Return the names of the active costs.
std::size_t get_nr() const
Return the dimension of the active residual vector.
CostModelSumTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nu)
Initialize the cost-sum model.
boost::shared_ptr< CostDataSum > createData(DataCollectorAbstract *const data)
Create the cost data.
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.
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.
Definition: state-base.hpp:42
friend std::ostream & operator<<(std::ostream &os, const CostItemTpl< Scalar > &model)
Print information on the cost item.
Definition: cost-sum.hpp:37