Public Member Functions | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes | List of all members
collision_detection::CollisionEnvFCL Class Reference

FCL implementation of the CollisionEnv. More...

#include <collision_env_fcl.h>

Inheritance diagram for collision_detection::CollisionEnvFCL:
Inheritance graph
[legend]

Public Member Functions

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 the world are considered. Self collisions are not checked. More...
 
void checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const override
 Check whether the robot model is in collision with the world. Allowed collisions are ignored. Self collisions are not checked. More...
 
void checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state1, const moveit::core::RobotState &state2) const override
 Check whether the robot model is in collision with the world in a continuous manner (between two robot states). Allowed collisions are ignored. Self collisions are not checked. More...
 
void checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const AllowedCollisionMatrix &acm) const override
 Check whether the robot model is in collision with the world in a continuous manner (between two robot states). Allowed collisions are ignored. Self collisions are not checked. More...
 
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, NO collisions are ignored. More...
 
void checkSelfCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const override
 Check for self collision. Allowed collisions specified by the allowed collision matrix are taken into account. More...
 
 CollisionEnvFCL ()=delete
 
 CollisionEnvFCL (const CollisionEnvFCL &other, const WorldPtr &world)
 
 CollisionEnvFCL (const moveit::core::RobotModelConstPtr &model, const WorldPtr &world, double padding=0.0, double scale=1.0)
 
 CollisionEnvFCL (const moveit::core::RobotModelConstPtr &model, double padding=0.0, double scale=1.0)
 
