9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/cost-sum.hpp" 14 template <
typename Scalar>
15 CostModelSumTpl<Scalar>::CostModelSumTpl(boost::shared_ptr<StateMultibody> state,
const std::size_t& nu)
16 : state_(state), nu_(nu), nr_(0), nr_total_(0) {}
18 template <
typename Scalar>
19 CostModelSumTpl<Scalar>::CostModelSumTpl(boost::shared_ptr<StateMultibody> state)
20 : state_(state), nu_(state->get_nv()), nr_(0), nr_total_(0) {}
22 template <
typename Scalar>
23 CostModelSumTpl<Scalar>::~CostModelSumTpl() {}
25 template <
typename Scalar>
26 void CostModelSumTpl<Scalar>::addCost(
const std::string& name, boost::shared_ptr<CostModelAbstract> cost,
27 const Scalar& weight,
bool active) {
28 if (cost->get_nu() != nu_) {
29 throw_pretty(
"Cost item doesn't have the same control dimension (it should be " + std::to_string(nu_) +
")");
31 std::pair<typename CostModelContainer::iterator, bool> ret =
32 costs_.insert(std::make_pair(name, boost::make_shared<CostItem>(name, cost, weight, active)));
33 if (ret.second ==
false) {
34 std::cout <<
"Warning: this cost item already existed, we cannot add it" << std::endl;
36 nr_ += cost->get_activation()->get_nr();
37 nr_total_ += cost->get_activation()->get_nr();
39 nr_total_ += cost->get_activation()->get_nr();
43 template <
typename Scalar>
44 void CostModelSumTpl<Scalar>::removeCost(
const std::string& name) {
45 typename CostModelContainer::iterator it = costs_.find(name);
46 if (it != costs_.end()) {
47 nr_ -= it->second->cost->get_activation()->get_nr();
48 nr_total_ -= it->second->cost->get_activation()->get_nr();
51 std::cout <<
"Warning: this cost item doesn't exist, we cannot remove it" << std::endl;
55 template <
typename Scalar>
56 void CostModelSumTpl<Scalar>::changeCostStatus(
const std::string& name,
bool active) {
57 typename CostModelContainer::iterator it = costs_.find(name);
58 if (it != costs_.end()) {
59 if (active && !it->second->active) {
60 nr_ += it->second->cost->get_activation()->get_nr();
61 }
else if (!active && it->second->active) {
62 nr_ -= it->second->cost->get_activation()->get_nr();
64 it->second->active = active;
66 std::cout <<
"Warning: this cost item doesn't exist, we cannot change its status" << std::endl;
70 template <
typename Scalar>
71 void CostModelSumTpl<Scalar>::calc(
const boost::shared_ptr<CostDataSumTpl<Scalar> >& data,
72 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u) {
73 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
74 throw_pretty(
"Invalid argument: " 75 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
77 if (static_cast<std::size_t>(u.size()) != nu_) {
78 throw_pretty(
"Invalid argument: " 79 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
81 if (data->costs.size() != costs_.size()) {
82 throw_pretty(
"Invalid argument: " 83 <<
"it doesn't match the number of cost datas and models");
87 typename CostModelContainer::iterator it_m, end_m;
88 typename CostDataContainer::iterator it_d, end_d;
89 for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(), end_d = data->costs.end();
90 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
91 const boost::shared_ptr<CostItem>& m_i = it_m->second;
93 const boost::shared_ptr<CostDataAbstract>& d_i = it_d->second;
94 assert_pretty(it_m->first == it_d->first,
"it doesn't match the cost name between data and model");
96 m_i->cost->calc(d_i, x, u);
97 data->cost += m_i->weight * d_i->cost;
102 template <
typename Scalar>
103 void CostModelSumTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataSumTpl<Scalar> >& data,
104 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u) {
105 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
106 throw_pretty(
"Invalid argument: " 107 <<
"x has wrong dimension (it should be " + std::to_string(state_->get_nx()) +
")");
109 if (static_cast<std::size_t>(u.size()) != nu_) {
110 throw_pretty(
"Invalid argument: " 111 <<
"u has wrong dimension (it should be " + std::to_string(nu_) +
")");
113 if (data->costs.size() != costs_.size()) {
114 throw_pretty(
"Invalid argument: " 115 <<
"it doesn't match the number of cost datas and models");
123 typename CostModelContainer::iterator it_m, end_m;
124 typename CostDataContainer::iterator it_d, end_d;
125 for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(), end_d = data->costs.end();
126 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
127 const boost::shared_ptr<CostItem>& m_i = it_m->second;
129 const boost::shared_ptr<CostDataAbstract>& d_i = it_d->second;
130 assert_pretty(it_m->first == it_d->first,
"it doesn't match the cost name between data and model");
132 m_i->cost->calcDiff(d_i, x, u);
133 data->Lx += m_i->weight * d_i->Lx;
134 data->Lu += m_i->weight * d_i->Lu;
135 data->Lxx += m_i->weight * d_i->Lxx;
136 data->Lxu += m_i->weight * d_i->Lxu;
137 data->Luu += m_i->weight * d_i->Luu;
142 template <
typename Scalar>
143 boost::shared_ptr<CostDataSumTpl<Scalar> > CostModelSumTpl<Scalar>::createData(DataCollectorAbstract*
const data) {
144 return boost::make_shared<CostDataSumTpl<Scalar> >(
this, data);
147 template <
typename Scalar>
148 void CostModelSumTpl<Scalar>::calc(
const boost::shared_ptr<CostDataSumTpl<Scalar> >& data,
149 const Eigen::Ref<const VectorXs>& x) {
150 calc(data, x, unone_);
153 template <
typename Scalar>
154 void CostModelSumTpl<Scalar>::calcDiff(
const boost::shared_ptr<CostDataSumTpl<Scalar> >& data,
155 const Eigen::Ref<const VectorXs>& x) {
156 calcDiff(data, x, unone_);
159 template <
typename Scalar>
160 const boost::shared_ptr<StateMultibodyTpl<Scalar> >& CostModelSumTpl<Scalar>::get_state()
const {
164 template <
typename Scalar>
165 const typename CostModelSumTpl<Scalar>::CostModelContainer& CostModelSumTpl<Scalar>::get_costs()
const {
169 template <
typename Scalar>
170 const std::size_t& CostModelSumTpl<Scalar>::get_nu()
const {
174 template <
typename Scalar>
175 const std::size_t& CostModelSumTpl<Scalar>::get_nr()
const {
179 template <
typename Scalar>
180 const std::size_t& CostModelSumTpl<Scalar>::get_nr_total()
const {
Definition: action-base.hxx:11