GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/costs/cost-sum.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 151 179 84.4%
Branches: 137 530 25.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include <iostream>
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 1800 CostModelSumTpl<Scalar>::CostModelSumTpl(std::shared_ptr<StateAbstract> state,
16 const std::size_t nu)
17 1800 : state_(state), nu_(nu), nr_(0), nr_total_(0) {}
18
19 template <typename Scalar>
20 65 CostModelSumTpl<Scalar>::CostModelSumTpl(std::shared_ptr<StateAbstract> state)
21
1/2
✓ Branch 4 taken 65 times.
✗ Branch 5 not taken.
 65 : state_(state), nu_(state->get_nv()), nr_(0), nr_total_(0) {}
22
23 template <typename Scalar>
24 1867 CostModelSumTpl<Scalar>::~CostModelSumTpl() {}
25
26 template <typename Scalar>
27 6121 void CostModelSumTpl<Scalar>::addCost(const std::string& name,
28 std::shared_ptr<CostModelAbstract> cost,
29 const Scalar weight, const bool active) {
30
2/4
✓ Branch 2 taken 6121 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 6121 times.
 6121 if (cost->get_nu() != nu_) {
31 ✗ throw_pretty(
32 name
33 << " cost item doesn't have the same control dimension (it should be " +
34 std::to_string(nu_) + ")");
35 }
36 std::pair<typename CostModelContainer::iterator, bool> ret =
37
3/6
✓ Branch 1 taken 6121 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6121 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6121 times.
✗ Branch 8 not taken.
 6121 costs_.insert(std::make_pair(
38 name, std::make_shared<CostItem>(name, cost, weight, active)));
39
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 6118 times.
 6121 if (ret.second == false) {
40 std::cerr << "Warning: we couldn't add the " << name
41
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 3 << " cost item, it already existed." << std::endl;
42
2/2
✓ Branch 0 taken 6115 times.
✓ Branch 1 taken 3 times.
 6118 } else if (active) {
43
2/4
✓ Branch 2 taken 6115 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 6115 times.
✗ Branch 7 not taken.
 6115 nr_ += cost->get_activation()->get_nr();
44
2/4
✓ Branch 2 taken 6115 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 6115 times.
✗ Branch 7 not taken.
 6115 nr_total_ += cost->get_activation()->get_nr();
45
1/2
✓ Branch 1 taken 6115 times.
✗ Branch 2 not taken.
 6115 active_set_.insert(name);
46
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 3 } else if (!active) {
47
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 nr_total_ += cost->get_activation()->get_nr();
48
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 inactive_set_.insert(name);
49 }
50 6121 }
51
52 template <typename Scalar>
53 151 void CostModelSumTpl<Scalar>::removeCost(const std::string& name) {
54
1/2
✓ Branch 1 taken 151 times.
✗ Branch 2 not taken.
 151 typename CostModelContainer::iterator it = costs_.find(name);
55
2/2
✓ Branch 2 taken 148 times.
✓ Branch 3 taken 3 times.
 151 if (it != costs_.end()) {
56
2/4
✓ Branch 4 taken 148 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 148 times.
✗ Branch 9 not taken.
 148 nr_ -= it->second->cost->get_activation()->get_nr();
57
2/4
✓ Branch 4 taken 148 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 148 times.
✗ Branch 9 not taken.
 148 nr_total_ -= it->second->cost->get_activation()->get_nr();
58
1/2
✓ Branch 1 taken 148 times.
✗ Branch 2 not taken.
 148 costs_.erase(it);
59
1/2
✓ Branch 1 taken 148 times.
✗ Branch 2 not taken.
 148 active_set_.erase(name);
60
1/2
✓ Branch 1 taken 148 times.
✗ Branch 2 not taken.
 148 inactive_set_.erase(name);
61 } else {
62 std::cerr << "Warning: we couldn't remove the " << name
63
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 3 << " cost item, it doesn't exist." << std::endl;
64 }
65 151 }
66
67 template <typename Scalar>
68 21 void CostModelSumTpl<Scalar>::changeCostStatus(const std::string& name,
69 const bool active) {
70
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 typename CostModelContainer::iterator it = costs_.find(name);
71
2/2
✓ Branch 2 taken 18 times.
✓ Branch 3 taken 3 times.
 21 if (it != costs_.end()) {
72
5/6
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 15 times.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3 times.
✓ Branch 7 taken 15 times.
 18 if (active && !it->second->active) {
73
2/4
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 3 nr_ += it->second->cost->get_activation()->get_nr();
74
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 active_set_.insert(name);
75
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 inactive_set_.erase(name);
76 3 it->second->active = active;
77
3/6
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 15 } else if (!active && it->second->active) {
78
2/4
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 15 times.
✗ Branch 9 not taken.
 15 nr_ -= it->second->cost->get_activation()->get_nr();
79
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 active_set_.erase(name);
80
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 inactive_set_.insert(name);
81 15 it->second->active = active;
82 }
83 } else {
84 std::cerr << "Warning: we couldn't change the status of the " << name
85
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 3 << " cost item, it doesn't exist." << std::endl;
86 }
87 21 }
88
89 template <typename Scalar>
90 77996 void CostModelSumTpl<Scalar>::calc(const std::shared_ptr<CostDataSum>& data,
91 const Eigen::Ref<const VectorXs>& x,
92 const Eigen::Ref<const VectorXs>& u) {
93
2/4
✓ Branch 3 taken 77996 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 77996 times.
 77996 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
94 ✗ throw_pretty(
95 "Invalid argument: " << "x has wrong dimension (it should be " +
96 std::to_string(state_->get_nx()) + ")");
97 }
98
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 77996 times.
 77996 if (static_cast<std::size_t>(u.size()) != nu_) {
99 ✗ throw_pretty(
100 "Invalid argument: " << "u has wrong dimension (it should be " +
101 std::to_string(nu_) + ")");
102 }
103
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 77996 times.
 77996 if (data->costs.size() != costs_.size()) {
104 ✗ throw_pretty("Invalid argument: "
105 << "it doesn't match the number of cost datas and models");
106 }
107 77996 data->cost = Scalar(0.);
108
109 77996 typename CostModelContainer::iterator it_m, end_m;
110 77996 typename CostDataContainer::iterator it_d, end_d;
111 155992 for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
112 77996 end_d = data->costs.end();
113
5/6
✓ Branch 3 taken 77996 times.
✓ Branch 4 taken 353757 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 77996 times.
✓ Branch 8 taken 353757 times.
✓ Branch 9 taken 77996 times.
 431753 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
