moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
collision_common.hpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ioan Sucan */
36
37#pragma once
38
43#include <geometric_shapes/check_isometry.h>
44
45#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
46#include <fcl/broadphase/broadphase_collision_manager.h>
47#include <fcl/narrowphase/collision.h>
48#include <fcl/narrowphase/distance.h>
49#else
50#include <fcl/broadphase/broadphase.h>
51#include <fcl/collision.h>
52#include <fcl/distance.h>
53#endif
54
55#include <memory>
56#include <set>
57
58namespace collision_detection
59{
61
64{
67 : type(BodyTypes::ROBOT_LINK), shape_index(index)
68 {
69 ptr.link = link;
70 }
71
74 : type(BodyTypes::ROBOT_ATTACHED), shape_index(index)
75 {
76 ptr.ab = ab;
77 }
78
80 CollisionGeometryData(const World::Object* obj, int index) : type(BodyTypes::WORLD_OBJECT), shape_index(index)
81 {
82 ptr.obj = obj;
83 }
84
86 const std::string& getID() const
87 {
88 switch (type)
89 {
91 return ptr.link->getName();
93 return ptr.ab->getName();
94 default:
95 break;
96 }
97 return ptr.obj->id_;
98 }
99
101 std::string getTypeString() const
102 {
103 switch (type)
104 {
106 return "Robot link";
108 return "Robot attached";
109 default:
110 break;
111 }
112 return "Object";
113 }
114
116 bool sameObject(const CollisionGeometryData& other) const
117 {
118 return type == other.type && ptr.raw == other.ptr.raw;
119 }
120
123
129
131 union
132 {
136 const void* raw;
138};
139
142{
143 CollisionData() : req_(nullptr), active_components_only_(nullptr), res_(nullptr), acm_(nullptr), done_(false)
144 {
145 }
146
148 : req_(req), active_components_only_(nullptr), res_(res), acm_(acm), done_(false)
149 {
150 }
151
153 {
154 }
155
157 void enableGroup(const moveit::core::RobotModelConstPtr& robot_model);
158
161
166 const std::set<const moveit::core::LinkModel*>* active_components_only_;
167
170
173
175 bool done_;
176};
177
180{
182 {
183 }
185 {
186 }
187
190
193
195 bool done;
196};
197
199
202{
204 {
205 }
206
208 FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::LinkModel* link, int shape_index)
209 : collision_geometry_(collision_geometry)
210 , collision_geometry_data_(std::make_shared<CollisionGeometryData>(link, shape_index))
211 {
213 }
214
216 FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const moveit::core::AttachedBody* ab, int shape_index)
217 : collision_geometry_(collision_geometry)
218 , collision_geometry_data_(std::make_shared<CollisionGeometryData>(ab, shape_index))
219 {
221 }
222
224 FCLGeometry(fcl::CollisionGeometryd* collision_geometry, const World::Object* obj, int shape_index)
225 : collision_geometry_(collision_geometry)
226 , collision_geometry_data_(std::make_shared<CollisionGeometryData>(obj, shape_index))
227 {
229 }
230
233 template <typename T>
234 void updateCollisionGeometryData(const T* data, int shape_index, bool newType)
235 {
236 if (!newType && collision_geometry_data_)
237 {
238 if (collision_geometry_data_->ptr.raw == reinterpret_cast<const void*>(data))
239 return;
240 }
241 collision_geometry_data_ = std::make_shared<CollisionGeometryData>(data, shape_index);
243 }
244
246 std::shared_ptr<fcl::CollisionGeometryd> collision_geometry_;
247
249 CollisionGeometryDataPtr collision_geometry_data_;
250};
251
252typedef std::shared_ptr<fcl::CollisionObjectd> FCLCollisionObjectPtr;
253typedef std::shared_ptr<const fcl::CollisionObjectd> FCLCollisionObjectConstPtr;
254
258{
261 void clear();
262
263 std::vector<FCLCollisionObjectPtr> collision_objects_;
264
266 std::vector<FCLGeometryConstPtr> collision_geometry_;
267};
268
271{
273 std::shared_ptr<fcl::BroadPhaseCollisionManagerd> manager_;
274};
275
284
292bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& min_dist);
293
295FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::LinkModel* link,
296 int shape_index);
297
299FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const moveit::core::AttachedBody* ab,
300 int shape_index);
301
305FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj);
306
308FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
309 const moveit::core::LinkModel* link, int shape_index);
310
312FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
313 const moveit::core::AttachedBody* ab, int shape_index);
314
316FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
317 const World::Object* obj);
318
321
323inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f)
324{
325 ASSERT_ISOMETRY(b);
326#if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
327 f = b.matrix();
328#else
329 Eigen::Quaterniond q(b.linear());
330 f.setTranslation(fcl::Vector3d(b.translation().x(), b.translation().y(), b.translation().z()));
331 f.setQuatRotation(fcl::Quaternion3f(q.w(), q.x(), q.y(), q.z()));
332#endif
333}
334
336inline fcl::Transform3d transform2fcl(const Eigen::Isometry3d& b)
337{
339 transform2fcl(b, t);
340 return t;
341}
342
344inline void fcl2contact(const fcl::Contactd& fc, Contact& c)
345{
346 c.pos = Eigen::Vector3d(fc.pos[0], fc.pos[1], fc.pos[2]);
347 c.normal = Eigen::Vector3d(fc.normal[0], fc.normal[1], fc.normal[2]);
348 c.depth = fc.penetration_depth;
349 const CollisionGeometryData* cgd1 = static_cast<const CollisionGeometryData*>(fc.o1->getUserData());
350 c.body_name_1 = cgd1->getID();
351 c.body_type_1 = cgd1->type;
352 const CollisionGeometryData* cgd2 = static_cast<const CollisionGeometryData*>(fc.o2->getUserData());
353 c.body_name_2 = cgd2->getID();
354 c.body_type_2 = cgd2->type;
355}
356
358inline void fcl2costsource(const fcl::CostSourced& fcs, CostSource& cs)
359{
360 cs.aabb_min[0] = fcs.aabb_min[0];
361 cs.aabb_min[1] = fcs.aabb_min[1];
362 cs.aabb_min[2] = fcs.aabb_min[2];
363
364 cs.aabb_max[0] = fcs.aabb_max[0];
365 cs.aabb_max[1] = fcs.aabb_max[1];
366 cs.aabb_max[2] = fcs.aabb_max[2];
367
368 cs.cost = fcs.cost_density;
369}
370} // namespace collision_detection
#define MOVEIT_STRUCT_FORWARD(C)
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Object defining bodies that can be attached to robot links.
A link from the robot. Contains the constant transform applied to the link and its geometry.
Type
The types of bodies that are considered for collision.
@ ROBOT_ATTACHED
A body attached to a robot link.
void fcl2contact(const fcl::Contactd &fc, Contact &c)
Transforms an FCL contact into a MoveIt contact point.
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const moveit::core::LinkModel *link, int shape_index)
Create new FCLGeometry object out of robot link model.
void cleanCollisionGeometryCache()
Increases the counter of the caches which can trigger the cleaning of expired entries from them.
bool collisionCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data)
Callback function used by the FCLManager used for each pair of collision objects to calculate object ...
std::shared_ptr< fcl::CollisionObjectd > FCLCollisionObjectPtr
void transform2fcl(const Eigen::Isometry3d &b, fcl::Transform3d &f)
Transforms an Eigen Isometry3d to FCL coordinate transformation.
std::shared_ptr< const fcl::CollisionObjectd > FCLCollisionObjectConstPtr
void fcl2costsource(const fcl::CostSourced &fcs, CostSource &cs)
Transforms the FCL internal representation to the MoveIt CostSource data structure.
bool distanceCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data, double &min_dist)
Callback function used by the FCLManager used for each pair of collision objects to calculate collisi...
fcl::CollisionObject CollisionObjectd
fcl::BroadPhaseCollisionManager BroadPhaseCollisionManagerd
fcl::CostSource CostSourced
fcl::CollisionGeometry CollisionGeometryd
fcl::Contact Contactd
fcl::Transform3f Transform3d
Data structure which is passed to the collision callback function of the collision manager.
const std::set< const moveit::core::LinkModel * > * active_components_only_
If the collision request includes a group name, this set contains the pointers to the link models tha...
CollisionData(const CollisionRequest *req, CollisionResult *res, const AllowedCollisionMatrix *acm)
const AllowedCollisionMatrix * acm_
The user-specified collision matrix (may be nullptr).
void enableGroup(const moveit::core::RobotModelConstPtr &robot_model)
Compute active_components_only_ based on the joint group specified in req_.
bool done_
Flag indicating whether collision checking is complete.
CollisionResult * res_
The user-specified response location.
const CollisionRequest * req_
The collision request passed by the user.
Wrapper around world, link and attached objects' geometry data.
CollisionGeometryData(const moveit::core::AttachedBody *ab, int index)
Constructor for a new collision geometry object which is attached to the robot.
bool sameObject(const CollisionGeometryData &other) const
Check if two CollisionGeometryData objects point to the same source object.
CollisionGeometryData(const moveit::core::LinkModel *link, int index)
Constructor for a robot link collision geometry object.
const std::string & getID() const
Returns the name which is saved in the member pointed to in ptr.
int shape_index
Multiple CollisionGeometryData objects construct a collision object. The collision object refers to a...
CollisionGeometryData(const World::Object *obj, int index)
Constructor for a new world collision geometry.
std::string getTypeString() const
Returns a string of the corresponding type.
const moveit::core::AttachedBody * ab
union collision_detection::CollisionGeometryData::@0 ptr
Points to the type of body which contains the geometry.
BodyType type
Indicates the body type of the object.
Representation of a collision checking request.
Representation of a collision checking result.
Definition of a contact point.
std::string body_name_2
The id of the second body involved in the contact.
std::string body_name_1
The id of the first body involved in the contact.
BodyType body_type_1
The type of the first body involved in the contact.
BodyType body_type_2
The type of the second body involved in the contact.
Eigen::Vector3d normal
normal unit vector at contact
double depth
depth (penetration between bodies)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d pos
contact position
When collision costs are computed, this structure contains information about the partial cost incurre...
std::array< double, 3 > aabb_max
The maximum bound of the AABB defining the volume responsible for this partial cost.
double cost
The partial cost (the probability of existence for the object there is a collision with)
std::array< double, 3 > aabb_min
The minimum bound of the AABB defining the volume responsible for this partial cost.
Data structure which is passed to the distance callback function of the collision manager.
DistanceResult * res
Distance query results information.
const DistanceRequest * req
Distance query request information.
DistanceData(const DistanceRequest *req, DistanceResult *res)
bool done
Indicates if distance query is finished.
Representation of a distance-reporting request.
Result of a distance request.
Bundles the CollisionGeometryData and FCL collision geometry representation into a single class.
CollisionGeometryDataPtr collision_geometry_data_
Pointer to the user-defined geometry data.
FCLGeometry(fcl::CollisionGeometryd *collision_geometry, const moveit::core::AttachedBody *ab, int shape_index)
Constructor for an attached body.
std::shared_ptr< fcl::CollisionGeometryd > collision_geometry_
Pointer to FCL collision geometry.
FCLGeometry(fcl::CollisionGeometryd *collision_geometry, const World::Object *obj, int shape_index)
Constructor for a world object.
FCLGeometry(fcl::CollisionGeometryd *collision_geometry, const moveit::core::LinkModel *link, int shape_index)
Constructor for a robot link.
void updateCollisionGeometryData(const T *data, int shape_index, bool newType)
Updates the collision_geometry_data_ with new data while also setting the collision_geometry_ to the ...
Bundles an FCLObject and a broadphase FCL collision manager.
std::shared_ptr< fcl::BroadPhaseCollisionManagerd > manager_
A general high-level object which consists of multiple FCLCollisionObjects. It is the top level data ...
std::vector< FCLCollisionObjectPtr > collision_objects_
void unregisterFrom(fcl::BroadPhaseCollisionManagerd *manager)
std::vector< FCLGeometryConstPtr > collision_geometry_
Geometry data corresponding to collision_objects_.
void registerTo(fcl::BroadPhaseCollisionManagerd *manager)
A representation of an object.
Definition world.hpp:79