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))
44 #else
45 #include <fcl/broadphase/broadphase.h>
46 #endif
47 
48 #include <memory>
49 
50 namespace collision_detection
51 {
53 class CollisionEnvFCL : public CollisionEnv
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 
67  void checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
68  const moveit::core::RobotState& state) const override;
69 
70  void checkSelfCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state,
71  const AllowedCollisionMatrix& acm) const override;
72 
73  void checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
74  const moveit::core::RobotState& state) const override;
75 
76  void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state,
77  const AllowedCollisionMatrix& acm) const override;
78 
79  void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1,
80  const moveit::core::RobotState& state2, const AllowedCollisionMatrix& acm) const override;
81 
82  void checkRobotCollision(const CollisionRequest& req, CollisionResult& res, const moveit::core::RobotState& state1,
83  const moveit::core::RobotState& state2) const override;
84 
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 
158 };
159 } // namespace collision_detection
collision_detection::CollisionEnvFCL::checkSelfCollisionHelper
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.
Definition: collision_env_fcl.cpp:290
collision_detection::CollisionEnvFCL::fcl_objs_
std::map< std::string, FCLObject > fcl_objs_
Definition: collision_env_fcl.h:215
collision_common.h
collision_detection::CollisionEnvFCL::checkRobotCollisionHelper
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.
Definition: collision_env_fcl.cpp:340
collision_detection::CollisionEnvFCL::updateFCLObject
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.
Definition: collision_env_fcl.cpp:390
collision_detection::CollisionEnvFCL::allocSelfCollisionBroadPhase
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...
Definition: collision_env_fcl.cpp:270
collision_detection::CollisionEnvFCL::manager_
std::unique_ptr< fcl::BroadPhaseCollisionManagerd > manager_
FCL collision manager which handles the collision checking process.
Definition: collision_env_fcl.h:213
collision_detection::CollisionEnvFCL::distanceRobot
void distanceRobot(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override
Compute the distance between a robot and the world.
Definition: collision_env_fcl.cpp:377
broadphase_collision_manager.h
collision_detection::World::Action
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:265
collision_detection::World::ObserverHandle
Definition: world.h:287
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
collision_detection::CollisionEnvFCL::robot_fcl_objs_
std::vector< FCLCollisionObjectConstPtr > robot_fcl_objs_
Vector of shared pointers to the FCL collision objects which make up the robot.
Definition: collision_env_fcl.h:210
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
moveit::core::AttachedBody
Object defining bodies that can be attached to robot links.
Definition: attached_body.h:121
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Definition: collision_matrix.h:112
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
collision_env.h
collision_detection::CollisionEnvFCL::constructFCLObjectWorld
void constructFCLObjectWorld(const World::Object *obj, FCLObject &fcl_obj) const
Construct an FCL collision object from MoveIt's World::Object.
Definition: collision_env_fcl.cpp:218
collision_detection::CollisionEnv::ObjectConstPtr
World::ObjectConstPtr ObjectConstPtr
Definition: collision_env.h:284
collision_detection::World::Object
A representation of an object.
Definition: world.h:79
collision_detection::FCLManager
Bundles an FCLObject and a broadphase FCL collision manager.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:297
collision_detection::CollisionEnvFCL::setWorld
void setWorld(const WorldPtr &world) override
Definition: collision_env_fcl.cpp:425
collision_detection::CollisionEnvFCL::distanceSelf
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.
Definition: collision_env_fcl.cpp:365
collision_detection::DistanceResult
Result of a distance request.
Definition: include/moveit/collision_detection/collision_common.h:390
collision_detection::CollisionEnvFCL::robot_geoms_
std::vector< FCLGeometryConstPtr > robot_geoms_
Vector of shared pointers to the FCL geometry for the objects in fcl_objs_.
Definition: collision_env_fcl.h:207
collision_detection::CollisionEnvFCL::observer_handle_
World::ObserverHandle observer_handle_
Definition: collision_env_fcl.h:221
collision_detection::CollisionEnvFCL::CollisionEnvFCL
CollisionEnvFCL()=delete
collision_detection::FCLObject
A general high-level object which consists of multiple FCLCollisionObjects. It is the top level data ...
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:284
collision_detection::CollisionEnvFCL::checkRobotCollision
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...
Definition: collision_env_fcl.cpp:312
collision_detection::CollisionEnvFCL::constructFCLObjectRobot
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 ...
Definition: collision_env_fcl.cpp:232
collision_detection::CollisionEnvFCL::~CollisionEnvFCL
~CollisionEnvFCL() override
Definition: collision_env_fcl.cpp:181
collision_detection::DistanceRequest
Representation of a distance-reporting request.
Definition: include/moveit/collision_detection/collision_common.h:274
collision_detection::CollisionEnvFCL::getAttachedBodyObjects
void getAttachedBodyObjects(const moveit::core::AttachedBody *ab, std::vector< FCLGeometryConstPtr > &geoms) const
Converts all shapes which make up an atttached body into a vector of FCLGeometryConstPtr.
Definition: collision_env_fcl.cpp:203
collision_detection::CollisionEnvFCL::notifyObjectChange
void notifyObjectChange(const ObjectConstPtr &obj, World::Action action)
Callback function executed for each change to the world environment.
Definition: collision_env_fcl.cpp:448
collision_detection::CollisionEnvFCL::updatedPaddingOrScaling
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...
Definition: collision_env_fcl.cpp:469
collision_detection::CollisionEnvFCL::checkSelfCollision
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,...
Definition: collision_env_fcl.cpp:278
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Dec 3 2021 03:23:21