Go to the documentation of this file.
42 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
45 #include <fcl/broadphase/broadphase.h>
53 class CollisionEnvFCL :
public CollisionEnv
58 CollisionEnvFCL(
const moveit::core::RobotModelConstPtr& model,
double padding = 0.0,
double scale = 1.0);
60 CollisionEnvFCL(
const moveit::core::RobotModelConstPtr& model,
const WorldPtr& world,
double padding = 0.0,
71 const AllowedCollisionMatrix& acm)
const override;
77 const AllowedCollisionMatrix& acm)
const override;
91 void setWorld(
const WorldPtr& world)
override;
149 std::unique_ptr<fcl::BroadPhaseCollisionManagerd>
manager_;
151 std::map<std::string, FCLObject>
fcl_objs_;
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::map< std::string, FCLObject > fcl_objs_
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 updateFCLObject(const std::string &id)
Updates the specified object in fcl_objs_ and in the manager from new data available in the World.
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...
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.
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Representation of a robot's state. This includes position, velocity, acceleration and effort.
std::vector< FCLCollisionObjectConstPtr > robot_fcl_objs_
Vector of shared pointers to the FCL collision objects which make up the robot.
Representation of a collision checking request.
Object defining bodies that can be attached to robot links.
Definition of a structure for the allowed collision matrix.
Representation of a collision checking result.
void constructFCLObjectWorld(const World::Object *obj, FCLObject &fcl_obj) const
Construct an FCL collision object from MoveIt's World::Object.
World::ObjectConstPtr ObjectConstPtr
A representation of an object.
Bundles an FCLObject and a broadphase FCL collision manager.
void setWorld(const WorldPtr &world) override
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.
Result of a distance request.
std::vector< FCLGeometryConstPtr > robot_geoms_
Vector of shared pointers to the FCL geometry for the objects in fcl_objs_.
World::ObserverHandle observer_handle_
A general high-level object which consists of multiple FCLCollisionObjects. It is the top level data ...
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 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 ...
~CollisionEnvFCL() override
Representation of a distance-reporting request.
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.
void notifyObjectChange(const ObjectConstPtr &obj, World::Action action)
Callback function executed for each change to the world environment.
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 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,...
moveit_core
Author(s): Ioan Sucan
, Sachin Chitta , Acorn Pooley
autogenerated on Tue Dec 24 2024 03:27:14