hpp-core  4.12.0
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 // This file is part of hpp-core
5 // hpp-core is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-core is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-core If not, see
16 // <http://www.gnu.org/licenses/>.
17 
18 #ifndef HPP_CORE_OBSTACLE_USER_HH
19 # define HPP_CORE_OBSTACLE_USER_HH
20 
21 # include <hpp/fcl/collision_data.h>
22 
23 # include <hpp/core/config.hh>
24 # include <hpp/core/fwd.hh>
27 
28 namespace hpp {
29  namespace core {
37  {
38  public:
39  virtual ~ObstacleUserInterface () = default;
40 
43  virtual void addObstacle (const CollisionObjectConstPtr_t& object) = 0;
44 
48  virtual void removeObstacleFromJoint(const JointPtr_t& joint,
49  const CollisionObjectConstPtr_t& object) = 0;
50 
58  virtual void filterCollisionPairs (const RelativeMotion::matrix_type& relMotion) = 0;
59 
68  virtual void setSecurityMargins(const matrix_t& securityMatrix) = 0;
69 
70 
74  virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
75  const std::string& body_b,
76  const value_type& margin) = 0;
77  }; // class ObstacleUserInterface
78 
95  template<typename Derived>
97  {
98  public:
99  virtual ~ObstacleUserVector () = default;
100 
107  {
108  for (std::size_t i = 0; i < validations_.size(); ++i) {
109  shared_ptr<ObstacleUserInterface> oui =
110  HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
111  if (oui) oui->addObstacle (object);
112  }
113  }
114 
121  const CollisionObjectConstPtr_t& object)
122  {
123  for (std::size_t i = 0; i < validations_.size(); ++i) {
124  shared_ptr<ObstacleUserInterface> oui =
125  HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
126  if (oui) oui->removeObstacleFromJoint (joint, object);
127  }
128  }
129 
136  {
137  for (std::size_t i = 0; i < validations_.size(); ++i) {
138  shared_ptr<ObstacleUserInterface> oui =
139  HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
140  if (oui) oui->filterCollisionPairs (relMotion);
141  }
142  }
143 
145  void setSecurityMargins(const matrix_t& securityMatrix)
146  {
147  for (std::size_t i = 0; i < validations_.size(); ++i) {
148  shared_ptr<ObstacleUserInterface> oui =
149  HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
150  if (oui) oui->setSecurityMargins (securityMatrix);
151  }
152  }
153 
155  void setSecurityMarginBetweenBodies(const std::string& body_a,
156  const std::string& body_b,
157  const value_type& margin)
158  {
159  for (std::size_t i = 0; i < validations_.size(); ++i) {
160  shared_ptr<ObstacleUserInterface> oui =
161  HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, validations_[i]);
162  if (oui)
163  oui->setSecurityMarginBetweenBodies (body_a, body_b, margin);
164  }
165  }
166 
167  // Clear the vector of config validations
168  void clear ()
169  {
170  validations_.clear ();
171  }
172 
173  protected:
174  typedef Derived value_t;
175  typedef std::vector<value_t> values_t;
176 
177  ObstacleUserVector() = default;
178  ObstacleUserVector(std::initializer_list<value_t> validations) :
179  validations_ (validations)
180  {};
181 
182  values_t validations_;
183  }; // class ObstacleUserVector
184 
187  {
188  public:
189  virtual ~ObstacleUser () = default;
190 
191  static bool collide (const CollisionPairs_t& pairs,
192  CollisionRequests_t& reqs,
193  fcl::CollisionResult& res,
194  std::size_t& i,
195  pinocchio::DeviceData& data);
196 
197  // Get pairs checked for collision
198  const CollisionPairs_t& pairs () const
199  {
200  return cPairs_;
201  }
202 
203  // Get pairs checked for collision
205  {
206  return cPairs_;
207  }
208 
209  // Get requests of collision pairs
211  {
212  return cRequests_;
213  }
214 
215  // Get requests of collision pairs
217  {
218  return cRequests_;
219  }
220 
221  fcl::CollisionRequest& defaultRequest()
222  {
223  return defaultRequest_;
224  }
225 
226  void setRequests (const fcl::CollisionRequest& r);
227 
230  virtual void addObstacle (const CollisionObjectConstPtr_t& object);
231 
237  virtual void addObstacleToJoint (const CollisionObjectConstPtr_t& object,
238  const JointPtr_t& joint, const bool includeChildren);
239 
243  virtual void removeObstacleFromJoint(const JointPtr_t& joint,
244  const CollisionObjectConstPtr_t& object);
245 
254  virtual void filterCollisionPairs (const RelativeMotion::matrix_type& relMotion);
255 
257  virtual void setSecurityMargins(const matrix_t& securityMatrix);
258 
260  virtual void setSecurityMarginBetweenBodies(const std::string& body_a,
261  const std::string& body_b,
262  const value_type& margin);
263 
264  protected:
267  : robot_ (robot), defaultRequest_ (fcl::NO_REQUEST,1)
268  {
269  defaultRequest_.enable_cached_gjk_guess = true;
270  }
271 
273  ObstacleUser (const ObstacleUser& other)
274  : robot_ (other.robot_),
275  defaultRequest_ (other.defaultRequest_),
276  cPairs_ (other.cPairs_),
277  pPairs_ (other.pPairs_),
278  dPairs_ (other.dPairs_),
279  cRequests_ (other.cRequests_),
280  pRequests_ (other.pRequests_),
281  dRequests_ (other.dRequests_)
282  {}
283 
284  void addRobotCollisionPairs ();
285 
287  fcl::CollisionRequest defaultRequest_;
288 
290  pPairs_,
291  dPairs_;
292  CollisionRequests_t cRequests_,
293  pRequests_,
294  dRequests_;
295  }; // class ObstacleUser
296  } // namespace core
297 } // namespace hpp
298 #endif // HPP_CORE_OBSTACLE_USER_HH
fcl::CollisionRequest & defaultRequest()
Definition: obstacle-user.hh:221
const CollisionPairs_t & pairs() const
Definition: obstacle-user.hh:198
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
Definition: obstacle-user.hh:96
Derived value_t
Definition: obstacle-user.hh:174
ObstacleUserVector(std::initializer_list< value_t > validations)
Definition: obstacle-user.hh:178
Definition: bi-rrt-planner.hh:24
ObstacleUser(DevicePtr_t robot)
Constructor of body pair collision.
Definition: obstacle-user.hh:266
void addObstacle(const CollisionObjectConstPtr_t &object)
Definition: obstacle-user.hh:106
CollisionRequests_t pRequests_
Active collision requests.
Definition: obstacle-user.hh:292
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:133
std::vector< CollisionPair_t > CollisionPairs_t
Definition: fwd.hh:221
std::vector< fcl::CollisionRequest > CollisionRequests_t
Definition: collision-pair.hh:32
CollisionPairs_t & pairs()
Definition: obstacle-user.hh:204
void setSecurityMarginBetweenBodies(const std::string &body_a, const std::string &body_b, const value_type &margin)
Definition: obstacle-user.hh:155
void clear()
Definition: obstacle-user.hh:168
DevicePtr_t robot_
Definition: obstacle-user.hh:286
pinocchio::matrix_t matrix_t
Definition: fwd.hh:145
fcl::CollisionRequest defaultRequest_
Definition: obstacle-user.hh:287
void filterCollisionPairs(const RelativeMotion::matrix_type &relMotion)
Definition: obstacle-user.hh:135
pinocchio::value_type value_type
Definition: fwd.hh:157
void setSecurityMargins(const matrix_t &securityMatrix)
Definition: obstacle-user.hh:145
void removeObstacleFromJoint(const JointPtr_t &joint, const CollisionObjectConstPtr_t &object)
Definition: obstacle-user.hh:120
CollisionRequests_t & requests()
Definition: obstacle-user.hh:216
Definition: obstacle-user.hh:36
const CollisionRequests_t & requests() const
Definition: obstacle-user.hh:210
std::vector< value_t > values_t
Definition: obstacle-user.hh:175
#define HPP_CORE_DLLAPI
Definition: config.hh:64
ObstacleUser(const ObstacleUser &other)
Copy constructor.
Definition: obstacle-user.hh:273
Eigen::Matrix< RelativeMotionType, Eigen::Dynamic, Eigen::Dynamic > matrix_type
Definition: relative-motion.hh:51
Stores a set of obstacles (movable or static).
Definition: obstacle-user.hh:186
CollisionPairs_t pPairs_
Active collision pairs.
Definition: obstacle-user.hh:289
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition: fwd.hh:90