Directory: | ./ |
---|---|
File: | include/hpp/core/obstacle-user.hh |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 16 | 38 | 42.1% |
Branches: | 6 | 34 | 17.6% |
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 | #ifndef HPP_CORE_OBSTACLE_USER_HH | ||
30 | #define HPP_CORE_OBSTACLE_USER_HH | ||
31 | |||
32 | #include <hpp/fcl/collision_data.h> | ||
33 | |||
34 | #include <hpp/core/collision-pair.hh> | ||
35 | #include <hpp/core/config.hh> | ||
36 | #include <hpp/core/fwd.hh> | ||
37 | #include <hpp/core/relative-motion.hh> | ||
38 | |||
39 | namespace hpp { | ||
40 | namespace core { | ||
41 | /// Abstract class for handling obstacles | ||
42 | /// | ||
43 | /// Several classes perform collision detection between the bodies | ||
44 | /// of a robot and a set of rigid-body obstacles of the environment. | ||
45 | /// | ||
46 | /// This class defines a common abstract interface for those classes. | ||
47 | class HPP_CORE_DLLAPI ObstacleUserInterface { | ||
48 | public: | ||
49 | 314 | virtual ~ObstacleUserInterface() = default; | |
50 | |||
51 | /// Add an obstacle | ||
52 | /// \param object obstacle added | ||
53 | virtual void addObstacle(const CollisionObjectConstPtr_t& object) = 0; | ||
54 | |||
55 | /// Remove a collision pair between a joint and an obstacle | ||
56 | /// \param joint that holds the inner objects, | ||
57 | /// \param obstacle to remove. | ||
58 | virtual void removeObstacleFromJoint( | ||
59 | const JointPtr_t& joint, const CollisionObjectConstPtr_t& object) = 0; | ||
60 | |||
61 | /// Filter collision pairs. | ||
62 | /// | ||
63 | /// Remove pairs of object that cannot be in collision. | ||
64 | /// This effectively disables collision detection between objects that | ||
65 | /// have no possible relative motion due to the constraints. | ||
66 | /// | ||
67 | /// \param relMotion square symmetric matrix of RelativeMotionType of size | ||
68 | /// numberDof x numberDof | ||
69 | virtual void filterCollisionPairs( | ||
70 | const RelativeMotion::matrix_type& relMotion) = 0; | ||
71 | |||
72 | /// Set different security margins for collision pairs | ||
73 | /// | ||
74 | /// This function works joint-wise. If you need a finer control, use | ||
75 | /// \ref setSecurityMarginBetweenBodies | ||
76 | /// | ||
77 | /// This method enables users to choose different security margins | ||
78 | /// for each pair of robot joint or each pair robot joint - obstacle. | ||
79 | /// \sa hpp::fcl::CollisionRequest::security_margin. | ||
80 | virtual void setSecurityMargins(const matrix_t& securityMatrix) = 0; | ||
81 | |||
82 | /// Set security margin for collision pair between the two bodies. | ||
83 | /// | ||
84 | /// \sa hpp::fcl::CollisionRequest::security_margin. | ||
85 | virtual void setSecurityMarginBetweenBodies(const std::string& body_a, | ||
86 | const std::string& body_b, | ||
87 | const value_type& margin) = 0; | ||
88 | }; // class ObstacleUserInterface | ||
89 | |||
90 | /// Vector of validation class instances. | ||
91 | /// | ||
92 | /// \tparam Derived must be a shared point to some validation class. For | ||
93 | /// instance \link ConfigValidation ConfigValidationPtr_t \endlink | ||
94 | /// or \link PathValidation PathValidationPtr_t \endlink. | ||
95 | /// | ||
96 | /// This class implements the abstract interface defined by | ||
97 | /// ObstacleUserInterface and stores a vector of path or config validation | ||
98 | /// class instances. Methods | ||
99 | /// \link ObstacleUserVector::addObstacle addObstacle \endlink, | ||
100 | /// \link ObstacleUserVector::removeObstacleFromJoint | ||
101 | /// removeObstacleFromJoint\endlink, and | ||
102 | /// \link ObstacleUserVector::filterCollisionPairs filterCollisionPairs | ||
103 | /// \endlink iteratively dynamic cast each instance into | ||
104 | /// Obstacle user interface and calls the derived implementation in case of | ||
105 | /// success. | ||
106 | template <typename Derived> | ||
107 | class HPP_CORE_DLLAPI ObstacleUserVector : public ObstacleUserInterface { | ||
108 | public: | ||
109 | 184 | virtual ~ObstacleUserVector() = default; | |
110 | |||
111 | /// Add obstacle to each element | ||
112 | /// | ||
113 | /// Dynamic cast into ObstacleUserInterface each element | ||
114 | /// of \link ObstacleUserVector::validations_ validations_ | ||
115 | /// \endlink and call method addObstacle of element. | ||
116 | 11 | void addObstacle(const CollisionObjectConstPtr_t& object) { | |
117 |
2/2✓ Branch 2 taken 17 times.
✓ Branch 3 taken 11 times.
|
28 | for (std::size_t i = 0; i < validations_.size(); ++i) { |
118 | 17 | shared_ptr<ObstacleUserInterface> oui = | |
119 | 17 | HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]); | |
120 |
3/4✓ Branch 1 taken 10 times.
✓ Branch 2 taken 7 times.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
17 | if (oui) oui->addObstacle(object); |
121 | } | ||
122 | 11 | } | |
123 | |||
124 | /// Remove obstacle from joint to each element | ||
125 | /// | ||
126 | /// Dynamic cast into ObstacleUserInterface each element | ||
127 | /// of \link ObstacleUserVector::validations_ validations_ | ||
128 | /// \endlink and call method removeObstacleFromJoint of element. | ||
129 | ✗ | void removeObstacleFromJoint(const JointPtr_t& joint, | |
130 | const CollisionObjectConstPtr_t& object) { | ||
131 | ✗ | for (std::size_t i = 0; i < validations_.size(); ++i) { | |
132 | ✗ | shared_ptr<ObstacleUserInterface> oui = | |
133 | ✗ | HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]); | |
134 | ✗ | if (oui) oui->removeObstacleFromJoint(joint, object); | |
135 | } | ||
136 | } | ||
137 | |||
138 | /// Filter collision pairs to each element | ||
139 | /// | ||
140 | /// Dynamic cast into ObstacleUserInterface each element | ||
141 | /// of \link ObstacleUserVector::validations_ validations_ | ||
142 | /// \endlink and call method filterCollisionPairs of element. | ||
143 | ✗ | void filterCollisionPairs(const RelativeMotion::matrix_type& relMotion) { | |
144 | ✗ | for (std::size_t i = 0; i < validations_.size(); ++i) { | |
145 | ✗ | shared_ptr<ObstacleUserInterface> oui = | |
146 | ✗ | HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]); | |
147 | ✗ | if (oui) oui->filterCollisionPairs(relMotion); | |
148 | } | ||
149 | } | ||
150 | |||
151 | /// \copydoc ObstacleUserInterface::setSecurityMargins | ||
152 | ✗ | void setSecurityMargins(const matrix_t& securityMatrix) { | |
153 | ✗ | for (std::size_t i = 0; i < validations_.size(); ++i) { | |
154 | ✗ | shared_ptr<ObstacleUserInterface> oui = | |
155 | ✗ | HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]); | |
156 | ✗ | if (oui) oui->setSecurityMargins(securityMatrix); | |
157 | } | ||
158 | } | ||
159 | |||
160 | /// \copydoc ObstacleUserInterface::setSecurityMarginBetweenBodies | ||
161 | ✗ | void setSecurityMarginBetweenBodies(const std::string& body_a, | |
162 | const std::string& body_b, | ||
163 | const value_type& margin) { | ||
164 | ✗ | for (std::size_t i = 0; i < validations_.size(); ++i) { | |
165 | ✗ | shared_ptr<ObstacleUserInterface> oui = | |
166 | ✗ | HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]); | |
167 | ✗ | if (oui) oui->setSecurityMarginBetweenBodies(body_a, body_b, margin); | |
168 | } | ||
169 | } | ||
170 | |||
171 | // Clear the vector of config validations | ||
172 | 12 | void clear() { validations_.clear(); } | |
173 | |||
174 | protected: | ||
175 | typedef Derived value_t; | ||
176 | typedef std::vector<value_t> values_t; | ||
177 | |||
178 | 106 | ObstacleUserVector() = default; | |
179 | ✗ | ObstacleUserVector(std::initializer_list<value_t> validations) | |
180 | ✗ | : validations_(validations) {}; | |
181 | |||
182 | values_t validations_; | ||
183 | }; // class ObstacleUserVector | ||
184 | |||
185 | /// Stores a set of obstacles (movable or static). | ||
186 | class HPP_CORE_DLLAPI ObstacleUser : public ObstacleUserInterface { | ||
187 | public: | ||
188 | 122 | virtual ~ObstacleUser() = default; | |
189 | |||
190 | static bool collide(const CollisionPairs_t& pairs, CollisionRequests_t& reqs, | ||
191 | fcl::CollisionResult& res, std::size_t& i, | ||
192 | pinocchio::DeviceData& data); | ||
193 | |||
194 | // Get pairs checked for collision | ||
195 | const CollisionPairs_t& pairs() const { return cPairs_; } | ||
196 | |||
197 | // Get pairs checked for collision | ||
198 | CollisionPairs_t& pairs() { return cPairs_; } | ||
199 | |||
200 | // Get requests of collision pairs | ||
201 | const CollisionRequests_t& requests() const { return cRequests_; } | ||
202 | |||
203 | // Get requests of collision pairs | ||
204 | CollisionRequests_t& requests() { return cRequests_; } | ||
205 | |||
206 | 75 | fcl::CollisionRequest& defaultRequest() { return defaultRequest_; } | |
207 | |||
208 | void setRequests(const fcl::CollisionRequest& r); | ||
209 | |||
210 | /// Add an obstacle | ||
211 | /// \param object obstacle added | ||
212 | virtual void addObstacle(const CollisionObjectConstPtr_t& object); | ||
213 | |||
214 | /// Add an obstacle to a specific joint | ||
215 | /// \param object obstacle added | ||
216 | /// \param joint concerned with obstacle addition | ||
217 | /// \param includeChildren whether to add obstacle to joint children | ||
218 | /// Store obstacle and build a collision pair with each body of the robot. | ||
219 | virtual void addObstacleToJoint(const CollisionObjectConstPtr_t& object, | ||
220 | const JointPtr_t& joint, | ||
221 | const bool includeChildren); | ||
222 | |||
223 | /// Remove a collision pair between a joint and an obstacle | ||
224 | /// \param joint that holds the inner objects, | ||
225 | /// \param obstacle to remove. | ||
226 | virtual void removeObstacleFromJoint(const JointPtr_t& joint, | ||
227 | const CollisionObjectConstPtr_t& object); | ||
228 | |||
229 | /// Filter collision pairs. | ||
230 | /// | ||
231 | /// Remove pairs of object that cannot be in collision. | ||
232 | /// This effectively disables collision detection between objects that | ||
233 | /// have no possible relative motion due to the constraints. | ||
234 | /// \todo Before disabling collision pair, check if there is a collision. | ||
235 | /// | ||
236 | /// \param relMotion square symmetric matrix of RelativeMotionType of size | ||
237 | /// numberDof x numberDof | ||
238 | virtual void filterCollisionPairs( | ||
239 | const RelativeMotion::matrix_type& relMotion); | ||
240 | |||
241 | /// \copydoc ObstacleUserInterface::setSecurityMargins | ||
242 | virtual void setSecurityMargins(const matrix_t& securityMatrix); | ||
243 | |||
244 | /// \copydoc ObstacleUserInterface::setSecurityMarginBetweenBodies | ||
245 | virtual void setSecurityMarginBetweenBodies(const std::string& body_a, | ||
246 | const std::string& body_b, | ||
247 | const value_type& margin); | ||
248 | |||
249 | protected: | ||
250 | /// Constructor of body pair collision | ||
251 | 75 | ObstacleUser(DevicePtr_t robot) | |
252 |
1/2✓ Branch 3 taken 75 times.
✗ Branch 4 not taken.
|
75 | : robot_(robot), defaultRequest_(fcl::NO_REQUEST, 1) { |
253 | 75 | defaultRequest_.enable_cached_gjk_guess = true; | |
254 | 75 | } | |
255 | |||
256 | /// Copy constructor | ||
257 | ObstacleUser(const ObstacleUser& other) | ||
258 | : robot_(other.robot_), | ||
259 | defaultRequest_(other.defaultRequest_), | ||
260 | cPairs_(other.cPairs_), | ||
261 | pPairs_(other.pPairs_), | ||
262 | dPairs_(other.dPairs_), | ||
263 | cRequests_(other.cRequests_), | ||
264 | pRequests_(other.pRequests_), | ||
265 | dRequests_(other.dRequests_) {} | ||
266 | |||
267 | void addRobotCollisionPairs(); | ||
268 | |||
269 | DevicePtr_t robot_; | ||
270 | fcl::CollisionRequest defaultRequest_; | ||
271 | |||
272 | CollisionPairs_t cPairs_, /// Active collision pairs | ||
273 | pPairs_, /// Parameterized collision pairs | ||
274 | dPairs_; /// Disabled collision pairs | ||
275 | CollisionRequests_t cRequests_, /// Active collision requests | ||
276 | pRequests_, /// Parameterized collision requests | ||
277 | dRequests_; /// Disabled collision requests | ||
278 | }; // class ObstacleUser | ||
279 | } // namespace core | ||
280 | } // namespace hpp | ||
281 | #endif // HPP_CORE_OBSTACLE_USER_HH | ||
282 |