77 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
79 typedef _Scalar Scalar;
87 typedef typename MathBase::VectorXs VectorXs;
88 typedef typename MathBase::MatrixXs MatrixXs;
90 typedef std::map<std::string, std::shared_ptr<CostItem> > CostModelContainer;
91 typedef std::map<std::string, std::shared_ptr<CostDataAbstract> >
120 void addCost(
const std::string& name, std::shared_ptr<CostModelAbstract> cost,
121 const Scalar weight,
const bool active =
true);
128 void addCost(
const std::shared_ptr<CostItem>& cost_item);
152 void calc(
const std::shared_ptr<CostDataSum>& data,
153 const Eigen::Ref<const VectorXs>& x,
154 const Eigen::Ref<const VectorXs>& u);
166 void calc(
const std::shared_ptr<CostDataSum>& data,
167 const Eigen::Ref<const VectorXs>& x);
176 void calcDiff(
const std::shared_ptr<CostDataSum>& data,
177 const Eigen::Ref<const VectorXs>& x,
178 const Eigen::Ref<const VectorXs>& u);
192 void calcDiff(
const std::shared_ptr<CostDataSum>& data,
193 const Eigen::Ref<const VectorXs>& x);
217 template <
typename NewScalar>
223 const std::shared_ptr<StateAbstract>&
get_state()
const;
256 "get_active() is deprecated and will be replaced with get_active_set()",
257 const std::vector<std::string>& get_active() {
259 active_.reserve(active_set_.size());
260 for (
const auto& contact : active_set_) {
261 active_.push_back(contact);
267 "get_inactive() is deprecated and will be replaced with "
268 "get_inactive_set()",
269 const std::vector<std::string>& get_inactive() {
271 inactive_.reserve(inactive_set_.size());
272 for (
const auto& contact : inactive_set_) {
273 inactive_.push_back(contact);
288 template <
class Scalar>
293 std::shared_ptr<StateAbstract> state_;
294 CostModelContainer costs_;
297 std::size_t nr_total_;
298 std::set<std::string> active_set_;
299 std::set<std::string>
305 std::vector<std::string> active_;
306 std::vector<std::string> inactive_;
311 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
313 typedef _Scalar Scalar;
317 typedef typename MathBase::VectorXs VectorXs;
318 typedef typename MathBase::MatrixXs MatrixXs;
320 template <
template <
typename Scalar>
class Model>
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()),
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(),
336 Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) {
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)));
350 template <
class ActionData>
351 void shareMemory(ActionData*
const data) {
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);
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(),
363 new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(),
365 new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(),
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; }
375 void set_Lx(
const VectorXs& _Lx) {
376 if (Lx.size() != _Lx.size()) {
378 "Invalid argument: " <<
"Lx has wrong dimension (it should be " +
379 std::to_string(Lx.size()) +
")");
383 void set_Lu(
const VectorXs& _Lu) {
384 if (Lu.size() != _Lu.size()) {
386 "Invalid argument: " <<
"Lu has wrong dimension (it should be " +
387 std::to_string(Lu.size()) +
")");
391 void set_Lxx(
const MatrixXs& _Lxx) {
392 if (Lxx.rows() != _Lxx.rows() || Lxx.cols() != _Lxx.cols()) {
394 "Invalid argument: " <<
"Lxx has wrong dimension (it should be " +
395 std::to_string(Lxx.rows()) +
", " +
396 std::to_string(Lxx.cols()) +
")");
400 void set_Lxu(
const MatrixXs& _Lxu) {
401 if (Lxu.rows() != _Lxu.rows() || Lxu.cols() != _Lxu.cols()) {
403 "Invalid argument: " <<
"Lxu has wrong dimension (it should be " +
404 std::to_string(Lxu.rows()) +
", " +
405 std::to_string(Lxu.cols()) +
")");
409 void set_Luu(
const MatrixXs& _Luu) {
410 if (Luu.rows() != _Luu.rows() || Luu.cols() != _Luu.cols()) {
412 "Invalid argument: " <<
"Luu has wrong dimension (it should be " +
413 std::to_string(Luu.rows()) +
", " +
414 std::to_string(Luu.cols()) +
")");
420 VectorXs Lx_internal;
421 VectorXs Lu_internal;
422 MatrixXs Lxx_internal;
423 MatrixXs Lxu_internal;
424 MatrixXs Luu_internal;
426 typename CostModelSumTpl<Scalar>::CostDataContainer costs;
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;