void distanceRobot (const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override
 Compute the distance between a robot and the world. More...
 
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. More...
 
void setWorld (const WorldPtr &world) override
 
 ~CollisionEnvFCL () override
 
- Public Member Functions inherited from collision_detection::CollisionEnv
virtual void checkCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const
 Check whether the robot model is in collision with itself or the world at a particular state. Any collision between any pair of links is checked for, NO collisions are ignored. More...
 
virtual void checkCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const
 Check whether the robot model is in collision with itself or the world at a particular state. Allowed collisions specified by the allowed collision matrix are taken into account. More...
 
 CollisionEnv ()=delete
 
 CollisionEnv (const CollisionEnv &other, const WorldPtr &world)
 Copy constructor. More...
 
 CollisionEnv (const moveit::core::RobotModelConstPtr &model, const WorldPtr &world, double padding=0.0, double scale=1.0)
 Constructor. More...
 
 CollisionEnv (const moveit::core::RobotModelConstPtr &model, double padding=0.0, double scale=1.0)
 Constructor. More...
 
double distanceRobot (const moveit::core::RobotState &state, bool verbose=false) const
 Compute the shortest distance between a robot and the world. More...
 
double distanceRobot (const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm, bool verbose=false) const
 Compute the shortest distance between a robot and the world. More...
 
double distanceSelf (const moveit::core::RobotState &state) const
 The distance to self-collision given the robot is at state state. More...
 
double distanceSelf (const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const
 The distance to self-collision given the robot is at state state, ignoring the distances between links that are allowed to always collide (as specified by acm) More...
 
const std::map< std::string, double > & getLinkPadding () const
 Get the link paddings as a map (from link names to padding value) More...
 
double getLinkPadding (const std::string &link_name) const
 Get the link padding for a particular link. More...
 
const std::map< std::string, double > & getLinkScale () const
 Get the link scaling as a map (from link names to scale value) More...
 
double getLinkScale (const std::string &link_name) const
 Set the scaling for a particular link. More...
 
void getPadding (std::vector< moveit_msgs::LinkPadding > &padding) const
 Get the link padding as a vector of messages. More...
 
const moveit::core::RobotModelConstPtr & getRobotModel () const
 The kinematic model corresponding to this collision model. More...
 
void getScale (std::vector< moveit_msgs::LinkScale > &scale) const
 Get the link scaling as a vector of messages. More...
 
const WorldPtr & getWorld ()
 
const WorldConstPtr & getWorld () const
 
void setLinkPadding (const std::map< std::string, double > &padding)
 Set the link paddings using a map (from link names to padding value) More...
 
void setLinkPadding (const std::string &link_name, double padding)
 Set the link padding for a particular link. More...
 
void setLinkScale (const std::map< std::string, double > &scale)
 Set the link scaling using a map (from link names to scale value) More...
 
void setLinkScale (const std::string &link_name, double scale)
 Set the scaling for a particular link. More...
 
void setPadding (const std::vector< moveit_msgs::LinkPadding > &padding)
 Set the link padding from a vector of messages. More...
 
void setPadding (double padding)
 Set the link padding (for every link) More...
 
void setScale (const std::vector< moveit_msgs::LinkScale > &scale)
 Set the link scaling from a vector of messages. More...
 
void setScale (double scale)
 Set the link scaling (for every link) More...
 
virtual ~CollisionEnv ()
 

Protected Member Functions

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 robot state and specifying a broadphase collision manager of FCL where the constructed object is registered to. More...
 
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. More...
 
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. More...
 
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 to check for collision. More...
 
void constructFCLObjectWorld (const World::Object *obj, FCLObject &fcl_obj) const
 Construct an FCL collision object from MoveIt's World::Object. More...
 
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. More...
 
void updatedPaddingOrScaling (const std::vector< std::string > &links) override
 Updates the FCL collision geometry and objects saved in the CollisionRobotFCL members to reflect a new padding or scaling of the robot links. More...
 
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. More...
 

Protected Attributes

std::map< std::string, FCLObjectfcl_objs_
 
std::unique_ptr< fcl::BroadPhaseCollisionManagerdmanager_
 FCL collision manager which handles the collision checking process. More...
 
std::vector< FCLCollisionObjectConstPtrrobot_fcl_objs_
 Vector of shared pointers to the FCL collision objects which make up the robot. More...
 
std::vector< FCLGeometryConstPtr > robot_geoms_
 Vector of shared pointers to the FCL geometry for the objects in fcl_objs_. More...
 
- Protected Attributes inherited from collision_detection::CollisionEnv
std::map< std::string, double > link_padding_
 The internally maintained map (from link names to padding) More...
 
std::map< std::string, double > link_scale_
 The internally maintained map (from link names to scaling) More...
 
moveit::core::RobotModelConstPtr robot_model_
 The kinematic model corresponding to this collision model. More...
 

Private Member Functions

void notifyObjectChange (const ObjectConstPtr &obj, World::Action action)
 Callback function executed for each change to the world environment. More...
 

Private Attributes

World::ObserverHandle observer_handle_
 

Additional Inherited Members

- Public Types inherited from collision_detection::CollisionEnv
using ObjectConstPtr = World::ObjectConstPtr
 
using ObjectPtr = World::ObjectPtr
 

Detailed Description

FCL implementation of the CollisionEnv.

Definition at line 85 of file collision_env_fcl.h.

Constructor & Destructor Documentation

◆ CollisionEnvFCL() [1/4]

collision_detection::CollisionEnvFCL::CollisionEnvFCL ( )
delete

◆ CollisionEnvFCL() [2/4]

collision_detection::CollisionEnvFCL::CollisionEnvFCL ( const moveit::core::RobotModelConstPtr &  model,
double  padding = 0.0,
double  scale = 1.0 
)

Definition at line 108 of file collision_env_fcl.cpp.

◆ CollisionEnvFCL() [3/4]

collision_detection::CollisionEnvFCL::CollisionEnvFCL ( const moveit::core::RobotModelConstPtr &  model,
const WorldPtr &  world,
double  padding = 0.0,
double  scale = 1.0 
)

Definition at line 143 of file collision_env_fcl.cpp.

◆ CollisionEnvFCL() [4/4]

collision_detection::CollisionEnvFCL::CollisionEnvFCL ( const CollisionEnvFCL other,
const WorldPtr &  world 
)

Definition at line 184 of file collision_env_fcl.cpp.

◆ ~CollisionEnvFCL()

collision_detection::CollisionEnvFCL::~CollisionEnvFCL ( )
override

Definition at line 179 of file collision_env_fcl.cpp.

Member Function Documentation

◆ allocSelfCollisionBroadPhase()

void collision_detection::CollisionEnvFCL::allocSelfCollisionBroadPhase ( const moveit::core::RobotState state,
FCLManager manager 
) const
protected

Prepares for the collision check through constructing an FCL collision object out of the current robot state and specifying a broadphase collision manager of FCL where the constructed object is registered to.

Definition at line 267 of file collision_env_fcl.cpp.

◆ checkRobotCollision() [1/4]

void collision_detection::CollisionEnvFCL::checkRobotCollision ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state 
) const
overridevirtual

Check whether the robot model is in collision with the world. Any collisions between a robot link and the world are considered. Self collisions are not checked.

Parameters
reqA CollisionRequest object that encapsulates the collision request
resA CollisionResult object that encapsulates the collision result @robot robot The collision model for the robot
stateThe kinematic state for which checks are being made

Implements collision_detection::CollisionEnv.

Definition at line 309 of file collision_env_fcl.cpp.

◆ checkRobotCollision() [2/4]

void collision_detection::CollisionEnvFCL::checkRobotCollision ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state,
const AllowedCollisionMatrix acm 
) const
overridevirtual

