#include <collision_world_distance_field.h>

Inheritance diagram for collision_detection::CollisionWorldDistanceField:

Classes
struct	DistanceFieldCacheEntry

Public Member Functions
virtual void	checkCollision (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::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 CollisionRobot &robot, const robot_state::RobotState &state, GroupStateRepresentationPtr &gsr) const

virtual void	checkCollision (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::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...

virtual void	checkCollision (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm, GroupStateRepresentationPtr &gsr) const

virtual void	checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const
	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...

virtual void	checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, GroupStateRepresentationPtr &gsr) const

virtual void	checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm) const
	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 CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm, GroupStateRepresentationPtr &gsr) const

virtual void	checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const
	Check whether the robot model is in collision with the world in a continuous manner (between two robot states). Any collisions between a robot link and the world are considered. Self collisions are not checked. More...

virtual void	checkRobotCollision (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const
	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...

virtual void	checkWorldCollision (const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world) const
	Check whether a given set of objects is in collision with objects from another world. Any contacts are considered. More...

virtual void	checkWorldCollision (const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world, const AllowedCollisionMatrix &acm) const
	Check whether a given set of objects is in collision with objects from another world. Allowed collisions are ignored. Any contacts are considered. More...

	CollisionWorldDistanceField (Eigen::Vector3d size=Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z), 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)

	CollisionWorldDistanceField (const WorldPtr &world, Eigen::Vector3d size=Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z), 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)

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

virtual double	distanceRobot (const CollisionRobot &robot, const robot_state::RobotState &state, bool verbose=false) const

virtual double	distanceRobot (const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix &acm, bool verbose=false) const

virtual void	distanceRobot (const DistanceRequest &req, DistanceResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const
	Compute the distance between a robot and the world. More...

virtual double	distanceWorld (const CollisionWorld &world, bool verbose=false) const

virtual double	distanceWorld (const CollisionWorld &world, const AllowedCollisionMatrix &acm, bool verbose=false) const

virtual void	distanceWorld (const DistanceRequest &req, DistanceResult &res, const CollisionWorld &world) const
	Compute the distance between another world. More...

void	generateEnvironmentDistanceField (bool redo=true)

void	getAllCollisions (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const

void	getCollisionGradients (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const

distance_field::DistanceFieldConstPtr	getDistanceField () const

collision_detection::GroupStateRepresentationConstPtr	getLastGroupStateRepresentation () const

virtual void	setWorld (const WorldPtr &world)

virtual	~CollisionWorldDistanceField ()

Public Member Functions inherited from collision_detection::CollisionWorld
virtual void	checkCollision (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2) const
	Check whether the robot model is in collision with itself or the world in a continuous manner (between two robot states) Any collision between any pair of links is checked for, NO collisions are ignored. More...

virtual void	checkCollision (const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state1, const robot_state::RobotState &state2, const AllowedCollisionMatrix &acm) const
	Check whether the robot model is in collision with itself or the world in a continuous manner (between two robot states). Allowed collisions specified by the allowed collision matrix are taken into account. More...

	CollisionWorld ()

	CollisionWorld (const WorldPtr &world)

	CollisionWorld (const CollisionWorld &other, const WorldPtr &world)
	A copy constructor. other should not be changed while the copy constructor is running. world must be the same world as used by other or a (not-yet-modified) copy of the world used by other. More...

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

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

double	distanceWorld (const CollisionWorld &world, bool verbose=false) const
	The shortest distance to another world instance (world) More...

double	distanceWorld (const CollisionWorld &world, const AllowedCollisionMatrix &acm, bool verbose=false) const
	The shortest distance to another world instance (world), ignoring the distances between world elements that are allowed to collide (as specified by acm) More...

const WorldPtr &	getWorld ()

const WorldConstPtr &	getWorld () const

virtual	~CollisionWorld ()

Protected Member Functions
DistanceFieldCacheEntryPtr	generateDistanceFieldCacheEntry ()

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	updateDistanceObject (const std::string &id, CollisionWorldDistanceField::DistanceFieldCacheEntryPtr &dfce, EigenSTL::vector_Vector3d &add_points, EigenSTL::vector_Vector3d &subtract_points)

Static Protected Member Functions
static void	notifyObjectChange (CollisionWorldDistanceField *self, const ObjectConstPtr &obj, World::Action action)

Protected Attributes
double	collision_tolerance_

DistanceFieldCacheEntryPtr	distance_field_cache_entry_

GroupStateRepresentationPtr	last_gsr_

double	max_propogation_distance_

World::ObserverHandle	observer_handle_

Eigen::Vector3d	origin_

double	resolution_

Eigen::Vector3d	size_

boost::mutex	update_cache_lock_

bool	use_signed_distance_field_

Additional Inherited Members
Public Types inherited from collision_detection::CollisionWorld
typedef World::ObjectConstPtr	ObjectConstPtr

typedef World::ObjectPtr	ObjectPtr

Detailed Description

Definition at line 49 of file collision_world_distance_field.h.

Constructor & Destructor Documentation

collision_detection::CollisionWorldDistanceField::CollisionWorldDistanceField	(	Eigen::Vector3d	size = `Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z)`,
		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`
	)

Definition at line 51 of file collision_world_distance_field.cpp.

collision_detection::CollisionWorldDistanceField::CollisionWorldDistanceField	(	const WorldPtr &	world,
		Eigen::Vector3d	size = `Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z)`,
		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`
	)

explicit

Definition at line 69 of file collision_world_distance_field.cpp.

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

Definition at line 89 of file collision_world_distance_field.cpp.

collision_detection::CollisionWorldDistanceField::~CollisionWorldDistanceField ( )

virtual

Definition at line 46 of file collision_world_distance_field.cpp.

Member Function Documentation

void collision_detection::CollisionWorldDistanceField::checkCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state
	)		const

virtual

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

req	A CollisionRequest object that encapsulates the collision request
res	A CollisionResult object that encapsulates the collision result
state	The kinematic state for which checks are being made

Reimplemented from collision_detection::CollisionWorld.

Definition at line 107 of file collision_world_distance_field.cpp.

void collision_detection::CollisionWorldDistanceField::checkCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state,
		GroupStateRepresentationPtr &	gsr
	)		const

