Directory: | ./ |
---|---|
File: | include/crocoddyl/core/constraints/constraint-manager.hxx |
Date: | 2025-02-24 23:41:29 |
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 720 times.
✓ Branch 3 taken 1027 times.
|
1747 | if (constraint->get_T_constraint()) { |
61 | 720 | ng_T_ += constraint->get_ng(); | |
62 | 720 | 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 3 times.
✓ Branch 5 taken 4 times.
|
7 | if (it->second->constraint->get_T_constraint()) { |
114 | 3 | ng_T_ -= it->second->constraint->get_ng(); | |
115 | 3 | 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 3583 times.
✓ Branch 7 taken 9840 times.
✓ Branch 8 taken 3583 times.
✓ Branch 9 taken 16838 times.
|
20421 | if (m_i->active && m_i->constraint->get_T_constraint()) { |
209 | 3583 | const std::shared_ptr<ConstraintDataAbstract>& d_i = it_d->second; | |
210 |
1/18✗ Branch 3 not taken.
✓ Branch 4 taken 3583 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.
|
3583 | 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 3583 times.
✗ Branch 4 not taken.
|
3583 | m_i->constraint->calc(d_i, x); |
216 | 3583 | const std::size_t ng = m_i->constraint->get_ng(); | |
217 | 3583 | const std::size_t nh = m_i->constraint->get_nh(); | |
218 |
2/4✓ Branch 3 taken 3583 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3583 times.
✗ Branch 7 not taken.
|
3583 | data->g.segment(ng_i, ng) = d_i->g; |
219 |
2/4✓ Branch 3 taken 3583 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3583 times.
✗ Branch 7 not taken.
|
3583 | data->h.segment(nh_i, nh) = d_i->h; |
220 |
2/4✓ Branch 4 taken 3583 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3583 times.
✗ Branch 8 not taken.
|
3583 | lb_.segment(ng_i, ng) = m_i->constraint->get_lb(); |
221 |
2/4✓ Branch 4 taken 3583 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3583 times.
✗ Branch 8 not taken.
|
3583 | ub_.segment(ng_i, ng) = m_i->constraint->get_ub(); |
222 | 3583 | ng_i += ng; | |
223 | 3583 | 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 262 times.
✓ Branch 7 taken 440 times.
✓ Branch 8 taken 262 times.
✓ Branch 9 taken 748 times.
|
1010 | if (m_i->active && m_i->constraint->get_T_constraint()) { |
315 | 262 | const std::shared_ptr<ConstraintDataAbstract>& d_i = it_d->second; | |
316 |
1/18✗ Branch 3 not taken.
✓ Branch 4 taken 262 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.
|
262 | 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 262 times.
✗ Branch 4 not taken.
|
262 | m_i->constraint->calcDiff(d_i, x); |
322 | 262 | const std::size_t ng = m_i->constraint->get_ng(); | |
323 | 262 | const std::size_t nh = m_i->constraint->get_nh(); | |
324 |
2/4✓ Branch 3 taken 262 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 262 times.
✗ Branch 7 not taken.
|
262 | data->Gx.block(ng_i, 0, ng, ndx) = d_i->Gx; |
325 |
2/4✓ Branch 3 taken 262 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 262 times.
✗ Branch 7 not taken.
|
262 | data->Hx.block(nh_i, 0, nh, ndx) = d_i->Hx; |
326 | 262 | ng_i += ng; | |
327 | 262 | 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 |