GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contacts/multiple-contacts.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 251 283 88.7%
Branches: 192 651 29.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2024, 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 766 : state_(state),
16 766 nc_(0),
17 766 nc_total_(0),
18 766 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 18 : state_(state),
25 18 nc_(0),
26 18 nc_total_(0),
27 18 nu_(state->get_nv()),
28 36 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
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1717 times.
 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 std::pair<typename ContactModelContainer::iterator, bool> ret =
44
3/6
✓ Branch 1 taken 1717 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1717 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1717 times.
✗ Branch 8 not taken.
 1717 contacts_.insert(std::make_pair(
45 name, boost::make_shared<ContactItem>(name, contact, active)));
46
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1716 times.
 1717 if (ret.second == false) {
47 std::cerr << "Warning: we couldn't add the " << name
48
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 << " contact item, it already existed." << std::endl;
49
2/2
✓ Branch 0 taken 1715 times.
✓ Branch 1 taken 1 times.
 1716 } else if (active) {
50 1715 nc_ += contact->get_nc();
51 1715 nc_total_ += contact->get_nc();
52
1/2
✓ Branch 1 taken 1715 times.
✗ Branch 2 not taken.
 1715 active_set_.insert(name);
53
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 } else if (!active) {
54 1 nc_total_ += contact->get_nc();
55
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 inactive_set_.insert(name);
56 }
57 1717 }
58
59 template <typename Scalar>
60 2 void ContactModelMultipleTpl<Scalar>::removeContact(const std::string& name) {
61
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 typename ContactModelContainer::iterator it = contacts_.find(name);
62
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 1 times.
 2 if (it != contacts_.end()) {
63 1 nc_ -= it->second->contact->get_nc();
64 1 nc_total_ -= it->second->contact->get_nc();
65
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 contacts_.erase(it);
66
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 active_set_.erase(name);
67
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 inactive_set_.erase(name);
68 } else {
69 std::cerr << "Warning: we couldn't remove the " << name
70
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 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
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 typename ContactModelContainer::iterator it = contacts_.find(name);
78
2/2
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 1 times.
 17 if (it != contacts_.end()) {
79
5/6
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 15 times.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✓ Branch 7 taken 15 times.
 16 if (active && !it->second->active) {
80 1 nc_ += it->second->contact->get_nc();
81
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 active_set_.insert(name);
82
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 inactive_set_.erase(name);
83
3/6
✓ Branch 0 taken 15 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 15 } else if (!active && it->second->active) {
84 15 nc_ -= it->second->contact->get_nc();
85
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 inactive_set_.insert(name);
86
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 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
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 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
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 62660 times.
 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
2/2
✓ Branch 0 taken 25930 times.
✓ Branch 1 taken 36730 times.
 62660 if (compute_all_contacts_) {
110 51860 for (it_m = contacts_.begin(), end_m = contacts_.end(),
111 25930 it_d = data->contacts.begin(), end_d = data->contacts.end();
112
5/6
✓ Branch 2 taken 25930 times.
✓ Branch 3 taken 63370 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 25930 times.
✓ Branch 7 taken 63370 times.
✓ Branch 8 taken 25930 times.
 89300 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
2/2
✓ Branch 1 taken 63366 times.
✓ Branch 2 taken 4 times.
 63370 if (m_i->active) {
116 63366 const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
117
1/18
✗ Branch 3 not taken.
✓ Branch 4 taken 63366 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.
 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
1/2
✓ Branch 3 taken 63366 times.
✗ Branch 4 not taken.
 63366 m_i->contact->calc(d_i, x);
122
2/4
✓ Branch 3 taken 63366 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 63366 times.
✗ Branch 7 not taken.
 63366 data->a0.segment(nc, nc_i) = d_i->a0;
123
2/4
✓ Branch 3 taken 63366 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 63366 times.
✗ Branch 7 not taken.
 63366 data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc;
124 } else {
125
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 4 data->a0.segment(nc, nc_i).setZero();
126
2/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 4 data->Jc.block(nc, 0, nc_i, nv).setZero();
127 }
128 63370 nc += nc_i;
129 }
130 } else {
131 73460 for (it_m = contacts_.begin(), end_m = contacts_.end(),
132 36730 it_d = data->contacts.begin(), end_d = data->contacts.end();
133
5/6
✓ Branch 3 taken 36730 times.
✓ Branch 4 taken 79602 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 36730 times.
✓ Branch 8 taken 79602 times.
✓ Branch 9 taken 36730 times.
 116332 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
2/2
✓ Branch 1 taken 79586 times.
✓ Branch 2 taken 16 times.
 79602 if (m_i->active) {
136 79586 const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
137
1/18
✗ Branch 3 not taken.
✓ Branch 4 taken 79586 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.
 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
1/2
✓ Branch 3 taken 79586 times.
✗ Branch 4 not taken.
 79586 m_i->contact->calc(d_i, x);
143 79586 const std::size_t nc_i = m_i->contact->get_nc();
144
2/4
✓ Branch 3 taken 79586 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 79586 times.
✗ Branch 7 not taken.
 79586 data->a0.segment(nc, nc_i) = d_i->a0;
145
2/4
✓ Branch 3 taken 79586 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 79586 times.
✗ Branch 7 not taken.
 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
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 8845 times.
 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
2/2
✓ Branch 0 taken 3762 times.
✓ Branch 1 taken 5083 times.
 8845 if (compute_all_contacts_) {
166 7524 for (it_m = contacts_.begin(), end_m = contacts_.end(),
167 3762 it_d = data->contacts.begin(), end_d = data->contacts.end();
168
5/6
✓ Branch 2 taken 3762 times.
✓ Branch 3 taken 8778 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3762 times.
✓ Branch 7 taken 8778 times.
✓ Branch 8 taken 3762 times.
 12540 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
1/2
✓ Branch 1 taken 8778 times.
✗ Branch 2 not taken.
 8778 if (m_i->active) {
172
1/18
✗ Branch 3 not taken.
✓ Branch 4 taken 8778 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.
 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
1/2
✓ Branch 3 taken 8778 times.
✗ Branch 4 not taken.
 8778 m_i->contact->calcDiff(d_i, x);
179
2/4
✓ Branch 3 taken 8778 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8778 times.
✗ Branch 7 not taken.
 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 10166 for (it_m = contacts_.begin(), end_m = contacts_.end(),
187 5083 it_d = data->contacts.begin(), end_d = data->contacts.end();
188
5/6
✓ Branch 3 taken 5083 times.
✓ Branch 4 taken 10220 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 5083 times.
✓ Branch 8 taken 10220 times.
✓ Branch 9 taken 5083 times.
 15303 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
2/2
✓ Branch 1 taken 10216 times.
✓ Branch 2 taken 4 times.
 10220 if (m_i->active) {
191 10216 const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
192
1/18
✗ Branch 3 not taken.
✓ Branch 4 taken 10216 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.
 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
1/2
✓ Branch 3 taken 10216 times.
✗ Branch 4 not taken.
 10216 m_i->contact->calcDiff(d_i, x);
198 10216 const std::size_t nc_i = m_i->contact->get_nc();
199
2/4
✓ Branch 3 taken 10216 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10216 times.
✗ Branch 7 not taken.
 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
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 32220 times.
 32220 if (static_cast<std::size_t>(dv.size()) != state_->get_nv()) {
211 ✗ throw_pretty(
212 "Invalid argument: " << "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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 58151 times.
 116302 if (static_cast<std::size_t>(force.size()) !=
222
2/2
✓ Branch 0 taken 25930 times.
✓ Branch 1 taken 32221 times.
 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
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 58151 times.
 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
2/2
✓ Branch 5 taken 1272028 times.
✓ Branch 6 taken 58151 times.
 1330179 for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) {
234
2/4
✓ Branch 1 taken 1272028 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1272028 times.
✗ Branch 6 not taken.
 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
2/2
✓ Branch 0 taken 25930 times.
✓ Branch 1 taken 32221 times.
 58151 if (compute_all_contacts_) {
241 51860 for (it_m = contacts_.begin(), end_m = contacts_.end(),
242 25930 it_d = data->contacts.begin(), end_d = data->contacts.end();
243
5/6
✓ Branch 2 taken 25930 times.
✓ Branch 3 taken 63370 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 25930 times.
✓ Branch 7 taken 63370 times.
✓ Branch 8 taken 25930 times.
 89300 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
1/10
✗ Branch 3 not taken.
✓ Branch 4 taken 63370 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.
 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
2/2
✓ Branch 1 taken 63366 times.
✓ Branch 2 taken 4 times.
 63370 if (m_i->active) {
250 const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i =
251
1/2
✓ Branch 1 taken 63366 times.
✗ Branch 2 not taken.
 63366 force.segment(nc, nc_i);
252
2/4
✓ Branch 3 taken 63366 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 63366 times.
✗ Branch 7 not taken.
 63366 m_i->contact->updateForce(d_i, force_i);
253 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
254 63366 const pinocchio::JointIndex joint =
255 63366 state_->get_pinocchio()->frames[d_i->frame].parentJoint;
256 #else
257 const pinocchio::JointIndex joint =
258 state_->get_pinocchio()->frames[d_i->frame].parent;
259 #endif
260
1/2
✓ Branch 4 taken 63366 times.
✗ Branch 5 not taken.
 63366 data->fext[joint] = d_i->fext;
261 } else {
262
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 m_i->contact->setZeroForce(d_i);
263 }
264 63370 nc += nc_i;
265 }
266 } else {
267 64442 for (it_m = contacts_.begin(), end_m = contacts_.end(),
268 32221 it_d = data->contacts.begin(), end_d = data->contacts.end();
269
5/6
✓ Branch 3 taken 32221 times.
✓ Branch 4 taken 69925 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 32221 times.
✓ Branch 8 taken 69925 times.
✓ Branch 9 taken 32221 times.
 102146 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
270 69925 const boost::shared_ptr<ContactItem>& m_i = it_m->second;
271 69925 const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
272
1/10
✗ Branch 3 not taken.
✓ Branch 4 taken 69925 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.
 69925 assert_pretty(it_m->first == it_d->first,
273 "it doesn't match the contact name between data and model");
274
2/2
✓ Branch 1 taken 69921 times.
✓ Branch 2 taken 4 times.
 69925 if (m_i->active) {
275 69921 const std::size_t nc_i = m_i->contact->get_nc();
276 const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i =
277
1/2
✓ Branch 1 taken 69921 times.
✗ Branch 2 not taken.
 69921 force.segment(nc, nc_i);
278
2/4
✓ Branch 3 taken 69921 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 69921 times.
✗ Branch 7 not taken.
 69921 m_i->contact->updateForce(d_i, force_i);
279 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
280 69921 const pinocchio::JointIndex joint =
281 69921 state_->get_pinocchio()->frames[d_i->frame].parentJoint;
282 #else
283 const pinocchio::JointIndex joint =
284 state_->get_pinocchio()->frames[d_i->frame].parent;
285 #endif
286
1/2
✓ Branch 4 taken 69921 times.
✗ Branch 5 not taken.
 69921 data->fext[joint] = d_i->fext;
287 69921 nc += nc_i;
288 } else {
289
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 m_i->contact->setZeroForce(d_i);
290 }
291 }
292 }
293 58151 }
294
295 template <typename Scalar>
296 5078 void ContactModelMultipleTpl<Scalar>::updateAccelerationDiff(
297 const boost::shared_ptr<ContactDataMultiple>& data,
298 const MatrixXs& ddv_dx) const {
299
3/6
✓ Branch 3 taken 5078 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 5078 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 5078 times.
 10156 if (static_cast<std::size_t>(ddv_dx.rows()) != state_->get_nv() ||
300 5078 static_cast<std::size_t>(ddv_dx.cols()) != state_->get_ndx()) {
301 ✗ throw_pretty(
302 "Invalid argument: " << "ddv_dx has wrong dimension (it should be " +
303 std::to_string(state_->get_nv()) + "," +
304 std::to_string(state_->get_ndx()) + ")");
305 }
306 5078 data->ddv_dx = ddv_dx;
307 5078 }
308
309 template <typename Scalar>
310 36507 void ContactModelMultipleTpl<Scalar>::updateForceDiff(
311 const boost::shared_ptr<ContactDataMultiple>& data, const MatrixXs& df_dx,
312 const MatrixXs& df_du) const {
313 36507 const std::size_t ndx = state_->get_ndx();
314 36507 if (static_cast<std::size_t>(df_dx.rows()) !=
315
4/6
✓ Branch 0 taken 31430 times.
✓ Branch 1 taken 5077 times.
✓ Branch 2 taken 36507 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 36507 times.
 73014 (compute_all_contacts_ ? nc_total_ : nc_) ||
316
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 36507 times.
 36507 static_cast<std::size_t>(df_dx.cols()) != ndx) {
317 ✗ throw_pretty(
318 "Invalid argument: "
319 << "df_dx has wrong dimension (it should be " +
320 std::to_string((compute_all_contacts_ ? nc_total_ : nc_)) + "," +
321 std::to_string(ndx) + ")");
322 }
323 36507 if (static_cast<std::size_t>(df_du.rows()) !=
324
4/6
✓ Branch 0 taken 31430 times.
✓ Branch 1 taken 5077 times.
✓ Branch 2 taken 36507 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 36507 times.
 73014 (compute_all_contacts_ ? nc_total_ : nc_) ||
325
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 36507 times.
 36507 static_cast<std::size_t>(df_du.cols()) != nu_) {
326 ✗ throw_pretty(
327 "Invalid argument: "
328 << "df_du has wrong dimension (it should be " +
329 std::to_string((compute_all_contacts_ ? nc_total_ : nc_)) + "," +
330 std::to_string(nu_) + ")");
331 }
332
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 36507 times.
 36507 if (static_cast<std::size_t>(data->contacts.size()) != contacts_.size()) {
333 ✗ throw_pretty("Invalid argument: "
334 << "it doesn't match the number of contact datas and models");
335 }
336
337 36507 std::size_t nc = 0;
338 36507 typename ContactModelContainer::const_iterator it_m, end_m;
339 36507 typename ContactDataContainer::const_iterator it_d, end_d;
340
2/2
✓ Branch 0 taken 31430 times.
✓ Branch 1 taken 5077 times.
 36507 if (compute_all_contacts_) {
341 31430 for (it_m = contacts_.begin(), end_m = contacts_.end(),
342 31430 it_d = data->contacts.begin(), end_d = data->contacts.end();
343
5/6
✓ Branch 2 taken 31430 times.
✓ Branch 3 taken 76196 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 31430 times.
✓ Branch 7 taken 76196 times.
✓ Branch 8 taken 31430 times.
 107626 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
344 76196 const boost::shared_ptr<ContactItem>& m_i = it_m->second;
345 76196 const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
346
1/10
✗ Branch 3 not taken.
✓ Branch 4 taken 76196 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.
 76196 assert_pretty(it_m->first == it_d->first,
347 "it doesn't match the contact name between data and model");
348 76196 const std::size_t nc_i = m_i->contact->get_nc();
349
1/2
✓ Branch 1 taken 76196 times.
✗ Branch 2 not taken.
 76196 if (m_i->active) {
350 const Eigen::Block<const MatrixXs> df_dx_i =
351
1/2
✓ Branch 1 taken 76196 times.
✗ Branch 2 not taken.
 76196 df_dx.block(nc, 0, nc_i, ndx);
352 const Eigen::Block<const MatrixXs> df_du_i =
353
1/2
✓ Branch 1 taken 76196 times.
✗ Branch 2 not taken.
 76196 df_du.block(nc, 0, nc_i, nu_);
354
3/6
✓ Branch 3 taken 76196 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 76196 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 76196 times.
✗ Branch 10 not taken.
 76196 m_i->contact->updateForceDiff(d_i, df_dx_i, df_du_i);
355 } else {
356 ✗ m_i->contact->setZeroForceDiff(d_i);
357 }
358 76196 nc += nc_i;
359 }
360 } else {
361 5077 for (it_m = contacts_.begin(), end_m = contacts_.end(),
362 5077 it_d = data->contacts.begin(), end_d = data->contacts.end();
363
5/6
✓ Branch 3 taken 5077 times.
✓ Branch 4 taken 10196 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 5077 times.
✓ Branch 8 taken 10196 times.
✓ Branch 9 taken 5077 times.
 15273 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
364 10196 const boost::shared_ptr<ContactItem>& m_i = it_m->second;
365 10196 const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
366
1/10
✗ Branch 3 not taken.
✓ Branch 4 taken 10196 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.
 10196 assert_pretty(it_m->first == it_d->first,
367 "it doesn't match the contact name between data and model");
368
1/2
✓ Branch 1 taken 10196 times.
✗ Branch 2 not taken.
 10196 if (m_i->active) {
369 10196 const std::size_t nc_i = m_i->contact->get_nc();
370 const Eigen::Block<const MatrixXs> df_dx_i =
371
1/2
✓ Branch 1 taken 10196 times.
✗ Branch 2 not taken.
 10196 df_dx.block(nc, 0, nc_i, ndx);
372 const Eigen::Block<const MatrixXs> df_du_i =
373
1/2
✓ Branch 1 taken 10196 times.
✗ Branch 2 not taken.
 10196 df_du.block(nc, 0, nc_i, nu_);
374
3/6
✓ Branch 3 taken 10196 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10196 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10196 times.
✗ Branch 10 not taken.
 10196 m_i->contact->updateForceDiff(d_i, df_dx_i, df_du_i);
375 10196 nc += nc_i;
376 } else {
377 ✗ m_i->contact->setZeroForceDiff(d_i);
378 }
379 }
380 }
381 36507 }
382
383 template <typename Scalar>
384 8839 void ContactModelMultipleTpl<Scalar>::updateRneaDiff(
385 const boost::shared_ptr<ContactDataMultiple>& data,
386 pinocchio::DataTpl<Scalar>& pinocchio) const {
387
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 8839 times.
 17678 if (static_cast<std::size_t>(data->contacts.size()) !=
388 8839 this->get_contacts().size()) {
389 ✗ throw_pretty("Invalid argument: "
390 << "it doesn't match the number of contact datas and models");
391 }
392 8839 typename ContactModelContainer::const_iterator it_m, end_m;
393 8839 typename ContactDataContainer::const_iterator it_d, end_d;
394 8839 for (it_m = contacts_.begin(), end_m = contacts_.end(),
395 8839 it_d = data->contacts.begin(), end_d = data->contacts.end();
396
5/6
✓ Branch 3 taken 8839 times.
✓ Branch 4 taken 18974 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 8839 times.
✓ Branch 8 taken 18974 times.
✓ Branch 9 taken 8839 times.
 27813 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
397 18974 const boost::shared_ptr<ContactItem>& m_i = it_m->second;
398 18974 const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
399
1/10
✗ Branch 3 not taken.
✓ Branch 4 taken 18974 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.
 18974 assert_pretty(it_m->first == it_d->first,
400 "it doesn't match the contact name between data and model");
401
1/2
✓ Branch 1 taken 18974 times.
✗ Branch 2 not taken.
 18974 if (m_i->active) {
402
2/3
✓ Branch 3 taken 11384 times.
✓ Branch 4 taken 7590 times.
✗ Branch 5 not taken.
 18974 switch (m_i->contact->get_type()) {
403 11384 case pinocchio::ReferenceFrame::LOCAL:
404 11384 break;
405 7590 case pinocchio::ReferenceFrame::WORLD:
406 case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
407
1/2
✓ Branch 2 taken 7590 times.
✗ Branch 3 not taken.
 7590 pinocchio.dtau_dq += d_i->dtau_dq;
408 7590 break;
409 }
410 }
411 }
412 8839 }
413
414 template <typename Scalar>
415 boost::shared_ptr<ContactDataMultipleTpl<Scalar> >
416 70999 ContactModelMultipleTpl<Scalar>::createData(
417 pinocchio::DataTpl<Scalar>* const data) {
418 return boost::allocate_shared<ContactDataMultiple>(
419
1/2
✓ Branch 2 taken 70999 times.
✗ Branch 3 not taken.
 141998 Eigen::aligned_allocator<ContactDataMultiple>(), this, data);
420 }
421
422 template <typename Scalar>
423 const boost::shared_ptr<StateMultibodyTpl<Scalar> >&
424 426022 ContactModelMultipleTpl<Scalar>::get_state() const {
425 426022 return state_;
426 }
427
428 template <typename Scalar>
429 const typename ContactModelMultipleTpl<Scalar>::ContactModelContainer&
430 459377 ContactModelMultipleTpl<Scalar>::get_contacts() const {
431 459377 return contacts_;
432 }
433
434 template <typename Scalar>
435 102819 std::size_t ContactModelMultipleTpl<Scalar>::get_nc() const {
436 102819 return nc_;
437 }
438
439 template <typename Scalar>
440 514944 std::size_t ContactModelMultipleTpl<Scalar>::get_nc_total() const {
441 514944 return nc_total_;
442 }
443
444 template <typename Scalar>
445 763 std::size_t ContactModelMultipleTpl<Scalar>::get_nu() const {
446 763 return nu_;
447 }
448
449 template <typename Scalar>
450 29693 const std::set<std::string>& ContactModelMultipleTpl<Scalar>::get_active_set()
451 const {
452 29693 return active_set_;
453 }
454
455 template <typename Scalar>
456 29693 const std::set<std::string>& ContactModelMultipleTpl<Scalar>::get_inactive_set()
457 const {
458 29693 return inactive_set_;
459 }
460
461 template <typename Scalar>
462 ✗ bool ContactModelMultipleTpl<Scalar>::getContactStatus(
463 const std::string& name) const {
464 ✗ typename ContactModelContainer::const_iterator it = contacts_.find(name);
465 ✗ if (it != contacts_.end()) {
466 ✗ return it->second->active;
467 } else {
468 std::cerr << "Warning: we couldn't get the status of the " << name
469 ✗ << " contact item, it doesn't exist." << std::endl;
470 ✗ return false;
471 }
472 }
473
474 template <typename Scalar>
475 ✗ bool ContactModelMultipleTpl<Scalar>::getComputeAllContacts() const {
476 ✗ return compute_all_contacts_;
477 }
478
479 template <typename Scalar>
480 4161 void ContactModelMultipleTpl<Scalar>::setComputeAllContacts(const bool status) {
481 4161 compute_all_contacts_ = status;
482 4161 }
483
484 template <class Scalar>
485 1 std::ostream& operator<<(std::ostream& os,
486 const ContactModelMultipleTpl<Scalar>& model) {
487 1 const auto& active = model.get_active_set();
488 1 const auto& inactive = model.get_inactive_set();
489 1 os << "ContactModelMultiple:" << std::endl;
490 1 os << " Active:" << std::endl;
491 1 for (std::set<std::string>::const_iterator it = active.begin();
492
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 it != active.end(); ++it) {
493 const boost::shared_ptr<
494 ✗ typename ContactModelMultipleTpl<Scalar>::ContactItem>& contact_item =
495 ✗ model.get_contacts().find(*it)->second;
496 ✗ if (it != --active.end()) {
497 ✗ os << " " << *it << ": " << *contact_item << std::endl;
498 } else {
499 ✗ os << " " << *it << ": " << *contact_item << std::endl;
500 }
501 }
502 1 os << " Inactive:" << std::endl;
503 1 for (std::set<std::string>::const_iterator it = inactive.begin();
504
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 it != inactive.end(); ++it) {
505 const boost::shared_ptr<
506 ✗ typename ContactModelMultipleTpl<Scalar>::ContactItem>& contact_item =
507 ✗ model.get_contacts().find(*it)->second;
508 ✗ if (it != --inactive.end()) {
509 ✗ os << " " << *it << ": " << *contact_item << std::endl;
510 } else {
511 ✗ os << " " << *it << ": " << *contact_item;
512 }
513 }
514 1 return os;
515 }
516
517 } // namespace crocoddyl
518