114 353757 const std::shared_ptr<CostItem>& m_i = it_m->second;
115
2/2
✓ Branch 1 taken 353745 times.
✓ Branch 2 taken 12 times.
 353757 if (m_i->active) {
116 353745 const std::shared_ptr<CostDataAbstract>& d_i = it_d->second;
117
1/18
✗ Branch 3 not taken.
✓ Branch 4 taken 353745 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
 353745 assert_pretty(it_m->first == it_d->first,
118 "it doesn't match the cost name between model and data ("
119 << it_m->first << " != " << it_d->first << ")");
120
121
1/2
✓ Branch 3 taken 353745 times.
✗ Branch 4 not taken.
 353745 m_i->cost->calc(d_i, x, u);
122 353745 data->cost += m_i->weight * d_i->cost;
123 }
124 }
125 77996 }
126
127 template <typename Scalar>
128 14190 void CostModelSumTpl<Scalar>::calc(const std::shared_ptr<CostDataSum>& data,
129 const Eigen::Ref<const VectorXs>& x) {
130
2/4
✓ Branch 3 taken 14190 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 14190 times.
 14190 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
131 ✗ throw_pretty(
132 "Invalid argument: " << "x has wrong dimension (it should be " +
133 std::to_string(state_->get_nx()) + ")");
134 }
135
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 14190 times.
 14190 if (data->costs.size() != costs_.size()) {
136 ✗ throw_pretty("Invalid argument: "
137 << "it doesn't match the number of cost datas and models");
138 }
139 14190 data->cost = Scalar(0.);
140
141 14190 typename CostModelContainer::iterator it_m, end_m;
142 14190 typename CostDataContainer::iterator it_d, end_d;
143 28380 for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
144 14190 end_d = data->costs.end();
145
5/6
✓ Branch 3 taken 14190 times.
✓ Branch 4 taken 69485 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 14190 times.
✓ Branch 8 taken 69485 times.
✓ Branch 9 taken 14190 times.
 83675 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