Check whether the robot model is in collision with the world. Allowed collisions are ignored. Self collisions are not checked.

Parameters
reqA CollisionRequest object that encapsulates the collision request
resA CollisionResult object that encapsulates the collision result @robot robot The collision model for the robot
stateThe kinematic state for which checks are being made
acmThe allowed collision matrix.

Implements collision_detection::CollisionEnv.

Definition at line 315 of file collision_env_fcl.cpp.

◆ checkRobotCollision() [3/4]

void collision_detection::CollisionEnvFCL::checkRobotCollision ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state1,
const moveit::core::RobotState state2 
) const
overridevirtual

Check whether the robot model is in collision with the world in a continuous manner (between two robot states). Allowed collisions are ignored. Self collisions are not checked.

Parameters
reqA CollisionRequest object that encapsulates the collision request
resA CollisionResult object that encapsulates the collision result @robot robot The collision model for the robot
state1The kinematic state at the start of the segment for which checks are being made
state2The kinematic state at the end of the segment for which checks are being made
acmThe allowed collision matrix.

Implements collision_detection::CollisionEnv.

Definition at line 322 of file collision_env_fcl.cpp.

◆ checkRobotCollision() [4/4]

void collision_detection::CollisionEnvFCL::checkRobotCollision ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state1,
const moveit::core::RobotState state2,
const AllowedCollisionMatrix acm 
) const
overridevirtual

Check whether the robot model is in collision with the world in a continuous manner (between two robot states). Allowed collisions are ignored. Self collisions are not checked.

Parameters
reqA CollisionRequest object that encapsulates the collision request
resA CollisionResult object that encapsulates the collision result @robot robot The collision model for the robot
state1The kinematic state at the start of the segment for which checks are being made
state2The kinematic state at the end of the segment for which checks are being made
acmThe allowed collision matrix.

Implements collision_detection::CollisionEnv.

Definition at line 329 of file collision_env_fcl.cpp.

◆ checkRobotCollisionHelper()

void collision_detection::CollisionEnvFCL::checkRobotCollisionHelper ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state,
const AllowedCollisionMatrix acm 
) const
protected

Bundles the different checkRobotCollision functions into a single function.

Definition at line 337 of file collision_env_fcl.cpp.

◆ checkSelfCollision() [1/2]

void collision_detection::CollisionEnvFCL::checkSelfCollision ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state 
) const
overridevirtual

Check for robot self collision. Any collision between any pair of links is checked for, NO collisions are ignored.

Parameters
reqA CollisionRequest object that encapsulates the collision request
resA CollisionResult object that encapsulates the collision result
stateThe kinematic state for which checks are being made

Implements collision_detection::CollisionEnv.

Definition at line 275 of file collision_env_fcl.cpp.

◆ checkSelfCollision() [2/2]

void collision_detection::CollisionEnvFCL::checkSelfCollision ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state,
const AllowedCollisionMatrix acm 
) const
overridevirtual

Check for self collision. Allowed collisions specified by the allowed collision matrix are taken into account.

Parameters
reqA CollisionRequest object that encapsulates the collision request
resA CollisionResult object that encapsulates the collision result
stateThe kinematic state for which checks are being made
acmThe allowed collision matrix.

Implements collision_detection::CollisionEnv.

Definition at line 281 of file collision_env_fcl.cpp.

◆ checkSelfCollisionHelper()

void collision_detection::CollisionEnvFCL::checkSelfCollisionHelper ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state,
const AllowedCollisionMatrix acm 
) const
protected

Bundles the different checkSelfCollision functions into a single function.

Definition at line 287 of file collision_env_fcl.cpp.

◆ constructFCLObjectRobot()

void collision_detection::CollisionEnvFCL::constructFCLObjectRobot ( const moveit::core::RobotState state,
FCLObject fcl_obj 
) const
protected

Out of the current robot state and its attached bodies construct an FCLObject which can then be used to check for collision.

The current state is used to recalculate the AABB of the FCL collision objects. However they are not computed from scratch (which would require call to computeLocalAABB()) but are only transformed according to the joint states.

Parameters
stateThe current robot state
fcl_objThe newly filled object

Definition at line 229 of file collision_env_fcl.cpp.

◆ constructFCLObjectWorld()

void collision_detection::CollisionEnvFCL::constructFCLObjectWorld ( const World::Object obj,
FCLObject fcl_obj 
) const
protected

Construct an FCL collision object from MoveIt's World::Object.

