GCC Code Coverage Report

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