Directory: | ./ |
---|---|
File: | src/obstacle-user.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 30 | 123 | 24.4% |
Branches: | 31 | 226 | 13.7% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // Copyright (c) 2019 CNRS | ||
2 | // Authors: Joseph Mirabel | ||
3 | // | ||
4 | |||
5 | // Redistribution and use in source and binary forms, with or without | ||
6 | // modification, are permitted provided that the following conditions are | ||
7 | // met: | ||
8 | // | ||
9 | // 1. Redistributions of source code must retain the above copyright | ||
10 | // notice, this list of conditions and the following disclaimer. | ||
11 | // | ||
12 | // 2. Redistributions in binary form must reproduce the above copyright | ||
13 | // notice, this list of conditions and the following disclaimer in the | ||
14 | // documentation and/or other materials provided with the distribution. | ||
15 | // | ||
16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
17 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
18 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
19 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
20 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
21 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
22 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
23 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
24 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
27 | // DAMAGE. | ||
28 | |||
29 | #include <hpp/fcl/collision.h> | ||
30 | |||
31 | #include <hpp/core/obstacle-user.hh> | ||
32 | #include <hpp/pinocchio/body.hh> | ||
33 | #include <hpp/pinocchio/collision-object.hh> | ||
34 | #include <hpp/pinocchio/device.hh> | ||
35 | #include <hpp/util/exception-factory.hh> | ||
36 | #include <pinocchio/multibody/geometry.hpp> | ||
37 | #include <pinocchio/spatial/fcl-pinocchio-conversions.hpp> | ||
38 | |||
39 | namespace hpp { | ||
40 | namespace core { | ||
41 | using ::pinocchio::toFclTransform3f; | ||
42 | 49385 | bool ObstacleUser::collide(const CollisionPairs_t& pairs, | |
43 | CollisionRequests_t& reqs, fcl::CollisionResult& res, | ||
44 | std::size_t& i, pinocchio::DeviceData& data) { | ||
45 |
2/2✓ Branch 1 taken 181811 times.
✓ Branch 2 taken 49278 times.
|
231087 | for (i = 0; i < pairs.size(); ++i) { |
46 | 181811 | res.clear(); | |
47 |
2/2✓ Branch 3 taken 109 times.
✓ Branch 4 taken 181702 times.
|
181807 | if (pairs[i].collide(data, reqs[i], res) != 0) return true; |
48 | } | ||
49 | 49278 | return false; | |
50 | } | ||
51 | |||
52 | 10 | void ObstacleUser::addObstacle(const CollisionObjectConstPtr_t& object) { | |
53 |
3/4✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 22 times.
✓ Branch 5 taken 10 times.
|
32 | for (size_type j = 0; j < robot_->nbJoints(); ++j) { |
54 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
22 | JointPtr_t joint = robot_->jointAt(j); |
55 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
22 | addObstacleToJoint(object, joint, false); |
56 | 22 | } | |
57 | 10 | } | |
58 | |||
59 | 22 | void ObstacleUser::addObstacleToJoint(const CollisionObjectConstPtr_t& object, | |
60 | const JointPtr_t& joint, | ||
61 | const bool includeChildren) { | ||
62 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
22 | BodyPtr_t body = joint->linkedBody(); |
63 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
22 | if (body) { |
64 |
3/4✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 22 times.
✓ Branch 5 taken 22 times.
|
44 | for (size_type o = 0; o < body->nbInnerObjects(); ++o) { |
65 | // TODO: check the objects are not in same joint | ||
66 |
3/6✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
|
22 | cPairs_.push_back(CollisionPair_t(body->innerObjectAt(o), object)); |
67 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
22 | cRequests_.push_back(defaultRequest_); |
68 | } | ||
69 | } | ||
70 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 22 times.
|
22 | if (includeChildren) { |
71 | ✗ | for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) { | |
72 | ✗ | addObstacleToJoint(object, joint->childJoint(i), includeChildren); | |
73 | } | ||
74 | } | ||
75 | 22 | } | |
76 | |||
77 | struct CollisionPairComparision { | ||
78 | CollisionPair_t a; | ||
79 | ✗ | CollisionPairComparision(const CollisionPair_t& p) : a(p) {} | |
80 | ✗ | bool operator()(const CollisionPair_t& b) { | |
81 | ✗ | return (&(a.first->pinocchio()) == &(b.first->pinocchio())) && | |
82 | ✗ | (&(a.second->pinocchio()) == &(b.second->pinocchio())); | |
83 | } | ||
84 | }; | ||
85 | |||
86 | ✗ | void ObstacleUser::removeObstacleFromJoint( | |
87 | const JointPtr_t& joint, const CollisionObjectConstPtr_t& obstacle) { | ||
88 | ✗ | BodyPtr_t body = joint->linkedBody(); | |
89 | ✗ | if (body) { | |
90 | ✗ | for (size_type o = 0; o < body->nbInnerObjects(); ++o) { | |
91 | ✗ | CollisionPair_t colPair(body->innerObjectAt(o), obstacle); | |
92 | ✗ | CollisionPairComparision compare(colPair); | |
93 | ✗ | std::size_t nbDelPairs = 0; | |
94 | ✗ | for (std::size_t i = 0; i < cPairs_.size();) { | |
95 | ✗ | if (compare(cPairs_[i])) { | |
96 | ✗ | cPairs_.erase(cPairs_.begin() + i); | |
97 | ✗ | cRequests_.erase(cRequests_.begin() + i); | |
98 | ✗ | ++nbDelPairs; | |
99 | } else | ||
100 | ✗ | ++i; | |
101 | } | ||
102 | ✗ | if (nbDelPairs == 0) { | |
103 | ✗ | std::ostringstream oss; | |
104 | oss << "ObstacleUser::removeObstacleFromJoint: obstacle \"" | ||
105 | ✗ | << obstacle->name() | |
106 | ✗ | << "\" is not registered as obstacle for joint \"" << joint->name() | |
107 | ✗ | << "\"."; | |
108 | ✗ | throw std::runtime_error(oss.str()); | |
109 | ✗ | } else if (nbDelPairs >= 2) { | |
110 | hppDout(error, "obstacle " << obstacle->name() << " was registered " | ||
111 | << nbDelPairs | ||
112 | << " times as obstacle for joint " | ||
113 | << joint->name() << "."); | ||
114 | } | ||
115 | } | ||
116 | } | ||
117 | } | ||
118 | |||
119 | ✗ | void ObstacleUser::filterCollisionPairs( | |
120 | const RelativeMotion::matrix_type& matrix) { | ||
121 | // Loop over collision pairs and remove disabled ones. | ||
122 | pinocchio::JointIndex j1, j2; | ||
123 | ✗ | fcl::CollisionResult unused; | |
124 | ✗ | for (std::size_t i = 0; i < cPairs_.size();) { | |
125 | ✗ | const CollisionPair_t& pair = cPairs_[i]; | |
126 | |||
127 | ✗ | j1 = pair.first->jointIndex(); | |
128 | ✗ | j2 = pair.second->jointIndex(); | |
129 | |||
130 | ✗ | switch (matrix(j1, j2)) { | |
131 | ✗ | case RelativeMotion::Parameterized: | |
132 | hppDout(info, "Parameterized collision pairs between " | ||
133 | << pair.first->name() << " and " | ||
134 | << pair.second->name()); | ||
135 | ✗ | pPairs_.push_back(pair); | |
136 | ✗ | pRequests_.push_back(cRequests_[i]); | |
137 | ✗ | cPairs_.erase(cPairs_.begin() + i); | |
138 | ✗ | cRequests_.erase(cRequests_.begin() + i); | |
139 | ✗ | break; | |
140 | ✗ | case RelativeMotion::Constrained: | |
141 | hppDout(info, "Disabling collision between " << pair.first->name() | ||
142 | << " and " | ||
143 | << pair.second->name()); | ||
144 | ✗ | if (pair.collide(cRequests_[i], unused) != 0) { | |
145 | hppDout(warning, | ||
146 | "Disabling collision detection between two " | ||
147 | "bodies in collision."); | ||
148 | } | ||
149 | ✗ | dPairs_.push_back(pair); | |
150 | ✗ | dRequests_.push_back(cRequests_[i]); | |
151 | ✗ | cPairs_.erase(cPairs_.begin() + i); | |
152 | ✗ | cRequests_.erase(cRequests_.begin() + i); | |
153 | ✗ | break; | |
154 | ✗ | case RelativeMotion::Unconstrained: | |
155 | ✗ | ++i; | |
156 | ✗ | break; | |
157 | ✗ | default: | |
158 | hppDout(warning, "RelativeMotionType not understood"); | ||
159 | ✗ | ++i; | |
160 | ✗ | break; | |
161 | } | ||
162 | } | ||
163 | } | ||
164 | |||
165 | ✗ | void ObstacleUser::setSecurityMargins(const matrix_t& securityMatrix) { | |
166 | ✗ | if (securityMatrix.rows() != robot_->nbJoints() + 1 || | |
167 | ✗ | securityMatrix.cols() != robot_->nbJoints() + 1) { | |
168 | ✗ | HPP_THROW(std::invalid_argument, | |
169 | "Wrong size of security margin matrix." | ||
170 | " Expected " | ||
171 | << robot_->nbJoints() + 1 << 'x' << robot_->nbJoints() + 1 | ||
172 | << ". Got " << securityMatrix.rows() << 'x' | ||
173 | << securityMatrix.cols()); | ||
174 | } | ||
175 | pinocchio::JointIndex j1, j2; | ||
176 | ✗ | fcl::CollisionResult unused; | |
177 | ✗ | for (std::size_t i = 0; i < cPairs_.size(); ++i) { | |
178 | ✗ | const CollisionPair_t& pair = cPairs_[i]; | |
179 | ✗ | j1 = pair.first->jointIndex(); | |
180 | ✗ | j2 = pair.second->jointIndex(); | |
181 | ✗ | cRequests_[i].security_margin = securityMatrix(j1, j2); | |
182 | } | ||
183 | ✗ | for (std::size_t i = 0; i < pPairs_.size(); ++i) { | |
184 | ✗ | const CollisionPair_t& pair = pPairs_[i]; | |
185 | ✗ | j1 = pair.first->jointIndex(); | |
186 | ✗ | j2 = pair.second->jointIndex(); | |
187 | ✗ | pRequests_[i].security_margin = securityMatrix(j1, j2); | |
188 | } | ||
189 | ✗ | for (std::size_t i = 0; i < dPairs_.size(); ++i) { | |
190 | ✗ | const CollisionPair_t& pair = dPairs_[i]; | |
191 | ✗ | j1 = pair.first->jointIndex(); | |
192 | ✗ | j2 = pair.second->jointIndex(); | |
193 | ✗ | dRequests_[i].security_margin = securityMatrix(j1, j2); | |
194 | } | ||
195 | } | ||
196 | |||
197 | ✗ | void ObstacleUser::setSecurityMarginBetweenBodies(const std::string& body_a, | |
198 | const std::string& body_b, | ||
199 | const value_type& margin) { | ||
200 | ✗ | auto setMargin = [&body_a, &body_b, &margin](const CollisionPairs_t& pairs, | |
201 | ✗ | CollisionRequests_t& requests) { | |
202 | ✗ | for (std::size_t i = 0; i < pairs.size(); ++i) { | |
203 | ✗ | const CollisionPair_t& pair = pairs[i]; | |
204 | ✗ | if ((pair.first->name() == body_a && pair.second->name() == body_b) || | |
205 | ✗ | (pair.first->name() == body_b && pair.second->name() == body_a)) { | |
206 | ✗ | requests[i].security_margin = margin; | |
207 | ✗ | return true; | |
208 | } | ||
209 | } | ||
210 | ✗ | return false; | |
211 | ✗ | }; | |
212 | ✗ | if (setMargin(cPairs_, cRequests_) || setMargin(pPairs_, pRequests_) || | |
213 | ✗ | setMargin(dPairs_, dRequests_)) | |
214 | ✗ | return; | |
215 | ✗ | throw std::invalid_argument( | |
216 | "Could not find a collision pair between body" | ||
217 | ✗ | " " + | |
218 | ✗ | body_a + " and " + body_b); | |
219 | } | ||
220 | |||
221 | 75 | void ObstacleUser::addRobotCollisionPairs() { | |
222 | 75 | const pinocchio::GeomModel& model = robot_->geomModel(); | |
223 | 75 | const pinocchio::GeomData& data = robot_->geomData(); | |
224 | |||
225 |
2/2✓ Branch 1 taken 5192 times.
✓ Branch 2 taken 75 times.
|
5267 | for (std::size_t i = 0; i < model.collisionPairs.size(); ++i) |
226 |
1/2✓ Branch 1 taken 5192 times.
✗ Branch 2 not taken.
|
5192 | if (data.activeCollisionPairs[i]) { |
227 | CollisionObjectConstPtr_t o1(new pinocchio::CollisionObject( | ||
228 |
3/6✓ Branch 3 taken 5192 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5192 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5192 times.
✗ Branch 10 not taken.
|
5192 | robot_, model.collisionPairs[i].first)); |
229 | CollisionObjectConstPtr_t o2(new pinocchio::CollisionObject( | ||
230 |
3/6✓ Branch 3 taken 5192 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5192 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5192 times.
✗ Branch 10 not taken.
|
5192 | robot_, model.collisionPairs[i].second)); |
231 |
2/4✓ Branch 3 taken 5192 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5192 times.
✗ Branch 7 not taken.
|
5192 | cPairs_.push_back(CollisionPair_t(o1, o2)); |
232 |
1/2✓ Branch 1 taken 5192 times.
✗ Branch 2 not taken.
|
5192 | cRequests_.push_back(defaultRequest_); |
233 | 5192 | } | |
234 | 75 | } | |
235 | |||
236 | ✗ | void ObstacleUser::setRequests(const fcl::CollisionRequest& r) { | |
237 | ✗ | for (std::size_t i = 0; i < cRequests_.size(); ++i) cRequests_[i] = r; | |
238 | ✗ | for (std::size_t i = 0; i < pRequests_.size(); ++i) pRequests_[i] = r; | |
239 | ✗ | for (std::size_t i = 0; i < dRequests_.size(); ++i) dRequests_[i] = r; | |
240 | } | ||
241 | } // namespace core | ||
242 | } // namespace hpp | ||
243 |