Definition at line 215 of file collision_env_fcl.cpp.

◆ distanceRobot()

void collision_detection::CollisionEnvFCL::distanceRobot ( const DistanceRequest req,
DistanceResult res,
const moveit::core::RobotState state 
) const
overridevirtual

Compute the distance between a robot and the world.

Parameters
reqA DistanceRequest object that encapsulates the distance request
resA DistanceResult object that encapsulates the distance result
robotThe robot to check distance for
stateThe state for the robot to check distances from

Implements collision_detection::CollisionEnv.

Definition at line 374 of file collision_env_fcl.cpp.

◆ distanceSelf()

void collision_detection::CollisionEnvFCL::distanceSelf ( const DistanceRequest req,
DistanceResult res,
const moveit::core::RobotState state 
) const
overridevirtual

The distance to self-collision given the robot is at state state.

Parameters
reqA DistanceRequest object that encapsulates the distance request
resA DistanceResult object that encapsulates the distance result
stateThe state of this robot to consider

Implements collision_detection::CollisionEnv.

Definition at line 362 of file collision_env_fcl.cpp.

◆ getAttachedBodyObjects()

void collision_detection::CollisionEnvFCL::getAttachedBodyObjects ( const moveit::core::AttachedBody ab,
std::vector< FCLGeometryConstPtr > &  geoms 
) const
protected

Converts all shapes which make up an atttached body into a vector of FCLGeometryConstPtr.

When they are converted, they can be added to the FCL representation of the robot for collision checking.

Parameters
abPointer to the attached body
geomsOutput vector of geometries

Definition at line 200 of file collision_env_fcl.cpp.

◆ notifyObjectChange()

void collision_detection::CollisionEnvFCL::notifyObjectChange ( const ObjectConstPtr obj,
World::Action  action 
)
private

Callback function executed for each change to the world environment.

Definition at line 444 of file collision_env_fcl.cpp.

◆ setWorld()

void collision_detection::CollisionEnvFCL::setWorld ( const WorldPtr &  world)
overridevirtual

set the world to use. This can be expensive unless the new and old world are empty. Passing NULL will result in a new empty world being created.

Reimplemented from collision_detection::CollisionEnv.

Reimplemented in collision_detection::CollisionEnvHybrid.

Definition at line 422 of file collision_env_fcl.cpp.

◆ updatedPaddingOrScaling()

void collision_detection::CollisionEnvFCL::updatedPaddingOrScaling ( const std::vector< std::string > &  links)
overrideprotectedvirtual

Updates the FCL collision geometry and objects saved in the CollisionRobotFCL members to reflect a new padding or scaling of the robot links.

It searches for the link through the pointed-to robot model of the CollisionRobot and then constructs new FCL collision objects and geometries depending on the changed robot model.

Parameters
linksThe names of the links which have been updated in the robot model

Reimplemented from collision_detection::CollisionEnv.

Definition at line 465 of file collision_env_fcl.cpp.

◆ updateFCLObject()

void collision_detection::CollisionEnvFCL::updateFCLObject ( const std::string id)
protected

Updates the specified object in \m fcl_objs_ and in the manager from new data available in the World.

If it does not exist in world, it is deleted. If it's not existing in \m fcl_objs_ yet, it's added there.

Definition at line 387 of file collision_env_fcl.cpp.

Member Data Documentation

◆ fcl_objs_

std::map<std::string, FCLObject> collision_detection::CollisionEnvFCL::fcl_objs_
protected

Definition at line 215 of file collision_env_fcl.h.

◆ manager_

std::unique_ptr<fcl::BroadPhaseCollisionManagerd> collision_detection::CollisionEnvFCL::manager_
protected

FCL collision manager which handles the collision checking process.

Definition at line 213 of file collision_env_fcl.h.

◆ observer_handle_

World::ObserverHandle collision_detection::CollisionEnvFCL::observer_handle_
private

Definition at line 221 of file collision_env_fcl.h.

◆ robot_fcl_objs_

std::vector<FCLCollisionObjectConstPtr> collision_detection::CollisionEnvFCL::robot_fcl_objs_
protected

Vector of shared pointers to the FCL collision objects which make up the robot.

Definition at line 210 of file collision_env_fcl.h.

◆ robot_geoms_

std::vector<FCLGeometryConstPtr> collision_detection::CollisionEnvFCL::robot_geoms_
protected

Vector of shared pointers to the FCL geometry for the objects in fcl_objs_.

Definition at line 207 of file collision_env_fcl.h.


The documentation for this class was generated from the following files:


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Oct 27 2021 02:25:58