Class for simple management of the arms and grippers. More...

Public Member Functions
def	__init__
	Initializes all of the servers, clients, and variables.
def	bias_guess
def	bias_wrist_ft
def	can_move_arm
	Evaluates IK for the given position and rotation to see if the arm could move.
def	cancel_trajectory
def	close_gripper
	Close the gripper.
def	create_JTG
	Create a joint configuration trajectory goal.
def	end_effector_pos
	Returns the current position, rotation of the arm.
def	execute_trajectory
	Executes a joint trajectory goal.
def	FK
	Performs Forward Kinematics on the given joint angles.
def	get_cep_jtt
def	get_ee_jtt
def	get_joint_angles
	Returns the list of 7 joint angles.
def	get_wrist_force
def	go_cep_jtt
def	grasp_biased_IK
	Same as move_arm but tries to keep the elbow up.
def	grasp_biased_move_arm
	Same as move_arm but tries to keep the elbow up.
def	grasp_biased_move_arm_trajectory
	Move the arm through a trajectory defined by a series of positions and rotations.
def	IK
	Performs Inverse Kinematics on the given position and rotation.
def	is_arm_in_motion
	Is the arm currently moving?
def	joint_states_cb
	Callback for /joint_states topic.
def	l_cart_state_cb
def	move_arm
	Moves the arm to the given position and rotation.
def	move_arm_trajectory
	Move the arm through a trajectory defined by a series of positions and rotations.
def	move_gripper
	Move the gripper the given amount with given amount of effort.
def	normalize_ang
def	open_gripper
	Open the gripper.
def	r_cart_state_cb
def	set_cep_jtt
def	set_joint_angles
	Move the arm to a joint configuration.
def	set_joint_angles_traj
	Move the arm through a joint configuration trajectory goal.
def	smooth_linear_arm_trajectory
	More specific functionality.
def	stop_trajectory
	Stop the current arm trajectory.
def	transform_in_frame
	Transforms the given position by the offset position in the given quaternion rotation frame.
def	wait_for_arm_completion
	Block until the arm has finished moving.
Public Attributes
	arm_angles
	arm_efforts
	arm_state_lock
	cur_traj
	cur_traj_pos
	cur_traj_timer
	fk_srv
	gripper_action_client
	ik_srv
	joint_action_client
	joint_names_list
	jtg
	l_arm_cart_pub
	l_cep_pos
	l_cep_rot
	marker_pub
	off_point
	r_arm_cart_pub
	r_arm_ftc
	r_cep_pos
	r_cep_pos_hooktip
	r_cep_rot
	r_ee_pos
	r_ee_rot
	send_delay
	tf_lstnr
Private Member Functions
def	_exec_traj
	Callback for periodic joint trajectory point throwing.

Detailed Description

Class for simple management of the arms and grippers.

Provides functionality for moving the arms, opening and closing the grippers, performing IK, and other functionality as it is developed.

Definition at line 88 of file pr2_arms.py.

Constructor & Destructor Documentation

def kelsey_sandbox.pr2_arms.PR2Arms.__init__	(	self,
		send_delay = `50000000`,
		gripper_point = `(0.23,0.0`,
		force_torque = `False`
	)

Initializes all of the servers, clients, and variables.

Parameters:

send_delay	send trajectory points send_delay nanoseconds into the future
gripper_point	given the frame of the wrist_roll_link, this point offsets the location used in FK and IK, preferably to the tip of the gripper

Definition at line 97 of file pr2_arms.py.

Member Function Documentation

def kelsey_sandbox.pr2_arms.PR2Arms._exec_traj	(	self,
		arm
	)	`[private]`

Callback for periodic joint trajectory point throwing.

Definition at line 364 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.bias_guess	(	self,
		q,
		joints_bias,
		bias_radius
	)

Definition at line 854 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.bias_wrist_ft	(	self,
		arm
	)

Definition at line 690 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.can_move_arm	(	self,
		arm,
		pos,
		rot
	)

Evaluates IK for the given position and rotation to see if the arm could move.

Parameters:

arm	0 for right, 1 for left
p	cartesian position in torso_lift_link frame
rot	quaternion rotation column or rotation matrix of wrist in torso_lift_link frame

Definition at line 577 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.cancel_trajectory	(	self,
		arm
	)

Definition at line 603 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.close_gripper	(	self,
		arm,
		effort = `15`
	)

Close the gripper.

Parameters:

arm	0 for right, 1 for left

Definition at line 778 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.create_JTG	(	self,
		arm,
		pos_arr,
		dur_arr,
		stamp = `None`,
		vel_arr = `None`,
		acc_arr = `None`
	)

Create a joint configuration trajectory goal.

Parameters:

arm	0 for right, 1 for left
pos_arr	list of lists of 7 joint angles in RADIANS.
dur_arr	list of how long (SECONDS) from the beginning of the trajectory before reaching the joint angles.
stamp	header (rospy.Duration) stamp to give the trajectory
vel_arr	list of lists of 7 joint velocities in RADIANS/sec.
acc_arr	list of lists of 7 joint accelerations in RADIANS/sec^2. Compute joint velocities and acclereations.

Definition at line 271 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.end_effector_pos	(	self,
		arm
	)

Returns the current position, rotation of the arm.

Parameters:

arm	0 for right, 1 for left

Returns:: rotation, position

Definition at line 657 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.execute_trajectory	(	self,
		arm,
		jtg
	)

Executes a joint trajectory goal.

This is the only function through which arm motion is performed.

Parameters:

arm	0 for right, 1 for left
jtg	the joint trajectory goal to execute

Definition at line 332 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.FK	(	self,
		arm,
		q
	)

Performs Forward Kinematics on the given joint angles.

Parameters:

arm	0 for right, 1 for left
q	list of 7 joint angles in radians

Returns:: column matrix of cartesian position, column matrix of quaternion

Definition at line 481 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.get_cep_jtt	(	self,
		arm,
		hook_tip = `False`
	)

Definition at line 703 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.get_ee_jtt	(	self,
		arm
	)

Definition at line 697 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.get_joint_angles	(	self,
		arm
	)

Returns the list of 7 joint angles.

Parameters:

arm	0 for right, 1 for left

Returns:: list of 7 joint angles

Definition at line 666 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.get_wrist_force	(	self,
		arm,
		bias = `True`,
		base_frame = `False`
	)

Definition at line 676 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.go_cep_jtt	(	self,
		arm,
		p
	)

Definition at line 741 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.grasp_biased_IK	(	self,
		arm,
		pos,
		rot,
		q_guess,
		joints_bias = `[0.]*7`,
		bias_radius = `0.`,
		num_iters = `5`
	)

Same as move_arm but tries to keep the elbow up.

Definition at line 870 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.grasp_biased_move_arm	(	self,
		arm,
		pos,
		rot = `None`,
		dur = `4.0`,
		num_iters = `20`
	)

Same as move_arm but tries to keep the elbow up.

Definition at line 879 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.grasp_biased_move_arm_trajectory	(	self,
		arm,
		pos_arr,
		dur = `1.`,
		rot_arr = `None`,
		dur_arr = `None`,
		freeze_wrist = `False`
	)

Move the arm through a trajectory defined by a series of positions and rotations.

Parameters:

arm	0 for right, 1 for left
pos_arr	list of positions to achieve during trajectory
dur	length of time to execute all trajectories in, all positions will be given the same amount of time to reach the next point (ignore if dur_arr is not None)
rot_arr	achieve these rotations along with those positions if None, maintain original rotation
dur_arr	use these times for the time positions
freeze_wrist	if True, keeps the rotation of the wrist constant

Definition at line 898 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.IK	(	self,
		arm,
		p,
		rot,
		q_guess
	)

Performs Inverse Kinematics on the given position and rotation.

Parameters:

arm	0 for right, 1 for left
p	cartesian position in torso_lift_link frame
rot	quaternion rotation column or rotation matrix of wrist in torso_lift_link frame
q_guess	initial joint angles to use for finding IK

Definition at line 524 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.is_arm_in_motion	(	self,
		arm
	)

Is the arm currently moving?

Parameters:

arm	0 for right, 1 for left

Returns:: True if moving, else False

Definition at line 442 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.joint_states_cb	(	self,
		data
	)

Callback for /joint_states topic.

Updates current joint angles and efforts for the arms constantly

Parameters:

data	JointState message recieved from the /joint_states topic

Definition at line 161 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.l_cart_state_cb	(	self,
		msg
	)

Definition at line 247 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.move_arm	(	self,
		arm,
		pos,
		begq = `None`,
		rot = `None`,
		dur = `4.0`
	)

Moves the arm to the given position and rotation.

Parameters:

arm	0 for right, 1 for left
pos	cartesian position of end effector
rot	quaterninon or rotation matrix of the end effector
dur	length of time to do the motion in

Definition at line 592 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.move_arm_trajectory	(	self,
		arm,
		pos_arr,
		dur = `1.`,
		rot_arr = `None`,
		dur_arr = `None`,
		freeze_wrist = `False`
	)

Move the arm through a trajectory defined by a series of positions and rotations.

Parameters:

arm	0 for right, 1 for left
pos_arr	list of positions to achieve during trajectory
dur	length of time to execute all trajectories in, all positions will be given the same amount of time to reach the next point (ignore if dur_arr is not None)
rot_arr	achieve these rotations along with those positions if None, maintain original rotation
dur_arr	use these times for the time positions
freeze_wrist	if True, keeps the rotation of the wrist constant

Definition at line 618 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.move_gripper	(	self,
		arm,
		amount = `0.08`,
		effort = `15`
	)

Move the gripper the given amount with given amount of effort.

Parameters:

arm	0 for right, 1 for left
amount	the amount the gripper should be opened
effort	- supposed to be in Newtons. (-ve number => max effort)

Definition at line 764 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.normalize_ang	(	self,
		ang
	)

Definition at line 254 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.open_gripper	(	self,
		arm
	)

Open the gripper.

Parameters:

arm	0 for right, 1 for left

Definition at line 771 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.r_cart_state_cb	(	self,
		msg
	)

Definition at line 196 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.set_cep_jtt	(	self,
		arm,
		p,
		rot = `None`
	)

Definition at line 713 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.set_joint_angles	(	self,
		arm,
		q,
		duration = `1.`,
		start_time = `0.`
	)

Move the arm to a joint configuration.

Parameters:

arm	0 for right, 1 for left
q	list of 7 joint angles in RADIANS.
start_time	time (in secs) from function call to start action
duration	how long (SECONDS) before reaching the joint angles.

Definition at line 418 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.set_joint_angles_traj	(	self,
		arm,
		q_arr,
		dur_arr,
		start_time = `0.`
	)

Move the arm through a joint configuration trajectory goal.

Parameters:

arm	0 for right, 1 for left
q_arr	list of lists of 7 joint angles in RADIANS.
dur_arr	list of how long (SECONDS) from the beginning of the trajectory before reaching the joint angles.
start_time	time (in secs) from function call to start action

Definition at line 429 of file pr2_arms.py.

def kelsey_sandbox.pr2_arms.PR2Arms.smooth_linear_arm_trajectory	(	self,
		arm,
		dist,
		dir = `(0.,0.`,
		max_jerk = `0.25`,
		delta = `0.005`,
		dur = `None`,
		freeze_wrist = `True`,
		is_grasp_biased = `True`
	)