146 69485 const std::shared_ptr<CostItem>& m_i = it_m->second;
147
2/2
✓ Branch 1 taken 69479 times.
✓ Branch 2 taken 6 times.
 69485 if (m_i->active) {
148 69479 const std::shared_ptr<CostDataAbstract>& d_i = it_d->second;
149
1/18
✗ Branch 3 not taken.
✓ Branch 4 taken 69479 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
 69479 assert_pretty(it_m->first == it_d->first,
150 "it doesn't match the cost name between model and data ("
151 << it_m->first << " != " << it_d->first << ")");
152
153
1/2
✓ Branch 3 taken 69479 times.
✗ Branch 4 not taken.
 69479 m_i->cost->calc(d_i, x);
154 69479 data->cost += m_i->weight * d_i->cost;
155 }
156 }
157 14190 }
158
159 template <typename Scalar>
160 13856 void CostModelSumTpl<Scalar>::calcDiff(const std::shared_ptr<CostDataSum>& data,
161 const Eigen::Ref<const VectorXs>& x,
162 const Eigen::Ref<const VectorXs>& u) {
163
2/4
✓ Branch 3 taken 13856 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 13856 times.
 13856 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
164 ✗ throw_pretty(
165 "Invalid argument: " << "x has wrong dimension (it should be " +
166 std::to_string(state_->get_nx()) + ")");
167 }
168
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 13856 times.
 13856 if (static_cast<std::size_t>(u.size()) != nu_) {
169 ✗ throw_pretty(
170 "Invalid argument: " << "u has wrong dimension (it should be " +
171 std::to_string(nu_) + ")");
172 }
173
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 13856 times.
 13856 if (data->costs.size() != costs_.size()) {
174 ✗ throw_pretty("Invalid argument: "
175 << "it doesn't match the number of cost datas and models");
176 }
177
1/2
✓ Branch 2 taken 13856 times.
✗ Branch 3 not taken.
 13856 data->Lx.setZero();
178
1/2
✓ Branch 2 taken 13856 times.
✗ Branch 3 not taken.
 13856 data->Lu.setZero();
179
1/2
✓ Branch 2 taken 13856 times.
✗ Branch 3 not taken.
 13856 data->Lxx.setZero();
180
1/2
✓ Branch 2 taken 13856 times.
✗ Branch 3 not taken.
 13856 data->Lxu.setZero();
181
1/2
✓ Branch 2 taken 13856 times.
✗ Branch 3 not taken.
 13856 data->Luu.setZero();
182
183 13856 typename CostModelContainer::iterator it_m, end_m;
184 13856 typename CostDataContainer::iterator it_d, end_d;
185 27712 for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
186 13856 end_d = data->costs.end();
187
5/6
✓ Branch 3 taken 13856 times.
✓ Branch 4 taken 59884 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 13856 times.
✓ Branch 8 taken 59884 times.
✓ Branch 9 taken 13856 times.
 73740 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
