This hybrid collision environment combines FCL and a distance field. Both can be used to calculate collisions. More...

#include <collision_env_hybrid.h>

Inheritance diagram for collision_detection::CollisionEnvHybrid:

Public Member Functions
void	checkCollisionDistanceField (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const

void	checkCollisionDistanceField (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const

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

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

void	checkRobotCollisionDistanceField (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const

void	checkRobotCollisionDistanceField (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm) const

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

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

void	checkSelfCollisionDistanceField (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state) const

void	checkSelfCollisionDistanceField (const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix &acm) const

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

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

	CollisionEnvHybrid (const CollisionEnvHybrid &other, const WorldPtr &world)

EIGEN_MAKE_ALIGNED_OPERATOR_NEW	CollisionEnvHybrid (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)

	CollisionEnvHybrid (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	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

const CollisionEnvDistanceFieldConstPtr	getCollisionRobotDistanceField () const

const CollisionEnvDistanceFieldConstPtr	getCollisionWorldDistanceField () const

void	initializeRobotDistanceField (const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions, double size_x, double size_y, double size_z, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance)

void	setWorld (const WorldPtr &world) override

	~CollisionEnvHybrid () override

Public Member Functions inherited from collision_detection::CollisionEnvFCL
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...

	~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. 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 Attributes
CollisionEnvDistanceFieldPtr	cenv_distance_

Protected Attributes inherited from collision_detection::CollisionEnvFCL
std::map< std::string, FCLObject >	fcl_objs_

std::unique_ptr< fcl::BroadPhaseCollisionManagerd >	manager_
	FCL collision manager which handles the collision checking process. More...

std::vector< FCLCollisionObjectConstPtr >	robot_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...

Additional Inherited Members
Public Types inherited from collision_detection::CollisionEnv
using	ObjectConstPtr = World::ObjectConstPtr

using	ObjectPtr = World::ObjectPtr

Protected Member Functions inherited from collision_detection::CollisionEnvFCL
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 `fcl_objs_` and in the manager from new data available in the World. More...

Detailed Description

This hybrid collision environment combines FCL and a distance field. Both can be used to calculate collisions.

Definition at line 82 of file collision_env_hybrid.h.

Constructor & Destructor Documentation

◆ CollisionEnvHybrid() [1/3]

collision_detection::CollisionEnvHybrid::CollisionEnvHybrid	(	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 79 of file collision_env_hybrid.cpp.

◆ CollisionEnvHybrid() [2/3]

collision_detection::CollisionEnvHybrid::CollisionEnvHybrid	(	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 91 of file collision_env_hybrid.cpp.

◆ CollisionEnvHybrid() [3/3]

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

Definition at line 103 of file collision_env_hybrid.cpp.

◆ ~CollisionEnvHybrid()

collision_detection::CollisionEnvHybrid::~CollisionEnvHybrid ( )

inlineoverride

Definition at line 141 of file collision_env_hybrid.h.

Member Function Documentation

◆ checkCollisionDistanceField() [1/4]

void collision_detection::CollisionEnvHybrid::checkCollisionDistanceField	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const moveit::core::RobotState &	state
	)		const

Definition at line 141 of file collision_env_hybrid.cpp.

◆ checkCollisionDistanceField() [2/4]

void collision_detection::CollisionEnvHybrid::checkCollisionDistanceField	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const moveit::core::RobotState &	state,
		const AllowedCollisionMatrix &	acm
	)		const

Definition at line 154 of file collision_env_hybrid.cpp.

◆ checkCollisionDistanceField() [3/4]

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

Definition at line 161 of file collision_env_hybrid.cpp.

◆ checkCollisionDistanceField() [4/4]

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

Definition at line 147 of file collision_env_hybrid.cpp.

◆ checkRobotCollisionDistanceField() [1/4]

void collision_detection::CollisionEnvHybrid::checkRobotCollisionDistanceField	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const moveit::core::RobotState &	state
	)		const

Definition at line 169 of file collision_env_hybrid.cpp.

◆ checkRobotCollisionDistanceField() [2/4]

void collision_detection::CollisionEnvHybrid::checkRobotCollisionDistanceField	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const moveit::core::RobotState &	state,
		const AllowedCollisionMatrix &	acm
	)		const

Definition at line 182 of file collision_env_hybrid.cpp.

◆ checkRobotCollisionDistanceField() [3/4]

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

Definition at line 189 of file collision_env_hybrid.cpp.

◆ checkRobotCollisionDistanceField() [4/4]

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

Definition at line 175 of file collision_env_hybrid.cpp.

◆ checkSelfCollisionDistanceField() [1/4]

void collision_detection::CollisionEnvHybrid::checkSelfCollisionDistanceField	(	const collision_detection::CollisionRequest &	req,
		collision_detection::CollisionResult &	res,
		const moveit::core::RobotState &	state
	)		const

Definition at line 109 of file collision_env_hybrid.cpp.

◆ checkSelfCollisionDistanceField() [2/4]

void collision_detection::CollisionEnvHybrid::checkSelfCollisionDistanceField	(	const collision_detection::CollisionRequest &	req,
		collision_detection::CollisionResult &	res,
		const moveit::core::RobotState &	state,
		const collision_detection::AllowedCollisionMatrix &	acm
	)		const

Definition at line 124 of file collision_env_hybrid.cpp.

◆ checkSelfCollisionDistanceField() [3/4]

void collision_detection::CollisionEnvHybrid::checkSelfCollisionDistanceField	(	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 132 of file collision_env_hybrid.cpp.

◆ checkSelfCollisionDistanceField() [4/4]

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

Definition at line 116 of file collision_env_hybrid.cpp.

◆ getAllCollisions()

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

Definition at line 213 of file collision_env_hybrid.cpp.

◆ getCollisionGradients()

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

Definition at line 206 of file collision_env_hybrid.cpp.

◆ getCollisionRobotDistanceField()

const CollisionEnvDistanceFieldConstPtr collision_detection::CollisionEnvHybrid::getCollisionRobotDistanceField ( ) const

inline

Definition at line 170 of file collision_env_hybrid.h.

◆ getCollisionWorldDistanceField()

const CollisionEnvDistanceFieldConstPtr collision_detection::CollisionEnvHybrid::getCollisionWorldDistanceField ( ) const

inline

Definition at line 209 of file collision_env_hybrid.h.

◆ initializeRobotDistanceField()

void collision_detection::CollisionEnvHybrid::initializeRobotDistanceField	(	const std::map< std::string, std::vector< CollisionSphere >> &	link_body_decompositions,
		double	size_x,
		double	size_y,
		double	size_z,
		bool	use_signed_distance_field,
		double	resolution,
		double	collision_tolerance,
		double	max_propogation_distance
	)

inline

Definition at line 145 of file collision_env_hybrid.h.

◆ setWorld()

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

Definition at line 197 of file collision_env_hybrid.cpp.

Member Data Documentation

◆ cenv_distance_

CollisionEnvDistanceFieldPtr collision_detection::CollisionEnvHybrid::cenv_distance_

protected

Definition at line 215 of file collision_env_hybrid.h.

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

Public Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CollisionEnvHybrid() [1/3]

◆ CollisionEnvHybrid() [2/3]

◆ CollisionEnvHybrid() [3/3]

◆ ~CollisionEnvHybrid()

Member Function Documentation

◆ checkCollisionDistanceField() [1/4]

◆ checkCollisionDistanceField() [2/4]

◆ checkCollisionDistanceField() [3/4]

◆ checkCollisionDistanceField() [4/4]

◆ checkRobotCollisionDistanceField() [1/4]

◆ checkRobotCollisionDistanceField() [2/4]

◆ checkRobotCollisionDistanceField() [3/4]

◆ checkRobotCollisionDistanceField() [4/4]

◆ checkSelfCollisionDistanceField() [1/4]

◆ checkSelfCollisionDistanceField() [2/4]

◆ checkSelfCollisionDistanceField() [3/4]

◆ checkSelfCollisionDistanceField() [4/4]

◆ getAllCollisions()

◆ getCollisionGradients()

◆ getCollisionRobotDistanceField()

◆ getCollisionWorldDistanceField()

◆ initializeRobotDistanceField()

◆ setWorld()

Member Data Documentation

◆ cenv_distance_