00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 from __future__ import division
00040 import roslib
00041 roslib.load_manifest('pr2_gripper_reactive_approach')
00042 import time
00043 import rospy
00044 import os
00045 import controller_manager
00046 import sys
00047 from arm_navigation_msgs.msg import OrderedCollisionOperations, CollisionOperation
00048 from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
00049 from object_manipulation_msgs.msg import GripperTranslation, ManipulationPhase
00050 import tf
00051 import scipy
00052 import math
00053 import pdb
00054 import hand_sensor_listeners
00055 from object_manipulator.convert_functions import *
00056 import copy
00057 from actionlib_msgs.msg import GoalStatus
00058
00059
00060 def keypause():
00061 print "press enter to continue"
00062 input = raw_input()
00063 return input
00064
00065
00066 class Aborted(Exception): pass
00067
00068
00069 class ReactiveGrasper:
00070
00071 def __init__(self, cm):
00072
00073 self.cm = cm
00074 self.whicharm = self.cm.whicharm
00075 print "initializing %s fingertip sensor listener"%self.whicharm
00076 self.pressure_listener = hand_sensor_listeners.FingertipSensorListener(self.whicharm)
00077
00078 print "done"
00079
00080
00081 self.using_slip_detection = self.cm.using_slip_detection
00082
00083
00084 self.close_force = 50
00085 if self.using_slip_detection:
00086 self.close_force = 10
00087
00088
00089 self._closed_hard = 0
00090
00091
00092 self._table_name = "table"
00093
00094
00095 self._photo = 0
00096
00097
00098 self._phase_pub = rospy.Publisher('/reactive_manipulation_phase', ManipulationPhase)
00099
00100
00101 self.manipulation_phase_dict = {}
00102 for element in dir(ManipulationPhase):
00103 if element[0].isupper():
00104 self.manipulation_phase_dict[eval('ManipulationPhase.'+element)] = element
00105
00106
00107 self.reactive_approach_result_dict = {"success":0, "within goal threshold":1, "both fingers touching":2, "ran out of tries":3, "touching table":4, "saw palm contact":5, "grasp infeasible":6}
00108
00109
00110 self.reactive_grasp_result_dict = {"success":0, "ran out of grasp tries":1, "ran out of approach tries":2, \
00111 "aborted":3, "grasp infeasible":4}
00112
00113 rospy.loginfo("done with ReactiveGrasper init for the %s arm"%self.whicharm)
00114
00115
00116
00117 def broadcast_phase(self, phase):
00118 rospy.loginfo("broadcasting reactive phase %s"%self.manipulation_phase_dict[phase])
00119 self._phase_pub.publish(phase)
00120
00121
00122
00123 def check_preempt(self):
00124 pass
00125
00126
00127
00128 def throw_exception(self):
00129 raise Aborted
00130
00131
00132
00133
00134 def get_fingertip_readings_during_find_contact(self, contacts_desired, timeout = 10.):
00135 saved_l_pressure = [0.]*22
00136 saved_r_pressure = [0.]*22
00137 start_time = rospy.get_rostime()
00138 left_touching = right_touching = left_force = right_force = 0.
00139
00140 while not rospy.is_shutdown():
00141 state = self.cm.get_find_gripper_contact_state()
00142
00143
00144 if state not in [GoalStatus.ACTIVE, GoalStatus.PENDING]:
00145
00146
00147 if state == GoalStatus.SUCCEEDED:
00148 (left_touching, right_touching, left_force, right_force) = self.cm.get_find_gripper_contact_result()
00149 rospy.loginfo("left_touching: %d, left_force: %0.3f, right_touching: %d, right_force: %0.3f"\
00150 %(left_touching, left_force, right_touching, right_force))
00151 else:
00152 rospy.loginfo("GoalStatus was not SUCCEEDED; state=%d"%state)
00153 break
00154
00155
00156 if rospy.get_rostime() - start_time > rospy.Duration(timeout):
00157 rospy.loginfo("timeout reached on find_gripper_contact")
00158 break
00159
00160
00161 (l_pressure, r_pressure) = self.pressure_listener.get_thresholded_readings()
00162 if any(l_pressure[7:22]) and not any(saved_l_pressure[7:22]):
00163 rospy.loginfo("saving l_pressure:"+pplist(l_pressure))
00164 saved_l_pressure = l_pressure
00165 if any(r_pressure[7:22]) and not any(saved_r_pressure[7:22]):
00166 rospy.loginfo("saving r_pressure:"+pplist(r_pressure))
00167 saved_r_pressure = r_pressure
00168
00169 time.sleep(.02)
00170
00171
00172 if contacts_desired == "either" and (not any(saved_l_pressure[7:22]) and not any(saved_r_pressure[7:22])) or \
00173 contacts_desired == "both" and (not any(saved_l_pressure[7:22]) or not any(saved_r_pressure[7:22])):
00174 start_time = rospy.get_rostime()
00175 while not rospy.is_shutdown():
00176
00177
00178 if rospy.get_rostime() - start_time > rospy.Duration(1.0):
00179 rospy.loginfo("timed out on extra second of waiting for readings")
00180 break
00181
00182
00183 (l_pressure, r_pressure) = self.pressure_listener.get_thresholded_readings()
00184 if any(l_pressure[7:22]) and not any(saved_l_pressure[7:22]):
00185 rospy.loginfo("saving l_pressure:"+pplist(l_pressure))
00186 saved_l_pressure = l_pressure
00187 if any(r_pressure[7:22]) and not any(saved_r_pressure[7:22]):
00188 rospy.loginfo("saving r_pressure:"+pplist(r_pressure))
00189 saved_r_pressure = r_pressure
00190 if contacts_desired == "either" and (any(saved_l_pressure[7:22]) or any(saved_r_pressure[7:22])):
00191 break
00192 if contacts_desired == "both" and (any(saved_l_pressure[7:22]) and any(saved_r_pressure[7:22])):
00193 break
00194 time.sleep(.02)
00195
00196
00197
00198 (rel_l_pressure, rel_r_pressure) = self.pressure_listener.get_relative_readings()
00199 if left_touching and not any(saved_l_pressure[7:22]):
00200 most_likely_on = 7+rel_l_pressure[7:22].index(max(rel_l_pressure[7:22]))
00201 saved_l_pressure[most_likely_on] = 1
00202 rospy.loginfo("left_touching but not any saved_l_pressure, fixing")
00203 if right_touching and not any(saved_r_pressure[7:22]):
00204 most_likely_on = 7+rel_r_pressure[7:22].index(max(rel_r_pressure[7:22]))
00205 saved_r_pressure[most_likely_on] = 1
00206 rospy.loginfo("right_touching but not any saved_r_pressure, fixing")
00207
00208 return (saved_l_pressure, saved_r_pressure, left_touching, right_touching, left_force, right_force)
00209
00210
00211
00212
00213 def compliant_close(self):
00214 self.check_preempt()
00215
00216 self.broadcast_phase(ManipulationPhase.CLOSING)
00217 self.cm.check_controllers_ok('cartesian')
00218
00219 use_slip_controller_to_close = self.using_slip_detection
00220
00221
00222 step_time = .05
00223 left_touching = right_touching = 0
00224
00225
00226 if use_slip_controller_to_close:
00227 rospy.loginfo("finding first gripper contact")
00228 self.cm.find_gripper_contact(contacts_desired = "either", zero_fingertips = 1, blocking = 0, timeout = 10.)
00229 (l_pressure, r_pressure, left_touching, right_touching, left_force, right_force) = \
00230 self.get_fingertip_readings_during_find_contact(contacts_desired = "either")
00231 else:
00232
00233 desired_pos = self.cm.get_current_gripper_opening()
00234 while(desired_pos > .01):
00235 self.cm.command_gripper(desired_pos, self.close_force)
00236 (l_pressure, r_pressure) = self.pressure_listener.get_thresholded_readings()
00237 if any(l_pressure):
00238 left_touching = 1
00239 break
00240 if any(r_pressure):
00241 right_touching = 1
00242 break
00243 desired_pos -= .001
00244 time.sleep(step_time)
00245
00246 self.check_preempt()
00247
00248
00249 if not (left_touching and right_touching):
00250
00251
00252 reloaded_controllers = 0
00253 if left_touching or right_touching:
00254
00255 if not use_slip_controller_to_close:
00256
00257 desired_pos += .002
00258 self.cm.command_gripper(desired_pos, 200)
00259
00260
00261 if self.cm.use_trajectory_cartesian:
00262 print "saw first contact! Reloading Cartesian controllers"
00263 if left_touching:
00264 self.cm.cart_params.tip_frame = self.whicharm+'_gripper_l_finger_tip_link'
00265 else:
00266 self.cm.cart_params.tip_frame = self.whicharm+'_gripper_r_finger_tip_link'
00267 self.cm.cart_params.set_params_to_defaults(set_tip_frame = 0, set_params_on_server = 1)
00268 self.cm.reload_cartesian_controllers(set_params = 0)
00269 reloaded_controllers = 1
00270 print "done reloading Cartesian controllers"
00271
00272 if self._photo:
00273 keypause()
00274
00275 self.check_preempt()
00276
00277
00278 if use_slip_controller_to_close:
00279 rospy.loginfo("finding second gripper contact")
00280 contacts_desired = "both"
00281 self.cm.find_gripper_contact(contacts_desired, zero_fingertips = 0, blocking = 0, timeout = 10.)
00282 (l_pressure, r_pressure, left_touching, right_touching, left_force, right_force) = \
00283 self.get_fingertip_readings_during_find_contact(contacts_desired = "both")
00284
00285 else:
00286 left_touching = right_touching = 0
00287 while(desired_pos > 0):
00288 self.cm.command_gripper(desired_pos, self.close_force)
00289 (l_pressure, r_pressure) = self.pressure_listener.get_thresholded_readings()
00290 if any(l_pressure):
00291 left_touching = 1
00292 if any(r_pressure):
00293 right_touching = 1
00294 if left_touching and right_touching:
00295 print "saw both left and right contacts, stopping"
00296 break
00297 desired_pos -= .001
00298 time.sleep(step_time)
00299
00300 if self._photo:
00301 keypause()
00302
00303 self.check_preempt()
00304
00305
00306 if reloaded_controllers:
00307 print "putting control frame back"
00308 self.cm.cart_params.set_params_to_gentle(set_tip_frame = 1, set_params_on_server = 1)
00309 self.cm.reload_cartesian_controllers(set_params = 0)
00310 print "done reloading Cartesian controllers"
00311
00312 self.check_preempt()
00313
00314 return (l_pressure, r_pressure)
00315
00316
00317
00318 def move_cartesian_step(self, pose, timeout = 10.0, settling_time = 3.0, blocking = 0, pos_thres = .0025, rot_thres = .05):
00319 self.check_preempt()
00320
00321 if type(pose) == list:
00322 pose = create_pose_stamped(pose, 'base_link')
00323 self.cm.move_cartesian(pose, blocking = blocking, \
00324 pos_thres = pos_thres, rot_thres = rot_thres, \
00325 timeout = rospy.Duration(timeout), \
00326 settling_time = rospy.Duration(settling_time))
00327
00328
00329
00330 def check_guarded_move_contacts(self):
00331
00332 left_touching = right_touching = palm_touching = 0
00333
00334
00335 (l_regions_touching, r_regions_touching) = self.pressure_listener.regions_touching()
00336 if self.palm_touching():
00337 palm_touching = 1
00338 rospy.loginfo("saw palm sensor, stopping")
00339 if any(l_regions_touching[0:3]) or l_regions_touching[4]:
00340 left_touching = 1
00341 rospy.loginfo("saw left fingertip tip or side, stopping")
00342 if any(r_regions_touching[0:3] or r_regions_touching[4]):
00343 right_touching = 1
00344 rospy.loginfo("saw right fingertip tip or side, stopping")
00345 return (left_touching, right_touching, palm_touching)
00346
00347
00348
00349
00350 def guarded_move_cartesian(self, pose_stamped, timeout = 3.0, settling_time = 0.5, \
00351 pos_thres = .0025, rot_thres = .05, overshoot_dist = 0., \
00352 overshoot_angle = 0., stuck_dist = 0, stuck_angle = 0):
00353
00354 self.check_preempt()
00355
00356
00357 rospy.loginfo("sending goal to Cartesian controllers")
00358 self.cm.move_cartesian(pose_stamped, blocking = 0, pos_thres = pos_thres, rot_thres = rot_thres, \
00359 timeout = rospy.Duration(timeout), settling_time = rospy.Duration(settling_time), \
00360 overshoot_dist = overshoot_dist, overshoot_angle = overshoot_angle)
00361
00362
00363 start_time = rospy.get_rostime()
00364 done_time = None
00365 (last_pos, last_rot) = self.cm.return_cartesian_pose()
00366 last_pos_array = [last_pos]*5
00367 last_rot_array = [last_rot]*5
00368 r = rospy.Rate(1./self.cm.timestep)
00369 while not rospy.is_shutdown():
00370
00371 self.check_preempt()
00372 now = rospy.get_rostime()
00373 (current_pos, current_rot) = self.cm.return_cartesian_pose()
00374
00375
00376 (left_touching, right_touching, palm_touching) = self.check_guarded_move_contacts()
00377
00378
00379 if left_touching or right_touching or palm_touching:
00380 rospy.loginfo("saw contact")
00381 if self.cm.use_trajectory_cartesian:
00382 self.cm.switch_to_joint_mode()
00383 self.cm.freeze_arm()
00384 else:
00385 self.cm.set_desired_cartesian_to_current()
00386 break
00387
00388
00389 if self.cm.check_cartesian_really_done(pose_stamped, pos_thres, rot_thres, current_pos, current_rot):
00390 rospy.loginfo("actually got there")
00391 break
00392
00393
00394 if stuck_dist or stuck_angle:
00395 (pos_change, angle_change) = self.cm.dist_from_current_pos_and_rot(last_pos_array[0], last_rot_array[0])
00396 for ind in range(len(last_pos_array)-1):
00397 last_pos_array[ind] = last_pos_array[ind+1]
00398 last_rot_array[ind] = last_rot_array[ind+1]
00399 last_pos_array[-1] = current_pos
00400 last_rot_array[-1] = current_rot
00401 if now - start_time > rospy.Duration(2.0) and\
00402 abs(pos_change) <= stuck_dist*self.cm.timestep and \
00403 abs(angle_change) <= stuck_angle*self.cm.timestep:
00404 rospy.loginfo("Cartesian controller is stuck, stopping. pos_change=%f, angle_change=%f", \
00405 pos_change, angle_change)
00406 break
00407
00408
00409 if not done_time and self.cm.check_cartesian_done():
00410 rospy.loginfo("check_cartesian_done returned 1")
00411 done_time = rospy.get_rostime()
00412
00413
00414 if self.cm.use_trajectory_cartesian and done_time and \
00415 rospy.get_rostime() - done_time > rospy.Duration(settling_time):
00416 rospy.loginfo("done settling")
00417 break
00418
00419
00420 if timeout != 0. and rospy.get_rostime() - start_time > rospy.Duration(timeout):
00421 rospy.loginfo("timed out")
00422 break
00423
00424 r.sleep()
00425
00426 self.cm.stop_cartesian_commands()
00427
00428 return (left_touching, right_touching, palm_touching)
00429
00430
00431
00432 def guarded_move_ik(self, pose_stamped, max_joint_vel = .05):
00433
00434 self.check_preempt()
00435
00436
00437 success = self.cm.command_interpolated_ik(pose_stamped, collision_aware = 0, step_size = .005, max_joint_vel = max_joint_vel)
00438
00439
00440 if not success:
00441 (left_touching, right_touching, palm_touching) = self.guarded_move_cartesian(pose_stamped, 10.0, 5.0)
00442 return (left_touching, right_touching, palm_touching)
00443
00444
00445 while(1):
00446
00447 self.check_preempt()
00448
00449
00450 (left_touching, right_touching, palm_touching) = self.check_guarded_move_contacts()
00451
00452
00453 if left_touching or right_touching or palm_touching:
00454 self.cm.freeze_arm()
00455 break
00456
00457
00458 if self.cm.check_joint_trajectory_done():
00459 break
00460
00461
00462 return (left_touching, right_touching, palm_touching)
00463
00464
00465
00466 def vect_norm(self, vect):
00467 return sum([x**2 for x in vect])**.5
00468
00469
00470 def normalize_vect(self, vect):
00471 return list(scipy.array(vect)/self.vect_norm(vect))
00472
00473
00474 def vect_dot(self, vect1, vect2):
00475 return sum([x*y for (x,y) in zip(vect1, vect2)])
00476
00477
00478 def vect_proj(self, vect1, vect2):
00479 norm_vect2 = self.normalize_vect(vect2)
00480 mag = self.vect_dot(vect1, norm_vect2)
00481 proj = [mag*x for x in norm_vect2]
00482 return proj
00483
00484
00485 def make_orthogonal(self, vect1, vect2):
00486 vect1_mag = self.vect_norm(vect1)
00487 projected_comp = self.vect_proj(vect1, vect2)
00488 orthogonal_comp = [x-y for (x,y) in zip(vect1, projected_comp)]
00489 orthogonal_comp_mag = self.vect_norm(orthogonal_comp)
00490 orthogonal_vect = [vect1_mag * x / orthogonal_comp_mag for x in orthogonal_comp]
00491 return orthogonal_vect
00492
00493
00494
00495
00496
00497 def return_rel_pose(self, vector, frame, start_pose = None, orthogonal_to_vect = None, orthogonal_to_vect_frame = 'base_link'):
00498
00499
00500 if start_pose == None:
00501 (start_trans, start_rot) = self.cm.return_cartesian_pose('base_link')
00502 else:
00503 start_pose.header.stamp = rospy.Time(0)
00504 (start_trans, start_rot) = pose_stamped_to_lists(self.cm.tf_listener, start_pose, 'base_link')
00505
00506
00507 if frame != 'base_link':
00508 vector3_stamped = create_vector3_stamped(vector, frame)
00509 base_link_vector = vector3_stamped_to_list(self.cm.tf_listener, vector3_stamped, 'base_link')
00510 else:
00511 base_link_vector = vector
00512
00513
00514
00515
00516
00517
00518 if orthogonal_to_vect != None:
00519
00520
00521 if orthogonal_to_vect_frame != 'base_link':
00522 ortho_vector3_stamped = create_vector3_stamped(orthogonal_to_vect, orthogonal_to_vect_frame)
00523 ortho_base_link_vector = vector3_stamped_to_list(self.cm.tf_listener, ortho_vector3_stamped, 'base_link')
00524 else:
00525 ortho_base_link_vector = orthogonal_to_vect
00526
00527
00528 base_link_vector = self.make_orthogonal(base_link_vector, ortho_base_link_vector)
00529
00530
00531 new_trans = [x+y for (x,y) in zip(start_trans, base_link_vector)]
00532
00533
00534 pose_stamped = create_pose_stamped(new_trans+start_rot)
00535
00536
00537
00538
00539
00540
00541
00542 return pose_stamped
00543
00544
00545
00546 def check_goal_reached(self, approach_vect, goal_pos, goal_pos_thres):
00547 (current_pos, current_rot) = self.cm.return_cartesian_pose()
00548
00549
00550 current_proj = sum([x*y for (x,y) in zip(current_pos, approach_vect)])
00551 goal_proj = sum([x*y for (x,y) in zip(goal_pos, approach_vect)])
00552
00553 posdiff = goal_proj-current_proj
00554
00555
00556
00557 if posdiff < goal_pos_thres:
00558 return 1
00559
00560 return 0
00561
00562
00563
00564 def compute_approach_dir(self, approach_pose, grasp_pose):
00565 approach_vect = self.compute_approach_vect(approach_pose, grasp_pose)
00566 approach_dir = approach_vect/scipy.dot(approach_vect, approach_vect)**.5
00567 return approach_dir
00568
00569
00570
00571 def compute_approach_vect(self, approach_pose, grasp_pose):
00572 (approach_pos, approach_rot) = pose_stamped_to_lists(self.cm.tf_listener, approach_pose, 'base_link')
00573 (grasp_pos, grasp_rot) = pose_stamped_to_lists(self.cm.tf_listener, grasp_pose, 'base_link')
00574 approach_vect = scipy.array(grasp_pos) - scipy.array(approach_pos)
00575 return approach_vect
00576
00577
00578
00579 def palm_touching(self):
00580
00581
00582 return 0
00583
00584
00585
00586
00587
00588
00589
00590
00591 def reactive_approach(self, approach_dir, grasp_pose, joint_path = None,
00592 side_step = .015, back_step = .03, \
00593 num_tries = 10, goal_pos_thres = 0.01):
00594
00595 self.check_preempt()
00596
00597 self.broadcast_phase(ManipulationPhase.MOVING_TO_GRASP)
00598
00599 ever_saw_left = ever_saw_right = 0
00600
00601
00602 (left_touching, right_touching, palm_touching) = self.check_guarded_move_contacts()
00603
00604 if not left_touching and not right_touching and not palm_touching:
00605 if 0:
00606
00607
00608
00609 self.cm.command_joint_trajectory(joint_path, 0.1)
00610 while not self.cm.check_joint_trajectory_done():
00611 (left_touching, right_touching, palm_touching) = self.check_guarded_move_contacts()
00612
00613 if left_touching or right_touching or palm_touching:
00614 self.cm.freeze_arm()
00615 break
00616 else:
00617
00618
00619 (left_touching, right_touching, palm_touching) = self.guarded_move_ik(grasp_pose)
00620
00621
00622 if not left_touching and not right_touching and not palm_touching:
00623 rospy.loginfo("approach move finished with no contacts")
00624 return self.reactive_approach_result_dict["success"]
00625
00626
00627
00628 time.sleep(1.0)
00629 (goal_pos, goal_rot) = pose_stamped_to_lists(self.cm.tf_listener, grasp_pose, 'base_link')
00630 if self.check_goal_reached(approach_dir, goal_pos, goal_pos_thres):
00631 rospy.loginfo("made it to within goal threshold")
00632 return self.reactive_approach_result_dict["within goal threshold"]
00633
00634
00635 if palm_touching:
00636 rospy.loginfo("saw palm contact")
00637 return self.reactive_approach_result_dict["saw palm contact"]
00638
00639
00640 if left_touching and right_touching:
00641 rospy.loginfo("both fingers touching")
00642 return self.reactive_approach_result_dict["both fingers touching"]
00643
00644
00645 if (left_touching or right_touching) and self.check_fingers_in_table():
00646 rospy.loginfo("touching the table! Returning")
00647 return self.reactive_approach_result_dict["touching table"]
00648
00649
00650 rospy.loginfo("saw one contact")
00651 tries = 0
00652 new_goal = grasp_pose
00653 while(tries < num_tries):
00654
00655 rospy.loginfo("approach step %d"%tries)
00656
00657 if self._photo:
00658 keypause()
00659
00660
00661 back_off_vect = -approach_dir*back_step
00662 pose = self.return_rel_pose(back_off_vect, 'base_link')
00663 self.cm.move_cartesian_ik(pose)
00664
00665
00666 self.check_preempt()
00667
00668
00669 self.pressure_listener.set_thresholds(short = 1)
00670
00671 self.check_preempt()
00672
00673 if self._photo:
00674 keypause()
00675
00676
00677 if left_touching:
00678 rospy.loginfo("side-stepping to the left")
00679 side_step_vect = [0., side_step, 0.]
00680
00681
00682 if right_touching:
00683 rospy.loginfo("side-stepping to the right")
00684 side_step_vect = [0., -side_step, 0.]
00685
00686
00687 pose = self.return_rel_pose(side_step_vect, self.whicharm+'_wrist_roll_link', orthogonal_to_vect = approach_dir)
00688 (left_touching, right_touching, palm_touching) = self.guarded_move_ik(pose)
00689
00690 new_goal = self.return_rel_pose(side_step_vect, self.whicharm+'_wrist_roll_link', new_goal)
00691
00692 self.check_preempt()
00693
00694 if self._photo:
00695 keypause()
00696
00697
00698
00699 if not left_touching and not right_touching:
00700 (goal_pos, goal_rot) = pose_stamped_to_lists(self.cm.tf_listener, new_goal, 'base_link')
00701 rospy.loginfo("going to new goal, pos: "+self.cm.pplist(goal_pos)+" rot: "+self.cm.pplist(goal_rot))
00702 (left_touching, right_touching, palm_touching) = self.guarded_move_ik(new_goal)
00703
00704
00705 self.check_preempt()
00706
00707 if self._photo:
00708 keypause()
00709
00710
00711 if not left_touching and not right_touching and not palm_touching:
00712 rospy.loginfo("made it all the way without touching anything")
00713 return self.reactive_approach_result_dict["success"]
00714
00715
00716
00717 time.sleep(1.0)
00718 if self.check_goal_reached(approach_dir, goal_pos, goal_pos_thres):
00719 rospy.loginfo("within goal threshold")
00720 return self.reactive_approach_result_dict["within goal threshold"]
00721
00722 else:
00723 rospy.loginfo("hit something while side-stepping")
00724
00725
00726 if palm_touching:
00727 rospy.loginfo("saw palm contact")
00728 return self.reactive_approach_result_dict["saw palm contact"]
00729
00730
00731 if left_touching and right_touching:
00732 rospy.loginfo("seeing both contacts! Returning")
00733 return self.reactive_approach_result_dict["both fingers touching"]
00734
00735
00736 if (left_touching or right_touching) and self.check_fingers_in_table():
00737 rospy.loginfo("touching the table! Returning")
00738 return self.reactive_approach_result_dict["touching table"]
00739
00740
00741 ever_saw_left = ever_saw_left or left_touching
00742 ever_saw_right = ever_saw_right or right_touching
00743
00744
00745 if ever_saw_left and ever_saw_right:
00746 side_step /= 2.
00747 rospy.loginfo("saw both contacts, halving the side-step size to %5.3f"%side_step)
00748 ever_saw_right = right_touching
00749 ever_saw_left = left_touching
00750
00751 tries += 1
00752
00753 rospy.loginfo("ran out of tries")
00754 return self.reactive_approach_result_dict["ran out of tries"]
00755
00756
00757
00758
00759
00760
00761 def check_if_contacts_need_adjusting(self, min_contact_row, l_readings = None, r_readings = None, check_table = 1):
00762
00763 (l_readings, r_readings, l_regions_touching, r_regions_touching, l_rows_touching, r_rows_touching, \
00764 l_columns_touching, r_columns_touching) = self.pressure_listener.get_sensor_status(l_readings, r_readings)
00765 self.pressure_listener.print_sensor_status(l_readings, r_readings)
00766
00767 front_touching = 1
00768 adjust_x = 0
00769 adjust_z = 0
00770 adjust_rot = 0
00771 fingers_in_table = 0
00772
00773
00774 if check_table and self.check_fingers_in_table():
00775 rospy.loginfo("fingers near table, grasp is probably fine despite not having any front contacts")
00776 return (1, 0, 0, 0)
00777
00778
00779 if not any(l_rows_touching) and not any(r_rows_touching):
00780
00781
00782 rospy.loginfo("no front elements touching")
00783 return (0, 0, 0, 0)
00784
00785
00786
00787 if min_contact_row >= 0:
00788 if self.palm_touching():
00789 rospy.loginfo("palm touching, can't move forward")
00790 else:
00791 r_contact_row = -1
00792 if any(r_rows_touching):
00793 r_contact_row = 4-r_rows_touching.index(1)
00794 l_contact_row = -1
00795 if any(l_rows_touching):
00796 l_contact_row = 4-l_rows_touching.index(1)
00797 rospy.loginfo("l contact row is %d, r contact row is %d"%(l_contact_row, r_contact_row))
00798 if l_contact_row < min_contact_row and r_contact_row < min_contact_row:
00799 rospy.loginfo("left and right contacts both not far enough in, need to move forward")
00800 adjust_x = 1
00801 if not adjust_x:
00802 rospy.loginfo("contacts are fine in x-direction")
00803
00804
00805
00806
00807 if l_columns_touching[1] or r_columns_touching[1]:
00808 rospy.loginfo("at least one center column touching, no need to adjust in z-direction")
00809
00810
00811 elif (l_columns_touching[0] or r_columns_touching[0]) and not (l_columns_touching[2] or r_columns_touching[2]):
00812 adjust_z = 1
00813
00814
00815 elif (l_columns_touching[2] or r_columns_touching[2]) and not (l_columns_touching[0] or r_columns_touching[0]):
00816 adjust_z = -1
00817
00818
00819 else:
00820 rospy.loginfo("contacts not all on one side, no need to adjust in z-direction")
00821
00822
00823
00824
00825 if l_columns_touching[0] and r_columns_touching[2] and not (l_columns_touching[2] or r_columns_touching[0]):
00826 adjust_rot = -1
00827
00828
00829 elif l_columns_touching[2] and r_columns_touching[0] and not(l_columns_touching[0] or r_columns_touching[2]):
00830 adjust_rot = 1
00831
00832 else:
00833 rospy.loginfo("no need to rotate about x")
00834
00835 rospy.loginfo("front_touching:%d, adjust_x:%d, adjust_z:%d, adjust_rot:%d"%(front_touching, adjust_x, adjust_z, adjust_rot))
00836 return (front_touching, adjust_x, adjust_z, adjust_rot)
00837
00838
00839
00840 def ppmat(self, mat):
00841 str = ''
00842 for i in range(mat.shape[0]):
00843 for j in range(mat.shape[1]):
00844 str += '%5.3f\t'%mat[i,j]
00845 str += '\n'
00846 return str
00847
00848
00849
00850 def check_gripper_opening(self, min_gripper_opening = .01, max_gripper_opening = .1):
00851 gripper_opening = self.cm.get_current_gripper_opening()
00852 rospy.loginfo("min_gripper_opening:"+str(min_gripper_opening))
00853 if min_gripper_opening < gripper_opening and gripper_opening < max_gripper_opening:
00854 rospy.loginfo("gripper opening was acceptable: %5.4f"%gripper_opening)
00855 return 1
00856
00857 rospy.loginfo("gripper opening unacceptable: %5.4f"%gripper_opening)
00858 return 0
00859
00860
00861
00862
00863
00864
00865 def lift_carefully(self, gripper_translation, min_gripper_opening = .0021, max_gripper_opening = .1, hardness_gain = 0.060):
00866
00867
00868 base_link_translation = change_vector3_stamped_frame(self.cm.tf_listener, gripper_translation.direction, 'base_link')
00869
00870
00871 start_wrist_pose = self.cm.get_current_wrist_pose_stamped()
00872
00873
00874
00875 if self.using_slip_detection:
00876 self.cm.start_gripper_grab(hardness_gain)
00877
00878 self.broadcast_phase(ManipulationPhase.LIFTING)
00879
00880
00881 desired_wrist_pose = copy.deepcopy(start_wrist_pose)
00882
00883
00884 desired_wrist_pose.header.stamp = rospy.Time(start_wrist_pose.header.stamp.secs)
00885 desired_wrist_pose.pose.position.x += base_link_translation.vector.x * gripper_translation.desired_distance
00886 desired_wrist_pose.pose.position.y += base_link_translation.vector.y * gripper_translation.desired_distance
00887 desired_wrist_pose.pose.position.z += base_link_translation.vector.z * gripper_translation.desired_distance
00888 self.move_cartesian_step(desired_wrist_pose, blocking = 1)
00889
00890
00891 success = self.check_gripper_opening(min_gripper_opening, max_gripper_opening)
00892
00893 return success
00894
00895
00896
00897
00898 def shake_object(self, arm_angles = None, min_gripper_opening = .0021, max_gripper_opening = .1,
00899 wrist_roll_shake_time = 0.5, wrist_flex_shake_time = 0.5, arm_pan_shake_time = 0.25):
00900
00901
00902 wrist_flex = -1.03
00903 wrist_roll = -.5
00904 if arm_angles == None:
00905 if self.whicharm == 'r':
00906 arm_angles = [-0.28, -0.21, -0.78, -1.02, -1.02, wrist_flex, wrist_roll]
00907 else:
00908 arm_angles = [0.28, -0.21, 0.78, -1.02, 1.02, wrist_flex, wrist_roll]
00909
00910 rospy.loginfo("moving to shake arm angles")
00911 self.broadcast_phase(ManipulationPhase.MOVING_WITH_OBJECT)
00912 self.cm.command_joint_trajectory([arm_angles], blocking = 1)
00913
00914
00915 self.broadcast_phase(ManipulationPhase.HOLDING_OBJECT)
00916 time.sleep(4)
00917
00918
00919 success = self.check_gripper_opening(min_gripper_opening, max_gripper_opening)
00920 if not success:
00921 return 0
00922
00923
00924 shake_amplitude = 1.75*1.57
00925 roll_pos_angles = arm_angles[:]
00926 roll_pos_angles[6] += shake_amplitude
00927 roll_neg_angles = arm_angles[:]
00928 roll_neg_angles[6] -= shake_amplitude
00929
00930 roll_trajectory = [roll_pos_angles, roll_neg_angles, roll_pos_angles, roll_neg_angles, arm_angles]
00931 times = [wrist_roll_shake_time*i+0.05 for i in range(len(roll_trajectory))]
00932 vels = [[0.0]*7 for i in range(len(roll_trajectory))]
00933
00934 rospy.loginfo("starting roll shake")
00935 self.broadcast_phase(ManipulationPhase.SHAKING)
00936 self.cm.command_joint_trajectory(roll_trajectory, blocking = 1, times_and_vels = (times, vels))
00937
00938
00939 time.sleep(4)
00940
00941
00942 success = self.check_gripper_opening(min_gripper_opening, max_gripper_opening)
00943 if not success:
00944 return 0
00945
00946
00947 flex_pos_angles = arm_angles[:]
00948 flex_pos_angles[5] += .78
00949 flex_neg_angles = arm_angles[:]
00950 flex_neg_angles[5] -= .78
00951
00952 flex_trajectory = [flex_pos_angles, flex_neg_angles, flex_pos_angles, flex_neg_angles, arm_angles]
00953 times = [wrist_flex_shake_time*i+0.05 for i in range(len(flex_trajectory))]
00954 vels = [[0.0]*7 for i in range(len(flex_trajectory))]
00955
00956 rospy.loginfo("starting flex shake")
00957 self.broadcast_phase(ManipulationPhase.SHAKING)
00958 self.cm.command_joint_trajectory(flex_trajectory, blocking = 1, times_and_vels = (times, vels))
00959
00960
00961 time.sleep(4)
00962
00963
00964 success = self.check_gripper_opening(min_gripper_opening, max_gripper_opening)
00965 if not success:
00966 return 0
00967
00968
00969 pan_pos_angles = arm_angles[:]
00970 pan_neg_angles = arm_angles[:]
00971 shake_amplitude = 0.125
00972 if self.whicharm == 'r':
00973 pan_pos_angles[0] += shake_amplitude
00974 pan_neg_angles[0] -= shake_amplitude
00975 else:
00976 pan_pos_angles[0] -= shake_amplitude
00977 pan_neg_angles[0] += shake_amplitude
00978 pan_trajectory = [pan_pos_angles, pan_neg_angles, pan_pos_angles, pan_neg_angles, arm_angles]
00979 times = [arm_pan_shake_time*i+0.05 for i in range(len(pan_trajectory))]
00980 vels = [[0.0]*7 for i in range(len(pan_trajectory))]
00981
00982 rospy.loginfo("starting pan shake")
00983 self.broadcast_phase(ManipulationPhase.SHAKING)
00984 self.cm.command_joint_trajectory(pan_trajectory, blocking = 1, times_and_vels = (times, vels))
00985
00986
00987 time.sleep(4)
00988
00989
00990 success = self.check_gripper_opening(min_gripper_opening, max_gripper_opening)
00991 if not success:
00992 return 0
00993
00994 return 1
00995
00996
00997
00998
00999
01000 def place_carefully(self, final_place_pose, place_overshoot, min_gripper_opening = .0021, \
01001 max_gripper_opening = .1, place_angle_max_diff = 1.05):
01002
01003 self.broadcast_phase(ManipulationPhase.PLACING)
01004
01005
01006 if not self.check_gripper_opening(min_gripper_opening, max_gripper_opening):
01007 self.broadcast_phase(ManipulationPhase.FAILED)
01008 self.cm.switch_to_joint_mode()
01009 return 0
01010
01011
01012 (start_wrist_pos, start_wrist_rot) = self.cm.return_cartesian_pose()
01013
01014
01015 (place_pos, place_rot) = pose_stamped_to_lists(self.cm.tf_listener, final_place_pose, 'base_link')
01016 place_vect = scipy.array(place_pos) - scipy.array(start_wrist_pos)
01017 place_vect_mag = (place_vect*place_vect).sum()
01018 overshoot_vect = place_vect / place_vect_mag * (place_vect_mag + place_overshoot)
01019 overshoot_pos = overshoot_vect + place_pos
01020 overshoot_pose = create_pose_stamped(overshoot_pos.tolist() + place_rot, 'base_link')
01021
01022
01023 self.cm.cart_params.set_params_to_gentle(set_tip_frame = 0, set_params_on_server = 1)
01024
01025
01026 self.check_preempt()
01027 if self.using_slip_detection:
01028 self.cm.start_gripper_event_detector(blocking = 0)
01029
01030
01031 self.move_cartesian_step(overshoot_pose, blocking = 0)
01032
01033
01034 start_time = rospy.get_rostime()
01035 timeout = 5.
01036 start_angles = self.cm.get_current_arm_angles()
01037 while not rospy.is_shutdown():
01038 self.check_preempt()
01039
01040
01041 if self.using_slip_detection:
01042 state = self.cm.get_gripper_event_detector_state()
01043
01044
01045 if state not in [GoalStatus.ACTIVE, GoalStatus.PENDING]:
01046 rospy.loginfo("place carefully saw the trigger, stopping the arm")
01047 break
01048
01049
01050 current_angles = self.cm.get_current_arm_angles()
01051 if any([abs(current - start) > place_angle_max_diff for (current, start) in zip(current_angles, start_angles)]):
01052 rospy.loginfo("arm angle change too great during place, stopping the arm")
01053 break
01054
01055
01056 if rospy.get_rostime() - start_time > rospy.Duration(timeout):
01057 rospy.loginfo("place carefully timed out before seeing the trigger, stopping the arm")
01058 break
01059
01060 time.sleep(0.1)
01061
01062
01063 if self.cm.use_trajectory_cartesian:
01064 self.cm.switch_to_joint_mode()
01065 self.cm.freeze_arm()
01066 else:
01067 self.cm.set_desired_cartesian_to_current()
01068
01069
01070 self.broadcast_phase(ManipulationPhase.OPENING)
01071 self.cm.command_gripper(.1, -1, 1)
01072
01073
01074 self.cm.cart_params.set_params_to_defaults(set_tip_frame = 0, set_params_on_server = 1)
01075
01076 return 1
01077
01078
01079
01080
01081 def close_hard(self, force = None):
01082
01083 if not self.using_slip_detection:
01084 self.broadcast_phase(ManipulationPhase.CLOSING)
01085 tries = 0
01086 self._closed_hard = 1
01087 if force == None:
01088 force = self.close_force
01089 self.cm.command_gripper(0, force, 1)
01090
01091
01092
01093 def open_and_reset_fingertips(self, reset = 0):
01094
01095
01096 rospy.loginfo("opening gripper")
01097 self.cm.command_gripper(.1, -1, 1)
01098
01099 self.check_preempt()
01100
01101
01102 if self._closed_hard:
01103 rospy.loginfo("waiting for the fingertips to release")
01104 time.sleep(2)
01105 self._closed_hard = 0
01106 else:
01107 time.sleep(1)
01108
01109 self.check_preempt()
01110
01111 if reset:
01112
01113 self.pressure_listener.set_thresholds(short = 1)
01114
01115 self.check_preempt()
01116
01117
01118
01119
01120 def check_fingers_in_table(self, gripper_pose = None):
01121
01122
01123 (l_regions_touching, r_regions_touching) = self.pressure_listener.regions_touching()
01124
01125
01126 if not (l_regions_touching[0] or r_regions_touching[0]):
01127 return 0
01128
01129 if gripper_pose == None:
01130 gripper_pose = self.cm.get_current_wrist_pose_stamped()
01131 current_angles = self.cm.get_current_arm_angles()
01132 collision_oper1 = CollisionOperation(object1 = CollisionOperation.COLLISION_SET_ALL, \
01133 object2 = CollisionOperation.COLLISION_SET_ALL, \
01134 operation = CollisionOperation.DISABLE)
01135 collision_oper2 = CollisionOperation(object1 = self.whicharm+"_gripper_r_finger_tip_link", \
01136 object2 = self._table_name, \
01137 operation = CollisionOperation.ENABLE)
01138 collision_oper3 = CollisionOperation(object1 = self.whicharm+"_gripper_l_finger_tip_link", \
01139 object2 = self._table_name, \
01140 operation = CollisionOperation.ENABLE)
01141
01142
01143
01144
01145
01146
01147
01148
01149
01150 return 0
01151
01152
01153
01154 def 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):
01155
01156 self.check_preempt()
01157
01158
01159 if not self.check_gripper_opening(min_gripper_opening, max_gripper_opening):
01160 return ("gripper opening unacceptable", grasp_pose)
01161
01162
01163 (front_touching, adjust_x, adjust_z, adjust_rot) = self.check_if_contacts_need_adjusting(min_contact_row, initial_l_readings, initial_r_readings)
01164
01165
01166 tries = 0
01167 current_grasp_pose = grasp_pose
01168 self._closed_hard = 0
01169 while(1):
01170
01171 self.check_preempt()
01172
01173
01174 if tries >= num_tries:
01175
01176
01177 rospy.loginfo("ran out of tries while adjusting the grasp, trying to close hard anyway")
01178 self.close_hard()
01179
01180
01181 if not self.check_gripper_opening(min_gripper_opening, max_gripper_opening):
01182 return ("gripper opening unacceptable", current_grasp_pose)
01183 return ("grasp okay", current_grasp_pose)
01184
01185
01186
01187 rospy.loginfo("adjust_grasp tries: "+str(tries))
01188
01189
01190 if adjust_x or adjust_z:
01191
01192 self.broadcast_phase(ManipulationPhase.ADJUSTING_GRASP)
01193
01194
01195 adjust_mat = scipy.matrix(scipy.identity(4))
01196 adjust_vect = scipy.matrix([0,0,0]).T
01197 if adjust_z:
01198 adjust_mat[2, 3] = adjust_z * z_step
01199 if adjust_x:
01200 adjust_mat[0, 3] = x_step
01201
01202 new_grasp_pose = transform_pose_stamped(current_grasp_pose, adjust_mat)
01203
01204 self.check_preempt()
01205
01206
01207 rospy.loginfo("opening gripper to adjust hand")
01208 self.open_and_reset_fingertips(reset = 1)
01209
01210 self.check_preempt()
01211 if self._photo:
01212 keypause()
01213
01214
01215 current_grasp_pose = new_grasp_pose
01216
01217
01218 (left_touching, right_touching, palm_touching) = self.guarded_move_ik(current_grasp_pose)
01219
01220 self.check_preempt()
01221 if self._photo:
01222 keypause()
01223
01224
01225 (l_readings, r_readings) = self.compliant_close()
01226
01227 self.check_preempt()
01228 if self._photo:
01229 keypause()
01230
01231
01232 current_grasp_pose = self.cm.get_current_wrist_pose_stamped()
01233
01234
01235 if not self.check_gripper_opening(min_gripper_opening, max_gripper_opening):
01236 return ("gripper opening unacceptable", current_grasp_pose)
01237
01238
01239 (front_touching, adjust_x, adjust_z, adjust_rot) = self.check_if_contacts_need_adjusting(min_contact_row, l_readings, r_readings)
01240
01241
01242 grasp_okay = 1
01243 if not front_touching or adjust_x or adjust_z:
01244 grasp_okay = 0
01245
01246
01247 if grasp_okay:
01248 self.check_preempt()
01249
01250 rospy.loginfo("contacts are fine")
01251 self.close_hard()
01252
01253 if self._photo:
01254 keypause()
01255
01256
01257 if not self.check_gripper_opening(min_gripper_opening, max_gripper_opening):
01258 return ("gripper opening unacceptable", current_grasp_pose)
01259
01260
01261 (front_touching, adjust_x, adjust_z, adjust_rot) = self.check_if_contacts_need_adjusting(min_contact_row)
01262 grasp_okay = 1
01263 if not front_touching or adjust_x or adjust_z:
01264 grasp_okay = 0
01265
01266
01267 if grasp_okay:
01268 rospy.loginfo("contacts are fine")
01269 result = "grasp okay"
01270 break
01271
01272 tries += 1
01273
01274 return (result, current_grasp_pose)
01275
01276
01277
01278
01279
01280
01281
01282
01283
01284
01285 def reactive_grasp(self, approach_pose, grasp_pose, joint_path = None, side_step = .015, back_step = .03, \
01286 approach_num_tries = 10, goal_pos_thres = 0.01, min_gripper_opening = 0.0021, max_gripper_opening = 0.1, \
01287 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):
01288
01289 self._table_name = table_name
01290
01291 self.check_preempt()
01292
01293
01294 if approach_pose == None:
01295 approach_pose = self.cm.get_current_wrist_pose_stamped()
01296
01297
01298 self.pressure_listener.set_thresholds()
01299
01300
01301 self.check_preempt()
01302
01303
01304 if approach_pose.header.frame_id != 'base_link':
01305 self.cm.tf_listener.waitForTransform('base_link', approach_pose.header.frame_id, rospy.Time.now(), rospy.Duration(5))
01306 try:
01307 approach_pose = self.cm.tf_listener.transformPose('base_link', approach_pose)
01308 except:
01309 rospy.logerr("tf transform was not there!")
01310 self.throw_exception()
01311 return self.reactive_grasp_result_dict["aborted"]
01312
01313 if grasp_pose.header.frame_id != 'base_link':
01314 self.cm.tf_listener.waitForTransform('base_link', grasp_pose.header.frame_id, rospy.Time.now(), rospy.Duration(5))
01315 try:
01316 grasp_pose = self.cm.tf_listener.transformPose('base_link', grasp_pose)
01317 except:
01318 rospy.logerr("tf transform was not there!")
01319 self.throw_exception()
01320 return self.reactive_grasp_result_dict["aborted"]
01321
01322
01323 approach_dir = self.compute_approach_dir(approach_pose, grasp_pose)
01324
01325 num_tries = 0
01326 current_goal_pose = grasp_pose
01327 while 1:
01328
01329 if approach_num_tries:
01330
01331 approach_result = self.reactive_approach(approach_dir, current_goal_pose, joint_path, \
01332 side_step, back_step, \
01333 approach_num_tries, goal_pos_thres)
01334
01335
01336
01337
01338
01339 if approach_result == self.reactive_approach_result_dict["grasp infeasible"]:
01340 return self.reactive_grasp_result_dict["grasp infeasible"]
01341
01342 else:
01343
01344
01345 self.broadcast_phase(ManipulationPhase.MOVING_TO_GRASP)
01346 rospy.loginfo("reactive approach is disabled, moving to the goal")
01347 self.move_cartesian_step(grasp_pose, timeout = 10.0, settling_time = 5.0, blocking = 1)
01348
01349
01350 rospy.loginfo("starting compliant_close")
01351 (l_readings, r_readings) = self.compliant_close()
01352
01353
01354 current_goal_pose = self.cm.get_current_wrist_pose_stamped()
01355
01356
01357 (adjust_result, current_goal_pose) = self.adjust_grasp(current_goal_pose, l_readings, r_readings, z_step = grasp_adjust_z_step, \
01358 x_step = grasp_adjust_x_step, min_gripper_opening = min_gripper_opening, max_gripper_opening = max_gripper_opening, \
01359 min_contact_row = min_contact_row, num_tries = grasp_adjust_num_tries)
01360 rospy.loginfo("adjust_result: "+adjust_result)
01361
01362
01363 if adjust_result == "grasp okay":
01364 return self.reactive_grasp_result_dict["success"]
01365
01366
01367 if self.palm_touching():
01368 return self.reactive_grasp_result_dict["success"]
01369
01370
01371 num_tries += 1
01372 if num_tries < grasp_num_tries:
01373
01374
01375 move_forward_vect = approach_dir*forward_step
01376 current_goal_pose = self.return_rel_pose(move_forward_vect, 'base_link', current_goal_pose)
01377
01378 joint_path = None
01379
01380
01381 (goal_pos, goal_rot) = pose_stamped_to_lists(self.cm.tf_listener, current_goal_pose, 'base_link')
01382 rospy.loginfo("trying approach again with new goal, pos: "+self.cm.pplist(goal_pos)+" rot: "+self.cm.pplist(goal_rot))
01383
01384 if self._photo:
01385 keypause()
01386
01387
01388 self.open_and_reset_fingertips(reset = 1)
01389
01390 if self._photo:
01391 keypause()
01392
01393
01394 else:
01395 rospy.loginfo("ran out of grasp tries!")
01396 return self.reactive_grasp_result_dict["ran out of grasp tries"]
01397
01398
01399
01400
01401
01402 if __name__ == "__main__":
01403
01404 rospy.init_node('grasp_demo', anonymous = True)
01405
01406 print "initializing controller manager"
01407 cm = controller_manager.ControllerManager('r')
01408
01409 print "opening gripper"
01410 cm.command_gripper(.1, -1, 1)
01411 print "done"
01412
01413 print "initializing reactive grasper"
01414 rg = ReactiveGrasper(cm)
01415 print "done initialization"
01416
01417 cm.start_joint_controllers()
01418 cm.start_gripper_controller()
01419
01420
01421
01422
01423 table_height = .7239
01424 tip_dist_to_table = .12
01425 wrist_height = table_height + tip_dist_to_table + .02
01426
01427 topapproachpos = [.52, -.05, wrist_height+.1]
01428 topapproachquat = [-0.5, 0.5, 0.5, 0.5]
01429 topgrasppos = topapproachpos[:]
01430 topgrasppos[2] -= .08
01431 top_approach_pose = create_pose_stamped(topapproachpos+topapproachquat)
01432 top_grasp_pose = create_pose_stamped(topgrasppos+topapproachquat)
01433
01434
01435 sideangles = [-0.447, -0.297, -2.229, -0.719, 0.734, -1.489, 1.355]
01436 side_tip_dist_to_table = .14
01437 start_angles = sideangles
01438 tiltangle = math.pi/18.
01439 sideapproachmat = scipy.array([[0., -1., 0., 0.],
01440 [math.cos(tiltangle), 0., math.sin(tiltangle), 0.],
01441 [-math.sin(tiltangle), 0., math.cos(tiltangle), 0.],
01442 [0., 0., 0., 1.]])
01443 sideapproachpos = [.63, -.3, table_height-.035+side_tip_dist_to_table]
01444 sideapproachquat = list(tf.transformations.quaternion_from_matrix(sideapproachmat))
01445 sidegrasppos = sideapproachpos[:]
01446 sidegrasppos[1] += .09
01447
01448 side_approach_pose = create_pose_stamped(sideapproachpos+sideapproachquat)
01449 side_grasp_pose = create_pose_stamped(sidegrasppos+sideapproachquat)
01450
01451
01452 top_approach_vect = rg.compute_approach_vect(top_approach_pose, top_grasp_pose)
01453 side_approach_vect = rg.compute_approach_vect(side_approach_pose, side_grasp_pose)
01454
01455 mode = 'none'
01456
01457 (pos, rot) = cm.return_cartesian_pose()
01458 currentgoal = pos+rot
01459
01460 small_step = .1
01461
01462 while(not rospy.is_shutdown()):
01463 print "enter top to go to the top approach position"
01464 print "side to go to the side approach position"
01465 print "rg to run the full reactive grasp"
01466 print "ra to approach once reactively"
01467 print "cc to close compliantly"
01468 print "c to close, o to open"
01469 print "u to go up, d to go down, r to go right, l to go left"
01470 print "a to go away from the robot, t to go toward the robot"
01471 print "j to go to a set of nearish-side-grasp angles"
01472 print "reset to reset the fingertip thresholds"
01473 print "f to print the current finger sensor values"
01474 print "s to stop"
01475 c = keypause()
01476
01477 if c == 'top':
01478 print "moving to top approach position"
01479 step_size = .01
01480 max_joint_vel = .05
01481 if mode == 'side' or mode == 'none':
01482 step_size = .03
01483 max_joint_vel = .15
01484 cm.command_interpolated_ik(top_approach_pose, collision_aware = 0, step_size = step_size, max_joint_vel = max_joint_vel)
01485 while not cm.check_joint_trajectory_done() and not rospy.is_shutdown():
01486 time.sleep(.1)
01487 print "done"
01488
01489
01490 (pos, rot) = cm.return_cartesian_pose()
01491 currentgoal = pos+rot
01492 mode = 'top'
01493
01494 elif c == 'side':
01495 print "moving to side approach position"
01496 step_size = .01
01497 max_joint_vel = .05
01498 if mode == 'top' or mode == 'none':
01499 step_size = .03
01500 max_joint_vel = .15
01501 cm.command_interpolated_ik(side_approach_pose, collision_aware = 0, step_size = step_size, max_joint_vel = max_joint_vel)
01502 while not cm.check_joint_trajectory_done():
01503 time.sleep(.1)
01504 print "done"
01505
01506
01507 (pos, rot) = cm.return_cartesian_pose()
01508 currentgoal = pos+rot
01509 mode = 'side'
01510
01511 elif c == 'j':
01512 print "moving to near-side-grasp angles"
01513 cm.command_joint(sideangles)
01514
01515
01516
01517
01518 elif c == 'rg' or c == 'ra':
01519 print "performing reactive grasp relative to current pose"
01520 current_pose = rg.cm.get_current_wrist_pose_stamped()
01521 if mode == 'top':
01522 new_grasp_pose = rg.return_rel_pose(top_approach_vect, 'base_link')
01523 grasp_num_tries = 2
01524 forward_step = .02
01525 elif mode == 'side':
01526 new_grasp_pose = rg.return_rel_pose(side_approach_vect, 'base_link')
01527 grasp_num_tries = 2
01528 forward_step = .06
01529 else:
01530 print "go to either top or side approach first"
01531 continue
01532
01533
01534 current_angles = cm.get_current_arm_angles()
01535 (trajectory, error_codes) = cm.ik_utilities.check_cartesian_path(current_pose, new_grasp_pose, current_angles, collision_aware = 0)
01536
01537
01538 if any(error_codes):
01539 rospy.loginfo("can't execute an interpolated IK trajectory to the grasp pose, using Cartesian controllers instead")
01540 if c == 'ra':
01541
01542
01543 approach_dir = rg.compute_approach_dir(current_pose, new_grasp_pose)
01544
01545 rg.reactive_approach(approach_dir, new_grasp_pose)
01546 else:
01547 result = rg.reactive_grasp(current_pose, new_grasp_pose, forward_step = forward_step, grasp_num_tries = grasp_num_tries)
01548
01549
01550 else:
01551 if c == 'ra':
01552
01553 approach_dir = rg.compute_approach_dir(current_pose, new_grasp_pose)
01554 print "approach_dir:", approach_dir
01555
01556 rg.reactive_approach(approach_dir, new_grasp_pose, trajectory)
01557 else:
01558 result = rg.reactive_grasp(current_pose, new_grasp_pose, trajectory, forward_step = forward_step, grasp_num_tries = grasp_num_tries)
01559
01560
01561 (pos, rot) = cm.return_cartesian_pose()
01562 currentgoal = pos+rot
01563
01564
01565 if c == 'rg' and result == 0:
01566 currentgoal[2] += .1
01567 rg.move_cartesian_step(currentgoal, blocking = 1)
01568
01569 elif c == 'cc':
01570 print "compliantly closing gripper"
01571 rg.compliant_close()
01572
01573
01574 (pos, rot) = cm.return_cartesian_pose()
01575 currentgoal = pos+rot
01576
01577 elif c == 'c':
01578 print "closing gripper"
01579 rg.close_hard()
01580
01581 elif c == 'o':
01582 print "opening gripper"
01583 rg.open_and_reset_fingertips(reset = 1)
01584
01585 elif c == 'u':
01586 print "going up"
01587 currentgoal[2] += .1
01588 rg.move_cartesian_step(currentgoal, blocking = 1)
01589 elif c == 'us':
01590 print "going up a small amount"
01591 currentgoal[2] += .02
01592 rg.move_cartesian_step(currentgoal, blocking = 1)
01593 elif c == 'd':
01594 print "going down"
01595 currentgoal[2] -= .05
01596 rg.move_cartesian_step(currentgoal, blocking = 1)
01597 elif c == 'ds':
01598 print "going down a small amount"
01599 currentgoal[2] -= .02
01600 rg.move_cartesian_step(currentgoal, blocking = 1)
01601 elif c == 'r':
01602 print "moving right"
01603 currentgoal[1] -= small_step
01604 rg.move_cartesian_step(currentgoal, blocking = 1)
01605 elif c == 'l':
01606 print "moving left"
01607 currentgoal[1] += small_step
01608 rg.move_cartesian_step(currentgoal, blocking = 1)
01609 elif c == 'a':
01610 print "moving away from robot"
01611 currentgoal[0] += small_step
01612 rg.move_cartesian_step(currentgoal, blocking = 1)
01613 elif c == 't':
01614 print "moving toward the robot"
01615 currentgoal[0] -= small_step
01616 rg.move_cartesian_step(currentgoal, blocking = 1)
01617 elif c == 'reset':
01618 print "resetting the fingertip thresholds"
01619 rg.pressure_listener.set_thresholds()
01620 elif c == 'f':
01621 print "printing the finger sensor values"
01622 rg.pressure_listener.print_sensor_status()
01623 elif c == 's':
01624 print "quitting"
01625 break
01626
01627 print "stopping Cartesian controllers"
01628 cm.stop_controllers(stop_cartesian = 1)
01629