#include <RobotArm.h>

Public Member Functions
void	bring_into_reach (tf::Stamped< tf::Pose > toolTargetPose)
btVector3	cartError ()
	cartesian difference at end effector resulting from difference between desired and actual joint state
bool	executeViaJointControl (const std::vector< tf::Stamped< tf::Pose > > &poses, int start=-1, int end=-1)
actionlib::SimpleActionClient < pr2_common_action_msgs::ArmMoveIKAction > *	getActionClient ()
void	getJointState (double state[])
void	getJointStateDes (double state[])
void	getJointStateErr (double state[])
actionlib::SimpleClientGoalState	getState ()
	Returns the current state of the action.
tf::Stamped< tf::Pose >	getToolPose (const char frame[]="base_link")
void	getToolPose (tf::Stamped< tf::Pose > &marker, const char frame[]="base_link")
void	getWristPose (tf::Stamped< tf::Pose > &marker, const char frame[]="base_link")
pr2_controllers_msgs::JointTrajectoryGoal	goalTraj (double a0, double a1, double a2, double a3, double a4, double a5, double a6, double dur=1.0)
pr2_controllers_msgs::JointTrajectoryGoal	goalTraj (double *poseA, double dur=1.0)
pr2_controllers_msgs::JointTrajectoryGoal	goalTraj (double poseA, double vel)
bool	isTucked ()
bool	move_ik (double x, double y, double z, double ox, double oy, double oz, double ow, double time=1.0)
bool	move_ik (tf::Stamped< tf::Pose > targetPose, double time=0.0)
bool	move_toolframe_ik (double x, double y, double z, double ox, double oy, double oz, double ow)
bool	move_toolframe_ik_pose (tf::Stamped< tf::Pose > toolTargetPose)
void	moveElbowOutOfWay (tf::Stamped< tf::Pose > toolTargetPose)
pr2_controllers_msgs::JointTrajectoryGoal	multiPointTrajectory (const std::vector< std::vector< double > > &poses, const double &duration=1.0)
pr2_controllers_msgs::JointTrajectoryGoal	multiPointTrajectory (const std::vector< std::vector< double > > &poses, const std::vector< double > &duration)
pr2_controllers_msgs::JointTrajectoryGoal	multiPointTrajectory (const std::vector< tf::Stamped< tf::Pose > > &poses, const std::vector< double > &duration)
bool	pose2Joint (const std::vector< tf::Stamped< tf::Pose > > &poses, std::vector< std::vector< double > > &joints)
bool	reachable (tf::Stamped< tf::Pose > target)
bool	rotate_toolframe_ik (double r_x, double r_y, double r_z)
tf::Stamped< tf::Pose >	rotate_toolframe_ik_p (double r_x, double r_y, double r_z)
bool	run_ik (geometry_msgs::PoseStamped pose, double start_angles[7], double solution[7], std::string link_name)
bool	run_ik (tf::Stamped< tf::Pose > pose, double start_angles[7], double solution[7], std::string link_name)
tf::Stamped< tf::Pose >	runFK (double jointAngles[], tf::Stamped< tf::Pose > *elbow=0)
void	stabilize_grip ()
void	startTrajectory (pr2_controllers_msgs::JointTrajectoryGoal goal, bool wait=true)
	Sends the command to start a given trajectory.
tf::Stamped< tf::Pose >	tool2wrist (tf::Stamped< tf::Pose > toolPose)
pr2_controllers_msgs::JointTrajectoryGoal	twoPointTrajectory (double poseA, double poseB)
btVector3	universal_move_toolframe_ik (double x, double y, double z, double ox, double oy, double oz, double ow, const char target_frame[]="base_link")
btVector3	universal_move_toolframe_ik_pose (tf::Stamped< tf::Pose > toolTargetPose)
	moves the toolframe to an ik position given in any frame, moving the base without rotating when necessary
btVector3	universal_move_toolframe_ik_pose_tolerance (tf::Stamped< tf::Pose > toolTargetPose, double tolerance, double timeout=5.0)
	move to the tool to the target pose with a goal tolerance, returns but does not stop arm when tolerance or timeout is reached
tf::Stamped< tf::Pose >	wrist2tool (tf::Stamped< tf::Pose > toolPose)
Static Public Member Functions
static bool	findBaseMovement (tf::Stamped< tf::Pose > &result, std::vector< int > arm, std::vector< tf::Stamped< tf::Pose > > goal, bool drive, bool reach)
static RobotArm *	getInstance (int side=0)
static void	init ()
static void	moveBothArms (tf::Stamped< tf::Pose > leftArm, tf::Stamped< tf::Pose > rightArm, double tolerance=0, bool wait=true)
static void	printPose (const tf::Stamped< tf::Pose > &toolTargetPose)
Public Attributes
bool	evil_switch
bool	excludeBaseProjectionFromWorkspace
double	preset_angle
bool	raise_elbow
int	retries
double	time_to_target
tf::Stamped< tf::Pose >	tool2wrist_
std::string	tool_frame
bool	tucked
tf::Stamped< tf::Pose >	wrist2tool_
std::string	wrist_frame
Private Member Functions
void	jointStateCallback (const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr &msg)
	RobotArm (int side)
	Initialize the action client and wait for action server to come up.
	~RobotArm ()
	Clean up the action client.
