GCC Code Coverage Report

Directory: ./
File: src/collision-validation.cc
Date: 2024-08-10 11:29:48
Exec Total Coverage
Lines: 31 45 68.9%
Branches: 22 68 32.4%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
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 <hpp/fcl/collision.h>
31
32 #include <hpp/core/collision-validation-report.hh>
33 #include <hpp/core/collision-validation.hh>
34 #include <hpp/core/relative-motion.hh>
35 #include <hpp/pinocchio/body.hh>
36 #include <hpp/pinocchio/collision-object.hh>
37 #include <hpp/pinocchio/configuration.hh>
38 #include <hpp/pinocchio/device.hh>
39 #include <pinocchio/multibody/geometry.hpp>
40
41 namespace hpp {
42 namespace core {
43 75 CollisionValidationPtr_t CollisionValidation::create(const DevicePtr_t& robot) {
44
1/2
✓ Branch 2 taken 75 times.
✗ Branch 3 not taken.
 75 CollisionValidation* ptr = new CollisionValidation(robot);
45 75 return CollisionValidationPtr_t(ptr);
46 }
47
48 49386 bool CollisionValidation::validate(const Configuration_t& config,
49 ValidationReportPtr_t& validationReport) {
50
1/2
✓ Branch 1 taken 49387 times.
✗ Branch 2 not taken.
 49386 pinocchio::DeviceSync device(robot_);
51
2/4
✓ Branch 1 taken 49387 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49386 times.
✗ Branch 5 not taken.
 49387 device.currentConfiguration(config);
52
1/2
✓ Branch 1 taken 49384 times.
✗ Branch 2 not taken.
 49385 device.computeForwardKinematics(pinocchio::JOINT_POSITION);
53
1/2
✓ Branch 1 taken 49385 times.
✗ Branch 2 not taken.
 49384 device.updateGeometryPlacements();
54
55
1/2
✓ Branch 1 taken 49386 times.
✗ Branch 2 not taken.
 49385 fcl::CollisionResult collisionResult;
56 49386 std::size_t iPair = 0;
57 49386 const CollisionPairs_t* pairs(&cPairs_);
58 49386 CollisionRequests_t* requests(&cRequests_);
59
60
2/4
✓ Branch 1 taken 49385 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49386 times.
✗ Branch 5 not taken.
 49386 bool collide = ObstacleUser::collide(cPairs_, cRequests_, collisionResult,
61 iPair, device.d());
62
3/4
✓ Branch 0 taken 49277 times.
✓ Branch 1 taken 109 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 49277 times.
 49386 if (!collide && checkParameterized_) {
63 ✗ collide = ObstacleUser::collide(pPairs_, pRequests_, collisionResult, iPair,
64 device.d());
65 ✗ pairs = &pPairs_;
66 ✗ requests = &pRequests_;
67 }
68
2/2
✓ Branch 0 taken 109 times.
✓ Branch 1 taken 49277 times.
 49386 if (collide) {
69 CollisionValidationReportPtr_t report(
70
3/6
✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 109 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 109 times.
✗ Branch 9 not taken.
 109 new CollisionValidationReport((*pairs)[iPair], collisionResult));
71
72
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 109 times.
 109 if (computeAllContacts_) {
73 // create report with the first collision
74 AllCollisionsValidationReportPtr_t allReport(
75 ✗ new AllCollisionsValidationReport((*pairs)[iPair], collisionResult));
76 ✗ allReport->collisionReports.push_back(report);
77 // then loop over all the remaining pairs :
78 ✗ ++iPair;
79 ✗ for (; iPair < pairs->size(); ++iPair) {
80 ✗ collisionResult.clear();
81 ✗ if ((*pairs)[iPair].collide(device.d(), (*requests)[iPair],
82 ✗ collisionResult) != 0) {
83 CollisionValidationReportPtr_t report(
84 ✗ new CollisionValidationReport((*pairs)[iPair], collisionResult));
85 ✗ allReport->collisionReports.push_back(report);
86 }
87 }
88 ✗ validationReport = allReport;
89 ✗ } else {
90 109 validationReport = report;
91 }
92 109 return false;
93 109 }
94 49277 return true;
95 49386 }
96
97 75 CollisionValidation::CollisionValidation(const DevicePtr_t& robot)
98 : ObstacleUser(robot),
99 75 robot_(robot),
100 75 checkParameterized_(false),
101
1/2
✓ Branch 3 taken 75 times.
✗ Branch 4 not taken.
 75 computeAllContacts_(false) {
102
1/2
✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
 75 fcl::CollisionRequest req(fcl::NO_REQUEST, 1);
103 75 req.enable_cached_gjk_guess = true;
104
1/2
✓ Branch 2 taken 75 times.
✗ Branch 3 not taken.
 75 defaultRequest() = req;
105
1/2
✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
 75 addRobotCollisionPairs();
106 75 }
107
108 } // namespace core
109 } // namespace hpp
110