SBPLArmModel Class Reference

#include <sbpl_arm_model.h>

Public Member Functions
bool	checkJointLimits (std::vector< double > angles, bool verbose)
	return true or false if angles are within specified joint limits
bool	computeEndEffPose (const std::vector< double > angles, double R_target[3][3], double x, double y, double z, double axis_angle)
	compute the position {x,y,z} of the planning_joint & the axis angle which represents the difference to the target frame
bool	computeFK (const std::vector< double > angles, int frame_num, std::vector< double > &xyzrpy, int sol_num)
	(added for debugging - can be removed) compute the forward kinematics to get the pose of the specified joint, use sol_num to choose which RPY representation you want
bool	computeFK (const std::vector< double > angles, int f_num, std::vector< double > &xyzrpy)
	compute the forward kinematics to get the pose of the specified joint
bool	computeFK (const std::vector< double > angles, int frame_num, KDL::Frame *frame_out)
	compute the forward kinematics to get the pose of the specified joint
bool	computeIK (const std::vector< double > pose, const std::vector< double > start, std::vector< double > &solution)
	compute the inverse kinematics for the planning_joint pose
bool	computePlanningJointPos (const std::vector< double > angles, double x, double y, double *z)
	compute the coordinates in the world frame of the planning_joint
void	getArmChainRootLinkName (std::string &name)
bool	getArmDescription (FILE *fCfg)
	parse the arm description text file
double	getAxisAngle (const double R1[3][3], const double R2[3][3])
	compute the axis angle between 2 rotation matrices
std::vector< std::vector < double > >	getCollisionCuboids ()
	get the self collision cuboids that the robot occupies
void	getJointLimits (int joint_num, double min, double max)
	get the joint postition limits of specified joint
bool	getJointPositions (const std::vector< double > angles, std::vector< std::vector< double > > &links, KDL::Frame &f_out)
bool	getJointPositions (const std::vector< double > angles, const double R_target[3][3], std::vector< std::vector< double > > &links, double *axis_angle)
	compute the positions of all the tips of the links (collision checking)
double	getLinkRadius (int link)
short unsigned int	getLinkRadiusCells (int link)
	get radius of link in grid cells
int	getMaxLinkRadius ()
	get the radius (in meters) of the widest link
bool	getPlanningJointPose (const std::vector< double > angles, double R_target[3][3], std::vector< double > &pose, double *axis_angle)
	compute the pose in the world frame of the planning_joint
bool	getPlanningJointPose (const std::vector< double > angles, std::vector< double > &pose)
short unsigned int	getPlanningJointRadius ()
	get radius of link in grid cells
bool	initKDLChain (const std::string &fKDL)
	initialize the KDL chain for FK and IK
bool	initKDLChainFromParamServer ()
	grab the robot description file from the param server
void	printArmDescription (FILE *fOut)
	print description of arm from SBPL arm text file
void	RPY2Rot (double roll, double pitch, double yaw, double Rot[3][3])
	convert RPY values to a rotation matrix
	SBPLArmModel (FILE *arm_file)
	default constructor
void	setDebugFile (FILE *file)
	set the file used for debug output
void	setRefFrameTransform (KDL::Frame f, std::string &name)
	set the transform from the torso frame to the base frame (really supposed to be a transform from the frame that the kinematics is done to the world frame that the planning is done)
void	setResolution (double resolution)
	set the resolution used for collision checking
	~SBPLArmModel ()
	destructor
Public Attributes
int	num_joints_
	number of joints to be planned for
int	num_links_
	number of links (for collision checking)
double	resolution_
Private Member Functions
double	frameToAxisAngle (const KDL::Frame frame, const double R_target[3][3])
	calculate axis angle between a KDL frame and a Rot matrix
void	getRPY (double Rot[3][3], double roll, double pitch, double *yaw, int solution_number)
	convert a rotation matrix into roll,pitch,yaw
void	parseKDLTree ()
Private Attributes
KDL::Chain	chain_
std::string	chain_root_name_
	the name of the root frame in the KDL chain
std::string	chain_tip_name_
	the name of the tip frame in the KDL chain
