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. |
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.
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.
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.
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.
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.
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.
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.
arm | 0 for right, 1 for left |
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.
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.
arm | 0 for right, 1 for left |
q | list of 7 joint angles in radians |
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.
arm | 0 for right, 1 for left |
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.
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.
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?
arm | 0 for right, 1 for left |
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
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.
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.
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.
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.
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.
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.
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 |
|||
) |
More specific functionality.
Moves arm smoothly through a linear trajectory.
arm |
Definition at line 792 of file pr2_arms.py.
def kelsey_sandbox.pr2_arms.PR2Arms.stop_trajectory | ( | self, | |
arm | |||
) |
Stop the current arm trajectory.
arm | 0 for right, 1 for left |
Definition at line 406 of file pr2_arms.py.
def kelsey_sandbox.pr2_arms.PR2Arms.transform_in_frame | ( | self, | |
pos, | |||
quat, | |||
off_point | |||
) |
Transforms the given position by the offset position in the given quaternion rotation frame.
pos | the current positions |
quat | quaternion representing the rotation of the frame |
off_point | offset to move the position inside the quat's frame |
Definition at line 466 of file pr2_arms.py.
def kelsey_sandbox.pr2_arms.PR2Arms.wait_for_arm_completion | ( | self, | |
arm, | |||
hz = 0.01 |
|||
) |
Block until the arm has finished moving.
arm | 0 for right, 1 for left |
hz | how often to check |
Definition at line 454 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 247 of file pr2_arms.py.
Definition at line 247 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 196 of file pr2_arms.py.
Definition at line 196 of file pr2_arms.py.
Definition at line 196 of file pr2_arms.py.
Definition at line 196 of file pr2_arms.py.
Definition at line 196 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.
Definition at line 97 of file pr2_arms.py.