reactive/guarded movement and grasping More...
Public Member Functions | |
def | __init__ |
def | adjust_grasp |
adjust the grasp in an attempt to center the object according to the contacts, if we appear to be in a marginal grasp assumes the fingers start closed around the object | |
def | broadcast_phase |
broadcast the current manipulation phase (of type ManipulationPhase) | |
def | check_fingers_in_table |
check to see if the fingers are colliding with the table in gripper_pose according to the collision model defaults to the current gripper pose | |
def | check_goal_reached |
check if we have gotten close enough to our goal along approach_vect (according to goal_pos_thres) | |
def | check_gripper_opening |
check the gripper opening to see if it's within our goal gripper opening range | |
def | check_guarded_move_contacts |
check for tip/side/back/palm contact | |
def | check_if_contacts_need_adjusting |
check for marginal grasps if we see only the edge sensors on both fingers, move in the z-axis direction if we see only the tip sensors, or front sensors not far enough in (and no palm sensors are pressed), move forward if we see only one edge on one finger and the opposite edge on the other finger, rotate the hand (disabled) | |
def | check_preempt |
check for a preempt (overload for your application) | |
def | close_hard |
close with a force of self.close_force, and record that the fingertips need a long-reset unless we're using the slip controller, in which case open and close to estimate stiffness | |
def | compliant_close |
close compliantly (controller manager should be in Cartesian mode) (if one finger touches first, counter-move the arm to keep that finger in-place while closing) | |
def | compute_approach_dir |
compute (unit) approach direction in base_link frame from an approach_pose and grasp_pose (returns scipy array) | |
def | compute_approach_vect |
compute approach vector in base_link frame from an approach_pose and grasp_pose (returns scipy array) | |
def | get_fingertip_readings_during_find_contact |
check the state of find_gripper_contact and try to collect the first positive thresholded fingertip readings only looks for front contacts | |
def | guarded_move_cartesian |
move the wrist to a desired Cartesian pose while watching the fingertip sensors settling_time is how long to wait after the controllers think we're there | |
def | guarded_move_ik |
move the wrist to a desired Cartesian pose using IK and watching the fingertip sensors | |
def | lift_carefully |
lift while checking for slip (assumes we already closed around the object) gripper_translation is a desired GripperTranslation for the lift slip servoing should continue after the lift (for moving the object through free space), until the gripper opens or closes returns 1 if the object is still in the hand at the end of the lift, 0 otherwise | |
def | make_orthogonal |
def | move_cartesian_step |
move to a Cartesian pose goal | |
def | normalize_vect |
def | open_and_reset_fingertips |
open the gripper, wait for the fingertips to release, and reset the thresholds if we're not touching anything | |
def | palm_touching |
are either of the palm sensors touching? (palm sensors removed; stubs left in just in case) | |
def | place_carefully |
place while checking for the gripper to report that the object has hit the table final_place_pose is a PoseStamped for the gripper in its desired place pose place_overshoot is the distance in m to go past the final_place_pose | |
def | ppmat |
pretty-print scipy matrix to string | |
def | reactive_approach |
reactive approach (stop if unexpected fingertip contact and go around) assumes we are already at the approach pose, going to grasp_pose along the direction approach_dir grasp_pose should be in base_link frame and have found and checked the joint-angle path side_step is how far to move right or left when you hit something num_tries is how many bump-and-moves to try before quitting | |
def | reactive_grasp |
do a reactive approach followed by a compliant close if the approach is successful then move forward more and try again along the approach if the gripper opening is not as expected succeeds if the gripper is closed with an opening between min_gripper_opening and max_gripper_opening tries the approach-and-grasp grasp_num_tries times (including the first approach) forward step is how far to move along the approach direction (past the goal) for the next approach-and-grasp try min_contact_row is how far in from the tip we require contact on the front of each sensor (farther in is okay), with values from 0-4 (-1 to ignore) possible return values: 0=success, 1=ran out of grasp tries, 2=ran out of approach tries, 3=aborted approach_pose, grasp_pose, joint_path, side_step, back_step, approach_num_tries, goal_pos_thres are inputs to the approach | |
def | return_rel_pose |
convert a relative vector in frame to a pose in the base_link frame if start_pose is not specified, uses current pose of the wrist if orthogonal_to_vect and orthogonal_to_vect_frame are specified, first convert vector to be orthogonal to orthogonal_to_vect | |
def | shake_object |
shake the object by rotating the wrist roll and flex joints and the arm pan joint at a particular set of arm angles | |
def | throw_exception |
throw an abort exception | |
def | vect_dot |
def | vect_norm |
def | vect_proj |
Public Attributes | |
close_force | |
cm | |
manipulation_phase_dict | |
pressure_listener | |
reactive_approach_result_dict | |
reactive_grasp_result_dict | |
using_slip_detection | |
whicharm | |
Private Attributes | |
_closed_hard | |
_phase_pub | |
_photo | |
_table_name |
reactive/guarded movement and grasping
Definition at line 69 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.adjust_grasp | ( | self, | |
grasp_pose, | |||
initial_l_readings, | |||
initial_r_readings, | |||
z_step = .015 , |
|||
x_step = .015 , |
|||
min_gripper_opening = .0021 , |
|||
max_gripper_opening = .1 , |
|||
min_contact_row = 1 , |
|||
num_tries = 4 |
|||
) |
adjust the grasp in an attempt to center the object according to the contacts, if we appear to be in a marginal grasp assumes the fingers start closed around the object
Definition at line 1154 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.broadcast_phase | ( | self, | |
phase | |||
) |
broadcast the current manipulation phase (of type ManipulationPhase)
Definition at line 117 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.check_fingers_in_table | ( | self, | |
gripper_pose = None |
|||
) |
check to see if the fingers are colliding with the table in gripper_pose according to the collision model defaults to the current gripper pose
Definition at line 1120 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.check_goal_reached | ( | self, | |
approach_vect, | |||
goal_pos, | |||
goal_pos_thres | |||
) |
check if we have gotten close enough to our goal along approach_vect (according to goal_pos_thres)
Definition at line 546 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.check_gripper_opening | ( | self, | |
min_gripper_opening = .01 , |
|||
max_gripper_opening = .1 |
|||
) |
check the gripper opening to see if it's within our goal gripper opening range
Definition at line 850 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.check_guarded_move_contacts | ( | self | ) |
check for tip/side/back/palm contact
Definition at line 330 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.check_if_contacts_need_adjusting | ( | self, | |
min_contact_row, | |||
l_readings = None , |
|||
r_readings = None , |
|||
check_table = 1 |
|||
) |
check for marginal grasps if we see only the edge sensors on both fingers, move in the z-axis direction if we see only the tip sensors, or front sensors not far enough in (and no palm sensors are pressed), move forward if we see only one edge on one finger and the opposite edge on the other finger, rotate the hand (disabled)
Definition at line 761 of file reactive_grasp.py.
check for a preempt (overload for your application)
Definition at line 123 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.close_hard | ( | self, | |
force = None |
|||
) |
close with a force of self.close_force, and record that the fingertips need a long-reset unless we're using the slip controller, in which case open and close to estimate stiffness
Definition at line 1081 of file reactive_grasp.py.
close compliantly (controller manager should be in Cartesian mode) (if one finger touches first, counter-move the arm to keep that finger in-place while closing)
Definition at line 213 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.compute_approach_dir | ( | self, | |
approach_pose, | |||
grasp_pose | |||
) |
compute (unit) approach direction in base_link frame from an approach_pose and grasp_pose (returns scipy array)
Definition at line 564 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.compute_approach_vect | ( | self, | |
approach_pose, | |||
grasp_pose | |||
) |
compute approach vector in base_link frame from an approach_pose and grasp_pose (returns scipy array)
Definition at line 571 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.get_fingertip_readings_during_find_contact | ( | self, | |
contacts_desired, | |||
timeout = 10. |
|||
) |
check the state of find_gripper_contact and try to collect the first positive thresholded fingertip readings only looks for front contacts
Definition at line 134 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.guarded_move_cartesian | ( | self, | |
pose_stamped, | |||
timeout = 3.0 , |
|||
settling_time = 0.5 , |
|||
pos_thres = .0025 , |
|||
rot_thres = .05 , |
|||
overshoot_dist = 0. , |
|||
overshoot_angle = 0. , |
|||
stuck_dist = 0 , |
|||
stuck_angle = 0 |
|||
) |
move the wrist to a desired Cartesian pose while watching the fingertip sensors settling_time is how long to wait after the controllers think we're there
Definition at line 350 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.guarded_move_ik | ( | self, | |
pose_stamped, | |||
max_joint_vel = .05 |
|||
) |
move the wrist to a desired Cartesian pose using IK and watching the fingertip sensors
Definition at line 432 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.lift_carefully | ( | self, | |
gripper_translation, | |||
min_gripper_opening = .0021 , |
|||
max_gripper_opening = .1 , |
|||
hardness_gain = 0.060 |
|||
) |
lift while checking for slip (assumes we already closed around the object) gripper_translation is a desired GripperTranslation for the lift slip servoing should continue after the lift (for moving the object through free space), until the gripper opens or closes returns 1 if the object is still in the hand at the end of the lift, 0 otherwise
Definition at line 865 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.make_orthogonal | ( | self, | |
vect1, | |||
vect2 | |||
) |
Definition at line 485 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.move_cartesian_step | ( | self, | |
pose, | |||
timeout = 10.0 , |
|||
settling_time = 3.0 , |
|||
blocking = 0 , |
|||
pos_thres = .0025 , |
|||
rot_thres = .05 |
|||
) |
move to a Cartesian pose goal
Definition at line 318 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.normalize_vect | ( | self, | |
vect | |||
) |
Definition at line 470 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.open_and_reset_fingertips | ( | self, | |
reset = 0 |
|||
) |
open the gripper, wait for the fingertips to release, and reset the thresholds if we're not touching anything
Definition at line 1093 of file reactive_grasp.py.
are either of the palm sensors touching? (palm sensors removed; stubs left in just in case)
Definition at line 579 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.place_carefully | ( | self, | |
final_place_pose, | |||
place_overshoot, | |||
min_gripper_opening = .0021 , |
|||
max_gripper_opening = .1 , |
|||
place_angle_max_diff = 1.05 |
|||
) |
place while checking for the gripper to report that the object has hit the table final_place_pose is a PoseStamped for the gripper in its desired place pose place_overshoot is the distance in m to go past the final_place_pose
Definition at line 1000 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.ppmat | ( | self, | |
mat | |||
) |
pretty-print scipy matrix to string
Definition at line 840 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.reactive_approach | ( | self, | |
approach_dir, | |||
grasp_pose, | |||
joint_path = None , |
|||
side_step = .015 , |
|||
back_step = .03 , |
|||
num_tries = 10 , |
|||
goal_pos_thres = 0.01 |
|||
) |
reactive approach (stop if unexpected fingertip contact and go around) assumes we are already at the approach pose, going to grasp_pose along the direction approach_dir grasp_pose should be in base_link frame and have found and checked the joint-angle path side_step is how far to move right or left when you hit something num_tries is how many bump-and-moves to try before quitting
Definition at line 591 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.reactive_grasp | ( | self, | |
approach_pose, | |||
grasp_pose, | |||
joint_path = None , |
|||
side_step = .015 , |
|||
back_step = .03 , |
|||
approach_num_tries = 10 , |
|||
goal_pos_thres = 0.01 , |
|||
min_gripper_opening = 0.0021 , |
|||
max_gripper_opening = 0.1 , |
|||
grasp_num_tries = 2 , |
|||
forward_step = 0.03 , |
|||
min_contact_row = 1 , |
|||
object_name = "points" , |
|||
table_name = "table" , |
|||
grasp_adjust_x_step = .02 , |
|||
grasp_adjust_z_step = .015 , |
|||
grasp_adjust_num_tries = 3 |
|||
) |
do a reactive approach followed by a compliant close if the approach is successful then move forward more and try again along the approach if the gripper opening is not as expected succeeds if the gripper is closed with an opening between min_gripper_opening and max_gripper_opening tries the approach-and-grasp grasp_num_tries times (including the first approach) forward step is how far to move along the approach direction (past the goal) for the next approach-and-grasp try min_contact_row is how far in from the tip we require contact on the front of each sensor (farther in is okay), with values from 0-4 (-1 to ignore) possible return values: 0=success, 1=ran out of grasp tries, 2=ran out of approach tries, 3=aborted approach_pose, grasp_pose, joint_path, side_step, back_step, approach_num_tries, goal_pos_thres are inputs to the approach
Definition at line 1285 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.return_rel_pose | ( | self, | |
vector, | |||
frame, | |||
start_pose = None , |
|||
orthogonal_to_vect = None , |
|||
orthogonal_to_vect_frame = 'base_link' |
|||
) |
convert a relative vector in frame to a pose in the base_link frame if start_pose is not specified, uses current pose of the wrist if orthogonal_to_vect and orthogonal_to_vect_frame are specified, first convert vector to be orthogonal to orthogonal_to_vect
Definition at line 497 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.shake_object | ( | self, | |
arm_angles = None , |
|||
min_gripper_opening = .0021 , |
|||
max_gripper_opening = .1 , |
|||
wrist_roll_shake_time = 0.5 , |
|||
wrist_flex_shake_time = 0.5 , |
|||
arm_pan_shake_time = 0.25 |
|||
) |
shake the object by rotating the wrist roll and flex joints and the arm pan joint at a particular set of arm angles
Definition at line 898 of file reactive_grasp.py.
throw an abort exception
Definition at line 128 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.vect_dot | ( | self, | |
vect1, | |||
vect2 | |||
) |
Definition at line 474 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.vect_norm | ( | self, | |
vect | |||
) |
Definition at line 466 of file reactive_grasp.py.
def pr2_gripper_reactive_approach.reactive_grasp.ReactiveGrasper.vect_proj | ( | self, | |
vect1, | |||
vect2 | |||
) |
Definition at line 478 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.
Definition at line 71 of file reactive_grasp.py.