std::vector< std::vector < double > >	collision_cuboids_
KDL::ChainFkSolverPos_recursive *	fk_solver_
FILE *	fOut_
KDL::Chain	ik_chain_
KDL::ChainFkSolverPos_recursive *	ik_fk_solver_
KDL::JntArray	ik_jnt_pos_in_
KDL::JntArray	ik_jnt_pos_out_
KDL::ChainIkSolverPos_NR *	ik_solver_
KDL::ChainIkSolverVel_pinv_givens *	ik_solver_vel_
KDL::JntArray	jnt_pos_in_
KDL::JntArray	jnt_pos_out_
std::vector< int >	joint_indeces_
	a vector of joint indeces used for collision checking.
std::map< std::string, std::string >	joint_segment_mapping_
std::vector< ArmJoint >	joints_
	a vector of joints
std::vector< std::string >	kdl_number_to_urdf_name_
KDL::Tree	kdl_tree_
	the kdl tree for the robot description
std::vector< ArmLink >	links_
	a vector of links
short unsigned int	max_radius_
	the radius of the widest link
int	num_collision_cuboids_
int	num_kdl_joints_
KDL::Frame	p_out_
int	planning_joint_
	the index of the joint that is being planned for
std::string	planning_joint_name_
	the name of the joint that is being planned for
pr2_arm_kinematics::PR2ArmIKSolver *	pr2_arm_ik_solver_
std::string	reference_frame_
	frame that the planning is done in
std::string	robot_description_
	a string containing the URDF that describes the robot arm
urdf::Model	robot_model_
	the robot model used by the KDL
std::map< std::string, std::string >	segment_joint_mapping_
KDL::Frame	transform_
KDL::Frame	transform_inverse_
std::map< std::string, int >	urdf_name_to_kdl_number_

Detailed Description

Definition at line 82 of file sbpl_arm_model.h.

Constructor & Destructor Documentation

SBPLArmModel::SBPLArmModel ( FILE * arm_file )

default constructor

Parameters:

the

filename of the sbpl arm description text file

Definition at line 36 of file sbpl_arm_model.cpp.

SBPLArmModel::~SBPLArmModel ( )

destructor

Definition at line 60 of file sbpl_arm_model.cpp.

Member Function Documentation

bool SBPLArmModel::checkJointLimits	(	std::vector< double >	angles,
		bool	verbose
	)

return true or false if angles are within specified joint limits

Definition at line 695 of file sbpl_arm_model.cpp.

bool SBPLArmModel::computeEndEffPose	(	const std::vector< double >	angles,
		double	R_target[3][3],
		double *	x,
		double *	y,
		double *	z,
		double *	axis_angle
	)

compute the position {x,y,z} of the planning_joint & the axis angle which represents the difference to the target frame

Definition at line 405 of file sbpl_arm_model.cpp.

bool SBPLArmModel::computeFK	(	const std::vector< double >	angles,
		int	frame_num,
		std::vector< double > &	xyzrpy,
		int	sol_num
	)

(added for debugging - can be removed) compute the forward kinematics to get the pose of the specified joint, use sol_num to choose which RPY representation you want

Definition at line 383 of file sbpl_arm_model.cpp.

bool SBPLArmModel::computeFK	(	const std::vector< double >	angles,
		int	f_num,
		std::vector< double > &	xyzrpy
	)

compute the forward kinematics to get the pose of the specified joint

Definition at line 366 of file sbpl_arm_model.cpp.

bool SBPLArmModel::computeFK	(	const std::vector< double >	angles,
		int	frame_num,
		KDL::Frame *	frame_out
	)

compute the forward kinematics to get the pose of the specified joint

Definition at line 335 of file sbpl_arm_model.cpp.

bool SBPLArmModel::computeIK	(	const std::vector< double >	pose,
		const std::vector< double >	start,
		std::vector< double > &	solution
	)

compute the inverse kinematics for the planning_joint pose

Definition at line 434 of file sbpl_arm_model.cpp.

bool SBPLArmModel::computePlanningJointPos	(	const std::vector< double >	angles,
		double *	x,
		double *	y,
		double *	z
	)

compute the coordinates in the world frame of the planning_joint

Definition at line 555 of file sbpl_arm_model.cpp.

double SBPLArmModel::frameToAxisAngle	(	const KDL::Frame	frame,
		const double	R_target[3][3]
	)			`[private]`

calculate axis angle between a KDL frame and a Rot matrix

Definition at line 421 of file sbpl_arm_model.cpp.

void SBPLArmModel::getArmChainRootLinkName ( std::string & name )