Private Attributes
actionlib::SimpleActionClient < pr2_common_action_msgs::ArmMoveIKAction > *	ac_
ros::ServiceClient	fk_client
volatile bool	haveJointState
ros::ServiceClient	ik_client
std::string	joint_names [7]
double	jointState [7]
double	jointStateDes [7]
double	jointStateErr [7]
ros::Subscriber	jointStateSubscriber_
boost::mutex	mutex_
ros::NodeHandle	n_
ros::ServiceClient	query_client
int	side_
TrajClient *	traj_client_
Static Private Attributes
static actionlib::SimpleActionClient < find_base_pose::FindBasePoseAction > *	ac_fbp_ = NULL
static RobotArm *	instance [] = {0,0}
static tf::TransformListener *	listener_ = 0

Detailed Description

Definition at line 62 of file RobotArm.h.

Constructor & Destructor Documentation

RobotArm::RobotArm ( int side ) [private]

Initialize the action client and wait for action server to come up.

Definition at line 78 of file RobotArm.cpp.

RobotArm::~RobotArm ( ) [private]

Clean up the action client.

Definition at line 141 of file RobotArm.cpp.

Member Function Documentation

void RobotArm::bring_into_reach ( tf::Stamped< tf::Pose > toolTargetPose )

Definition at line 1253 of file RobotArm.cpp.

btVector3 RobotArm::cartError ( )

cartesian difference at end effector resulting from difference between desired and actual joint state

Definition at line 1509 of file RobotArm.cpp.

bool RobotArm::executeViaJointControl	(	const std::vector< tf::Stamped< tf::Pose > > &	poses,
		int	start = `-1`,
		int	end = `-1`
	)

Definition at line 1699 of file RobotArm.cpp.

bool RobotArm::findBaseMovement	(	tf::Stamped< tf::Pose > &	result,
		std::vector< int >	arm,
		std::vector< tf::Stamped< tf::Pose > >	goal,
		bool	drive,
		bool	reach
	)		`[static]`

Definition at line 1078 of file RobotArm.cpp.

actionlib::SimpleActionClient<pr2_common_action_msgs::ArmMoveIKAction>* RobotArm::getActionClient ( ) [inline]

Definition at line 107 of file RobotArm.h.

RobotArm * RobotArm::getInstance ( int side = 0 ) [static]

Definition at line 148 of file RobotArm.cpp.

void RobotArm::getJointState ( double state[] )

Definition at line 155 of file RobotArm.cpp.

void RobotArm::getJointStateDes ( double state[] )

Definition at line 170 of file RobotArm.cpp.

void RobotArm::getJointStateErr ( double state[] )

Definition at line 184 of file RobotArm.cpp.

actionlib::SimpleClientGoalState RobotArm::getState ( )

Returns the current state of the action.

Definition at line 645 of file RobotArm.cpp.

tf::Stamped< tf::Pose > RobotArm::getToolPose ( const char frame[] = "base_link" )

Definition at line 659 of file RobotArm.cpp.

void RobotArm::getToolPose	(	tf::Stamped< tf::Pose > &	marker,
		const char	frame[] = `"base_link"`
	)

Definition at line 691 of file RobotArm.cpp.

void RobotArm::getWristPose	(	tf::Stamped< tf::Pose > &	marker,
		const char	frame[] = `"base_link"`
	)

Definition at line 719 of file RobotArm.cpp.

pr2_controllers_msgs::JointTrajectoryGoal RobotArm::goalTraj	(	double	a0,
		double	a1,
		double	a2,
		double	a3,
		double	a4,
		double	a5,
		double	a6,
		double	dur = `1.0`
	)

Definition at line 349 of file RobotArm.cpp.

pr2_controllers_msgs::JointTrajectoryGoal RobotArm::goalTraj	(	double *	poseA,
		double	dur = `1.0`
	)

Definition at line 362 of file RobotArm.cpp.

pr2_controllers_msgs::JointTrajectoryGoal RobotArm::goalTraj	(	double *	poseA,
		double *	vel
	)

Definition at line 411 of file RobotArm.cpp.

void RobotArm::init ( ) [static]

Definition at line 62 of file RobotArm.cpp.