188 59884 const std::shared_ptr<CostItem>& m_i = it_m->second;
189
2/2
✓ Branch 1 taken 59878 times.
✓ Branch 2 taken 6 times.
 59884 if (m_i->active) {
190 59878 const std::shared_ptr<CostDataAbstract>& d_i = it_d->second;
191
1/18
✗ Branch 3 not taken.
✓ Branch 4 taken 59878 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
 59878 assert_pretty(it_m->first == it_d->first,
192 "it doesn't match the cost name between model and data ("
193 << it_m->first << " != " << it_d->first << ")");
194
195
1/2
✓ Branch 3 taken 59878 times.
✗ Branch 4 not taken.
 59878 m_i->cost->calcDiff(d_i, x, u);
196
2/4
✓ Branch 3 taken 59878 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 59878 times.
✗ Branch 8 not taken.
 59878 data->Lx += m_i->weight * d_i->Lx;
197
2/4
✓ Branch 3 taken 59878 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 59878 times.
✗ Branch 8 not taken.
 59878 data->Lu += m_i->weight * d_i->Lu;
198
2/4
✓ Branch 3 taken 59878 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 59878 times.
✗ Branch 8 not taken.
 59878 data->Lxx += m_i->weight * d_i->Lxx;
199
2/4
✓ Branch 3 taken 59878 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 59878 times.
✗ Branch 8 not taken.
 59878 data->Lxu += m_i->weight * d_i->Lxu;
200
2/4
✓ Branch 3 taken 59878 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 59878 times.
✗ Branch 8 not taken.
 59878 data->Luu += m_i->weight * d_i->Luu;
201 }
202 }
203 13856 }
204
205 template <typename Scalar>
206 639 void CostModelSumTpl<Scalar>::calcDiff(const std::shared_ptr<CostDataSum>& data,
207 const Eigen::Ref<const VectorXs>& x) {
208
2/4
✓ Branch 3 taken 639 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 639 times.
 639 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
209 ✗ throw_pretty(
210 "Invalid argument: " << "x has wrong dimension (it should be " +
211 std::to_string(state_->get_nx()) + ")");
212 }
213
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 639 times.
 639 if (data->costs.size() != costs_.size()) {
214 ✗ throw_pretty("Invalid argument: "
215 << "it doesn't match the number of cost datas and models");
216 }
217
1/2
✓ Branch 2 taken 639 times.
✗ Branch 3 not taken.
 639 data->Lx.setZero();
218
1/2
✓ Branch 2 taken 639 times.
✗ Branch 3 not taken.
 639 data->Lxx.setZero();