Definition at line 782 of file sbpl_arm_model.cpp.

bool SBPLArmModel::getArmDescription ( FILE * fCfg )

parse the arm description text file

Definition at line 79 of file sbpl_arm_model.cpp.

double SBPLArmModel::getAxisAngle	(	const double	R1[3][3],
		const double	R2[3][3]
	)

compute the axis angle between 2 rotation matrices

Definition at line 481 of file sbpl_arm_model.cpp.

std::vector< std::vector< double > > SBPLArmModel::getCollisionCuboids ( ) [inline]

get the self collision cuboids that the robot occupies

Definition at line 285 of file sbpl_arm_model.h.

void SBPLArmModel::getJointLimits	(	int	joint_num,
		double *	min,
		double *	max
	)

get the joint postition limits of specified joint

Definition at line 765 of file sbpl_arm_model.cpp.

bool SBPLArmModel::getJointPositions	(	const std::vector< double >	angles,
		std::vector< std::vector< double > > &	links,
		KDL::Frame &	f_out
	)

Definition at line 507 of file sbpl_arm_model.cpp.

bool SBPLArmModel::getJointPositions	(	const std::vector< double >	angles,
		const double	R_target[3][3],
		std::vector< std::vector< double > > &	links,
		double *	axis_angle
	)

compute the positions of all the tips of the links (collision checking)

Definition at line 530 of file sbpl_arm_model.cpp.

double SBPLArmModel::getLinkRadius ( int link ) [inline]

Definition at line 273 of file sbpl_arm_model.h.

short unsigned int SBPLArmModel::getLinkRadiusCells ( int link ) [inline]

get radius of link in grid cells

Definition at line 266 of file sbpl_arm_model.h.

int SBPLArmModel::getMaxLinkRadius ( ) [inline]

get the radius (in meters) of the widest link

Definition at line 280 of file sbpl_arm_model.h.

bool SBPLArmModel::getPlanningJointPose	(	const std::vector< double >	angles,
		double	R_target[3][3],
		std::vector< double > &	pose,
		double *	axis_angle
	)

compute the pose in the world frame of the planning_joint

Definition at line 570 of file sbpl_arm_model.cpp.

bool SBPLArmModel::getPlanningJointPose	(	const std::vector< double >	angles,
		std::vector< double > &	pose
	)

Definition at line 591 of file sbpl_arm_model.cpp.

short unsigned int SBPLArmModel::getPlanningJointRadius ( )

get radius of link in grid cells

void SBPLArmModel::getRPY	(	double	Rot[3][3],
		double *	roll,
		double *	pitch,
		double *	yaw,
		int	solution_number
	)			`[private]`

convert a rotation matrix into roll,pitch,yaw

convert a rotation matrix into the RPY format (copied from getEulerZYX() in Bullet physics library)

Definition at line 635 of file sbpl_arm_model.cpp.

bool SBPLArmModel::initKDLChain ( const std::string & fKDL )

initialize the KDL chain for FK and IK

Definition at line 243 of file sbpl_arm_model.cpp.

bool SBPLArmModel::initKDLChainFromParamServer ( )

grab the robot description file from the param server

Definition at line 225 of file sbpl_arm_model.cpp.

void SBPLArmModel::parseKDLTree ( ) [private]

Definition at line 292 of file sbpl_arm_model.cpp.

void SBPLArmModel::printArmDescription ( FILE * fOut )

print description of arm from SBPL arm text file

Definition at line 727 of file sbpl_arm_model.cpp.

void SBPLArmModel::RPY2Rot	(	double	roll,
		double	pitch,
		double	yaw,
		double	Rot[3][3]
	)

convert RPY values to a rotation matrix

Definition at line 610 of file sbpl_arm_model.cpp.

void SBPLArmModel::setDebugFile ( FILE * file )

set the file used for debug output

Definition at line 74 of file sbpl_arm_model.cpp.

void SBPLArmModel::setRefFrameTransform	(	KDL::Frame	f,
		std::string &	name
	)

set the transform from the torso frame to the base frame (really supposed to be a transform from the frame that the kinematics is done to the world frame that the planning is done)

Definition at line 771 of file sbpl_arm_model.cpp.

void SBPLArmModel::setResolution ( double resolution )

set the resolution used for collision checking

Definition at line 210 of file sbpl_arm_model.cpp.