9 #ifndef CROCODDYL_MULTIBODY_COSTS_COST_SUM_HPP_ 10 #define CROCODDYL_MULTIBODY_COSTS_COST_SUM_HPP_ 16 #include "crocoddyl/multibody/fwd.hpp" 17 #include "crocoddyl/core/utils/exception.hpp" 18 #include "crocoddyl/multibody/cost-base.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;
40 template <
typename _Scalar>
43 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 typedef _Scalar Scalar;
53 typedef typename MathBase::VectorXs VectorXs;
54 typedef typename MathBase::MatrixXs MatrixXs;
56 typedef std::map<std::string, boost::shared_ptr<CostItem> > CostModelContainer;
57 typedef std::map<std::string, boost::shared_ptr<CostDataAbstract> > CostDataContainer;
59 CostModelSumTpl(boost::shared_ptr<StateMultibody> state,
const std::size_t& nu);
63 void addCost(
const std::string& name, boost::shared_ptr<CostModelAbstract> cost,
const Scalar& weight,
65 void removeCost(
const std::string& name);
66 void changeCostStatus(
const std::string& name,
bool active);
69 const Eigen::Ref<const VectorXs>& u);
71 const Eigen::Ref<const VectorXs>& u);
72 boost::shared_ptr<CostDataSumTpl<Scalar> > createData(DataCollectorAbstract*
const data);
75 void calcDiff(
const boost::shared_ptr<
CostDataSumTpl<Scalar> >& data,
const Eigen::Ref<const VectorXs>& x);
77 const boost::shared_ptr<StateMultibody>& get_state()
const;
78 const CostModelContainer& get_costs()
const;
79 const std::size_t& get_nu()
const;
80 const std::size_t& get_nr()
const;
81 const std::size_t& get_nr_total()
const;
82 const std::vector<std::string>& get_active()
const;
83 const std::vector<std::string>& get_inactive()
const;
84 bool getCostStatus(
const std::string& name)
const;
87 boost::shared_ptr<StateMultibody> state_;
88 CostModelContainer costs_;
91 std::size_t nr_total_;
92 std::vector<std::string> active_;
93 std::vector<std::string> inactive_;
97 template <
typename _Scalar>
99 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
101 typedef _Scalar Scalar;
105 typedef typename MathBase::VectorXs VectorXs;
106 typedef typename MathBase::MatrixXs MatrixXs;
108 template <
template <
typename Scalar>
class Model>
109 CostDataSumTpl(Model<Scalar>*
const model, DataCollectorAbstract*
const data)
110 : Lx_internal(model->get_state()->get_ndx()),
111 Lu_internal(model->get_nu()),
112 Lxx_internal(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
113 Lxu_internal(model->get_state()->get_ndx(), model->get_nu()),
114 Luu_internal(model->get_nu(), model->get_nu()),
117 Lx(Lx_internal.data(), model->get_state()->get_ndx()),
118 Lu(Lu_internal.data(), model->get_nu()),
119 Lxx(Lxx_internal.data(), model->get_state()->get_ndx(), model->get_state()->get_ndx()),
120 Lxu(Lxu_internal.data(), model->get_state()->get_ndx(), model->get_nu()),
121 Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) {
128 it != model->get_costs().end(); ++it) {
129 const boost::shared_ptr<CostItem>& item = it->second;
130 costs.insert(std::make_pair(item->name, item->cost->createData(data)));
134 template <
class ActionData>
135 void shareMemory(ActionData*
const data) {
137 new (&Lx) Eigen::Map<VectorXs>(data->Lx.data(), data->Lx.size());
138 new (&Lu) Eigen::Map<VectorXs>(data->Lu.data(), data->Lu.size());
139 new (&Lxx) Eigen::Map<MatrixXs>(data->Lxx.data(), data->Lxx.rows(), data->Lxx.cols());
140 new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(), data->Lxu.cols());
141 new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(), data->Luu.cols());
144 VectorXs get_Lx()
const {
return Lx; }
145 VectorXs get_Lu()
const {
return Lu; }
146 MatrixXs get_Lxx()
const {
return Lxx; }
147 MatrixXs get_Lxu()
const {
return Lxu; }
148 MatrixXs get_Luu()
const {
return Luu; }
150 void set_Lx(
const VectorXs& _Lx) {
151 if (Lx.size() != _Lx.size()) {
152 throw_pretty(
"Invalid argument: " 153 <<
"Lx has wrong dimension (it should be " + std::to_string(Lx.size()) +
")");
157 void set_Lu(
const VectorXs& _Lu) {
158 if (Lu.size() != _Lu.size()) {
159 throw_pretty(
"Invalid argument: " 160 <<
"Lu has wrong dimension (it should be " + std::to_string(Lu.size()) +
")");
164 void set_Lxx(
const MatrixXs& _Lxx) {
165 if (Lxx.rows() != _Lxx.rows() || Lxx.cols() != _Lxx.cols()) {
166 throw_pretty(
"Invalid argument: " 167 <<
"Lxx has wrong dimension (it should be " + std::to_string(Lxx.rows()) +
", " +
168 std::to_string(Lxx.cols()) +
")");
172 void set_Lxu(
const MatrixXs& _Lxu) {
173 if (Lxu.rows() != _Lxu.rows() || Lxu.cols() != _Lxu.cols()) {
174 throw_pretty(
"Invalid argument: " 175 <<
"Lxu has wrong dimension (it should be " + std::to_string(Lxu.rows()) +
", " +
176 std::to_string(Lxu.cols()) +
")");
180 void set_Luu(
const MatrixXs& _Luu) {
181 if (Luu.rows() != _Luu.rows() || Luu.cols() != _Luu.cols()) {
182 throw_pretty(
"Invalid argument: " 183 <<
"Luu has wrong dimension (it should be " + std::to_string(Luu.rows()) +
", " +
184 std::to_string(Luu.cols()) +
")");
190 VectorXs Lx_internal;
191 VectorXs Lu_internal;
192 MatrixXs Lxx_internal;
193 MatrixXs Lxu_internal;
194 MatrixXs Luu_internal;
197 DataCollectorAbstract* shared;
199 Eigen::Map<VectorXs> Lx;
200 Eigen::Map<VectorXs> Lu;
201 Eigen::Map<MatrixXs> Lxx;
202 Eigen::Map<MatrixXs> Lxu;
203 Eigen::Map<MatrixXs> Luu;
211 #include "crocoddyl/multibody/costs/cost-sum.hxx" 213 #endif // CROCODDYL_MULTIBODY_COSTS_COST_SUM_HPP_