sbpl_arm_planner::SBPLCollisionSpace Class Reference

#include <sbpl_collision_space.h>

List of all members.

Public Member Functions
void	addArmCuboidsToGrid ()
void	addCollisionObject (const mapping_msgs::CollisionObject &object)
	add a collision object to the environment
void	attachCylinderToGripper (std::string frame, geometry_msgs::Pose pose, double radius, double length)
void	attachMeshToGripper (const std::string frame, const geometry_msgs::Pose pose, const std::vector< int32_t > &triangles, const std::vector< geometry_msgs::Point > &vertices)
void	attachMeshToGripper (const std::string frame, const geometry_msgs::Pose pose, const bodies::BoundingCylinder &cyl)
void	attachSphereToGripper (std::string frame, geometry_msgs::Pose pose, double radius)
bool	checkCollision (const std::vector< double > &angles, bool verbose, bool check_mesh, unsigned char &dist)
	check joint configuration for collision (0: collision)
bool	checkLinkForCollision (const std::vector< double > &angles, int link_num, bool verbose, unsigned char &dist)
	check if a specific link is in collision (0: collision)
bool	checkLinkPathForCollision (const std::vector< double > &start, const std::vector< double > &end, int link_num, bool verbose, unsigned char &dist)
	check linearly interpolated path for collision of a specific link
bool	checkPathForCollision (const std::vector< double > &start, const std::vector< double > &end, bool verbose, unsigned char &dist)
	check linearly interpolated path for collisions
bool	getAttachedObject (const std::vector< double > &angles, std::vector< std::vector< double > > &xyz)
	get the voxels that the attached object occupies
double	getAttachedObjectRadius ()
	get the radius of the attached object (sphere or cylinder)
bool	getCollisionCylinders (const std::vector< double > &angles, std::vector< std::vector< double > > &cylinders)
void	getCollisionObjectVoxelPoses (std::vector< geometry_msgs::Pose > &points)
void	getInterpolatedPath (const std::vector< double > &start, const std::vector< double > &end, std::vector< double > &inc, std::vector< std::vector< double > > &path)
void	getInterpolatedPath (const std::vector< double > &start, const std::vector< double > &end, double inc, std::vector< std::vector< double > > &path)
bool	getJointPosesInGrid (std::vector< double > angles, std::vector< std::vector< int > > &jnts, KDL::Frame &f_out, bool verbose)
	get the xyz coords of each joint in the arm
void	getLineSegment (const std::vector< int > a, const std::vector< int > b, std::vector< std::vector< int > > &points)
	get coordinates of cells that a line segment intersects
bool	isValidCell (const int x, const int y, const int z, const int radius)
	check if the cell's distance to nearest obstacle > radius
unsigned char	isValidLineSegment (const std::vector< int > a, const std::vector< int > b, const short unsigned int radius)
	check if a line segment lies on an invalid cell (0: collision)
void	processCollisionObjectMsg (const mapping_msgs::CollisionObject &object)
	process a collision object message
void	putCollisionObjectsInGrid ()
void	removeAllCollisionObjects ()
void	removeAttachedObject ()
void	removeCollisionObject (const mapping_msgs::CollisionObject &object)
	SBPLCollisionSpace (SBPLArmModel *arm, OccupancyGrid *grid)
	default constructor
void	setDebugFile (FILE *file_ptr)
	choose the file to output debug information
void	transformPose (const std::string &current_frame, const std::string &desired_frame, const geometry_msgs::Pose &pose_in, geometry_msgs::Pose &pose_out)
void	transformPose (std::string current_frame, std::string desired_frame, geometry_msgs::Pose &pose_in, geometry_msgs::Pose &pose_out)
	transform a pose from one frame to another
	~SBPLCollisionSpace ()
	destructor
Private Member Functions
double	distanceBetween3DLineSegments (std::vector< int > l1a, std::vector< int > l1b, std::vector< int > l2a, std::vector< int > l2b)
	get the shortest distance between two 3D line segments
void	getBoundingCylinderOfMesh (const std::vector< int32_t > &triangles, const std::vector< geometry_msgs::Point > &vertices, bodies::BoundingCylinder &cyl)
bool	getBoundingCylinderOfMesh (std::string mesh_file, shapes::Shape *mesh, bodies::BoundingCylinder &cyl)
bool	isValidAttachedObject (const std::vector< double > &angles, unsigned char &dist, std::vector< int > j1, std::vector< int > j2)
bool	isValidAttachedObject (const std::vector< double > &angles, unsigned char &dist)
bool	isValidPoint (double &x, double &y, double &z, short unsigned int &radius, unsigned char &dist)
Private Attributes
SBPLArmModel *	arm_
	arm model used by planner
int	attached_object_frame_num_
short unsigned int	attached_object_radius_
FILE *	fOut_
	the file for dumping debug output
