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) {}
38 os <<
"{w=" << model.weight <<
", " << *model.cost <<
"}";
43 boost::shared_ptr<CostModelAbstract> cost;
64 template <
typename _Scalar>
67 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69 typedef _Scalar Scalar;
77 typedef typename MathBase::VectorXs VectorXs;
78 typedef typename MathBase::MatrixXs MatrixXs;
80 typedef std::map<std::string, boost::shared_ptr<CostItem> > CostModelContainer;
81 typedef std::map<std::string, boost::shared_ptr<CostDataAbstract> > CostDataContainer;
109 void addCost(
const std::string& name, boost::shared_ptr<CostModelAbstract> cost,
const Scalar weight,
110 const bool active =
true);
134 void calc(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x,
135 const Eigen::Ref<const VectorXs>& u);
144 void calcDiff(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x,
145 const Eigen::Ref<const VectorXs>& u);
165 void calc(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x);
173 void calcDiff(
const boost::shared_ptr<CostDataSum>& data,
const Eigen::Ref<const VectorXs>& x);
178 const boost::shared_ptr<StateAbstract>&
get_state()
const;
220 template <
class Scalar>
224 boost::shared_ptr<StateAbstract> state_;
225 CostModelContainer costs_;
228 std::size_t nr_total_;
229 std::vector<std::string> active_;
230 std::vector<std::string> inactive_;
234 template <
typename _Scalar>
236 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
238 typedef _Scalar Scalar;
242 typedef typename MathBase::VectorXs VectorXs;
243 typedef typename MathBase::MatrixXs MatrixXs;
245 template <
template <
typename Scalar>
class Model>
247 : Lx_internal(model->get_state()->get_ndx()),
248 Lu_internal(model->get_nu()),
249 Lxx_internal(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
250 Lxu_internal(model->get_state()->get_ndx(), model->get_nu()),
251 Luu_internal(model->get_nu(), model->get_nu()),
254 Lx(Lx_internal.data(), model->get_state()->get_ndx()),
255 Lu(Lu_internal.data(), model->get_nu()),
256 Lxx(Lxx_internal.data(), model->get_state()->get_ndx(), model->get_state()->get_ndx()),
257 Lxu(Lxu_internal.data(), model->get_state()->get_ndx(), model->get_nu()),
258 Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) {
265 it != model->get_costs().end(); ++it) {
266 const boost::shared_ptr<CostItem>& item = it->second;
267 costs.insert(std::make_pair(item->name, item->cost->createData(data)));
271 template <
class ActionData>
272 void shareMemory(ActionData*
const data) {
274 new (&Lx) Eigen::Map<VectorXs>(data->Lx.data(), data->Lx.size());
275 new (&Lu) Eigen::Map<VectorXs>(data->Lu.data(), data->Lu.size());
276 new (&Lxx) Eigen::Map<MatrixXs>(data->Lxx.data(), data->Lxx.rows(), data->Lxx.cols());
277 new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(), data->Lxu.cols());
278 new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(), data->Luu.cols());
281 VectorXs get_Lx()
const {
return Lx; }
282 VectorXs get_Lu()
const {
return Lu; }
283 MatrixXs get_Lxx()
const {
return Lxx; }
284 MatrixXs get_Lxu()
const {
return Lxu; }
285 MatrixXs get_Luu()
const {
return Luu; }
287 void set_Lx(
const VectorXs& _Lx) {
288 if (Lx.size() != _Lx.size()) {
289 throw_pretty(
"Invalid argument: "
290 <<
"Lx has wrong dimension (it should be " + std::to_string(Lx.size()) +
")");
294 void set_Lu(
const VectorXs& _Lu) {
295 if (Lu.size() != _Lu.size()) {
296 throw_pretty(
"Invalid argument: "
297 <<
"Lu has wrong dimension (it should be " + std::to_string(Lu.size()) +
")");
301 void set_Lxx(
const MatrixXs& _Lxx) {
302 if (Lxx.rows() != _Lxx.rows() || Lxx.cols() != _Lxx.cols()) {
303 throw_pretty(
"Invalid argument: "
304 <<
"Lxx has wrong dimension (it should be " + std::to_string(Lxx.rows()) +
", " +
305 std::to_string(Lxx.cols()) +
")");
309 void set_Lxu(
const MatrixXs& _Lxu) {
310 if (Lxu.rows() != _Lxu.rows() || Lxu.cols() != _Lxu.cols()) {
311 throw_pretty(
"Invalid argument: "
312 <<
"Lxu has wrong dimension (it should be " + std::to_string(Lxu.rows()) +
", " +
313 std::to_string(Lxu.cols()) +
")");
317 void set_Luu(
const MatrixXs& _Luu) {
318 if (Luu.rows() != _Luu.rows() || Luu.cols() != _Luu.cols()) {
319 throw_pretty(
"Invalid argument: "
320 <<
"Luu has wrong dimension (it should be " + std::to_string(Luu.rows()) +
", " +
321 std::to_string(Luu.cols()) +
")");
327 VectorXs Lx_internal;
328 VectorXs Lu_internal;
329 MatrixXs Lxx_internal;
330 MatrixXs Lxu_internal;
331 MatrixXs Luu_internal;
336 Eigen::Map<VectorXs> Lx;
337 Eigen::Map<VectorXs> Lu;
338 Eigen::Map<MatrixXs> Lxx;
339 Eigen::Map<MatrixXs> Lxu;
340 Eigen::Map<MatrixXs> Luu;
348 #include "crocoddyl/core/costs/cost-sum.hxx"
Abstract class for cost models.
Summation of individual cost models.
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.
void removeCost(const std::string &name)
Remove a cost item.
const std::vector< std::string > & get_inactive() const
Return the names of the inactive costs.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
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.
boost::shared_ptr< CostDataSum > createData(DataCollectorAbstract *const data)
Create the cost data.
friend std::ostream & operator<<(std::ostream &os, const CostModelSumTpl< Scalar > &model)
Print information on the stack of costs.
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.
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.
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.
const CostModelContainer & get_costs() const
Return the stack of cost models.
bool getCostStatus(const std::string &name) const
Return the status of a given cost name.
const std::vector< std::string > & get_active() const
Return the names of the active 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.