virtual

Definition at line 115 of file collision_world_distance_field.cpp.

void collision_detection::CollisionWorldDistanceField::checkCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state,
		const AllowedCollisionMatrix &	acm
	)		const

virtual

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

req	A CollisionRequest object that encapsulates the collision request
res	A CollisionResult object that encapsulates the collision result
state	The kinematic state for which checks are being made
acm	The allowed collision matrix.

Reimplemented from collision_detection::CollisionWorld.

Definition at line 149 of file collision_world_distance_field.cpp.

void collision_detection::CollisionWorldDistanceField::checkCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state,
		const AllowedCollisionMatrix &	acm,
		GroupStateRepresentationPtr &	gsr
	)		const

virtual

Definition at line 157 of file collision_world_distance_field.cpp.

void collision_detection::CollisionWorldDistanceField::checkRobotCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state
	)		const

virtual

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

req	A CollisionRequest object that encapsulates the collision request
res	A CollisionResult object that encapsulates the collision result robot The collision model for the robot
state	The kinematic state for which checks are being made

Implements collision_detection::CollisionWorld.

Definition at line 192 of file collision_world_distance_field.cpp.

void collision_detection::CollisionWorldDistanceField::checkRobotCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state,
		GroupStateRepresentationPtr &	gsr
	)		const

virtual

Definition at line 200 of file collision_world_distance_field.cpp.

void collision_detection::CollisionWorldDistanceField::checkRobotCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state,
		const AllowedCollisionMatrix &	acm
	)		const

virtual

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

Parameters

req	A CollisionRequest object that encapsulates the collision request
res	A CollisionResult object that encapsulates the collision result robot The collision model for the robot
state	The kinematic state for which checks are being made
acm	The allowed collision matrix.

Implements collision_detection::CollisionWorld.

Definition at line 229 of file collision_world_distance_field.cpp.

void collision_detection::CollisionWorldDistanceField::checkRobotCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state,
		const AllowedCollisionMatrix &	acm,
		GroupStateRepresentationPtr &	gsr
	)		const

virtual

Definition at line 237 of file collision_world_distance_field.cpp.

virtual void collision_detection::CollisionWorldDistanceField::checkRobotCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state1,
		const robot_state::RobotState &	state2
	)		const

inlinevirtual

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

Parameters

req	A CollisionRequest object that encapsulates the collision request
res	A CollisionResult object that encapsulates the collision result robot The collision model for the robot
state1	The kinematic state at the start of the segment for which checks are being made
state2	The kinematic state at the end of the segment for which checks are being made

Implements collision_detection::CollisionWorld.

Definition at line 105 of file collision_world_distance_field.h.

virtual void collision_detection::CollisionWorldDistanceField::checkRobotCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state1,
		const robot_state::RobotState &	state2,
		const AllowedCollisionMatrix &	acm
	)		const

inlinevirtual

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

req	A CollisionRequest object that encapsulates the collision request
res	A CollisionResult object that encapsulates the collision result robot The collision model for the robot
state1	The kinematic state at the start of the segment for which checks are being made
state2	The kinematic state at the end of the segment for which checks are being made
acm	The allowed collision matrix.

Implements collision_detection::CollisionWorld.

Definition at line 109 of file collision_world_distance_field.h.

virtual void collision_detection::CollisionWorldDistanceField::checkWorldCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionWorld &	other_world
	)		const

inlinevirtual

Check whether a given set of objects is in collision with objects from another world. Any contacts are considered.

Parameters

req	A CollisionRequest object that encapsulates the collision request
res	A CollisionResult object that encapsulates the collision result
other_world	The other collision world

Implements collision_detection::CollisionWorld.

Definition at line 114 of file collision_world_distance_field.h.