OccupancyGrid *	grid_
	occupancy grid used by planner
std::vector< double >	inc_
std::vector< std::string >	known_objects_
bool	object_attached_
std::map< std::string, mapping_msgs::CollisionObject >	object_map_
	map from object id to object details
std::vector< KDL::Frame >	object_points_
std::map< std::string, std::vector< btVector3 > >	object_voxel_map_
	map from object id to list of occupying voxels
tf::TransformListener	tf_

---

Detailed Description

Definition at line 61 of file sbpl_collision_space.h.

---

Constructor & Destructor Documentation

sbpl_arm_planner::SBPLCollisionSpace::SBPLCollisionSpace	(	SBPLArmModel *	arm,
		OccupancyGrid *	grid
	)

default constructor

Parameters:

	a	pointer to the arm object used for planning
	a	pointer to an occupancy grid used for planning

Definition at line 37 of file sbpl_collision_space.cpp.

sbpl_arm_planner::SBPLCollisionSpace::~SBPLCollisionSpace

(

)

[inline]

destructor

Definition at line 71 of file sbpl_collision_space.h.

---

Member Function Documentation

void sbpl_arm_planner::SBPLCollisionSpace::addArmCuboidsToGrid

(

)

Definition at line 465 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::addCollisionObject

(

const mapping_msgs::CollisionObject &

object

)

add a collision object to the environment

Definition at line 991 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::attachCylinderToGripper	(	std::string	frame,
		geometry_msgs::Pose	pose,
		double	radius,
		double	length
	)

Definition at line 631 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::attachMeshToGripper	(	const std::string	frame,
		const geometry_msgs::Pose	pose,
		const std::vector< int32_t > &	triangles,
		const std::vector< geometry_msgs::Point > &	vertices
	)

Definition at line 657 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::attachMeshToGripper	(	const std::string	frame,
		const geometry_msgs::Pose	pose,
		const bodies::BoundingCylinder &	cyl
	)

Definition at line 665 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::attachSphereToGripper	(	std::string	frame,
		geometry_msgs::Pose	pose,
		double	radius
	)

Definition at line 615 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::checkCollision	(	const std::vector< double > &	angles,
		bool	verbose,
		bool	check_mesh,
		unsigned char &	dist
	)

check joint configuration for collision (0: collision)

Definition at line 59 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::checkLinkForCollision	(	const std::vector< double > &	angles,
		int	link_num,
		bool	verbose,
		unsigned char &	dist
	)

check if a specific link is in collision (0: collision)

Definition at line 112 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::checkLinkPathForCollision	(	const std::vector< double > &	start,
		const std::vector< double > &	end,
		int	link_num,
		bool	verbose,
		unsigned char &	dist
	)

check linearly interpolated path for collision of a specific link

Definition at line 237 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::checkPathForCollision	(	const std::vector< double > &	start,
		const std::vector< double > &	end,
		bool	verbose,
		unsigned char &	dist
	)

check linearly interpolated path for collisions

Definition at line 166 of file sbpl_collision_space.cpp.

double sbpl_arm_planner::SBPLCollisionSpace::distanceBetween3DLineSegments	(	std::vector< int >	l1a,
		std::vector< int >	l1b,
		std::vector< int >	l2a,
		std::vector< int >	l2b
	)			[private]

get the shortest distance between two 3D line segments

Definition at line 369 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::getAttachedObject	(	const std::vector< double > &	angles,
		std::vector< std::vector< double > > &	xyz
	)

get the voxels that the attached object occupies

Definition at line 843 of file sbpl_collision_space.cpp.

double sbpl_arm_planner::SBPLCollisionSpace::getAttachedObjectRadius

(

)

get the radius of the attached object (sphere or cylinder)

Definition at line 888 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::getBoundingCylinderOfMesh	(	const std::vector< int32_t > &	triangles,
		const std::vector< geometry_msgs::Point > &	vertices,
		bodies::BoundingCylinder &	cyl
	)			[private]

Definition at line 941 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::getBoundingCylinderOfMesh	(	std::string	mesh_file,
		shapes::Shape *	mesh,
		bodies::BoundingCylinder &	cyl
	)			[private]

Definition at line 893 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::getCollisionCylinders	(	const std::vector< double > &	angles,
		std::vector< std::vector< double > > &	cylinders
	)

Definition at line 480 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::getCollisionObjectVoxelPoses

(

std::vector< geometry_msgs::Pose > &

points

)

Definition at line 1053 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::getInterpolatedPath	(	const std::vector< double > &	start,
		const std::vector< double > &	end,
		std::vector< double > &	inc,
		std::vector< std::vector< double > > &	path
	)

Definition at line 574 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::getInterpolatedPath	(	const std::vector< double > &	start,
		const std::vector< double > &	end,
		double	inc,
		std::vector< std::vector< double > > &	path
	)