219
220 639 typename CostModelContainer::iterator it_m, end_m;
221 639 typename CostDataContainer::iterator it_d, end_d;
222 1278 for (it_m = costs_.begin(), end_m = costs_.end(), it_d = data->costs.begin(),
223 639 end_d = data->costs.end();
224
5/6
✓ Branch 3 taken 639 times.
✓ Branch 4 taken 2858 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 639 times.
✓ Branch 8 taken 2858 times.
✓ Branch 9 taken 639 times.
 3497 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
225 2858 const std::shared_ptr<CostItem>& m_i = it_m->second;
226
2/2
✓ Branch 1 taken 2852 times.
✓ Branch 2 taken 6 times.
 2858 if (m_i->active) {
227 2852 const std::shared_ptr<CostDataAbstract>& d_i = it_d->second;
228
1/18
✗ Branch 3 not taken.
✓ Branch 4 taken 2852 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
 2852 assert_pretty(it_m->first == it_d->first,
229 "it doesn't match the cost name between model and data ("
230 << it_m->first << " != " << it_d->first << ")");
231
232
1/2
✓ Branch 3 taken 2852 times.
✗ Branch 4 not taken.
 2852 m_i->cost->calcDiff(d_i, x);
233
2/4
✓ Branch 3 taken 2852 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 2852 times.
✗ Branch 8 not taken.
 2852 data->Lx += m_i->weight * d_i->Lx;
234
2/4
✓ Branch 3 taken 2852 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 2852 times.
✗ Branch 8 not taken.
 2852 data->Lxx += m_i->weight * d_i->Lxx;
235 }
236 }
237 639 }
238
239 template <typename Scalar>
240 95880 std::shared_ptr<CostDataSumTpl<Scalar> > CostModelSumTpl<Scalar>::createData(
241 DataCollectorAbstract* const data) {
242 return std::allocate_shared<CostDataSum>(
243
1/2
✓ Branch 2 taken 95880 times.
✗ Branch 3 not taken.
 191760 Eigen::aligned_allocator<CostDataSum>(), this, data);
244 }
245
246 template <typename Scalar>
247 template <typename NewScalar>
248 6 CostModelSumTpl<NewScalar> CostModelSumTpl<Scalar>::cast() const {
249 typedef CostModelSumTpl<NewScalar> ReturnType;
250 typedef CostItemTpl<NewScalar> CostType;
251
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 6 ReturnType ret(state_->template cast<NewScalar>(), nu_);
252 6 typename CostModelContainer::const_iterator it_m, end_m;
253
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 6 times.
 6 for (it_m = costs_.begin(), end_m = costs_.end(); it_m != end_m; ++it_m) {
254 ✗ const std::string name = it_m->first;
255 ✗ const CostType& m_i = it_m->second->template cast<NewScalar>();
256 ✗ ret.addCost(name, m_i.cost, m_i.weight, m_i.active);
257 }
258 12 return ret;
259 }
260
261 template <typename Scalar>
262 const std::shared_ptr<StateAbstractTpl<Scalar> >&
263 768376 CostModelSumTpl<Scalar>::get_state() const {
264 768376 return state_;
265 }
266
267 template <typename Scalar>
268 const typename CostModelSumTpl<Scalar>::CostModelContainer&
269 626456 CostModelSumTpl<Scalar>::get_costs() const {
270 626456 return costs_;
271 }
272
273 template <typename Scalar>
274 768472 std::size_t CostModelSumTpl<Scalar>::get_nu() const {
275 768472 return nu_;
276 }
277
278 template <typename Scalar>
279 1511 std::size_t CostModelSumTpl<Scalar>::get_nr() const {
280 1511 return nr_;
281 }
282
283 template <typename Scalar>
284 15 std::size_t CostModelSumTpl<Scalar>::get_nr_total() const {
285 15 return nr_total_;
286 }
287
288 template <typename Scalar>
289 3 const std::set<std::string>& CostModelSumTpl<Scalar>::get_active_set() const {
290 3 return active_set_;
291 }
292
293 template <typename Scalar>
294 3 const std::set<std::string>& CostModelSumTpl<Scalar>::get_inactive_set() const {
295 3 return inactive_set_;
296 }
297
298 template <typename Scalar>
299 ✗ bool CostModelSumTpl<Scalar>::getCostStatus(const std::string& name) const {
300 ✗ typename CostModelContainer::const_iterator it = costs_.find(name);
301 ✗ if (it != costs_.end()) {
302 ✗ return it->second->active;
303 } else {
304 std::cerr << "Warning: we couldn't get the status of the " << name
305 ✗ << " cost item, it doesn't exist." << std::endl;
306 ✗ return false;
307 }
308 }
309
310 template <typename Scalar>
311 3 std::ostream& operator<<(std::ostream& os,
312 const CostModelSumTpl<Scalar>& model) {
313 3 const std::set<std::string>& active = model.get_active_set();
314 3 const std::set<std::string>& inactive = model.get_inactive_set();
315 3 os << "CostModelSum:" << std::endl;
316 3 os << " Active:" << std::endl;
317 3 for (std::set<std::string>::const_iterator it = active.begin();
318
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
 3 it != active.end(); ++it) {
319 const std::shared_ptr<typename CostModelSumTpl<Scalar>::CostItem>&
320 ✗ cost_item = model.get_costs().find(*it)->second;
321 ✗ if (it != --active.end()) {
322 ✗ os << " " << *it << ": " << *cost_item << std::endl;
323 } else {
324 ✗ os << " " << *it << ": " << *cost_item << std::endl;
325 }
326 }
327 3 os << " Inactive:" << std::endl;
328 3 for (std::set<std::string>::const_iterator it = inactive.begin();
329
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
 3 it != inactive.end(); ++it) {
330 const std::shared_ptr<typename CostModelSumTpl<Scalar>::CostItem>&
331 ✗ cost_item = model.get_costs().find(*it)->second;
332 ✗ if (it != --inactive.end()) {
333 ✗ os << " " << *it << ": " << *cost_item << std::endl;
334 } else {
335 ✗ os << " " << *it << ": " << *cost_item;
336 }
337 }
338 3 return os;
339 }
340
341 } // namespace crocoddyl
342