virtual void collision_detection::CollisionWorldDistanceField::checkWorldCollision	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionWorld &	other_world,
		const AllowedCollisionMatrix &	acm
	)		const

inlinevirtual

Check whether a given set of objects is in collision with objects from another world. Allowed collisions are ignored. Any contacts are considered.

Parameters

req	A CollisionRequest object that encapsulates the collision request
res	A CollisionResult object that encapsulates the collision result
other_world	The other collision world
acm	The allowed collision matrix.

Implements collision_detection::CollisionWorld.

Definition at line 118 of file collision_world_distance_field.h.

virtual double collision_detection::CollisionWorldDistanceField::distanceRobot	(	const CollisionRobot &	robot,
		const robot_state::RobotState &	state,
		bool	verbose = `false`
	)		const

inlinevirtual

Definition at line 123 of file collision_world_distance_field.h.

virtual double collision_detection::CollisionWorldDistanceField::distanceRobot	(	const CollisionRobot &	robot,
		const robot_state::RobotState &	state,
		const AllowedCollisionMatrix &	acm,
		bool	verbose = `false`
	)		const

inlinevirtual

Definition at line 128 of file collision_world_distance_field.h.

virtual void collision_detection::CollisionWorldDistanceField::distanceRobot	(	const DistanceRequest &	req,
		DistanceResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state
	)		const

inlinevirtual

Compute the distance between a robot and the world.

Parameters

req	A DistanceRequest object that encapsulates the distance request
res	A DistanceResult object that encapsulates the distance result
robot	The robot to check distance for
state	The state for the robot to check distances from

Implements collision_detection::CollisionWorld.

Definition at line 143 of file collision_world_distance_field.h.

virtual double collision_detection::CollisionWorldDistanceField::distanceWorld	(	const CollisionWorld &	world,
		bool	verbose = `false`
	)		const

inlinevirtual

Definition at line 133 of file collision_world_distance_field.h.

virtual double collision_detection::CollisionWorldDistanceField::distanceWorld	(	const CollisionWorld &	world,
		const AllowedCollisionMatrix &	acm,
		bool	verbose = `false`
	)		const

inlinevirtual

Definition at line 137 of file collision_world_distance_field.h.

virtual void collision_detection::CollisionWorldDistanceField::distanceWorld	(	const DistanceRequest &	req,
		DistanceResult &	res,
		const CollisionWorld &	world
	)		const

inlinevirtual

Compute the distance between another world.

Parameters

req	A DistanceRequest object that encapsulates the distance request
res	A DistanceResult object that encapsulates the distance result
world	The world to check distance for

Implements collision_detection::CollisionWorld.

Definition at line 149 of file collision_world_distance_field.h.

CollisionWorldDistanceField::DistanceFieldCacheEntryPtr collision_detection::CollisionWorldDistanceField::generateDistanceFieldCacheEntry ( )

protected

Definition at line 549 of file collision_world_distance_field.cpp.

void collision_detection::CollisionWorldDistanceField::generateEnvironmentDistanceField ( bool redo = true )

void collision_detection::CollisionWorldDistanceField::getAllCollisions	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state,
		const AllowedCollisionMatrix *	acm,
		GroupStateRepresentationPtr &	gsr
	)		const

Definition at line 298 of file collision_world_distance_field.cpp.

void collision_detection::CollisionWorldDistanceField::getCollisionGradients	(	const CollisionRequest &	req,
		CollisionResult &	res,
		const CollisionRobot &	robot,
		const robot_state::RobotState &	state,
		const AllowedCollisionMatrix *	acm,
		GroupStateRepresentationPtr &	gsr
	)		const

Definition at line 267 of file collision_world_distance_field.cpp.

distance_field::DistanceFieldConstPtr collision_detection::CollisionWorldDistanceField::getDistanceField ( ) const

inline

Definition at line 158 of file collision_world_distance_field.h.

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

protected

Definition at line 328 of file collision_world_distance_field.cpp.

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

protected

Definition at line 412 of file collision_world_distance_field.cpp.

collision_detection::GroupStateRepresentationConstPtr collision_detection::CollisionWorldDistanceField::getLastGroupStateRepresentation ( ) const

inline

Definition at line 163 of file collision_world_distance_field.h.

void collision_detection::CollisionWorldDistanceField::notifyObjectChange	(	CollisionWorldDistanceField *	self,
		const ObjectConstPtr &	obj,
		World::Action	action
	)

staticprotected

Definition at line 471 of file collision_world_distance_field.cpp.

void collision_detection::CollisionWorldDistanceField::setWorld ( const WorldPtr & world )

virtual

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::CollisionWorld.

Definition at line 450 of file collision_world_distance_field.cpp.

void collision_detection::CollisionWorldDistanceField::updateDistanceObject	(	const std::string &	id,
		CollisionWorldDistanceField::DistanceFieldCacheEntryPtr &	dfce,
		EigenSTL::vector_Vector3d &	add_points,
		EigenSTL::vector_Vector3d &	subtract_points
	)