moveit2
The MoveIt Motion Planning Framework for ROS 2.
collision_env_fcl.h
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 the copyright holder 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, Jens Petit */
36 
37 #pragma once
38 
41 
42 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
43 #include <fcl/broadphase/broadphase_collision_manager.h>
44 #else
45 #include <fcl/broadphase/broadphase.h>
46 #endif
47 
48 #include <memory>
49 
50 namespace collision_detection
51 {
54 {
55 public:
56  CollisionEnvFCL() = delete;
57 
58  CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, double padding = 0.0, double scale = 1.0);
59 
60  CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding = 0.0,
61  double scale = 1.0);
62 
63  CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world);
64 
65  ~CollisionEnvFCL() override;
66 
68  const moveit::core::RobotState& state) const override;
69 
71  const AllowedCollisionMatrix& acm) const override;
72 
74  const moveit::core::RobotState& state) const override;
75 
77  const AllowedCollisionMatrix& acm) const override;
78 
80  const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override;
81 
83  const moveit::core::RobotState& state2) const override;
84 
85  void distanceSelf(const DistanceRequest& req, DistanceResult& res,
86  const moveit::core::RobotState& state) const override;
87 
88  void distanceRobot(const DistanceRequest& req, DistanceResult& res,
89  const moveit::core::RobotState& state) const override;
90 
91  void setWorld(const WorldPtr& world) override;
92 
93 protected:
101  void updatedPaddingOrScaling(const std::vector<std::string>& links) override;
102 
105  const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const;
106 
109  const moveit::core::RobotState& state, const AllowedCollisionMatrix* acm) const;
110 
112  void constructFCLObjectWorld(const World::Object* obj, FCLObject& fcl_obj) const;
113 
117  void updateFCLObject(const std::string& id);
118 
127  void constructFCLObjectRobot(const moveit::core::RobotState& state, FCLObject& fcl_obj) const;
128 
131  void allocSelfCollisionBroadPhase(const moveit::core::RobotState& state, FCLManager& manager) const;
132 
140  void getAttachedBodyObjects(const moveit::core::AttachedBody* ab, std::vector<FCLGeometryConstPtr>& geoms) const;
141 
143  std::vector<FCLGeometryConstPtr> robot_geoms_;
144 
146  std::vector<FCLCollisionObjectConstPtr> robot_fcl_objs_;
147 
149  std::unique_ptr<fcl::BroadPhaseCollisionManagerd> manager_;
150 
151  std::map<std::string, FCLObject> fcl_objs_;
152 
153 private:
155  void notifyObjectChange(const ObjectConstPtr& obj, World::Action action);
156 
157  World::ObserverHandle observer_handle_;
158 };
159 } // namespace collision_detection
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
FCL implementation of the CollisionEnv.
std::vector< FCLCollisionObjectConstPtr > robot_fcl_objs_
Vector of shared pointers to the FCL collision objects which make up the robot.
void constructFCLObjectRobot(const moveit::core::RobotState &state, FCLObject &fcl_obj) const
Out of the current robot state and its attached bodies construct an FCLObject which can then be used ...
void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check for robot self collision. Any collision between any pair of links is checked for,...
void setWorld(const WorldPtr &world) override
std::unique_ptr< fcl::BroadPhaseCollisionManagerd > manager_
FCL collision manager which handles the collision checking process.
void distanceRobot(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override
Compute the distance between a robot and the world.
void checkRobotCollisionHelper(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm) const
Bundles the different checkRobotCollision functions into a single function.
void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
void distanceSelf(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override
The distance to self-collision given the robot is at state state.
void updatedPaddingOrScaling(const std::vector< std::string > &links) override
Updates the FCL collision geometry and objects saved in the CollisionRobotFCL members to reflect a ne...
void allocSelfCollisionBroadPhase(const moveit::core::RobotState &state, FCLManager &manager) const
Prepares for the collision check through constructing an FCL collision object out of the current robo...
void updateFCLObject(const std::string &id)
Updates the specified object in \m fcl_objs_ and in the manager from new data available in the World.
void constructFCLObjectWorld(const World::Object *obj, FCLObject &fcl_obj) const
Construct an FCL collision object from MoveIt's World::Object.
void checkSelfCollisionHelper(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm) const
Bundles the different checkSelfCollision functions into a single function.
std::vector< FCLGeometryConstPtr > robot_geoms_
Vector of shared pointers to the FCL geometry for the objects in fcl_objs_.
std::map< std::string, FCLObject > fcl_objs_
void getAttachedBodyObjects(const moveit::core::AttachedBody *ab, std::vector< FCLGeometryConstPtr > &geoms) const
Converts all shapes which make up an attached body into a vector of FCLGeometryConstPtr.
Provides the interface to the individual collision checking libraries.
Definition: collision_env.h:52
World::ObjectConstPtr ObjectConstPtr
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:265
Object defining bodies that can be attached to robot links.
Definition: attached_body.h:58
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
Representation of a collision checking request.
Representation of a collision checking result.
Representation of a distance-reporting request.
Result of a distance request.
Bundles an FCLObject and a broadphase FCL collision manager.
A general high-level object which consists of multiple FCLCollisionObjects. It is the top level data ...
A representation of an object.
Definition: world.h:79