hpp-core: include/hpp/core/obstacle-user.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
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 <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
39namespace hpp {
40namespace core {
47 class HPP_CORE_DLLAPI ObstacleUserInterface {
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
106template <typename Derived>
107 class HPP_CORE_DLLAPI ObstacleUserVector : public ObstacleUserInterface {
108 public:
109 virtual ~ObstacleUserVector() = default;
110
116 void addObstacle(const CollisionObjectConstPtr_t& object) {
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
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
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
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
186 class HPP_CORE_DLLAPI ObstacleUser : public ObstacleUserInterface {
187 public:
188 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 coal::CollisionRequest& defaultRequest() { return defaultRequest_; }
207
208 void setRequests(const coal::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:
251 ObstacleUser(DevicePtr_t robot)
252 : robot_(robot), defaultRequest_(coal::NO_REQUEST, 1) {
253 defaultRequest_.enable_cached_gjk_guess = true;
254 }
255
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_,
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
hpp::core::ObstacleUserInterface
Definition obstacle-user.hh:47
hpp::core::ObstacleUserInterface::removeObstacleFromJoint
virtual void removeObstacleFromJoint(const JointPtr_t &joint, const CollisionObjectConstPtr_t &object)=0
hpp::core::ObstacleUserInterface::addObstacle
virtual void addObstacle(const CollisionObjectConstPtr_t &object)=0
hpp::core::ObstacleUserInterface::filterCollisionPairs
virtual void filterCollisionPairs(const RelativeMotion::matrix_type &relMotion)=0
hpp::core::ObstacleUserInterface::~ObstacleUserInterface
virtual ~ObstacleUserInterface()=default
hpp::core::ObstacleUserInterface::setSecurityMarginBetweenBodies
virtual void setSecurityMarginBetweenBodies(const std::string &body_a, const std::string &body_b, const value_type &margin)=0
hpp::core::ObstacleUserInterface::setSecurityMargins
virtual void setSecurityMargins(const matrix_t &securityMatrix)=0
hpp::core::ObstacleUserVector
Definition obstacle-user.hh:107
hpp::core::ObstacleUserVector::ObstacleUserVector
ObstacleUserVector(std::initializer_list< value_t > validations)
Definition obstacle-user.hh:179
hpp::core::ObstacleUserVector::filterCollisionPairs
void filterCollisionPairs(const RelativeMotion::matrix_type &relMotion)
Definition obstacle-user.hh:143
hpp::core::ObstacleUserVector::clear
void clear()
Definition obstacle-user.hh:172
hpp::core::ObstacleUserVector::~ObstacleUserVector
virtual ~ObstacleUserVector()=default
hpp::core::ObstacleUserVector::validations_
values_t validations_
Definition obstacle-user.hh:182
hpp::core::ObstacleUserVector::value_t
Derived value_t
Definition obstacle-user.hh:175
hpp::core::ObstacleUserVector::setSecurityMarginBetweenBodies
void setSecurityMarginBetweenBodies(const std::string &body_a, const std::string &body_b, const value_type &margin)
Definition obstacle-user.hh:161
hpp::core::ObstacleUserVector::addObstacle
void addObstacle(const CollisionObjectConstPtr_t &object)
Definition obstacle-user.hh:116
hpp::core::ObstacleUserVector::ObstacleUserVector
ObstacleUserVector()=default
hpp::core::ObstacleUserVector::removeObstacleFromJoint
void removeObstacleFromJoint(const JointPtr_t &joint, const CollisionObjectConstPtr_t &object)
Definition obstacle-user.hh:129
hpp::core::ObstacleUserVector::values_t
std::vector< value_t > values_t
Definition obstacle-user.hh:176
hpp::core::ObstacleUserVector::setSecurityMargins
void setSecurityMargins(const matrix_t &securityMatrix)
Definition obstacle-user.hh:152
hpp::core::ObstacleUser
Stores a set of obstacles (movable or static).
Definition obstacle-user.hh:186
hpp::core::ObstacleUser::setSecurityMarginBetweenBodies
virtual void setSecurityMarginBetweenBodies(const std::string &body_a, const std::string &body_b, const value_type &margin)
hpp::core::ObstacleUser::setSecurityMargins
virtual void setSecurityMargins(const matrix_t &securityMatrix)
hpp::core::ObstacleUser::cRequests_
CollisionRequests_t cRequests_
Disabled collision pairs.
Definition obstacle-user.hh:275
hpp::core::ObstacleUser::pairs
CollisionPairs_t & pairs()
Definition obstacle-user.hh:198
hpp::core::ObstacleUser::~ObstacleUser
virtual ~ObstacleUser()=default
hpp::core::ObstacleUser::requests
CollisionRequests_t & requests()
Definition obstacle-user.hh:204
hpp::core::ObstacleUser::pPairs_
CollisionPairs_t pPairs_
Active collision pairs.
Definition obstacle-user.hh:273
hpp::core::ObstacleUser::pairs
const CollisionPairs_t & pairs() const
Definition obstacle-user.hh:195
hpp::core::ObstacleUser::ObstacleUser
ObstacleUser(DevicePtr_t robot)
Constructor of body pair collision.
Definition obstacle-user.hh:251
hpp::core::ObstacleUser::dRequests_
CollisionRequests_t dRequests_
Parameterized collision requests.
Definition obstacle-user.hh:277
hpp::core::ObstacleUser::dPairs_
CollisionPairs_t dPairs_
Parameterized collision pairs.
Definition obstacle-user.hh:274
hpp::core::ObstacleUser::defaultRequest_
coal::CollisionRequest defaultRequest_
Definition obstacle-user.hh:270
hpp::core::ObstacleUser::collide
static bool collide(const CollisionPairs_t &pairs, CollisionRequests_t &reqs, coal::CollisionResult &res, std::size_t &i, pinocchio::DeviceData &data)
hpp::core::ObstacleUser::robot_
DevicePtr_t robot_
Definition obstacle-user.hh:269
hpp::core::ObstacleUser::removeObstacleFromJoint
virtual void removeObstacleFromJoint(const JointPtr_t &joint, const CollisionObjectConstPtr_t &object)
hpp::core::ObstacleUser::ObstacleUser
ObstacleUser(const ObstacleUser &other)
Copy constructor.
Definition obstacle-user.hh:257
hpp::core::ObstacleUser::defaultRequest
coal::CollisionRequest & defaultRequest()
Definition obstacle-user.hh:206
hpp::core::ObstacleUser::addObstacleToJoint
virtual void addObstacleToJoint(const CollisionObjectConstPtr_t &object, const JointPtr_t &joint, const bool includeChildren)
hpp::core::ObstacleUser::pRequests_
CollisionRequests_t pRequests_
Active collision requests.
Definition obstacle-user.hh:276
hpp::core::ObstacleUser::addRobotCollisionPairs
void addRobotCollisionPairs()
hpp::core::ObstacleUser::filterCollisionPairs
virtual void filterCollisionPairs(const RelativeMotion::matrix_type &relMotion)
hpp::core::ObstacleUser::addObstacle
virtual void addObstacle(const CollisionObjectConstPtr_t &object)
hpp::core::ObstacleUser::requests
const CollisionRequests_t & requests() const
Definition obstacle-user.hh:201
hpp::core::ObstacleUser::setRequests
void setRequests(const coal::CollisionRequest &r)
hpp::core::ObstacleUser::cPairs_
CollisionPairs_t cPairs_
Definition obstacle-user.hh:272
collision-pair.hh
config.hh
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
fwd.hh
hpp
hpp::core::CollisionPairs_t
std::vector< CollisionPair_t > CollisionPairs_t
Definition fwd.hh:239
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::CollisionRequests_t
std::vector< coal::CollisionRequest > CollisionRequests_t
Definition collision-pair.hh:43
hpp::core::JointPtr_t
pinocchio::JointPtr_t JointPtr_t
Definition fwd.hh:151
hpp::core::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
hpp::core::CollisionObjectConstPtr_t
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition fwd.hh:100
hpp::core::matrix_t
pinocchio::matrix_t matrix_t
Definition fwd.hh:162
HPP_DYNAMIC_PTR_CAST
#define HPP_DYNAMIC_PTR_CAST(t, x)
relative-motion.hh
hpp::core::RelativeMotion::matrix_type
Eigen::Matrix< RelativeMotionType, Eigen::Dynamic, Eigen::Dynamic > matrix_type
Definition relative-motion.hh:62
hpp::pinocchio::DeviceData