Classes | Public Member Functions | Protected Member Functions | Static Protected Member Functions | Protected Attributes | List of all members
collision_detection::CollisionEnvDistanceField Class Reference

#include <collision_env_distance_field.h>

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

Classes

struct  DistanceFieldCacheEntryWorld
 

Public Member Functions

void checkCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
 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...
 
void checkCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const override
 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...
 
virtual void checkCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm, GroupStateRepresentationPtr &gsr) const
 
virtual void checkCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
 
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...
 
virtual void checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm, GroupStateRepresentationPtr &gsr) const
 
virtual void checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
 
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 collision_detection::CollisionRequest &req, collision_detection::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 collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix &acm) const override
 Check for self collision. Allowed collisions specified by the allowed collision matrix are taken into account. More...
 
void checkSelfCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix &acm, GroupStateRepresentationPtr &gsr) const
 
void checkSelfCollision (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
 
 CollisionEnvDistanceField (const CollisionEnvDistanceField &other, const WorldPtr &world)
 
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CollisionEnvDistanceField (const moveit::core::RobotModelConstPtr &robot_model, const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions=std::map< std::string, std::vector< CollisionSphere >>(), double size_x=DEFAULT_SIZE_X, double size_y=DEFAULT_SIZE_Y, double size_z=DEFAULT_SIZE_Z, const Eigen::Vector3d &origin=Eigen::Vector3d(0, 0, 0), bool use_signed_distance_field=DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution=DEFAULT_RESOLUTION, double collision_tolerance=DEFAULT_COLLISION_TOLERANCE, double max_propogation_distance=DEFAULT_MAX_PROPOGATION_DISTANCE, double padding=0.0, double scale=1.0)
 
 CollisionEnvDistanceField (const moveit::core::RobotModelConstPtr &robot_model, const WorldPtr &world, const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions=std::map< std::string, std::vector< CollisionSphere >>(), double size_x=DEFAULT_SIZE_X, double size_y=DEFAULT_SIZE_Y, double size_z=DEFAULT_SIZE_Z, const Eigen::Vector3d &origin=Eigen::Vector3d(0, 0, 0), bool use_signed_distance_field=DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution=DEFAULT_RESOLUTION, double collision_tolerance=DEFAULT_COLLISION_TOLERANCE, double max_propogation_distance=DEFAULT_MAX_PROPOGATION_DISTANCE, double padding=0.0, double scale=1.0)
 
void createCollisionModelMarker (const moveit::core::RobotState &state, visualization_msgs::MarkerArray &model_markers) const
 
void distanceRobot (const DistanceRequest &, DistanceResult &, const moveit::core::RobotState &) const override
 Compute the distance between a robot and the world. More...
 
virtual double distanceRobot (const moveit::core::RobotState &state, bool verbose=false) const
 
virtual double distanceRobot (const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm, bool verbose=false) const
 
void distanceSelf (const DistanceRequest &, DistanceResult &, const moveit::core::RobotState &) const override
 The distance to self-collision given the robot is at state state. More...
 
virtual double distanceSelf (const moveit::core::RobotState &) const
 
virtual double distanceSelf (const moveit::core::RobotState &, const collision_detection::AllowedCollisionMatrix &) const
 
void getAllCollisions (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
 
void getCollisionGradients (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
 
distance_field::DistanceFieldConstPtr getDistanceField () const
 
DistanceFieldCacheEntryConstPtr getLastDistanceFieldEntry () const
 
collision_detection::GroupStateRepresentationConstPtr getLastGroupStateRepresentation () const
 
void initialize (const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions, const Eigen::Vector3d &size, const Eigen::Vector3d &origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance)
 
 MOVEIT_STRUCT_FORWARD (DistanceFieldCacheEntryWorld)
 
void setWorld (const WorldPtr &world) override
 
 ~CollisionEnvDistanceField () override
 
- Public Member Functions inherited from collision_detection::CollisionEnv
 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 addLinkBodyDecompositions (double resolution)
 
void addLinkBodyDecompositions (double resolution, const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions)
 
void checkSelfCollisionHelper (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
 
bool compareCacheEntryToAllowedCollisionMatrix (const DistanceFieldCacheEntryConstPtr &dfce, const collision_detection::AllowedCollisionMatrix &acm) const
 
bool compareCacheEntryToState (const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state) const
 
void generateCollisionCheckingStructures (const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr, bool generate_distance_field) const
 
DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry (const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, bool generate_distance_field) const
 
DistanceFieldCacheEntryWorldPtr generateDistanceFieldCacheEntryWorld ()
 
DistanceFieldCacheEntryConstPtr getDistanceFieldCacheEntry (const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm) const
 
bool getEnvironmentCollisions (const CollisionRequest &req, CollisionResult &res, const distance_field::DistanceFieldConstPtr &env_distance_field, GroupStateRepresentationPtr &gsr) const
 
bool getEnvironmentProximityGradients (const distance_field::DistanceFieldConstPtr &env_distance_field, GroupStateRepresentationPtr &gsr) const
 
void getGroupStateRepresentation (const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
 
bool getIntraGroupCollisions (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
 
bool getIntraGroupProximityGradients (GroupStateRepresentationPtr &gsr) const
 
PosedBodyPointDecompositionPtr getPosedLinkBodyPointDecomposition (const moveit::core::LinkModel *ls) const
 
PosedBodySphereDecompositionPtr getPosedLinkBodySphereDecomposition (const moveit::core::LinkModel *ls, unsigned int ind) const
 
bool getSelfCollisions (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
 
bool getSelfProximityGradients (GroupStateRepresentationPtr &gsr) const
 
void updateDistanceObject (const std::string &id, CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr &dfce, EigenSTL::vector_Vector3d &add_points, EigenSTL::vector_Vector3d &subtract_points)
 
void updatedPaddingOrScaling (const std::vector< std::string > &) override
 When the scale or padding is changed for a set of links by any of the functions in this class, updatedPaddingOrScaling() function is called. This function has an empty default implementation. The intention is to override this function in a derived class to allow for updating additional structures that may need such updating when link scale or padding changes. More...
 
void updateGroupStateRepresentationState (const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
 

Static Protected Member Functions

static void notifyObjectChange (CollisionEnvDistanceField *self, const ObjectConstPtr &obj, World::Action action)
 

Protected Attributes

double collision_tolerance_
 
DistanceFieldCacheEntryPtr distance_field_cache_entry_
 
DistanceFieldCacheEntryWorldPtr distance_field_cache_entry_world_
 
std::map< std::string, std::map< std::string, bool > > in_group_update_map_
 
GroupStateRepresentationPtr last_gsr_
 
std::map< std::string, unsigned int > link_body_decomposition_index_map_
 
std::vector< BodyDecompositionConstPtr > link_body_decomposition_vector_
 
double max_propogation_distance_
 
World::ObserverHandle observer_handle_
 
Eigen::Vector3d origin_
 
planning_scene::PlanningScenePtr planning_scene_
 
std::map< std::string, GroupStateRepresentationPtr > pregenerated_group_state_representation_map_
 
double resolution_
 
Eigen::Vector3d size_
 
boost::mutex update_cache_lock_
 
boost::mutex update_cache_lock_world_
 
bool use_signed_distance_field_
 
- 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...
 

Additional Inherited Members

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

Detailed Description

Definition at line 90 of file collision_env_distance_field.h.

Constructor & Destructor Documentation

◆ CollisionEnvDistanceField() [1/3]

collision_detection::CollisionEnvDistanceField::CollisionEnvDistanceField ( const moveit::core::RobotModelConstPtr &  robot_model,
const std::map< std::string, std::vector< CollisionSphere >> &  link_body_decompositions = std::map<std::string, std::vector<CollisionSphere>>(),
double  size_x = DEFAULT_SIZE_X,
double  size_y = DEFAULT_SIZE_Y,
double  size_z = DEFAULT_SIZE_Z,
const Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0),
bool  use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD,
double  resolution = DEFAULT_RESOLUTION,
double  collision_tolerance = DEFAULT_COLLISION_TOLERANCE,
double  max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE,
double  padding = 0.0,
double  scale = 1.0 
)

Definition at line 89 of file collision_env_distance_field.cpp.

◆ CollisionEnvDistanceField() [2/3]

collision_detection::CollisionEnvDistanceField::CollisionEnvDistanceField ( const moveit::core::RobotModelConstPtr &  robot_model,
const WorldPtr &  world,
const std::map< std::string, std::vector< CollisionSphere >> &  link_body_decompositions = std::map<std::string, std::vector<CollisionSphere>>(),
double  size_x = DEFAULT_SIZE_X,
double  size_y = DEFAULT_SIZE_Y,
double  size_z = DEFAULT_SIZE_Z,
const Eigen::Vector3d origin = Eigen::Vector3d(0, 0, 0),
bool  use_signed_distance_field = DEFAULT_USE_SIGNED_DISTANCE_FIELD,
double  resolution = DEFAULT_RESOLUTION,
double  collision_tolerance = DEFAULT_COLLISION_TOLERANCE,
double  max_propogation_distance = DEFAULT_MAX_PROPOGATION_DISTANCE,
double  padding = 0.0,
double  scale = 1.0 
)

Definition at line 108 of file collision_env_distance_field.cpp.

◆ CollisionEnvDistanceField() [3/3]

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

Definition at line 127 of file collision_env_distance_field.cpp.

◆ ~CollisionEnvDistanceField()

collision_detection::CollisionEnvDistanceField::~CollisionEnvDistanceField ( )
override

Definition at line 150 of file collision_env_distance_field.cpp.

Member Function Documentation

◆ addLinkBodyDecompositions() [1/2]

void collision_detection::CollisionEnvDistanceField::addLinkBodyDecompositions ( double  resolution)
protected

Definition at line 1004 of file collision_env_distance_field.cpp.

◆ addLinkBodyDecompositions() [2/2]

void collision_detection::CollisionEnvDistanceField::addLinkBodyDecompositions ( double  resolution,
const std::map< std::string, std::vector< CollisionSphere >> &  link_body_decompositions 
)
protected

Definition at line 1092 of file collision_env_distance_field.cpp.

◆ checkCollision() [1/4]

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

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.

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

Reimplemented from collision_detection::CollisionEnv.

Definition at line 1428 of file collision_env_distance_field.cpp.

◆ checkCollision() [2/4]

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

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.

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.

Reimplemented from collision_detection::CollisionEnv.

Definition at line 1460 of file collision_env_distance_field.cpp.

◆ checkCollision() [3/4]

void collision_detection::CollisionEnvDistanceField::checkCollision ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state,
const AllowedCollisionMatrix acm,
GroupStateRepresentationPtr &  gsr 
) const
virtual

Definition at line 1468 of file collision_env_distance_field.cpp.

◆ checkCollision() [4/4]

void collision_detection::CollisionEnvDistanceField::checkCollision ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state,
GroupStateRepresentationPtr &  gsr 
) const
virtual

Definition at line 1435 of file collision_env_distance_field.cpp.

◆ checkRobotCollision() [1/6]

void collision_detection::CollisionEnvDistanceField::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 1493 of file collision_env_distance_field.cpp.

◆ checkRobotCollision() [2/6]

void collision_detection::CollisionEnvDistanceField::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 1519 of file collision_env_distance_field.cpp.

◆ checkRobotCollision() [3/6]

void collision_detection::CollisionEnvDistanceField::checkRobotCollision ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state,
const AllowedCollisionMatrix acm,
GroupStateRepresentationPtr &  gsr 
) const
virtual

Definition at line 1527 of file collision_env_distance_field.cpp.

◆ checkRobotCollision() [4/6]

void collision_detection::CollisionEnvDistanceField::checkRobotCollision ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state,
GroupStateRepresentationPtr &  gsr 
) const
virtual

Definition at line 1500 of file collision_env_distance_field.cpp.

◆ checkRobotCollision() [5/6]

void collision_detection::CollisionEnvDistanceField::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 1556 of file collision_env_distance_field.cpp.

◆ checkRobotCollision() [6/6]

void collision_detection::CollisionEnvDistanceField::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 1548 of file collision_env_distance_field.cpp.

◆ checkSelfCollision() [1/4]

void collision_detection::CollisionEnvDistanceField::checkSelfCollision ( const collision_detection::CollisionRequest req,
collision_detection::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 262 of file collision_env_distance_field.cpp.

◆ checkSelfCollision() [2/4]

void collision_detection::CollisionEnvDistanceField::checkSelfCollision ( const collision_detection::CollisionRequest req,
collision_detection::CollisionResult res,
const moveit::core::RobotState state,
const collision_detection::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 278 of file collision_env_distance_field.cpp.

◆ checkSelfCollision() [3/4]

void collision_detection::CollisionEnvDistanceField::checkSelfCollision ( const collision_detection::CollisionRequest req,
collision_detection::CollisionResult res,
const moveit::core::RobotState state,
const collision_detection::AllowedCollisionMatrix acm,
GroupStateRepresentationPtr &  gsr 
) const

Definition at line 287 of file collision_env_distance_field.cpp.

◆ checkSelfCollision() [4/4]

void collision_detection::CollisionEnvDistanceField::checkSelfCollision ( const collision_detection::CollisionRequest req,
collision_detection::CollisionResult res,
const moveit::core::RobotState state,
GroupStateRepresentationPtr &  gsr 
) const

Definition at line 270 of file collision_env_distance_field.cpp.

◆ checkSelfCollisionHelper()

void collision_detection::CollisionEnvDistanceField::checkSelfCollisionHelper ( const collision_detection::CollisionRequest req,
collision_detection::CollisionResult res,
const moveit::core::RobotState state,
const collision_detection::AllowedCollisionMatrix acm,
GroupStateRepresentationPtr &  gsr 
) const
protected

Definition at line 206 of file collision_env_distance_field.cpp.

◆ compareCacheEntryToAllowedCollisionMatrix()

bool collision_detection::CollisionEnvDistanceField::compareCacheEntryToAllowedCollisionMatrix ( const DistanceFieldCacheEntryConstPtr &  dfce,
const collision_detection::AllowedCollisionMatrix acm 
) const
protected

Definition at line 1357 of file collision_env_distance_field.cpp.

◆ compareCacheEntryToState()

bool collision_detection::CollisionEnvDistanceField::compareCacheEntryToState ( const DistanceFieldCacheEntryConstPtr &  dfce,
const moveit::core::RobotState state 
) const
protected

Definition at line 1297 of file collision_env_distance_field.cpp.

◆ createCollisionModelMarker()

void collision_detection::CollisionEnvDistanceField::createCollisionModelMarker ( const moveit::core::RobotState state,
visualization_msgs::MarkerArray &  model_markers 
) const

Definition at line 1024 of file collision_env_distance_field.cpp.

◆ distanceRobot() [1/3]

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

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 226 of file collision_env_distance_field.h.

◆ distanceRobot() [2/3]

virtual double collision_detection::CollisionEnvDistanceField::distanceRobot ( const moveit::core::RobotState state,
bool  verbose = false 
) const
inlinevirtual

Definition at line 210 of file collision_env_distance_field.h.

◆ distanceRobot() [3/3]

virtual double collision_detection::CollisionEnvDistanceField::distanceRobot ( const moveit::core::RobotState state,
const AllowedCollisionMatrix acm,
bool  verbose = false 
) const
inlinevirtual

Definition at line 217 of file collision_env_distance_field.h.

◆ distanceSelf() [1/3]

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

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 151 of file collision_env_distance_field.h.

◆ distanceSelf() [2/3]

virtual double collision_detection::CollisionEnvDistanceField::distanceSelf ( const moveit::core::RobotState ) const
inlinevirtual

Definition at line 140 of file collision_env_distance_field.h.

◆ distanceSelf() [3/3]

virtual double collision_detection::CollisionEnvDistanceField::distanceSelf ( const moveit::core::RobotState ,
const collision_detection::AllowedCollisionMatrix  
) const
inlinevirtual

Definition at line 145 of file collision_env_distance_field.h.

◆ generateCollisionCheckingStructures()

void collision_detection::CollisionEnvDistanceField::generateCollisionCheckingStructures ( const std::string group_name,
const moveit::core::RobotState state,
const collision_detection::AllowedCollisionMatrix acm,
GroupStateRepresentationPtr &  gsr,
bool  generate_distance_field 
) const
protected

Definition at line 187 of file collision_env_distance_field.cpp.

◆ generateDistanceFieldCacheEntry()

DistanceFieldCacheEntryPtr collision_detection::CollisionEnvDistanceField::generateDistanceFieldCacheEntry ( const std::string group_name,
const moveit::core::RobotState state,
const collision_detection::AllowedCollisionMatrix acm,
bool  generate_distance_field 
) const
protected

Definition at line 752 of file collision_env_distance_field.cpp.

◆ generateDistanceFieldCacheEntryWorld()

CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr collision_detection::CollisionEnvDistanceField::generateDistanceFieldCacheEntryWorld ( )
protected

Definition at line 1829 of file collision_env_distance_field.cpp.

◆ getAllCollisions()

void collision_detection::CollisionEnvDistanceField::getAllCollisions ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state,
const AllowedCollisionMatrix acm,
GroupStateRepresentationPtr &  gsr 
) const

Definition at line 1586 of file collision_env_distance_field.cpp.

◆ getCollisionGradients()

void collision_detection::CollisionEnvDistanceField::getCollisionGradients ( const CollisionRequest req,
CollisionResult res,
const moveit::core::RobotState state,
const AllowedCollisionMatrix acm,
GroupStateRepresentationPtr &  gsr 
) const

Definition at line 1563 of file collision_env_distance_field.cpp.

◆ getDistanceField()

distance_field::DistanceFieldConstPtr collision_detection::CollisionEnvDistanceField::getDistanceField ( ) const
inline

Definition at line 234 of file collision_env_distance_field.h.

◆ getDistanceFieldCacheEntry()

DistanceFieldCacheEntryConstPtr collision_detection::CollisionEnvDistanceField::getDistanceFieldCacheEntry ( const std::string group_name,
const moveit::core::RobotState state,
const collision_detection::AllowedCollisionMatrix acm 
) const
protected

Definition at line 231 of file collision_env_distance_field.cpp.

◆ getEnvironmentCollisions()

bool collision_detection::CollisionEnvDistanceField::getEnvironmentCollisions ( const CollisionRequest req,
CollisionResult res,
const distance_field::DistanceFieldConstPtr &  env_distance_field,
GroupStateRepresentationPtr &  gsr 
) const
protected

Definition at line 1607 of file collision_env_distance_field.cpp.

◆ getEnvironmentProximityGradients()

bool collision_detection::CollisionEnvDistanceField::getEnvironmentProximityGradients ( const distance_field::DistanceFieldConstPtr &  env_distance_field,
GroupStateRepresentationPtr &  gsr 
) const
protected

Definition at line 1691 of file collision_env_distance_field.cpp.

◆ getGroupStateRepresentation()

void collision_detection::CollisionEnvDistanceField::getGroupStateRepresentation ( const DistanceFieldCacheEntryConstPtr &  dfce,
const moveit::core::RobotState state,
GroupStateRepresentationPtr &  gsr 
) const
protected

std::cerr << "Attached " << dfce->attached_body_names_[i] << " index " << dfce->attached_body_link_state_indices_[i] << std::endl;

Definition at line 1199 of file collision_env_distance_field.cpp.

◆ getIntraGroupCollisions()

bool collision_detection::CollisionEnvDistanceField::getIntraGroupCollisions ( const collision_detection::CollisionRequest req,
collision_detection::CollisionResult res,
GroupStateRepresentationPtr &  gsr 
) const
protected

Definition at line 457 of file collision_env_distance_field.cpp.

◆ getIntraGroupProximityGradients()

bool collision_detection::CollisionEnvDistanceField::getIntraGroupProximityGradients ( GroupStateRepresentationPtr &  gsr) const
protected

Definition at line 686 of file collision_env_distance_field.cpp.

◆ getLastDistanceFieldEntry()

DistanceFieldCacheEntryConstPtr collision_detection::CollisionEnvDistanceField::getLastDistanceFieldEntry ( ) const
inline

Definition at line 157 of file collision_env_distance_field.h.

◆ getLastGroupStateRepresentation()

collision_detection::GroupStateRepresentationConstPtr collision_detection::CollisionEnvDistanceField::getLastGroupStateRepresentation ( ) const
inline

Definition at line 239 of file collision_env_distance_field.h.

◆ getPosedLinkBodyPointDecomposition()

PosedBodyPointDecompositionPtr collision_detection::CollisionEnvDistanceField::getPosedLinkBodyPointDecomposition ( const moveit::core::LinkModel ls) const
protected

Definition at line 1132 of file collision_env_distance_field.cpp.

◆ getPosedLinkBodySphereDecomposition()

PosedBodySphereDecompositionPtr collision_detection::CollisionEnvDistanceField::getPosedLinkBodySphereDecomposition ( const moveit::core::LinkModel ls,
unsigned int  ind 
) const
protected

Definition at line 1123 of file collision_env_distance_field.cpp.

◆ getSelfCollisions()

bool collision_detection::CollisionEnvDistanceField::getSelfCollisions ( const collision_detection::CollisionRequest req,
collision_detection::CollisionResult res,
GroupStateRepresentationPtr &  gsr 
) const
protected

Definition at line 301 of file collision_env_distance_field.cpp.

◆ getSelfProximityGradients()

bool collision_detection::CollisionEnvDistanceField::getSelfProximityGradients ( GroupStateRepresentationPtr &  gsr) const
protected

Definition at line 382 of file collision_env_distance_field.cpp.

◆ initialize()

void collision_detection::CollisionEnvDistanceField::initialize ( const std::map< std::string, std::vector< CollisionSphere >> &  link_body_decompositions,
const Eigen::Vector3d size,
const Eigen::Vector3d origin,
bool  use_signed_distance_field,
double  resolution,
double  collision_tolerance,
double  max_propogation_distance 
)

Definition at line 155 of file collision_env_distance_field.cpp.

◆ MOVEIT_STRUCT_FORWARD()

collision_detection::CollisionEnvDistanceField::MOVEIT_STRUCT_FORWARD ( DistanceFieldCacheEntryWorld  )

◆ notifyObjectChange()

void collision_detection::CollisionEnvDistanceField::notifyObjectChange ( CollisionEnvDistanceField self,
const ObjectConstPtr obj,
World::Action  action 
)
staticprotected

Definition at line 1750 of file collision_env_distance_field.cpp.

◆ setWorld()

void collision_detection::CollisionEnvDistanceField::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.

Definition at line 1729 of file collision_env_distance_field.cpp.

◆ updateDistanceObject()

void collision_detection::CollisionEnvDistanceField::updateDistanceObject ( const std::string id,
CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr &  dfce,
EigenSTL::vector_Vector3d &  add_points,
EigenSTL::vector_Vector3d &  subtract_points 
)
protected

Definition at line 1777 of file collision_env_distance_field.cpp.

◆ updatedPaddingOrScaling()

void collision_detection::CollisionEnvDistanceField::updatedPaddingOrScaling ( const std::vector< std::string > &  links)
inlineoverrideprotectedvirtual

When the scale or padding is changed for a set of links by any of the functions in this class, updatedPaddingOrScaling() function is called. This function has an empty default implementation. The intention is to override this function in a derived class to allow for updating additional structures that may need such updating when link scale or padding changes.

Parameters
linksthe names of the links whose padding or scaling were updated

Reimplemented from collision_detection::CollisionEnv.

Definition at line 301 of file collision_env_distance_field.h.

◆ updateGroupStateRepresentationState()

void collision_detection::CollisionEnvDistanceField::updateGroupStateRepresentationState ( const moveit::core::RobotState state,
GroupStateRepresentationPtr &  gsr 
) const
protected

Definition at line 1145 of file collision_env_distance_field.cpp.

Member Data Documentation

◆ collision_tolerance_

double collision_detection::CollisionEnvDistanceField::collision_tolerance_
protected

Definition at line 321 of file collision_env_distance_field.h.

◆ distance_field_cache_entry_

DistanceFieldCacheEntryPtr collision_detection::CollisionEnvDistanceField::distance_field_cache_entry_
protected

Definition at line 328 of file collision_env_distance_field.h.

◆ distance_field_cache_entry_world_

DistanceFieldCacheEntryWorldPtr collision_detection::CollisionEnvDistanceField::distance_field_cache_entry_world_
protected

Definition at line 335 of file collision_env_distance_field.h.

◆ in_group_update_map_

std::map<std::string, std::map<std::string, bool> > collision_detection::CollisionEnvDistanceField::in_group_update_map_
protected

Definition at line 329 of file collision_env_distance_field.h.

◆ last_gsr_

GroupStateRepresentationPtr collision_detection::CollisionEnvDistanceField::last_gsr_
protected

Definition at line 336 of file collision_env_distance_field.h.

◆ link_body_decomposition_index_map_

std::map<std::string, unsigned int> collision_detection::CollisionEnvDistanceField::link_body_decomposition_index_map_
protected

Definition at line 325 of file collision_env_distance_field.h.

◆ link_body_decomposition_vector_

std::vector<BodyDecompositionConstPtr> collision_detection::CollisionEnvDistanceField::link_body_decomposition_vector_
protected

Definition at line 324 of file collision_env_distance_field.h.

◆ max_propogation_distance_

double collision_detection::CollisionEnvDistanceField::max_propogation_distance_
protected

Definition at line 322 of file collision_env_distance_field.h.

◆ observer_handle_

World::ObserverHandle collision_detection::CollisionEnvDistanceField::observer_handle_
protected

Definition at line 337 of file collision_env_distance_field.h.

◆ origin_

Eigen::Vector3d collision_detection::CollisionEnvDistanceField::origin_
protected

Definition at line 318 of file collision_env_distance_field.h.

◆ planning_scene_

planning_scene::PlanningScenePtr collision_detection::CollisionEnvDistanceField::planning_scene_
protected

Definition at line 332 of file collision_env_distance_field.h.

◆ pregenerated_group_state_representation_map_

std::map<std::string, GroupStateRepresentationPtr> collision_detection::CollisionEnvDistanceField::pregenerated_group_state_representation_map_
protected

Definition at line 330 of file collision_env_distance_field.h.

◆ resolution_

double collision_detection::CollisionEnvDistanceField::resolution_
protected

Definition at line 320 of file collision_env_distance_field.h.

◆ size_

Eigen::Vector3d collision_detection::CollisionEnvDistanceField::size_
protected

Definition at line 317 of file collision_env_distance_field.h.

◆ update_cache_lock_

boost::mutex collision_detection::CollisionEnvDistanceField::update_cache_lock_
mutableprotected

Definition at line 327 of file collision_env_distance_field.h.

◆ update_cache_lock_world_

boost::mutex collision_detection::CollisionEnvDistanceField::update_cache_lock_world_
mutableprotected

Definition at line 334 of file collision_env_distance_field.h.

◆ use_signed_distance_field_

bool collision_detection::CollisionEnvDistanceField::use_signed_distance_field_
protected

Definition at line 319 of file collision_env_distance_field.h.


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


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