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,
32 : name(name), cost(cost), weight(weight), active(active) {}
35 boost::shared_ptr<CostModelAbstract> cost;
56 template <
typename _Scalar>
59 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
61 typedef _Scalar Scalar;
69 typedef typename MathBase::VectorXs VectorXs;
70 typedef typename MathBase::MatrixXs MatrixXs;
72 typedef std::map<std::string, boost::shared_ptr<CostItem> > CostModelContainer;
73 typedef std::map<std::string, boost::shared_ptr<CostDataAbstract> > CostDataContainer;
81 CostModelSumTpl(boost::shared_ptr<StateAbstract> state,
const std::size_t& nu);
101 void addCost(
const std::string& name, boost::shared_ptr<CostModelAbstract> cost,
const Scalar& weight,
126 void calc(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x,
127 const Eigen::Ref<const VectorXs>& u);
136 void calcDiff(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x,
137 const Eigen::Ref<const VectorXs>& u);
157 void calc(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x);
165 void calcDiff(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x);
170 const boost::shared_ptr<StateAbstract>&
get_state()
const;
175 const CostModelContainer&
get_costs()
const;
180 const std::size_t&
get_nu()
const;
185 const std::size_t&
get_nr()
const;
195 const std::vector<std::string>&
get_active()
const;
210 boost::shared_ptr<StateAbstract> state_;
211 CostModelContainer costs_;
214 std::size_t nr_total_;
215 std::vector<std::string> active_;
216 std::vector<std::string> inactive_;
220 template <
typename _Scalar>
222 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
224 typedef _Scalar Scalar;
228 typedef typename MathBase::VectorXs VectorXs;
229 typedef typename MathBase::MatrixXs MatrixXs;
231 template <
template <
typename Scalar>
class Model>
233 : Lx_internal(model->get_state()->get_ndx()),
234 Lu_internal(model->get_nu()),
235 Lxx_internal(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
236 Lxu_internal(model->get_state()->get_ndx(), model->get_nu()),
237 Luu_internal(model->get_nu(), model->get_nu()),
240 Lx(Lx_internal.data(), model->get_state()->get_ndx()),
241 Lu(Lu_internal.data(), model->get_nu()),
242 Lxx(Lxx_internal.data(), model->get_state()->get_ndx(), model->get_state()->get_ndx()),
243 Lxu(Lxu_internal.data(), model->get_state()->get_ndx(), model->get_nu()),
244 Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) {
251 it != model->get_costs().end(); ++it) {
252 const boost::shared_ptr<CostItem>& item = it->second;
253 costs.insert(std::make_pair(item->name, item->cost->createData(data)));
257 template <
class ActionData>
258 void shareMemory(ActionData*
const data) {
260 new (&Lx) Eigen::Map<VectorXs>(data->Lx.data(), data->Lx.size());
261 new (&Lu) Eigen::Map<VectorXs>(data->Lu.data(), data->Lu.size());
262 new (&Lxx) Eigen::Map<MatrixXs>(data->Lxx.data(), data->Lxx.rows(), data->Lxx.cols());
263 new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(), data->Lxu.cols());
264 new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(), data->Luu.cols());
267 VectorXs get_Lx()
const {
return Lx; }
268 VectorXs get_Lu()
const {
return Lu; }
269 MatrixXs get_Lxx()
const {
return Lxx; }
270 MatrixXs get_Lxu()
const {
return Lxu; }
271 MatrixXs get_Luu()
const {
return Luu; }
273 void set_Lx(
const VectorXs& _Lx) {
274 if (Lx.size() != _Lx.size()) {
275 throw_pretty(
"Invalid argument: "
276 <<
"Lx has wrong dimension (it should be " + std::to_string(Lx.size()) +
")");
280 void set_Lu(
const VectorXs& _Lu) {
281 if (Lu.size() != _Lu.size()) {
282 throw_pretty(
"Invalid argument: "
283 <<
"Lu has wrong dimension (it should be " + std::to_string(Lu.size()) +
")");
287 void set_Lxx(
const MatrixXs& _Lxx) {
288 if (Lxx.rows() != _Lxx.rows() || Lxx.cols() != _Lxx.cols()) {
289 throw_pretty(
"Invalid argument: "
290 <<
"Lxx has wrong dimension (it should be " + std::to_string(Lxx.rows()) +
", " +
291 std::to_string(Lxx.cols()) +
")");
295 void set_Lxu(
const MatrixXs& _Lxu) {
296 if (Lxu.rows() != _Lxu.rows() || Lxu.cols() != _Lxu.cols()) {
297 throw_pretty(
"Invalid argument: "
298 <<
"Lxu has wrong dimension (it should be " + std::to_string(Lxu.rows()) +
", " +
299 std::to_string(Lxu.cols()) +
")");
303 void set_Luu(
const MatrixXs& _Luu) {
304 if (Luu.rows() != _Luu.rows() || Luu.cols() != _Luu.cols()) {
305 throw_pretty(
"Invalid argument: "
306 <<
"Luu has wrong dimension (it should be " + std::to_string(Luu.rows()) +
", " +
307 std::to_string(Luu.cols()) +
")");
313 VectorXs Lx_internal;
314 VectorXs Lu_internal;
315 MatrixXs Lxx_internal;
316 MatrixXs Lxu_internal;
317 MatrixXs Luu_internal;
322 Eigen::Map<VectorXs> Lx;
323 Eigen::Map<VectorXs> Lu;
324 Eigen::Map<MatrixXs> Lxx;
325 Eigen::Map<MatrixXs> Lxu;
326 Eigen::Map<MatrixXs> Luu;
334 #include "crocoddyl/core/costs/cost-sum.hxx"
336 #endif // CROCODDYL_CORE_COSTS_COST_SUM_HPP_