Definition at line 540 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::getJointPosesInGrid	(	std::vector< double >	angles,
		std::vector< std::vector< int > > &	jnts,
		KDL::Frame &	f_out,
		bool	verbose
	)

get the xyz coords of each joint in the arm

Definition at line 297 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::getLineSegment	(	const std::vector< int >	a,
		const std::vector< int >	b,
		std::vector< std::vector< int > > &	points
	)

get coordinates of cells that a line segment intersects

Definition at line 526 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::isValidAttachedObject	(	const std::vector< double > &	angles,
		unsigned char &	dist,
		std::vector< int >	j1,
		std::vector< int >	j2
	)			[private]

Definition at line 708 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::isValidAttachedObject	(	const std::vector< double > &	angles,
		unsigned char &	dist
	)			[private]

Definition at line 771 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::isValidCell	(	const int	x,
		const int	y,
		const int	z,
		const int	radius
	)			[inline]

check if the cell's distance to nearest obstacle > radius

Definition at line 191 of file sbpl_collision_space.h.

unsigned char sbpl_arm_planner::SBPLCollisionSpace::isValidLineSegment	(	const std::vector< int >	a,
		const std::vector< int >	b,
		const short unsigned int	radius
	)

check if a line segment lies on an invalid cell (0: collision)

Definition at line 321 of file sbpl_collision_space.cpp.

bool sbpl_arm_planner::SBPLCollisionSpace::isValidPoint	(	double &	x,
		double &	y,
		double &	z,
		short unsigned int &	radius,
		unsigned char &	dist
	)			[private]

Definition at line 829 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::processCollisionObjectMsg

(

const mapping_msgs::CollisionObject &

object

)

process a collision object message

Definition at line 963 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::putCollisionObjectsInGrid

(

)

Definition at line 1042 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::removeAllCollisionObjects

(

)

Definition at line 1037 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::removeAttachedObject

(

)

Definition at line 608 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::removeCollisionObject

(

const mapping_msgs::CollisionObject &

object

)

Definition at line 1025 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::setDebugFile

(

FILE *

file_ptr

)

choose the file to output debug information

Definition at line 54 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::transformPose	(	const std::string &	current_frame,
		const std::string &	desired_frame,
		const geometry_msgs::Pose &	pose_in,
		geometry_msgs::Pose &	pose_out
	)

Definition at line 1074 of file sbpl_collision_space.cpp.

void sbpl_arm_planner::SBPLCollisionSpace::transformPose	(	std::string	current_frame,
		std::string	desired_frame,
		geometry_msgs::Pose &	pose_in,
		geometry_msgs::Pose &	pose_out
	)

transform a pose from one frame to another

---

Member Data Documentation

SBPLArmModel* sbpl_arm_planner::SBPLCollisionSpace::arm_ [private]

arm model used by planner

Definition at line 153 of file sbpl_collision_space.h.

int sbpl_arm_planner::SBPLCollisionSpace::attached_object_frame_num_ [private]

Definition at line 174 of file sbpl_collision_space.h.

short unsigned int sbpl_arm_planner::SBPLCollisionSpace::attached_object_radius_ [private]

Definition at line 175 of file sbpl_collision_space.h.

FILE* sbpl_arm_planner::SBPLCollisionSpace::fOut_ [private]

the file for dumping debug output

Definition at line 159 of file sbpl_collision_space.h.

OccupancyGrid* sbpl_arm_planner::SBPLCollisionSpace::grid_ [private]

occupancy grid used by planner

Definition at line 156 of file sbpl_collision_space.h.

std::vector<double> sbpl_arm_planner::SBPLCollisionSpace::inc_ [private]

Definition at line 171 of file sbpl_collision_space.h.

std::vector<std::string> sbpl_arm_planner::SBPLCollisionSpace::known_objects_ [private]

Definition at line 167 of file sbpl_collision_space.h.

bool sbpl_arm_planner::SBPLCollisionSpace::object_attached_ [private]

Definition at line 173 of file sbpl_collision_space.h.

std::map<std::string, mapping_msgs::CollisionObject> sbpl_arm_planner::SBPLCollisionSpace::object_map_ [private]

map from object id to object details

Definition at line 162 of file sbpl_collision_space.h.

std::vector<KDL::Frame> sbpl_arm_planner::SBPLCollisionSpace::object_points_ [private]

Definition at line 176 of file sbpl_collision_space.h.

std::map<std::string, std::vector<btVector3> > sbpl_arm_planner::SBPLCollisionSpace::object_voxel_map_ [private]

map from object id to list of occupying voxels

Definition at line 165 of file sbpl_collision_space.h.

tf::TransformListener sbpl_arm_planner::SBPLCollisionSpace::tf_ [private]

Definition at line 169 of file sbpl_collision_space.h.

---

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

• sbpl_collision_space.h
• sbpl_collision_space.cpp