GCC Code Coverage Report

Directory: ./
File: src/continuous-validation/solid-solid-collision.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 136 172 79.1%
Branches: 107 260 41.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014,2015,2016, 2018 CNRS
3 // Authors: Florent Lamiraux, Joseph Mirabel, Diane Bury
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <coal/collision.h>
31 #include <coal/collision_data.h>
32
33 #include <hpp/core/continuous-validation/solid-solid-collision.hh>
34 #include <hpp/core/deprecated.hh>
35 #include <hpp/pinocchio/body.hh>
36 #include <hpp/pinocchio/collision-object.hh>
37 #include <hpp/pinocchio/device.hh>
38 #include <hpp/pinocchio/joint.hh>
39 #include <pinocchio/multibody/model.hpp>
40
41 namespace hpp {
42 namespace core {
43 namespace continuousValidation {
44
45 10 SolidSolidCollisionPtr_t SolidSolidCollision::create(
46 const JointPtr_t& joint_a, const ConstObjectStdVector_t& objects_b,
47 value_type tolerance) {
48 SolidSolidCollision* ptr(
49
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 new SolidSolidCollision(joint_a, objects_b, tolerance));
50 10 SolidSolidCollisionPtr_t shPtr(ptr);
51
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 ptr->init(shPtr);
52 10 return shPtr;
53 }
54
55 39 SolidSolidCollisionPtr_t SolidSolidCollision::create(const JointPtr_t& joint_a,
56 const JointPtr_t& joint_b,
57 value_type tolerance) {
58 SolidSolidCollision* ptr =
59
1/2
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
 39 new SolidSolidCollision(joint_a, joint_b, tolerance);
60 39 SolidSolidCollisionPtr_t shPtr(ptr);
61
1/2
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
 39 ptr->init(shPtr);
62 39 return shPtr;
63 }
64
65 153 SolidSolidCollisionPtr_t SolidSolidCollision::createCopy(
66 const SolidSolidCollisionPtr_t& other) {
67
1/2
✓ Branch 3 taken 153 times.
✗ Branch 4 not taken.
 153 SolidSolidCollision* ptr = new SolidSolidCollision(*other);
68 153 SolidSolidCollisionPtr_t shPtr(ptr);
69
1/2
✓ Branch 2 taken 153 times.
✗ Branch 3 not taken.
 153 ptr->init(shPtr);
70 153 return shPtr;
71 }
72 153 IntervalValidationPtr_t SolidSolidCollision::copy() const {
73
1/2
✓ Branch 2 taken 153 times.
✗ Branch 3 not taken.
 153 return createCopy(weak_.lock());
74 }
75
76 37617 value_type SolidSolidCollision::computeMaximalVelocity(vector_t& Vb) const {
77 37617 value_type maximalVelocity_a_b = 0;
78
2/2
✓ Branch 5 taken 140632 times.
✓ Branch 6 taken 37602 times.
 178114 for (const auto& coeff : m_->coefficients) {
79 140607 const JointPtr_t& joint = coeff.joint_;
80 140291 maximalVelocity_a_b +=
81 280898 coeff.value_ *
82
4/8
✓ Branch 2 taken 140435 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 140768 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 141708 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 140291 times.
✗ Branch 13 not taken.
 140607 Vb.segment(joint->rankInVelocity(), joint->numberDof()).norm();
83 }
84
85
2/2
✓ Branch 2 taken 21645 times.
✓ Branch 3 taken 16370 times.
 37602 if (m_->joint_b) {
86 21645 value_type maximalVelocity_b_a = 0;
87
2/2
✓ Branch 5 taken 71429 times.
✓ Branch 6 taken 21693 times.
 93195 for (const auto& coeff : m_->coefficients_reverse) {
88 71574 const JointPtr_t& joint = coeff.joint_;
89 71512 maximalVelocity_b_a +=
90 143086 coeff.value_ *
91
4/8
✓ Branch 2 taken 71494 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 71504 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 71704 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 71512 times.
✗ Branch 13 not taken.
 71574 Vb.segment(joint->rankInVelocity(), joint->numberDof()).norm();
92 }
93
94
2/2
✓ Branch 0 taken 6030 times.
✓ Branch 1 taken 15663 times.
 21693 if (maximalVelocity_b_a < maximalVelocity_a_b) {
95 6030 return maximalVelocity_b_a;
96 }
97 }
98 32033 return maximalVelocity_a_b;
99 }
100
101 ✗ bool SolidSolidCollision::removeObjectTo_b(
102 const CollisionObjectConstPtr_t& object) {
103 ✗ CollisionPairs_t& prs(pairs());
104 ✗ CollisionRequests_t& rqsts(requests());
105 ✗ const int s = (int)prs.size();
106
107 // Remove all reference to object
108 ✗ int last = 0;
109 ✗ for (int i = 0; i < s; ++i) {
110 ✗ if (object != prs[i].second) { // Different -> keep
111 ✗ if (last != i) { // If one has been removed, then move.
112 ✗ prs[last] = std::move(prs[i]);
113 ✗ rqsts[last] = std::move(rqsts[i]);
114 }
115 ✗ last++;
116 }
117 }
118 ✗ prs.erase(prs.begin() + last, prs.end());
119 ✗ rqsts.erase(rqsts.begin() + last, rqsts.end());
120
121 ✗ return last != s;
122 }
123
124 45 void SolidSolidCollision::breakDistance(value_type breakDistance) {
125
2/2
✓ Branch 5 taken 9 times.
✓ Branch 6 taken 45 times.
 54 for (coal::CollisionRequest& req : requests())
126 9 req.break_distance = breakDistance;
127 45 }
128
129 54 void SolidSolidCollision::addCollisionPair(
130 const CollisionObjectConstPtr_t& left,
131 const CollisionObjectConstPtr_t& right) {
132 // std::cout << "size = " << pairs().size() << std::endl;
133 // std::cout << "capacity = " << pairs().capacity() << std::endl;
134 54 pairs().emplace_back(left, right);
135
1/2
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
 54 requests().emplace_back(coal::DISTANCE_LOWER_BOUND, 1);
136 54 requests().back().enable_cached_gjk_guess = true;
137 54 }
138
139 ✗ std::string SolidSolidCollision::name() const {
140 ✗ std::ostringstream oss;
141 ✗ oss << "(" << m_->joint_a->name() << ",";
142 ✗ if (m_->joint_b)
143 ✗ oss << m_->joint_b->name();
144 else
145 ✗ oss << "obstacles";
146 ✗ oss << ")";
147 ✗ return oss.str();
148 }
149
150 ✗ std::ostream& SolidSolidCollision::print(std::ostream& os) const {
151 ✗ const pinocchio::Model& model = joint_a()->robot()->model();
152 ✗ os << "SolidSolidCollision: " << m_->joint_a->name() << " - "
153 ✗ << (m_->joint_b ? m_->joint_b->name() : model.names[0]) << '\n';
154 ✗ JointIndices_t joints = m_->computeSequenceOfJoints();
155 ✗ for (auto i : joints) os << model.names[i] << ", ";
156 ✗ os << '\n';
157 ✗ for (std::size_t i = 0; i < m_->coefficients.size(); ++i)
158 ✗ os << m_->coefficients[i].value_ << ", ";
159 ✗ return os;
160 }
161
162 SolidSolidCollision::JointIndices_t
163 49 SolidSolidCollision::Model::computeSequenceOfJoints() const {
164 49 JointIndices_t joints;
165
166
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 49 times.
 49 assert(joint_a);
167 49 const pinocchio::Model& model = joint_a->robot()->model();
168 49 const JointIndex id_a = joint_a->index(),
169
2/2
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 22 times.
 49 id_b = (joint_b ? joint_b->index() : 0);
170 49 JointIndex ia = id_a, ib = id_b;
171
172 49 std::vector<JointIndex> fromA, fromB;
173
2/2
✓ Branch 0 taken 169 times.
✓ Branch 1 taken 49 times.
 218 while (ia != ib) {
174
2/2
✓ Branch 0 taken 160 times.
✓ Branch 1 taken 9 times.
 169 if (ia > ib) {
175
1/2
✓ Branch 1 taken 160 times.
✗ Branch 2 not taken.
 160 fromA.push_back(ia);
176 160 ia = model.parents[ia];
177 } else /* if (ia < ib) */ {
178
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 fromB.push_back(ib);
179 9 ib = model.parents[ib];
180 }
181 }
182
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 49 times.
 49 assert(ia == ib);
183
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 fromA.push_back(ia);
184
185 // Check joint vectors
186
2/2
✓ Branch 1 taken 47 times.
✓ Branch 2 taken 2 times.
 49 if (fromB.empty())
187
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 47 times.
 47 assert(fromA.back() == id_b);
188 else
189
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 2 assert(model.parents[fromB.back()] == ia);
190
191 // Build sequence
192 49 joints = std::move(fromA);
193
1/2
✓ Branch 5 taken 49 times.
✗ Branch 6 not taken.
 49 joints.insert(joints.end(), fromB.rbegin(), fromB.rend());
194
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 49 times.
 49 assert(joints.front() == id_a);
195
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 49 times.
 49 assert(joints.back() == id_b);
196
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 49 times.
 49 assert(joints.size() > 1);
197 98 return joints;
198 49 }
199
200 76 CoefficientVelocities_t SolidSolidCollision::Model::computeCoefficients(
201 const JointIndices_t& joints) const {
202 76 const pinocchio::Model& model = joint_a->robot()->model();
203
204 76 JointPtr_t child;
205
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 76 times.
 76 assert(joints.size() > 1);
206 76 CoefficientVelocities_t coeff;
207
1/2
✓ Branch 2 taken 76 times.
✗ Branch 3 not taken.
 76 coeff.resize(joints.size() - 1);
208 76 pinocchio::DevicePtr_t robot = joint_a->robot();
209 // Store r0 + sum of T_{i/i+1} in a variable
210
2/4
✓ Branch 2 taken 76 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 76 times.
✗ Branch 7 not taken.
 76 value_type cumulativeLength = joint_a->linkedBody()->radius();
211 value_type distance;
212 76 std::size_t i = 0;
213
2/2
✓ Branch 1 taken 265 times.
✓ Branch 2 taken 76 times.
 341 while (i + 1 < joints.size()) {
214
2/2
✓ Branch 3 taken 169 times.
✓ Branch 4 taken 96 times.
 265 if (model.parents[joints[i]] == joints[i + 1])
215
1/2
✓ Branch 3 taken 169 times.
✗ Branch 4 not taken.
 169 child = Joint::create(robot, joints[i]);
216
1/2
✓ Branch 3 taken 96 times.
✗ Branch 4 not taken.
 96 else if (model.parents[joints[i + 1]] == joints[i])
217
1/2
✓ Branch 3 taken 96 times.
✗ Branch 4 not taken.
 96 child = Joint::create(robot, joints[i + 1]);
218 else
219 ✗ abort();
220
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 265 times.
 265 assert(child);
221 265 coeff[i].joint_ = child;
222 // Go through all known types of joints
223 // TODO: REPLACE THESE FUNCTIONS WITH NEW API
224
1/2
✓ Branch 2 taken 265 times.
✗ Branch 3 not taken.
 265 distance = child->maximalDistanceToParent();
225
1/2
✓ Branch 2 taken 265 times.
✗ Branch 3 not taken.
 265 coeff[i].value_ = child->upperBoundLinearVelocity() +
226
1/2
✓ Branch 2 taken 265 times.
✗ Branch 3 not taken.
 265 cumulativeLength * child->upperBoundAngularVelocity();
227 265 cumulativeLength += distance;
228
229 265 ++i;
230 }
231 152 return coeff;
232 76 }
233
234 49 void SolidSolidCollision::Model::setCoefficients(const JointIndices_t& joints) {
235 // Compute coefficients going from joint a to joint b
236 49 coefficients = computeCoefficients(joints);
237
238 // Compute coefficients going from joint b to joint a
239
2/2
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 22 times.
 49 if (joint_b) {
240
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 JointIndices_t joints_reverse(joints);
241
1/2
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 27 std::reverse(joints_reverse.begin(), joints_reverse.end());
242
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 coefficients_reverse = computeCoefficients(joints_reverse);
243 27 }
244 49 }
245
246 39 SolidSolidCollision::SolidSolidCollision(const JointPtr_t& joint_a,
247 const JointPtr_t& joint_b,
248 39 value_type tolerance)
249
2/4
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 39 times.
✗ Branch 7 not taken.
 39 : BodyPairCollision(tolerance), m_(new Model) {
250 39 m_->joint_a = joint_a;
251 39 m_->joint_b = joint_b;
252
253
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 39 times.
 39 assert(joint_a);
254
8/10
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 27 times.
✓ Branch 10 taken 27 times.
✓ Branch 11 taken 12 times.
✓ Branch 13 taken 27 times.
✓ Branch 14 taken 12 times.
✗ Branch 16 not taken.
✓ Branch 17 taken 39 times.
 39 if (joint_b && joint_b->robot() != joint_a->robot()) {
255 ✗ throw std::runtime_error("Joints do not belong to the same device.");
256 }
257
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 39 times.
 39 if (indexJointA() == indexJointB()) {
258 ✗ throw std::runtime_error("Bodies should be different");
259 }
260
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 39 times.
 39 if (tolerance < 0) {
261 ✗ throw std::runtime_error("tolerance should be non-negative.");
262 }
263
264
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 39 if (joint_a) {
265
2/4
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 39 times.
 39 assert(joint_a->linkedBody());
266 }
267
2/2
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 12 times.
 39 if (joint_b) {
268
2/4
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 27 times.
 27 assert(joint_b->linkedBody());
269 }
270 // Find sequence of joints
271
1/2
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
 39 JointIndices_t joints(m_->computeSequenceOfJoints());
272
1/2
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
 39 m_->setCoefficients(joints);
273 39 }
274
275 10 SolidSolidCollision::SolidSolidCollision(
276 const JointPtr_t& joint_a, const ConstObjectStdVector_t& objects_b,
277 10 value_type tolerance)
278
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
 10 : BodyPairCollision(tolerance), m_(new Model) {
279 10 m_->joint_a = joint_a;
280
281
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
 10 assert(joint_a);
282
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 BodyPtr_t body_a = joint_a->linkedBody();
283
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 10 times.
 10 assert(body_a);
284
3/4
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 9 times.
✓ Branch 5 taken 10 times.
 19 for (size_type i = 0; i < body_a->nbInnerObjects(); ++i) {
285
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 CollisionObjectConstPtr_t obj = body_a->innerObjectAt(i);
286 9 for (ConstObjectStdVector_t::const_iterator it = objects_b.begin();
287
2/2
✓ Branch 3 taken 9 times.
✓ Branch 4 taken 9 times.
 18 it != objects_b.end(); ++it) {
288
6/24
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
✗ Branch 21 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 24 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 27 not taken.
✓ Branch 28 taken 9 times.
✓ Branch 30 taken 9 times.
✗ Branch 31 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
 9 assert(!(*it)->joint() || (*it)->joint()->robot() != joint_a->robot());
289
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 addCollisionPair(obj, *it);
290 }
291 9 }
292 // Find sequence of joints
293
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 JointIndices_t joints(m_->computeSequenceOfJoints());
294
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 m_->setCoefficients(joints);
295 10 }
296
297 202 void SolidSolidCollision::init(const SolidSolidCollisionWkPtr_t& weak) {
298 202 weak_ = weak;
299 202 }
300
301 } // namespace continuousValidation
302 } // namespace core
303 } // namespace hpp
304