GCC Code Coverage Report

Directory: ./
File: include/hpp/core/obstacle-user.hh
Date: 2025-03-10 11:18:21
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 <coal/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 coal::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 coal::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 coal::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 coal::CollisionRequest& defaultRequest() { return defaultRequest_; }
207
208 void setRequests(const coal::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_(coal::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 coal::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