| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2020-2022, University of Edinburgh, Heriot-Watt University |
||
5 |
// Copyright note valid unless otherwise stated in individual files. |
||
6 |
// All rights reserved. |
||
7 |
/////////////////////////////////////////////////////////////////////////////// |
||
8 |
|||
9 |
#include <iostream> |
||
10 |
|||
11 |
#include "crocoddyl/core/utils/exception.hpp" |
||
12 |
|||
13 |
namespace crocoddyl { |
||
14 |
|||
15 |
template <typename Scalar> |
||
16 |
717 |
ConstraintModelManagerTpl<Scalar>::ConstraintModelManagerTpl( |
|
17 |
boost::shared_ptr<StateAbstract> state, const std::size_t nu) |
||
18 |
✓✗✓✗ |
717 |
: state_(state), lb_(0), ub_(0), nu_(nu), ng_(0), nh_(0) {} |
19 |
|||
20 |
template <typename Scalar> |
||
21 |
27 |
ConstraintModelManagerTpl<Scalar>::ConstraintModelManagerTpl( |
|
22 |
boost::shared_ptr<StateAbstract> state) |
||
23 |
✓✗✓✗ |
27 |
: state_(state), lb_(0), ub_(0), nu_(state->get_nv()), ng_(0), nh_(0) {} |
24 |
|||
25 |
template <typename Scalar> |
||
26 |
746 |
ConstraintModelManagerTpl<Scalar>::~ConstraintModelManagerTpl() {} |
|
27 |
|||
28 |
template <typename Scalar> |
||
29 |
2453 |
void ConstraintModelManagerTpl<Scalar>::addConstraint( |
|
30 |
const std::string& name, |
||
31 |
boost::shared_ptr<ConstraintModelAbstract> constraint, const bool active) { |
||
32 |
✗✓ | 2453 |
if (constraint->get_nu() != nu_) { |
33 |
throw_pretty(name << " constraint item doesn't have the same control " |
||
34 |
"dimension (it should be " + |
||
35 |
std::to_string(nu_) + ")"); |
||
36 |
} |
||
37 |
✓✗✓✗ ✓✗ |
2453 |
std::pair<typename ConstraintModelContainer::iterator, bool> ret = |
38 |
constraints_.insert(std::make_pair( |
||
39 |
name, boost::make_shared<ConstraintItem>(name, constraint, active))); |
||
40 |
✓✓ | 2453 |
if (ret.second == false) { |
41 |
std::cout << "Warning: we couldn't add the " << name |
||
42 |
✓✗✓✗ ✓✗✓✗ |
3 |
<< " constraint item, it already existed." << std::endl; |
43 |
✓✓ | 2450 |
} else if (active) { |
44 |
1747 |
ng_ += constraint->get_ng(); |
|
45 |
1747 |
nh_ += constraint->get_nh(); |
|
46 |
✓✗ | 1747 |
active_set_.insert(name); |
47 |
✓✗ | 1747 |
lb_.resize(ng_); |
48 |
✓✗ | 1747 |
ub_.resize(ng_); |
49 |
✓✗ | 703 |
} else if (!active) { |
50 |
✓✗ | 703 |
inactive_set_.insert(name); |
51 |
} |
||
52 |
2453 |
} |
|
53 |
|||
54 |
template <typename Scalar> |
||
55 |
6 |
void ConstraintModelManagerTpl<Scalar>::removeConstraint( |
|
56 |
const std::string& name) { |
||
57 |
✓✗ | 6 |
typename ConstraintModelContainer::iterator it = constraints_.find(name); |
58 |
✓✓ | 6 |
if (it != constraints_.end()) { |
59 |
3 |
ng_ -= it->second->constraint->get_ng(); |
|
60 |
3 |
nh_ -= it->second->constraint->get_nh(); |
|
61 |
✓✗ | 3 |
constraints_.erase(it); |
62 |
✓✗ | 3 |
inactive_set_.erase(name); |
63 |
✓✗ | 3 |
lb_.resize(ng_); |
64 |
✓✗ | 3 |
ub_.resize(ng_); |
65 |
} else { |
||
66 |
std::cout << "Warning: we couldn't remove the " << name |
||
67 |
✓✗✓✗ ✓✗✓✗ |
3 |
<< " constraint item, it doesn't exist." << std::endl; |
68 |
} |
||
69 |
6 |
} |
|
70 |
|||
71 |
template <typename Scalar> |
||
72 |
144305 |
void ConstraintModelManagerTpl<Scalar>::changeConstraintStatus( |
|
73 |
const std::string& name, bool active) { |
||
74 |
✓✗ | 144305 |
typename ConstraintModelContainer::iterator it = constraints_.find(name); |
75 |
✓✓ | 144305 |
if (it != constraints_.end()) { |
76 |
✓✓✓✓ ✓✓ |
144302 |
if (active && !it->second->active) { |
77 |
7 |
ng_ += it->second->constraint->get_ng(); |
|
78 |
7 |
nh_ += it->second->constraint->get_nh(); |
|
79 |
✓✗ | 7 |
active_set_.insert(name); |
80 |
✓✗ | 7 |
inactive_set_.erase(name); |
81 |
7 |
it->second->active = active; |
|
82 |
✓✗ | 7 |
lb_.resize(ng_); |
83 |
✓✗ | 7 |
ub_.resize(ng_); |
84 |
✓✓✓✓ ✓✓ |
144295 |
} else if (!active && it->second->active) { |
85 |
7 |
ng_ -= it->second->constraint->get_ng(); |
|
86 |
7 |
nh_ -= it->second->constraint->get_nh(); |
|
87 |
✓✗ | 7 |
active_set_.erase(name); |
88 |
✓✗ | 7 |
inactive_set_.insert(name); |
89 |
7 |
it->second->active = active; |
|
90 |
✓✗ | 7 |
lb_.resize(ng_); |
91 |
✓✗ | 7 |
ub_.resize(ng_); |
92 |
} |
||
93 |
} else { |
||
94 |
std::cout << "Warning: we couldn't change the status of the " << name |
||
95 |
✓✗✓✗ ✓✗✓✗ |
3 |
<< " constraint item, it doesn't exist." << std::endl; |
96 |
} |
||
97 |
144305 |
} |
|
98 |
|||
99 |
template <typename Scalar> |
||
100 |
39932 |
void ConstraintModelManagerTpl<Scalar>::calc( |
|
101 |
const boost::shared_ptr<ConstraintDataManager>& data, |
||
102 |
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { |
||
103 |
✓✗✗✓ |
39932 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
104 |
throw_pretty("Invalid argument: " |
||
105 |
<< "x has wrong dimension (it should be " + |
||
106 |
std::to_string(state_->get_nx()) + ")"); |
||
107 |
} |
||
108 |
✓✗✗✓ |
39932 |
if (static_cast<std::size_t>(u.size()) != nu_) { |
109 |
throw_pretty("Invalid argument: " |
||
110 |
<< "u has wrong dimension (it should be " + |
||
111 |
std::to_string(nu_) + ")"); |
||
112 |
} |
||
113 |
✗✓ | 39932 |
if (data->constraints.size() != constraints_.size()) { |
114 |
throw_pretty( |
||
115 |
"Invalid argument: " |
||
116 |
<< "it doesn't match the number of constraint datas and models"); |
||
117 |
} |
||
118 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
39932 |
assert_pretty(static_cast<std::size_t>(data->g.size()) == ng_, |
119 |
"the dimension of data.g doesn't correspond with ng=" << ng_); |
||
120 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
39932 |
assert_pretty(static_cast<std::size_t>(data->h.size()) == nh_, |
121 |
"the dimension of data.h doesn't correspond with nh=" << nh_); |
||
122 |
39932 |
std::size_t ng_i = 0; |
|
123 |
39932 |
std::size_t nh_i = 0; |
|
124 |
|||
125 |
39932 |
typename ConstraintModelContainer::iterator it_m, end_m; |
|
126 |
39932 |
typename ConstraintDataContainer::iterator it_d, end_d; |
|
127 |
220256 |
for (it_m = constraints_.begin(), end_m = constraints_.end(), |
|
128 |
39932 |
it_d = data->constraints.begin(), end_d = data->constraints.end(); |
|
129 |
✓✓✗✓ ✓✓ |
400580 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
130 |
180324 |
const boost::shared_ptr<ConstraintItem>& m_i = it_m->second; |
|
131 |
✓✓ | 180324 |
if (m_i->active) { |
132 |
116954 |
const boost::shared_ptr<ConstraintDataAbstract>& d_i = it_d->second; |
|
133 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
116954 |
assert_pretty( |
134 |
it_m->first == it_d->first, |
||
135 |
"it doesn't match the constraint name between model and data (" |
||
136 |
<< it_m->first << " != " << it_d->first << ")"); |
||
137 |
|||
138 |
✓✗ | 116954 |
m_i->constraint->calc(d_i, x, u); |
139 |
116954 |
const std::size_t ng = m_i->constraint->get_ng(); |
|
140 |
116954 |
const std::size_t nh = m_i->constraint->get_nh(); |
|
141 |
✓✗✓✗ |
116954 |
data->g.segment(ng_i, ng) = d_i->g; |
142 |
✓✗✓✗ |
116954 |
data->h.segment(nh_i, nh) = d_i->h; |
143 |
✓✗✓✗ |
116954 |
lb_.segment(ng_i, ng) = m_i->constraint->get_lb(); |
144 |
✓✗✓✗ |
116954 |
ub_.segment(ng_i, ng) = m_i->constraint->get_ub(); |
145 |
116954 |
ng_i += ng; |
|
146 |
116954 |
nh_i += nh; |
|
147 |
} |
||
148 |
} |
||
149 |
39932 |
} |
|
150 |
|||
151 |
template <typename Scalar> |
||
152 |
4664 |
void ConstraintModelManagerTpl<Scalar>::calc( |
|
153 |
const boost::shared_ptr<ConstraintDataManager>& data, |
||
154 |
const Eigen::Ref<const VectorXs>& x) { |
||
155 |
✓✗✗✓ |
4664 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
156 |
throw_pretty("Invalid argument: " |
||
157 |
<< "x has wrong dimension (it should be " + |
||
158 |
std::to_string(state_->get_nx()) + ")"); |
||
159 |
} |
||
160 |
✗✓ | 4664 |
if (data->constraints.size() != constraints_.size()) { |
161 |
throw_pretty( |
||
162 |
"Invalid argument: " |
||
163 |
<< "it doesn't match the number of constraint datas and models"); |
||
164 |
} |
||
165 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
4664 |
assert_pretty(static_cast<std::size_t>(data->g.size()) == ng_, |
166 |
"the dimension of data.g doesn't correspond with ng=" << ng_); |
||
167 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
4664 |
assert_pretty(static_cast<std::size_t>(data->h.size()) == nh_, |
168 |
"the dimension of data.h doesn't correspond with nh=" << nh_); |
||
169 |
4664 |
std::size_t ng_i = 0; |
|
170 |
4664 |
std::size_t nh_i = 0; |
|
171 |
|||
172 |
4664 |
typename ConstraintModelContainer::iterator it_m, end_m; |
|
173 |
4664 |
typename ConstraintDataContainer::iterator it_d, end_d; |
|
174 |
25085 |
for (it_m = constraints_.begin(), end_m = constraints_.end(), |
|
175 |
4664 |
it_d = data->constraints.begin(), end_d = data->constraints.end(); |
|
176 |
✓✓✗✓ ✓✓ |
45506 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
177 |
20421 |
const boost::shared_ptr<ConstraintItem>& m_i = it_m->second; |
|
178 |
✓✓ | 20421 |
if (m_i->active) { |
179 |
13423 |
const boost::shared_ptr<ConstraintDataAbstract>& d_i = it_d->second; |
|
180 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
13423 |
assert_pretty( |
181 |
it_m->first == it_d->first, |
||
182 |
"it doesn't match the constraint name between model and data (" |
||
183 |
<< it_m->first << " != " << it_d->first << ")"); |
||
184 |
|||
185 |
✓✗ | 13423 |
m_i->constraint->calc(d_i, x); |
186 |
13423 |
const std::size_t ng = m_i->constraint->get_ng(); |
|
187 |
13423 |
const std::size_t nh = m_i->constraint->get_nh(); |
|
188 |
✓✗✓✗ |
13423 |
data->g.segment(ng_i, ng) = d_i->g; |
189 |
✓✗✓✗ |
13423 |
data->h.segment(nh_i, nh) = d_i->h; |
190 |
✓✗✓✗ |
13423 |
lb_.segment(ng_i, ng) = m_i->constraint->get_lb(); |
191 |
✓✗✓✗ |
13423 |
ub_.segment(ng_i, ng) = m_i->constraint->get_ub(); |
192 |
13423 |
ng_i += ng; |
|
193 |
13423 |
nh_i += nh; |
|
194 |
} |
||
195 |
} |
||
196 |
4664 |
} |
|
197 |
|||
198 |
template <typename Scalar> |
||
199 |
6995 |
void ConstraintModelManagerTpl<Scalar>::calcDiff( |
|
200 |
const boost::shared_ptr<ConstraintDataManager>& data, |
||
201 |
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { |
||
202 |
✓✗✗✓ |
6995 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
203 |
throw_pretty("Invalid argument: " |
||
204 |
<< "x has wrong dimension (it should be " + |
||
205 |
std::to_string(state_->get_nx()) + ")"); |
||
206 |
} |
||
207 |
✓✗✗✓ |
6995 |
if (static_cast<std::size_t>(u.size()) != nu_) { |
208 |
throw_pretty("Invalid argument: " |
||
209 |
<< "u has wrong dimension (it should be " + |
||
210 |
std::to_string(nu_) + ")"); |
||
211 |
} |
||
212 |
✗✓ | 6995 |
if (data->constraints.size() != constraints_.size()) { |
213 |
throw_pretty( |
||
214 |
"Invalid argument: " |
||
215 |
<< "it doesn't match the number of constraint datas and models"); |
||
216 |
} |
||
217 |
✓✓✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
6995 |
assert_pretty(static_cast<std::size_t>(data->Gx.rows()) == ng_, |
218 |
"the dimension of data.Gx doesn't correspond with ng=" << ng_); |
||
219 |
✓✓✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
6995 |
assert_pretty(static_cast<std::size_t>(data->Gu.rows()) == ng_, |
220 |
"the dimension of data.Gu doesn't correspond with ng=" << ng_); |
||
221 |
✓✓✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
6995 |
assert_pretty( |
222 |
static_cast<std::size_t>(data->Hx.rows()) == nh_, |
||
223 |
"the dimension of data.Hx,u doesn't correspond with nh=" << nh_); |
||
224 |
6995 |
const std::size_t ndx = state_->get_ndx(); |
|
225 |
6995 |
std::size_t ng_i = 0; |
|
226 |
6995 |
std::size_t nh_i = 0; |
|
227 |
|||
228 |
6995 |
typename ConstraintModelContainer::iterator it_m, end_m; |
|
229 |
6995 |
typename ConstraintDataContainer::iterator it_d, end_d; |
|
230 |
34721 |
for (it_m = constraints_.begin(), end_m = constraints_.end(), |
|
231 |
6995 |
it_d = data->constraints.begin(), end_d = data->constraints.end(); |
|
232 |
✓✓✗✓ ✓✓ |
62447 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
233 |
27726 |
const boost::shared_ptr<ConstraintItem>& m_i = it_m->second; |
|
234 |
✓✓ | 27726 |
if (m_i->active) { |
235 |
18948 |
const boost::shared_ptr<ConstraintDataAbstract>& d_i = it_d->second; |
|
236 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
18948 |
assert_pretty( |
237 |
it_m->first == it_d->first, |
||
238 |
"it doesn't match the constraint name between model and data (" |
||
239 |
<< it_m->first << " != " << it_d->first << ")"); |
||
240 |
|||
241 |
✓✗ | 18948 |
m_i->constraint->calcDiff(d_i, x, u); |
242 |
18948 |
const std::size_t ng = m_i->constraint->get_ng(); |
|
243 |
18948 |
const std::size_t nh = m_i->constraint->get_nh(); |
|
244 |
✓✗✓✗ |
18948 |
data->Gx.block(ng_i, 0, ng, ndx) = d_i->Gx; |
245 |
✓✗✓✗ |
18948 |
data->Gu.block(ng_i, 0, ng, nu_) = d_i->Gu; |
246 |
✓✗✓✗ |
18948 |
data->Hx.block(nh_i, 0, nh, ndx) = d_i->Hx; |
247 |
✓✗✓✗ |
18948 |
data->Hu.block(nh_i, 0, nh, nu_) = d_i->Hu; |
248 |
18948 |
ng_i += ng; |
|
249 |
18948 |
nh_i += nh; |
|
250 |
} |
||
251 |
} |
||
252 |
6995 |
} |
|
253 |
|||
254 |
template <typename Scalar> |
||
255 |
258 |
void ConstraintModelManagerTpl<Scalar>::calcDiff( |
|
256 |
const boost::shared_ptr<ConstraintDataManager>& data, |
||
257 |
const Eigen::Ref<const VectorXs>& x) { |
||
258 |
✓✗✗✓ |
258 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
259 |
throw_pretty("Invalid argument: " |
||
260 |
<< "x has wrong dimension (it should be " + |
||
261 |
std::to_string(state_->get_nx()) + ")"); |
||
262 |
} |
||
263 |
✗✓ | 258 |
if (data->constraints.size() != constraints_.size()) { |
264 |
throw_pretty( |
||
265 |
"Invalid argument: " |
||
266 |
<< "it doesn't match the number of constraint datas and models"); |
||
267 |
} |
||
268 |
✓✓✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
258 |
assert_pretty( |
269 |
static_cast<std::size_t>(data->Gx.rows()) == ng_, |
||
270 |
"the dimension of data.Gx,u doesn't correspond with ng=" << ng_); |
||
271 |
✓✓✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
258 |
assert_pretty( |
272 |
static_cast<std::size_t>(data->Hx.rows()) == nh_, |
||
273 |
"the dimension of data.Hx,u doesn't correspond with nh=" << nh_); |
||
274 |
258 |
const std::size_t ndx = state_->get_ndx(); |
|
275 |
258 |
std::size_t ng_i = 0; |
|
276 |
258 |
std::size_t nh_i = 0; |
|
277 |
|||
278 |
258 |
typename ConstraintModelContainer::iterator it_m, end_m; |
|
279 |
258 |
typename ConstraintDataContainer::iterator it_d, end_d; |
|
280 |
1268 |
for (it_m = constraints_.begin(), end_m = constraints_.end(), |
|
281 |
258 |
it_d = data->constraints.begin(), end_d = data->constraints.end(); |
|
282 |
✓✓✗✓ ✓✓ |
2278 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
283 |
1010 |
const boost::shared_ptr<ConstraintItem>& m_i = it_m->second; |
|
284 |
✓✓ | 1010 |
if (m_i->active) { |
285 |
702 |
const boost::shared_ptr<ConstraintDataAbstract>& d_i = it_d->second; |
|
286 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
702 |
assert_pretty( |
287 |
it_m->first == it_d->first, |
||
288 |
"it doesn't match the constraint name between model and data (" |
||
289 |
<< it_m->first << " != " << it_d->first << ")"); |
||
290 |
|||
291 |
✓✗ | 702 |
m_i->constraint->calcDiff(d_i, x); |
292 |
702 |
const std::size_t ng = m_i->constraint->get_ng(); |
|
293 |
702 |
const std::size_t nh = m_i->constraint->get_nh(); |
|
294 |
✓✗✓✗ |
702 |
data->Gx.block(ng_i, 0, ng, ndx) = d_i->Gx; |
295 |
✓✗✓✗ |
702 |
data->Hx.block(nh_i, 0, nh, ndx) = d_i->Hx; |
296 |
702 |
ng_i += ng; |
|
297 |
702 |
nh_i += nh; |
|
298 |
} |
||
299 |
} |
||
300 |
258 |
} |
|
301 |
|||
302 |
template <typename Scalar> |
||
303 |
boost::shared_ptr<ConstraintDataManagerTpl<Scalar> > |
||
304 |
50024 |
ConstraintModelManagerTpl<Scalar>::createData( |
|
305 |
DataCollectorAbstract* const data) { |
||
306 |
return boost::allocate_shared<ConstraintDataManager>( |
||
307 |
✓✗ | 50024 |
Eigen::aligned_allocator<ConstraintDataManager>(), this, data); |
308 |
} |
||
309 |
|||
310 |
template <typename Scalar> |
||
311 |
const boost::shared_ptr<StateAbstractTpl<Scalar> >& |
||
312 |
200385 |
ConstraintModelManagerTpl<Scalar>::get_state() const { |
|
313 |
200385 |
return state_; |
|
314 |
} |
||
315 |
|||
316 |
template <typename Scalar> |
||
317 |
const typename ConstraintModelManagerTpl<Scalar>::ConstraintModelContainer& |
||
318 |
320702 |
ConstraintModelManagerTpl<Scalar>::get_constraints() const { |
|
319 |
320702 |
return constraints_; |
|
320 |
} |
||
321 |
|||
322 |
template <typename Scalar> |
||
323 |
200329 |
std::size_t ConstraintModelManagerTpl<Scalar>::get_nu() const { |
|
324 |
200329 |
return nu_; |
|
325 |
} |
||
326 |
|||
327 |
template <typename Scalar> |
||
328 |
555276 |
std::size_t ConstraintModelManagerTpl<Scalar>::get_ng() const { |
|
329 |
555276 |
return ng_; |
|
330 |
} |
||
331 |
|||
332 |
template <typename Scalar> |
||
333 |
555276 |
std::size_t ConstraintModelManagerTpl<Scalar>::get_nh() const { |
|
334 |
555276 |
return nh_; |
|
335 |
} |
||
336 |
|||
337 |
template <typename Scalar> |
||
338 |
3 |
const std::set<std::string>& ConstraintModelManagerTpl<Scalar>::get_active_set() |
|
339 |
const { |
||
340 |
3 |
return active_set_; |
|
341 |
} |
||
342 |
|||
343 |
template <typename Scalar> |
||
344 |
const std::set<std::string>& |
||
345 |
3 |
ConstraintModelManagerTpl<Scalar>::get_inactive_set() const { |
|
346 |
3 |
return inactive_set_; |
|
347 |
} |
||
348 |
|||
349 |
template <typename Scalar> |
||
350 |
const typename MathBaseTpl<Scalar>::VectorXs& |
||
351 |
ConstraintModelManagerTpl<Scalar>::get_lb() const { |
||
352 |
return lb_; |
||
353 |
} |
||
354 |
|||
355 |
template <typename Scalar> |
||
356 |
const typename MathBaseTpl<Scalar>::VectorXs& |
||
357 |
ConstraintModelManagerTpl<Scalar>::get_ub() const { |
||
358 |
return ub_; |
||
359 |
} |
||
360 |
|||
361 |
template <typename Scalar> |
||
362 |
bool ConstraintModelManagerTpl<Scalar>::getConstraintStatus( |
||
363 |
const std::string& name) const { |
||
364 |
typename ConstraintModelContainer::const_iterator it = |
||
365 |
constraints_.find(name); |
||
366 |
if (it != constraints_.end()) { |
||
367 |
return it->second->active; |
||
368 |
} else { |
||
369 |
std::cout << "Warning: we couldn't get the status of the " << name |
||
370 |
<< " constraint item, it doesn't exist." << std::endl; |
||
371 |
return false; |
||
372 |
} |
||
373 |
} |
||
374 |
|||
375 |
template <typename Scalar> |
||
376 |
3 |
std::ostream& operator<<(std::ostream& os, |
|
377 |
const ConstraintModelManagerTpl<Scalar>& model) { |
||
378 |
3 |
const std::set<std::string>& active = model.get_active_set(); |
|
379 |
3 |
const std::set<std::string>& inactive = model.get_inactive_set(); |
|
380 |
3 |
os << "ConstraintModelManagerTpl:" << std::endl; |
|
381 |
3 |
os << " Active:" << std::endl; |
|
382 |
3 |
for (std::set<std::string>::const_iterator it = active.begin(); |
|
383 |
✗✓ | 3 |
it != active.end(); ++it) { |
384 |
const boost::shared_ptr< |
||
385 |
typename ConstraintModelManagerTpl<Scalar>::ConstraintItem>& |
||
386 |
constraint_item = model.get_constraints().find(*it)->second; |
||
387 |
if (it != --active.end()) { |
||
388 |
os << " " << *it << ": " << *constraint_item << std::endl; |
||
389 |
} else { |
||
390 |
os << " " << *it << ": " << *constraint_item << std::endl; |
||
391 |
} |
||
392 |
} |
||
393 |
3 |
os << " Inactive:" << std::endl; |
|
394 |
3 |
for (std::set<std::string>::const_iterator it = inactive.begin(); |
|
395 |
✗✓ | 3 |
it != inactive.end(); ++it) { |
396 |
const boost::shared_ptr< |
||
397 |
typename ConstraintModelManagerTpl<Scalar>::ConstraintItem>& |
||
398 |
constraint_item = model.get_constraints().find(*it)->second; |
||
399 |
if (it != --inactive.end()) { |
||
400 |
os << " " << *it << ": " << *constraint_item << std::endl; |
||
401 |
} else { |
||
402 |
os << " " << *it << ": " << *constraint_item; |
||
403 |
} |
||
404 |
} |
||
405 |
3 |
return os; |
|
406 |
} |
||
407 |
|||
408 |
} // namespace crocoddyl |
| Generated by: GCOVR (Version 4.2) |