bool RobotArm::isTucked ( ) [inline]

Definition at line 114 of file RobotArm.h.

void RobotArm::jointStateCallback ( const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr & msg ) [private]

Definition at line 45 of file RobotArm.cpp.

bool RobotArm::move_ik	(	double	x,
		double	y,
		double	z,
		double	ox,
		double	oy,
		double	oz,
		double	ow,
		double	time = `1.0`
	)

Definition at line 932 of file RobotArm.cpp.

bool RobotArm::move_ik	(	tf::Stamped< tf::Pose >	targetPose,
		double	time = `0.0`
	)

Definition at line 923 of file RobotArm.cpp.

bool RobotArm::move_toolframe_ik	(	double	x,
		double	y,
		double	z,
		double	ox,
		double	oy,
		double	oz,
		double	ow
	)

Definition at line 801 of file RobotArm.cpp.

bool RobotArm::move_toolframe_ik_pose ( tf::Stamped< tf::Pose > toolTargetPose )

Definition at line 784 of file RobotArm.cpp.

void RobotArm::moveBothArms	(	tf::Stamped< tf::Pose >	leftArm,
		tf::Stamped< tf::Pose >	rightArm,
		double	tolerance = `0`,
		bool	wait = `true`
	)		`[static]`

Definition at line 1628 of file RobotArm.cpp.

void RobotArm::moveElbowOutOfWay ( tf::Stamped< tf::Pose > toolTargetPose )

Definition at line 1523 of file RobotArm.cpp.

pr2_controllers_msgs::JointTrajectoryGoal RobotArm::multiPointTrajectory	(	const std::vector< std::vector< double > > &	poses,
		const double &	duration = `1.0`
	)

Definition at line 461 of file RobotArm.cpp.

pr2_controllers_msgs::JointTrajectoryGoal RobotArm::multiPointTrajectory	(	const std::vector< std::vector< double > > &	poses,
		const std::vector< double > &	duration
	)

Definition at line 517 of file RobotArm.cpp.

pr2_controllers_msgs::JointTrajectoryGoal RobotArm::multiPointTrajectory	(	const std::vector< tf::Stamped< tf::Pose > > &	poses,
		const std::vector< double > &	duration
	)

Definition at line 572 of file RobotArm.cpp.

bool RobotArm::pose2Joint	(	const std::vector< tf::Stamped< tf::Pose > > &	poses,
		std::vector< std::vector< double > > &	joints
	)

Definition at line 1671 of file RobotArm.cpp.

void RobotArm::printPose ( const tf::Stamped< tf::Pose > & toolTargetPose ) [static]

Definition at line 650 of file RobotArm.cpp.

bool RobotArm::reachable ( tf::Stamped< tf::Pose > target )

Definition at line 1267 of file RobotArm.cpp.

bool RobotArm::rotate_toolframe_ik	(	double	r_x,
		double	r_y,
		double	r_z
	)

Definition at line 773 of file RobotArm.cpp.

tf::Stamped< tf::Pose > RobotArm::rotate_toolframe_ik_p	(	double	r_x,
		double	r_y,
		double	r_z
	)

Definition at line 747 of file RobotArm.cpp.

bool RobotArm::run_ik	(	geometry_msgs::PoseStamped	pose,
		double	start_angles[7],
		double	solution[7],
		std::string	link_name
	)

Definition at line 206 of file RobotArm.cpp.

bool RobotArm::run_ik	(	tf::Stamped< tf::Pose >	pose,
		double	start_angles[7],
		double	solution[7],
		std::string	link_name
	)

Definition at line 305 of file RobotArm.cpp.

tf::Stamped< tf::Pose > RobotArm::runFK	(	double	jointAngles[],
		tf::Stamped< tf::Pose > *	elbow = `0`
	)

Definition at line 1407 of file RobotArm.cpp.

void RobotArm::stabilize_grip ( )

Definition at line 834 of file RobotArm.cpp.

void RobotArm::startTrajectory	(	pr2_controllers_msgs::JointTrajectoryGoal	goal,
		bool	wait = `true`
	)

Sends the command to start a given trajectory.

Definition at line 320 of file RobotArm.cpp.

tf::Stamped< tf::Pose > RobotArm::tool2wrist ( tf::Stamped< tf::Pose > toolPose )

Definition at line 1056 of file RobotArm.cpp.

pr2_controllers_msgs::JointTrajectoryGoal RobotArm::twoPointTrajectory	(	double *	poseA,
		double *	poseB
	)

Definition at line 580 of file RobotArm.cpp.

btVector3 RobotArm::universal_move_toolframe_ik	(	double	x,
		double	y,
		double	z,
		double	ox,
		double	oy,
		double	oz,
		double	ow,
		const char	target_frame[] = `"base_link"`
	)