GCC Code Coverage Report

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