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:

[legend]

Collaboration diagram for collision_detection::CollisionEnvHybrid:

[legend]

Public Member Functions
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)

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

	~CollisionEnvHybrid () override

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	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, GroupStateRepresentationPtr &gsr) 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

const CollisionEnvDistanceFieldConstPtr	getCollisionRobotDistanceField () const

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, GroupStateRepresentationPtr &gsr) 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	checkRobotCollisionDistanceField (const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const

void	checkRobotCollisionDistanceField (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 AllowedCollisionMatrix &acm) const

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

void	setWorld (const WorldPtr &world) override

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

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

const CollisionEnvDistanceFieldConstPtr	getCollisionWorldDistanceField () const

Public Member Functions inherited from collision_detection::CollisionEnvFCL
	CollisionEnvFCL ()=delete

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

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

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

	~CollisionEnvFCL () override

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.

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.

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.

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.

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.

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.

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.

void	distanceRobot (const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override
	Compute the distance between a robot and the world.

Public Member Functions inherited from collision_detection::CollisionEnv
	CollisionEnv ()=delete

	CollisionEnv (const moveit::core::RobotModelConstPtr &model, double padding=0.0, double scale=1.0)
	Constructor.

	CollisionEnv (const moveit::core::RobotModelConstPtr &model, const WorldPtr &world, double padding=0.0, double scale=1.0)
	Constructor.

	CollisionEnv (const CollisionEnv &other, const WorldPtr &world)
	Copy constructor.

virtual	~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.

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.

double	distanceSelf (const moveit::core::RobotState &state) const
	The distance to self-collision given the robot is at state state.

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)

double	distanceRobot (const moveit::core::RobotState &state, bool verbose=false) const
	Compute the shortest distance between a robot and the world.

double	distanceRobot (const moveit::core::RobotState &state, const AllowedCollisionMatrix &acm, bool verbose=false) const
	Compute the shortest distance between a robot and the world.

const WorldPtr &	getWorld ()

const WorldConstPtr &	getWorld () const

const moveit::core::RobotModelConstPtr &	getRobotModel () const
	The kinematic model corresponding to this collision model.

void	setLinkPadding (const std::string &link_name, double padding)
	Set the link padding for a particular link.

double	getLinkPadding (const std::string &link_name) const
	Get the link padding for a particular link.

void	setLinkPadding (const std::map< std::string, double > &padding)
	Set the link paddings using a map (from link names to padding value)

const std::map< std::string, double > &	getLinkPadding () const
	Get the link paddings as a map (from link names to padding value)

void	setLinkScale (const std::string &link_name, double scale)
	Set the scaling for a particular link.

double	getLinkScale (const std::string &link_name) const
	Set the scaling for a particular link.

void	setLinkScale (const std::map< std::string, double > &scale)
	Set the link scaling using a map (from link names to scale value)

const std::map< std::string, double > &	getLinkScale () const
	Get the link scaling as a map (from link names to scale value)

void	setPadding (double padding)
	Set the link padding (for every link)

void	setScale (double scale)
	Set the link scaling (for every link)

void	setPadding (const std::vector< moveit_msgs::msg::LinkPadding > &padding)
	Set the link padding from a vector of messages.

void	getPadding (std::vector< moveit_msgs::msg::LinkPadding > &padding) const
	Get the link padding as a vector of messages.

void	setScale (const std::vector< moveit_msgs::msg::LinkScale > &scale)
	Set the link scaling from a vector of messages.

void	getScale (std::vector< moveit_msgs::msg::LinkScale > &scale) const
	Get the link scaling as a vector of messages.

Protected Attributes
CollisionEnvDistanceFieldPtr	cenv_distance_

Protected Attributes inherited from collision_detection::CollisionEnvFCL
std::vector< FCLGeometryConstPtr >	robot_geoms_
	Vector of shared pointers to the FCL geometry for the objects in fcl_objs_.

std::vector< FCLCollisionObjectConstPtr >	robot_fcl_objs_
	Vector of shared pointers to the FCL collision objects which make up the robot.

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

std::map< std::string, FCLObject >	fcl_objs_

Protected Attributes inherited from collision_detection::CollisionEnv
moveit::core::RobotModelConstPtr	robot_model_
	The kinematic model corresponding to this collision model.

std::map< std::string, double >	link_padding_
	The internally maintained map (from link names to padding)

std::map< std::string, double >	link_scale_
	The internally maintained map (from link names to scaling)

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

using	ObjectConstPtr = World::ObjectConstPtr

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

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.

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	constructFCLObjectWorld (const World::Object *obj, FCLObject &fcl_obj) const
	Construct an FCL collision object from MoveIt's World::Object.

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.

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.

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.

void	getAttachedBodyObjects (const moveit::core::AttachedBody *ab, std::vector< FCLGeometryConstPtr > &geoms) const
	Converts all shapes which make up an attached body into a vector of FCLGeometryConstPtr.

Detailed Description

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

Definition at line 48 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 44 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 56 of file collision_env_hybrid.cpp.

◆ CollisionEnvHybrid() [3/3]

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

Definition at line 68 of file collision_env_hybrid.cpp.

◆ ~CollisionEnvHybrid()

collision_detection::CollisionEnvHybrid::~CollisionEnvHybrid ( )

inlineoverride

Definition at line 75 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 107 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 120 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 127 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 113 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 135 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 148 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 155 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 141 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 75 of file collision_env_hybrid.cpp.

Here is the call graph for this function:

◆ 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 90 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 98 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 82 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 179 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 172 of file collision_env_hybrid.cpp.

◆ getCollisionRobotDistanceField()

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

inline

Definition at line 104 of file collision_env_hybrid.h.

◆ getCollisionWorldDistanceField()

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

inline

Definition at line 143 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 79 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 nullptr will result in a new empty world being created.

Reimplemented from collision_detection::CollisionEnvFCL.

Definition at line 163 of file collision_env_hybrid.cpp.

Here is the call graph for this function:

Member Data Documentation

◆ cenv_distance_

CollisionEnvDistanceFieldPtr collision_detection::CollisionEnvHybrid::cenv_distance_

protected

Definition at line 149 of file collision_env_hybrid.h.

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

moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_env_hybrid.h
moveit_core/collision_distance_field/src/collision_env_hybrid.cpp

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_