| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, |
||
5 |
// Heriot-Watt University |
||
6 |
// Copyright note valid unless otherwise stated in individual files. |
||
7 |
// All rights reserved. |
||
8 |
/////////////////////////////////////////////////////////////////////////////// |
||
9 |
|||
10 |
namespace crocoddyl { |
||
11 |
|||
12 |
template <typename Scalar> |
||
13 |
766 |
ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl( |
|
14 |
boost::shared_ptr<StateMultibody> state, const std::size_t nu) |
||
15 |
: state_(state), |
||
16 |
nc_(0), |
||
17 |
nc_total_(0), |
||
18 |
nu_(nu), |
||
19 |
766 |
compute_all_contacts_(false) {} |
|
20 |
|||
21 |
template <typename Scalar> |
||
22 |
18 |
ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl( |
|
23 |
boost::shared_ptr<StateMultibody> state) |
||
24 |
: state_(state), |
||
25 |
nc_(0), |
||
26 |
nc_total_(0), |
||
27 |
18 |
nu_(state->get_nv()), |
|
28 |
18 |
compute_all_contacts_(false) {} |
|
29 |
|||
30 |
template <typename Scalar> |
||
31 |
786 |
ContactModelMultipleTpl<Scalar>::~ContactModelMultipleTpl() {} |
|
32 |
|||
33 |
template <typename Scalar> |
||
34 |
1717 |
void ContactModelMultipleTpl<Scalar>::addContact( |
|
35 |
const std::string& name, boost::shared_ptr<ContactModelAbstract> contact, |
||
36 |
const bool active) { |
||
37 |
✗✓ | 1717 |
if (contact->get_nu() != nu_) { |
38 |
throw_pretty("Invalid argument: " |
||
39 |
<< name |
||
40 |
<< " contact item doesn't have the same control dimension (" + |
||
41 |
std::to_string(nu_) + ")"); |
||
42 |
} |
||
43 |
✓✗✓✗ ✓✗ |
1717 |
std::pair<typename ContactModelContainer::iterator, bool> ret = |
44 |
contacts_.insert(std::make_pair( |
||
45 |
name, boost::make_shared<ContactItem>(name, contact, active))); |
||
46 |
✓✓ | 1717 |
if (ret.second == false) { |
47 |
std::cerr << "Warning: we couldn't add the " << name |
||
48 |
✓✗✓✗ ✓✗✓✗ |
1 |
<< " contact item, it already existed." << std::endl; |
49 |
✓✓ | 1716 |
} else if (active) { |
50 |
1715 |
nc_ += contact->get_nc(); |
|
51 |
1715 |
nc_total_ += contact->get_nc(); |
|
52 |
✓✗ | 1715 |
active_set_.insert(name); |
53 |
✓✗ | 1 |
} else if (!active) { |
54 |
1 |
nc_total_ += contact->get_nc(); |
|
55 |
✓✗ | 1 |
inactive_set_.insert(name); |
56 |
} |
||
57 |
1717 |
} |
|
58 |
|||
59 |
template <typename Scalar> |
||
60 |
2 |
void ContactModelMultipleTpl<Scalar>::removeContact(const std::string& name) { |
|
61 |
✓✗ | 2 |
typename ContactModelContainer::iterator it = contacts_.find(name); |
62 |
✓✓ | 2 |
if (it != contacts_.end()) { |
63 |
1 |
nc_ -= it->second->contact->get_nc(); |
|
64 |
1 |
nc_total_ -= it->second->contact->get_nc(); |
|
65 |
✓✗ | 1 |
contacts_.erase(it); |
66 |
✓✗ | 1 |
active_set_.erase(name); |
67 |
✓✗ | 1 |
inactive_set_.erase(name); |
68 |
} else { |
||
69 |
std::cerr << "Warning: we couldn't remove the " << name |
||
70 |
✓✗✓✗ ✓✗✓✗ |
1 |
<< " contact item, it doesn't exist." << std::endl; |
71 |
} |
||
72 |
2 |
} |
|
73 |
|||
74 |
template <typename Scalar> |
||
75 |
17 |
void ContactModelMultipleTpl<Scalar>::changeContactStatus( |
|
76 |
const std::string& name, const bool active) { |
||
77 |
✓✗ | 17 |
typename ContactModelContainer::iterator it = contacts_.find(name); |
78 |
✓✓ | 17 |
if (it != contacts_.end()) { |
79 |
✓✓✓✗ ✓✓ |
16 |
if (active && !it->second->active) { |
80 |
1 |
nc_ += it->second->contact->get_nc(); |
|
81 |
✓✗ | 1 |
active_set_.insert(name); |
82 |
✓✗ | 1 |
inactive_set_.erase(name); |
83 |
✓✗✓✗ ✓✗ |
15 |
} else if (!active && it->second->active) { |
84 |
15 |
nc_ -= it->second->contact->get_nc(); |
|
85 |
✓✗ | 15 |
inactive_set_.insert(name); |
86 |
✓✗ | 15 |
active_set_.erase(name); |
87 |
} |
||
88 |
// "else" case: Contact status unchanged - already in desired state |
||
89 |
16 |
it->second->active = active; |
|
90 |
} else { |
||
91 |
std::cerr << "Warning: we couldn't change the status of the " << name |
||
92 |
✓✗✓✗ ✓✗✓✗ |
1 |
<< " contact item, it doesn't exist." << std::endl; |
93 |
} |
||
94 |
17 |
} |
|
95 |
|||
96 |
template <typename Scalar> |
||
97 |
62660 |
void ContactModelMultipleTpl<Scalar>::calc( |
|
98 |
const boost::shared_ptr<ContactDataMultiple>& data, |
||
99 |
const Eigen::Ref<const VectorXs>& x) { |
||
100 |
✗✓ | 62660 |
if (data->contacts.size() != contacts_.size()) { |
101 |
throw_pretty("Invalid argument: " |
||
102 |
<< "it doesn't match the number of contact datas and models"); |
||
103 |
} |
||
104 |
|||
105 |
62660 |
std::size_t nc = 0; |
|
106 |
62660 |
const std::size_t nv = state_->get_nv(); |
|
107 |
62660 |
typename ContactModelContainer::iterator it_m, end_m; |
|
108 |
62660 |
typename ContactDataContainer::iterator it_d, end_d; |
|
109 |
✓✓ | 62660 |
if (compute_all_contacts_) { |
110 |
89300 |
for (it_m = contacts_.begin(), end_m = contacts_.end(), |
|
111 |
25930 |
it_d = data->contacts.begin(), end_d = data->contacts.end(); |
|
112 |
✓✓✗✓ ✓✓ |
152670 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
113 |
63370 |
const boost::shared_ptr<ContactItem>& m_i = it_m->second; |
|
114 |
63370 |
const std::size_t nc_i = m_i->contact->get_nc(); |
|
115 |
✓✓ | 63370 |
if (m_i->active) { |
116 |
63366 |
const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second; |
|
117 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
63366 |
assert_pretty( |
118 |
it_m->first == it_d->first, |
||
119 |
"it doesn't match the contact name between model and data (" |
||
120 |
<< it_m->first << " != " << it_d->first << ")"); |
||
121 |
✓✗ | 63366 |
m_i->contact->calc(d_i, x); |
122 |
✓✗✓✗ |
63366 |
data->a0.segment(nc, nc_i) = d_i->a0; |
123 |
✓✗✓✗ |
63366 |
data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc; |
124 |
} else { |
||
125 |
✓✗✓✗ |
4 |
data->a0.segment(nc, nc_i).setZero(); |
126 |
✓✗✓✗ |
4 |
data->Jc.block(nc, 0, nc_i, nv).setZero(); |
127 |
} |
||
128 |
63370 |
nc += nc_i; |
|
129 |
} |
||
130 |
} else { |
||
131 |
116332 |
for (it_m = contacts_.begin(), end_m = contacts_.end(), |
|
132 |
36730 |
it_d = data->contacts.begin(), end_d = data->contacts.end(); |
|
133 |
✓✓✗✓ ✓✓ |
195934 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
134 |
79602 |
const boost::shared_ptr<ContactItem>& m_i = it_m->second; |
|
135 |
✓✓ | 79602 |
if (m_i->active) { |
136 |
79586 |
const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second; |
|
137 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
79586 |
assert_pretty( |
138 |
it_m->first == it_d->first, |
||
139 |
"it doesn't match the contact name between model and data (" |
||
140 |
<< it_m->first << " != " << it_d->first << ")"); |
||
141 |
|||
142 |
✓✗ | 79586 |
m_i->contact->calc(d_i, x); |
143 |
79586 |
const std::size_t nc_i = m_i->contact->get_nc(); |
|
144 |
✓✗✓✗ |
79586 |
data->a0.segment(nc, nc_i) = d_i->a0; |
145 |
✓✗✓✗ |
79586 |
data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc; |
146 |
79586 |
nc += nc_i; |
|
147 |
} |
||
148 |
} |
||
149 |
} |
||
150 |
62660 |
} |
|
151 |
|||
152 |
template <typename Scalar> |
||
153 |
8845 |
void ContactModelMultipleTpl<Scalar>::calcDiff( |
|
154 |
const boost::shared_ptr<ContactDataMultiple>& data, |
||
155 |
const Eigen::Ref<const VectorXs>& x) { |
||
156 |
✗✓ | 8845 |
if (data->contacts.size() != contacts_.size()) { |
157 |
throw_pretty("Invalid argument: " |
||
158 |
<< "it doesn't match the number of contact datas and models"); |
||
159 |
} |
||
160 |
|||
161 |
8845 |
std::size_t nc = 0; |
|
162 |
8845 |
const std::size_t ndx = state_->get_ndx(); |
|
163 |
8845 |
typename ContactModelContainer::iterator it_m, end_m; |
|
164 |
8845 |
typename ContactDataContainer::iterator it_d, end_d; |
|
165 |
✓✓ | 8845 |
if (compute_all_contacts_) { |
166 |
12540 |
for (it_m = contacts_.begin(), end_m = contacts_.end(), |
|
167 |
3762 |
it_d = data->contacts.begin(), end_d = data->contacts.end(); |
|
168 |
✓✓✗✓ ✓✓ |
21318 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
169 |
8778 |
const boost::shared_ptr<ContactItem>& m_i = it_m->second; |
|
170 |
8778 |
const std::size_t nc_i = m_i->contact->get_nc(); |
|
171 |
✓✗ | 8778 |
if (m_i->active) { |
172 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
8778 |
assert_pretty( |
173 |
it_m->first == it_d->first, |
||
174 |
"it doesn't match the contact name between model and data (" |
||
175 |
<< it_m->first << " != " << it_d->first << ")"); |
||
176 |
8778 |
const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second; |
|
177 |
|||
178 |
✓✗ | 8778 |
m_i->contact->calcDiff(d_i, x); |
179 |
✓✗✓✗ |
8778 |
data->da0_dx.block(nc, 0, nc_i, ndx) = d_i->da0_dx; |
180 |
} else { |
||
181 |
data->da0_dx.block(nc, 0, nc_i, ndx).setZero(); |
||
182 |
} |
||
183 |
8778 |
nc += nc_i; |
|
184 |
} |
||
185 |
} else { |
||
186 |
15303 |
for (it_m = contacts_.begin(), end_m = contacts_.end(), |
|
187 |
5083 |
it_d = data->contacts.begin(), end_d = data->contacts.end(); |
|
188 |
✓✓✗✓ ✓✓ |
25523 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
189 |
10220 |
const boost::shared_ptr<ContactItem>& m_i = it_m->second; |
|
190 |
✓✓ | 10220 |
if (m_i->active) { |
191 |
10216 |
const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second; |
|
192 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
10216 |
assert_pretty( |
193 |
it_m->first == it_d->first, |
||
194 |
"it doesn't match the contact name between model and data (" |
||
195 |
<< it_m->first << " != " << it_d->first << ")"); |
||
196 |
|||
197 |
✓✗ | 10216 |
m_i->contact->calcDiff(d_i, x); |
198 |
10216 |
const std::size_t nc_i = m_i->contact->get_nc(); |
|
199 |
✓✗✓✗ |
10216 |
data->da0_dx.block(nc, 0, nc_i, ndx) = d_i->da0_dx; |
200 |
10216 |
nc += nc_i; |
|
201 |
} |
||
202 |
} |
||
203 |
} |
||
204 |
8845 |
} |
|
205 |
|||
206 |
template <typename Scalar> |
||
207 |
32220 |
void ContactModelMultipleTpl<Scalar>::updateAcceleration( |
|
208 |
const boost::shared_ptr<ContactDataMultiple>& data, |
||
209 |
const VectorXs& dv) const { |
||
210 |
✗✓ | 32220 |
if (static_cast<std::size_t>(dv.size()) != state_->get_nv()) { |
211 |
throw_pretty("Invalid argument: " |
||
212 |
<< "dv has wrong dimension (it should be " + |
||
213 |
std::to_string(state_->get_nv()) + ")"); |
||
214 |
} |
||
215 |
32220 |
data->dv = dv; |
|
216 |
32220 |
} |
|
217 |
|||
218 |
template <typename Scalar> |
||
219 |
58151 |
void ContactModelMultipleTpl<Scalar>::updateForce( |
|
220 |
const boost::shared_ptr<ContactDataMultiple>& data, const VectorXs& force) { |
||
221 |
✓✗✗✓ |
116302 |
if (static_cast<std::size_t>(force.size()) != |
222 |
✓✓ | 58151 |
(compute_all_contacts_ ? nc_total_ : nc_)) { |
223 |
throw_pretty( |
||
224 |
"Invalid argument: " |
||
225 |
<< "force has wrong dimension (it should be " + |
||
226 |
std::to_string((compute_all_contacts_ ? nc_total_ : nc_)) + ")"); |
||
227 |
} |
||
228 |
✗✓ | 58151 |
if (static_cast<std::size_t>(data->contacts.size()) != contacts_.size()) { |
229 |
throw_pretty("Invalid argument: " |
||
230 |
<< "it doesn't match the number of contact datas and models"); |
||
231 |
} |
||
232 |
|||
233 |
✓✓ | 1330179 |
for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) { |
234 |
✓✗✓✗ |
1272028 |
*it = pinocchio::ForceTpl<Scalar>::Zero(); |
235 |
} |
||
236 |
|||
237 |
58151 |
std::size_t nc = 0; |
|
238 |
58151 |
typename ContactModelContainer::iterator it_m, end_m; |
|
239 |
58151 |
typename ContactDataContainer::iterator it_d, end_d; |
|
240 |
✓✓ | 58151 |
if (compute_all_contacts_) { |
241 |
89300 |
for (it_m = contacts_.begin(), end_m = contacts_.end(), |
|
242 |
25930 |
it_d = data->contacts.begin(), end_d = data->contacts.end(); |
|
243 |
✓✓✗✓ ✓✓ |
152670 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
244 |
63370 |
const boost::shared_ptr<ContactItem>& m_i = it_m->second; |
|
245 |
63370 |
const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second; |
|
246 |
✗✓✗✗ ✗✗✗✗ ✗✗ |
63370 |
assert_pretty(it_m->first == it_d->first, |
247 |
"it doesn't match the contact name between data and model"); |
||
248 |
63370 |
const std::size_t nc_i = m_i->contact->get_nc(); |
|
249 |
✓✓ | 63370 |
if (m_i->active) { |
250 |
✓✗ | 63366 |
const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i = |
251 |
force.segment(nc, nc_i); |
||
252 |
✓✗✓✗ |
63366 |
m_i->contact->updateForce(d_i, force_i); |
253 |
63366 |
const pinocchio::JointIndex joint = |
|
254 |
63366 |
state_->get_pinocchio()->frames[d_i->frame].parent; |
|
255 |
✓✗ | 63366 |
data->fext[joint] = d_i->fext; |
256 |
} else { |
||
257 |
✓✗ | 4 |
m_i->contact->setZeroForce(d_i); |
258 |
} |
||
259 |
63370 |
nc += nc_i; |
|
260 |
} |
||
261 |
} else { |
||
262 |
102146 |
for (it_m = contacts_.begin(), end_m = contacts_.end(), |
|
263 |
32221 |
it_d = data->contacts.begin(), end_d = data->contacts.end(); |
|
264 |
✓✓✗✓ ✓✓ |
172071 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
265 |
69925 |
const boost::shared_ptr<ContactItem>& m_i = it_m->second; |
|
266 |
69925 |
const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second; |
|
267 |
✗✓✗✗ ✗✗✗✗ ✗✗ |
69925 |
assert_pretty(it_m->first == it_d->first, |
268 |
"it doesn't match the contact name between data and model"); |
||
269 |
✓✓ | 69925 |
if (m_i->active) { |
270 |
69921 |
const std::size_t nc_i = m_i->contact->get_nc(); |
|
271 |
✓✗ | 69921 |
const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i = |
272 |
force.segment(nc, nc_i); |
||
273 |
✓✗✓✗ |
69921 |
m_i->contact->updateForce(d_i, force_i); |
274 |
69921 |
const pinocchio::JointIndex joint = |
|
275 |
69921 |
state_->get_pinocchio()->frames[d_i->frame].parent; |
|
276 |
✓✗ | 69921 |
data->fext[joint] = d_i->fext; |
277 |
69921 |
nc += nc_i; |
|
278 |
} else { |
||
279 |
✓✗ | 4 |
m_i->contact->setZeroForce(d_i); |
280 |
} |
||
281 |
} |
||
282 |
} |
||
283 |
58151 |
} |
|
284 |
|||
285 |
template <typename Scalar> |
||
286 |
5078 |
void ContactModelMultipleTpl<Scalar>::updateAccelerationDiff( |
|
287 |
const boost::shared_ptr<ContactDataMultiple>& data, |
||
288 |
const MatrixXs& ddv_dx) const { |
||
289 |
✓✗✗✓ ✗✓ |
10156 |
if (static_cast<std::size_t>(ddv_dx.rows()) != state_->get_nv() || |
290 |
5078 |
static_cast<std::size_t>(ddv_dx.cols()) != state_->get_ndx()) { |
|
291 |
throw_pretty("Invalid argument: " |
||
292 |
<< "ddv_dx has wrong dimension (it should be " + |
||
293 |
std::to_string(state_->get_nv()) + "," + |
||
294 |
std::to_string(state_->get_ndx()) + ")"); |
||
295 |
} |
||
296 |
5078 |
data->ddv_dx = ddv_dx; |
|
297 |
5078 |
} |
|
298 |
|||
299 |
template <typename Scalar> |
||
300 |
36507 |
void ContactModelMultipleTpl<Scalar>::updateForceDiff( |
|
301 |
const boost::shared_ptr<ContactDataMultiple>& data, const MatrixXs& df_dx, |
||
302 |
const MatrixXs& df_du) const { |
||
303 |
36507 |
const std::size_t ndx = state_->get_ndx(); |
|
304 |
✓✗ | 73014 |
if (static_cast<std::size_t>(df_dx.rows()) != |
305 |
✓✓✓✗ ✗✓ |
109521 |
(compute_all_contacts_ ? nc_total_ : nc_) || |
306 |
✓✗✗✓ |
36507 |
static_cast<std::size_t>(df_dx.cols()) != ndx) { |
307 |
throw_pretty( |
||
308 |
"Invalid argument: " |
||
309 |
<< "df_dx has wrong dimension (it should be " + |
||
310 |
std::to_string((compute_all_contacts_ ? nc_total_ : nc_)) + "," + |
||
311 |
std::to_string(ndx) + ")"); |
||
312 |
} |
||
313 |
✓✗ | 73014 |
if (static_cast<std::size_t>(df_du.rows()) != |
314 |
✓✓✓✗ ✗✓ |
109521 |
(compute_all_contacts_ ? nc_total_ : nc_) || |
315 |
✓✗✗✓ |
36507 |
static_cast<std::size_t>(df_du.cols()) != nu_) { |
316 |
throw_pretty( |
||
317 |
"Invalid argument: " |
||
318 |
<< "df_du has wrong dimension (it should be " + |
||
319 |
std::to_string((compute_all_contacts_ ? nc_total_ : nc_)) + "," + |
||
320 |
std::to_string(nu_) + ")"); |
||
321 |
} |
||
322 |
✗✓ | 36507 |
if (static_cast<std::size_t>(data->contacts.size()) != contacts_.size()) { |
323 |
throw_pretty("Invalid argument: " |
||
324 |
<< "it doesn't match the number of contact datas and models"); |
||
325 |
} |
||
326 |
|||
327 |
36507 |
std::size_t nc = 0; |
|
328 |
36507 |
typename ContactModelContainer::const_iterator it_m, end_m; |
|
329 |
36507 |
typename ContactDataContainer::const_iterator it_d, end_d; |
|
330 |
✓✓ | 36507 |
if (compute_all_contacts_) { |
331 |
107626 |
for (it_m = contacts_.begin(), end_m = contacts_.end(), |
|
332 |
31430 |
it_d = data->contacts.begin(), end_d = data->contacts.end(); |
|
333 |
✓✓✗✓ ✓✓ |
183822 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
334 |
76196 |
const boost::shared_ptr<ContactItem>& m_i = it_m->second; |
|
335 |
76196 |
const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second; |
|
336 |
✗✓✗✗ ✗✗✗✗ ✗✗ |
76196 |
assert_pretty(it_m->first == it_d->first, |
337 |
"it doesn't match the contact name between data and model"); |
||
338 |
76196 |
const std::size_t nc_i = m_i->contact->get_nc(); |
|
339 |
✓✗ | 76196 |
if (m_i->active) { |
340 |
✓✗ | 76196 |
const Eigen::Block<const MatrixXs> df_dx_i = |
341 |
df_dx.block(nc, 0, nc_i, ndx); |
||
342 |
76196 |
const Eigen::Block<const MatrixXs> df_du_i = |
|
343 |
✓✗ | 76196 |
df_du.block(nc, 0, nc_i, nu_); |
344 |
✓✗✓✗ ✓✗ |
76196 |
m_i->contact->updateForceDiff(d_i, df_dx_i, df_du_i); |
345 |
} else { |
||
346 |
m_i->contact->setZeroForceDiff(d_i); |
||
347 |
} |
||
348 |
76196 |
nc += nc_i; |
|
349 |
} |
||
350 |
} else { |
||
351 |
15273 |
for (it_m = contacts_.begin(), end_m = contacts_.end(), |
|
352 |
5077 |
it_d = data->contacts.begin(), end_d = data->contacts.end(); |
|
353 |
✓✓✗✓ ✓✓ |
25469 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
354 |
10196 |
const boost::shared_ptr<ContactItem>& m_i = it_m->second; |
|
355 |
10196 |
const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second; |
|
356 |
✗✓✗✗ ✗✗✗✗ ✗✗ |
10196 |
assert_pretty(it_m->first == it_d->first, |
357 |
"it doesn't match the contact name between data and model"); |
||
358 |
✓✗ | 10196 |
if (m_i->active) { |
359 |
10196 |
const std::size_t nc_i = m_i->contact->get_nc(); |
|
360 |
✓✗ | 10196 |
const Eigen::Block<const MatrixXs> df_dx_i = |
361 |
df_dx.block(nc, 0, nc_i, ndx); |
||
362 |
10196 |
const Eigen::Block<const MatrixXs> df_du_i = |
|
363 |
✓✗ | 10196 |
df_du.block(nc, 0, nc_i, nu_); |
364 |
✓✗✓✗ ✓✗ |
10196 |
m_i->contact->updateForceDiff(d_i, df_dx_i, df_du_i); |
365 |
10196 |
nc += nc_i; |
|
366 |
} else { |
||
367 |
m_i->contact->setZeroForceDiff(d_i); |
||
368 |
} |
||
369 |
} |
||
370 |
} |
||
371 |
36507 |
} |
|
372 |
|||
373 |
template <typename Scalar> |
||
374 |
8839 |
void ContactModelMultipleTpl<Scalar>::updateRneaDiff( |
|
375 |
const boost::shared_ptr<ContactDataMultiple>& data, |
||
376 |
pinocchio::DataTpl<Scalar>& pinocchio) const { |
||
377 |
✗✓ | 17678 |
if (static_cast<std::size_t>(data->contacts.size()) != |
378 |
8839 |
this->get_contacts().size()) { |
|
379 |
throw_pretty("Invalid argument: " |
||
380 |
<< "it doesn't match the number of contact datas and models"); |
||
381 |
} |
||
382 |
8839 |
typename ContactModelContainer::const_iterator it_m, end_m; |
|
383 |
8839 |
typename ContactDataContainer::const_iterator it_d, end_d; |
|
384 |
27813 |
for (it_m = contacts_.begin(), end_m = contacts_.end(), |
|
385 |
8839 |
it_d = data->contacts.begin(), end_d = data->contacts.end(); |
|
386 |
✓✓✗✓ ✓✓ |
46787 |
it_m != end_m || it_d != end_d; ++it_m, ++it_d) { |
387 |
18974 |
const boost::shared_ptr<ContactItem>& m_i = it_m->second; |
|
388 |
18974 |
const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second; |
|
389 |
✗✓✗✗ ✗✗✗✗ ✗✗ |
18974 |
assert_pretty(it_m->first == it_d->first, |
390 |
"it doesn't match the contact name between data and model"); |
||
391 |
✓✗ | 18974 |
if (m_i->active) { |
392 |
✓✓✗ | 18974 |
switch (m_i->contact->get_type()) { |
393 |
11384 |
case pinocchio::ReferenceFrame::LOCAL: |
|
394 |
11384 |
break; |
|
395 |
7590 |
case pinocchio::ReferenceFrame::WORLD: |
|
396 |
case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED: |
||
397 |
✓✗ | 7590 |
pinocchio.dtau_dq += d_i->dtau_dq; |
398 |
7590 |
break; |
|
399 |
} |
||
400 |
} |
||
401 |
} |
||
402 |
8839 |
} |
|
403 |
|||
404 |
template <typename Scalar> |
||
405 |
boost::shared_ptr<ContactDataMultipleTpl<Scalar> > |
||
406 |
70999 |
ContactModelMultipleTpl<Scalar>::createData( |
|
407 |
pinocchio::DataTpl<Scalar>* const data) { |
||
408 |
return boost::allocate_shared<ContactDataMultiple>( |
||
409 |
✓✗ | 70999 |
Eigen::aligned_allocator<ContactDataMultiple>(), this, data); |
410 |
} |
||
411 |
|||
412 |
template <typename Scalar> |
||
413 |
const boost::shared_ptr<StateMultibodyTpl<Scalar> >& |
||
414 |
426022 |
ContactModelMultipleTpl<Scalar>::get_state() const { |
|
415 |
426022 |
return state_; |
|
416 |
} |
||
417 |
|||
418 |
template <typename Scalar> |
||
419 |
const typename ContactModelMultipleTpl<Scalar>::ContactModelContainer& |
||
420 |
459377 |
ContactModelMultipleTpl<Scalar>::get_contacts() const { |
|
421 |
459377 |
return contacts_; |
|
422 |
} |
||
423 |
|||
424 |
template <typename Scalar> |
||
425 |
102819 |
std::size_t ContactModelMultipleTpl<Scalar>::get_nc() const { |
|
426 |
102819 |
return nc_; |
|
427 |
} |
||
428 |
|||
429 |
template <typename Scalar> |
||
430 |
514944 |
std::size_t ContactModelMultipleTpl<Scalar>::get_nc_total() const { |
|
431 |
514944 |
return nc_total_; |
|
432 |
} |
||
433 |
|||
434 |
template <typename Scalar> |
||
435 |
763 |
std::size_t ContactModelMultipleTpl<Scalar>::get_nu() const { |
|
436 |
763 |
return nu_; |
|
437 |
} |
||
438 |
|||
439 |
template <typename Scalar> |
||
440 |
29693 |
const std::set<std::string>& ContactModelMultipleTpl<Scalar>::get_active_set() |
|
441 |
const { |
||
442 |
29693 |
return active_set_; |
|
443 |
} |
||
444 |
|||
445 |
template <typename Scalar> |
||
446 |
29693 |
const std::set<std::string>& ContactModelMultipleTpl<Scalar>::get_inactive_set() |
|
447 |
const { |
||
448 |
29693 |
return inactive_set_; |
|
449 |
} |
||
450 |
|||
451 |
template <typename Scalar> |
||
452 |
bool ContactModelMultipleTpl<Scalar>::getContactStatus( |
||
453 |
const std::string& name) const { |
||
454 |
typename ContactModelContainer::const_iterator it = contacts_.find(name); |
||
455 |
if (it != contacts_.end()) { |
||
456 |
return it->second->active; |
||
457 |
} else { |
||
458 |
std::cerr << "Warning: we couldn't get the status of the " << name |
||
459 |
<< " contact item, it doesn't exist." << std::endl; |
||
460 |
return false; |
||
461 |
} |
||
462 |
} |
||
463 |
|||
464 |
template <typename Scalar> |
||
465 |
bool ContactModelMultipleTpl<Scalar>::getComputeAllContacts() const { |
||
466 |
return compute_all_contacts_; |
||
467 |
} |
||
468 |
|||
469 |
template <typename Scalar> |
||
470 |
4161 |
void ContactModelMultipleTpl<Scalar>::setComputeAllContacts(const bool status) { |
|
471 |
4161 |
compute_all_contacts_ = status; |
|
472 |
4161 |
} |
|
473 |
|||
474 |
template <class Scalar> |
||
475 |
1 |
std::ostream& operator<<(std::ostream& os, |
|
476 |
const ContactModelMultipleTpl<Scalar>& model) { |
||
477 |
1 |
const auto& active = model.get_active_set(); |
|
478 |
1 |
const auto& inactive = model.get_inactive_set(); |
|
479 |
1 |
os << "ContactModelMultiple:" << std::endl; |
|
480 |
1 |
os << " Active:" << std::endl; |
|
481 |
1 |
for (std::set<std::string>::const_iterator it = active.begin(); |
|
482 |
✗✓ | 1 |
it != active.end(); ++it) { |
483 |
const boost::shared_ptr< |
||
484 |
typename ContactModelMultipleTpl<Scalar>::ContactItem>& contact_item = |
||
485 |
model.get_contacts().find(*it)->second; |
||
486 |
if (it != --active.end()) { |
||
487 |
os << " " << *it << ": " << *contact_item << std::endl; |
||
488 |
} else { |
||
489 |
os << " " << *it << ": " << *contact_item << std::endl; |
||
490 |
} |
||
491 |
} |
||
492 |
1 |
os << " Inactive:" << std::endl; |
|
493 |
1 |
for (std::set<std::string>::const_iterator it = inactive.begin(); |
|
494 |
✗✓ | 1 |
it != inactive.end(); ++it) { |
495 |
const boost::shared_ptr< |
||
496 |
typename ContactModelMultipleTpl<Scalar>::ContactItem>& contact_item = |
||
497 |
model.get_contacts().find(*it)->second; |
||
498 |
if (it != --inactive.end()) { |
||
499 |
os << " " << *it << ": " << *contact_item << std::endl; |
||
500 |
} else { |
||
501 |
os << " " << *it << ": " << *contact_item; |
||
502 |
} |
||
503 |
} |
||
504 |
1 |
return os; |
|
505 |
} |
||
506 |
|||
507 |
} // namespace crocoddyl |
| Generated by: GCOVR (Version 4.2) |