hpp-core  4.15.1
Implement basic classes for canonical path planning for kinematic chains.
obstacle-user.hh
Go to the documentation of this file.
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 
35 #include <hpp/core/config.hh>
36 #include <hpp/core/fwd.hh>
38 
39 namespace hpp {
40 namespace core {
48  public:
49  virtual ~ObstacleUserInterface() = default;
50 
53  virtual void addObstacle(const CollisionObjectConstPtr_t& object) = 0;
54 
58  virtual void removeObstacleFromJoint(
59  const JointPtr_t& joint, const CollisionObjectConstPtr_t& object) = 0;
60 
69  virtual void filterCollisionPairs(
70  const RelativeMotion::matrix_type& relMotion) = 0;
71 
80  virtual void setSecurityMargins(const matrix_t& securityMatrix) = 0;
81 
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 
106 template <typename Derived>
108  public:
109  virtual ~ObstacleUserVector() = default;
110 
117  for (std::size_t i = 0; i < validations_.size(); ++i) {
118  shared_ptr<ObstacleUserInterface> oui =
119  HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
120  if (oui) oui->addObstacle(object);
121  }
122  }
123 
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 
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 
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 
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  void clear() { validations_.clear(); }
173 
174  protected:
175  typedef Derived value_t;
176  typedef std::vector<value_t> values_t;
177 
178  ObstacleUserVector() = default;
179  ObstacleUserVector(std::initializer_list<value_t> validations)
180  : validations_(validations){};
181 
182  values_t validations_;
183 }; // class ObstacleUserVector
184 
187  public:
188  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  fcl::CollisionRequest& defaultRequest() { return defaultRequest_; }
207 
208  void setRequests(const fcl::CollisionRequest& r);
209 
212  virtual void addObstacle(const CollisionObjectConstPtr_t& object);
213 
219  virtual void addObstacleToJoint(const CollisionObjectConstPtr_t& object,
220  const JointPtr_t& joint,
221  const bool includeChildren);
222 
226  virtual void removeObstacleFromJoint(const JointPtr_t& joint,
227  const CollisionObjectConstPtr_t& object);
228 
238  virtual void filterCollisionPairs(
239  const RelativeMotion::matrix_type& relMotion);
240 
242  virtual void setSecurityMargins(const matrix_t& securityMatrix);
243 
245  virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
246  const std::string& body_b,
247  const value_type& margin);
248 
249  protected:
252  : robot_(robot), defaultRequest_(fcl::NO_REQUEST, 1) {
253  defaultRequest_.enable_cached_gjk_guess = true;
254  }
255 
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 
270  fcl::CollisionRequest defaultRequest_;
271 
273  pPairs_,
274  dPairs_;
275  CollisionRequests_t cRequests_,
276  pRequests_,
277  dRequests_;
278 }; // class ObstacleUser
279 } // namespace core
280 } // namespace hpp
281 #endif // HPP_CORE_OBSTACLE_USER_HH
fcl::CollisionRequest & defaultRequest()
Definition: obstacle-user.hh:206
const CollisionPairs_t & pairs() const
Definition: obstacle-user.hh:195
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:134
Definition: obstacle-user.hh:107
values_t validations_
Definition: obstacle-user.hh:180
Derived value_t
Definition: obstacle-user.hh:175
ObstacleUserVector(std::initializer_list< value_t > validations)
Definition: obstacle-user.hh:179
Definition: bi-rrt-planner.hh:35
ObstacleUser(DevicePtr_t robot)
Constructor of body pair collision.
Definition: obstacle-user.hh:251
void addObstacle(const CollisionObjectConstPtr_t &object)
Definition: obstacle-user.hh:116
CollisionRequests_t pRequests_
Active collision requests.
Definition: obstacle-user.hh:275
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:151
std::vector< CollisionPair_t > CollisionPairs_t
Definition: fwd.hh:239
std::vector< fcl::CollisionRequest > CollisionRequests_t
Definition: collision-pair.hh:43
CollisionPairs_t & pairs()
Definition: obstacle-user.hh:198
void setSecurityMarginBetweenBodies(const std::string &body_a, const std::string &body_b, const value_type &margin)
Definition: obstacle-user.hh:161
void clear()
Definition: obstacle-user.hh:172
DevicePtr_t robot_
Definition: obstacle-user.hh:269
pinocchio::matrix_t matrix_t
Definition: fwd.hh:162
fcl::CollisionRequest defaultRequest_
Definition: obstacle-user.hh:270
void filterCollisionPairs(const RelativeMotion::matrix_type &relMotion)
Definition: obstacle-user.hh:143
pinocchio::value_type value_type
Definition: fwd.hh:174
void setSecurityMargins(const matrix_t &securityMatrix)
Definition: obstacle-user.hh:152
void removeObstacleFromJoint(const JointPtr_t &joint, const CollisionObjectConstPtr_t &object)
Definition: obstacle-user.hh:129
CollisionRequests_t & requests()
Definition: obstacle-user.hh:204
Definition: obstacle-user.hh:47
const CollisionRequests_t & requests() const
Definition: obstacle-user.hh:201
std::vector< value_t > values_t
Definition: obstacle-user.hh:176
#define HPP_CORE_DLLAPI
Definition: config.hh:64
ObstacleUser(const ObstacleUser &other)
Copy constructor.
Definition: obstacle-user.hh:257
Eigen::Matrix< RelativeMotionType, Eigen::Dynamic, Eigen::Dynamic > matrix_type
Definition: relative-motion.hh:62
Stores a set of obstacles (movable or static).
Definition: obstacle-user.hh:186
CollisionPairs_t pPairs_
Active collision pairs.
Definition: obstacle-user.hh:272
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition: fwd.hh:99