$search
00001 #!/usr/bin/env python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2009, Willow Garage, Inc. 00005 # All rights reserved. 00006 # 00007 # Redistribution and use in source and binary forms, with or without 00008 # modification, are permitted provided that the following conditions 00009 # are met: 00010 # 00011 # * Redistributions of source code must retain the above copyright 00012 # notice, this list of conditions and the following disclaimer. 00013 # * Redistributions in binary form must reproduce the above 00014 # copyright notice, this list of conditions and the following 00015 # disclaimer in the documentation and/or other materials provided 00016 # with the distribution. 00017 # * Neither the name of the Willow Garage nor the names of its 00018 # contributors may be used to endorse or promote products derived 00019 # from this software without specific prior written permission. 00020 # 00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 # POSSIBILITY OF SUCH DAMAGE. 00033 # 00034 # author: Kaijen Hsiao 00035 00036 ## @package reactive_grasp 00037 #Reactive grasping based on fingertip readings 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 ##pause for input 00060 def keypause(): 00061 print "press enter to continue" 00062 input = raw_input() 00063 return input 00064 00065 ##abort exception 00066 class Aborted(Exception): pass 00067 00068 ##reactive/guarded movement and grasping 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 #self.palm_listener = hand_sensor_listeners.PalmSensorListener() 00078 print "done" 00079 00080 #are we using slip detection? 00081 self.using_slip_detection = self.cm.using_slip_detection 00082 00083 #force to use when closing 00084 self.close_force = 50 00085 if self.using_slip_detection: 00086 self.close_force = 10 00087 00088 #has the hand been closed hard recently? 00089 self._closed_hard = 0 00090 00091 #collision name of support surface 00092 self._table_name = "table" 00093 00094 #for taking photos of each step 00095 self._photo = 0 00096 00097 #broadcast the current phase of the manipulation 00098 self._phase_pub = rospy.Publisher('/reactive_manipulation_phase', ManipulationPhase) 00099 00100 #dictionary for ManipulationPhase 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 #dictionary of return values for reactive approach 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 #dictionary of return values for reactive_grasp 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 ##broadcast the current manipulation phase (of type ManipulationPhase) 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 ##check for a preempt (overload for your application) 00123 def check_preempt(self): 00124 pass 00125 00126 00127 ##throw an abort exception 00128 def throw_exception(self): 00129 raise Aborted 00130 00131 00132 ##check the state of find_gripper_contact and try to collect the first positive thresholded fingertip readings 00133 #only looks for front contacts 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 #action finished 00144 if state not in [GoalStatus.ACTIVE, GoalStatus.PENDING]: 00145 00146 #get the end result, if successful 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 #timeout reached 00156 if rospy.get_rostime() - start_time > rospy.Duration(timeout): 00157 rospy.loginfo("timeout reached on find_gripper_contact") 00158 break 00159 00160 #check the current thresholded readings and save the first ones seen for each fingertip 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 #our desired fingertip readings haven't tripped yet; wait a bit longer and see if some readings show up 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 #timeout reached 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 #check the current thresholded readings and save the first ones seen for each fingertip 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 #if there's still a discrepancy in contacts, figure out which front contact is closest 00197 #to being triggered and set that to 1 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 ##close compliantly (controller manager should be in Cartesian mode) 00212 #(if one finger touches first, counter-move the arm to keep that finger in-place while closing) 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 #use_slip_controller_to_close = 0 00221 00222 step_time = .05 00223 left_touching = right_touching = 0 00224 00225 #ask the gripper to close until first contact 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 #close slowly until first front contact (or until the gripper gets to 1 cm open) 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 #if we saw both contacts at once, skip this part 00249 if not (left_touching and right_touching): 00250 00251 #if one contact was seen, change the Cartesian control frame to the contacting finger 00252 reloaded_controllers = 0 00253 if left_touching or right_touching: 00254 00255 if not use_slip_controller_to_close: 00256 #stop the gripper in a hurry 00257 desired_pos += .002 00258 self.cm.command_gripper(desired_pos, 200) 00259 00260 #move the cartesian arm control frame to the appropriate fingertip frame 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 #continue closing the gripper slowly until both sensors contact, or until the gripper's closed 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 #put the cartesian control frame back to the normal gripper frame 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 ##move to a Cartesian pose goal 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 ##check for tip/side/back/palm contact 00330 def check_guarded_move_contacts(self): 00331 00332 left_touching = right_touching = palm_touching = 0 00333 00334 #regions are (tip, plus_z_side, neg_z_side, front, back) 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 ##move the wrist to a desired Cartesian pose while watching the fingertip sensors 00349 #settling_time is how long to wait after the controllers think we're there 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 #send the goal to the Cartesian controllers 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 #watch the fingertip/palm sensors until the controllers are done and then some 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 #stop if you hit a tip, side, back, or palm 00376 (left_touching, right_touching, palm_touching) = self.check_guarded_move_contacts() 00377 00378 #saw a contact, freeze the arm 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 #check if we're actually there 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 #check if we're stuck 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 #check if the controllers think we're done 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 #done settling 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 #timed out 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 #return whether the left and right fingers were touching 00428 return (left_touching, right_touching, palm_touching) 00429 00430 00431 ##move the wrist to a desired Cartesian pose using IK and watching the fingertip sensors 00432 def guarded_move_ik(self, pose_stamped, max_joint_vel = .05): 00433 00434 self.check_preempt() 00435 00436 #send the goal to the interpolated IK controllers (ignore collisions in IK) 00437 success = self.cm.command_interpolated_ik(pose_stamped, collision_aware = 0, step_size = .005, max_joint_vel = max_joint_vel) 00438 00439 #if can't move using IK (out of reach), try to move using the Cartesian controllers instead 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 #watch the fingertip sensors until the controllers are done and then some 00445 while(1): 00446 00447 self.check_preempt() 00448 00449 #stop if you hit a tip, side, or palm 00450 (left_touching, right_touching, palm_touching) = self.check_guarded_move_contacts() 00451 00452 #saw a contact, freeze the arm 00453 if left_touching or right_touching or palm_touching: 00454 self.cm.freeze_arm() 00455 break 00456 00457 #check if the controllers say they're done 00458 if self.cm.check_joint_trajectory_done(): 00459 break 00460 00461 #return whether the left and right fingers were touching 00462 return (left_touching, right_touching, palm_touching) 00463 00464 00465 #vector norm of a list 00466 def vect_norm(self, vect): 00467 return sum([x**2 for x in vect])**.5 00468 00469 #normalize a vector 00470 def normalize_vect(self, vect): 00471 return list(scipy.array(vect)/self.vect_norm(vect)) 00472 00473 #dot product of two vectors (as lists) 00474 def vect_dot(self, vect1, vect2): 00475 return sum([x*y for (x,y) in zip(vect1, vect2)]) 00476 00477 #projection of vect1 onto vect2 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 #make vect1 orthogonal to vect2 while retaining the magnitude of vect1 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 ##convert a relative vector in frame to a pose in the base_link frame 00495 #if start_pose is not specified, uses current pose of the wrist 00496 #if orthogonal_to_vect and orthogonal_to_vect_frame are specified, first convert vector to be orthogonal to orthogonal_to_vect 00497 def return_rel_pose(self, vector, frame, start_pose = None, orthogonal_to_vect = None, orthogonal_to_vect_frame = 'base_link'): 00498 00499 #if start_pose is not specified, use the current Cartesian pose of the wrist 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 #convert the vector in frame to base_link frame 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 # print "start_trans:", self.cm.pplist(start_trans), "start_rot:", self.cm.pplist(start_rot) 00514 # print "frame:", frame 00515 # print "base_link_vector:", self.cm.pplist(base_link_vector) 00516 00517 #if orthogonal_to_vect and orthogonal_to_vect_frame are specified, make the vector orthogonal to that vector 00518 if orthogonal_to_vect != None: 00519 00520 #first convert it to the base_link frame if it's not already 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 #now project the desired vector onto the orthogonal vector and subtract out that component 00528 base_link_vector = self.make_orthogonal(base_link_vector, ortho_base_link_vector) 00529 00530 #add the desired vector to the position 00531 new_trans = [x+y for (x,y) in zip(start_trans, base_link_vector)] 00532 00533 #create a new poseStamped 00534 pose_stamped = create_pose_stamped(new_trans+start_rot) 00535 00536 # if orthogonal_to_vect != None: 00537 # print "orthogonal_to_vect:", self.cm.pplist(orthogonal_to_vect) 00538 # print "base_link_vector after being made orthogonal:", self.cm.pplist(base_link_vector) 00539 # print "new_trans:", self.cm.pplist(new_trans) 00540 #print "resulting pose_stamped:\n", pose_stamped 00541 00542 return pose_stamped 00543 00544 00545 ##check if we have gotten close enough to our goal along approach_vect (according to goal_pos_thres) 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 #project the current position and the goal position onto the (already-normalized) approach vector 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 #if we overshot due to the controllers, that's fine too 00554 #print "current_proj = %5.3f, goal_proj = %5.3f, posdiff:%5.3f"%(current_proj, goal_proj, posdiff) 00555 #print "current_pos:", self.cm.pplist(current_pos) 00556 00557 if posdiff < goal_pos_thres: 00558 return 1 00559 00560 return 0 00561 00562 00563 ##compute (unit) approach direction in base_link frame from an approach_pose and grasp_pose (returns scipy array) 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 ##compute approach vector in base_link frame from an approach_pose and grasp_pose (returns scipy array) 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 ##are either of the palm sensors touching? (palm sensors removed; stubs left in just in case) 00579 def palm_touching(self): 00580 #palm_state = self.palm_listener.get_state() 00581 #return any(palm_state) 00582 return 0 00583 00584 00585 ##reactive approach (stop if unexpected fingertip contact and go around) 00586 #assumes we are already at the approach pose, going to grasp_pose along the direction approach_dir 00587 #grasp_pose should be in base_link frame 00588 #and have found and checked the joint-angle path 00589 #side_step is how far to move right or left when you hit something 00590 #num_tries is how many bump-and-moves to try before quitting 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 #self.cm.reload_joint_controllers(.2) 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 #if joint_path != None: 00607 #use the joint trajectory action to move through joint_path slowly 00608 #while checking the fingertip/palm sensors 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 #saw a contact, freeze the arm 00613 if left_touching or right_touching or palm_touching: 00614 self.cm.freeze_arm() 00615 break 00616 else: 00617 #use the Cartesian controllers to move through the approach path 00618 #(left_touching, right_touching, palm_touching) = self.guarded_move_cartesian(grasp_pose, 10.0, 5.0) 00619 (left_touching, right_touching, palm_touching) = self.guarded_move_ik(grasp_pose) 00620 00621 #made it all the way through without touching anything, return 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 #wait a bit to settle, then check if we are close enough to the goal 00627 #even though we touched something 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 #saw palm contact! Return 00635 if palm_touching: 00636 rospy.loginfo("saw palm contact") 00637 return self.reactive_approach_result_dict["saw palm contact"] 00638 00639 #saw both contacts at once! Return 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 #colliding with the table! Return 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 #saw only one contact, try to go around 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 #back off a bit along the approach vector 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 #self.move_cartesian_step(pose, blocking = 1) 00665 00666 self.check_preempt() 00667 00668 #reset the fingertip sensors (hopefully we're no longer touching) 00669 self.pressure_listener.set_thresholds(short = 1) 00670 00671 self.check_preempt() 00672 00673 if self._photo: 00674 keypause() 00675 00676 #left finger was touching; go to the left by side_step 00677 if left_touching: 00678 rospy.loginfo("side-stepping to the left") 00679 side_step_vect = [0., side_step, 0.] 00680 00681 #right finger touching; go to the right by side_step 00682 if right_touching: 00683 rospy.loginfo("side-stepping to the right") 00684 side_step_vect = [0., -side_step, 0.] 00685 00686 #execute the side step in guarded fashion 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 #(left_touching, right_touching, palm_touching) = self.guarded_move_cartesian(pose, 3.0, 1.0) 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 #if we didn't hit while moving sideways, 00698 #move to our new (offset) goal in guarded fashion 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 #(left_touching, right_touching, palm_touching) = self.guarded_move_cartesian(new_goal, 5.0, 3.0) 00704 00705 self.check_preempt() 00706 00707 if self._photo: 00708 keypause() 00709 00710 #made it all the way through without touching anything, return 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 #wait a bit to settle, then check if we are close enough to the goal 00716 #even though we touched something 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 #saw palm contact! Return 00726 if palm_touching: 00727 rospy.loginfo("saw palm contact") 00728 return self.reactive_approach_result_dict["saw palm contact"] 00729 00730 #seeing both contacts now! Return 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 #colliding with the table! Return 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 #update the history 00741 ever_saw_left = ever_saw_left or left_touching 00742 ever_saw_right = ever_saw_right or right_touching 00743 00744 #saw both contacts at some point! Halve the side-step size 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 ##check for marginal grasps 00758 #if we see only the edge sensors on both fingers, move in the z-axis direction 00759 #if we see only the tip sensors, or front sensors not far enough in (and no palm sensors are pressed), move forward 00760 #if we see only one edge on one finger and the opposite edge on the other finger, rotate the hand (disabled) 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 #check if our fingers are touching the table (if so, the grasp is probably fine, just return) 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 #check if we have any contacts on the front of either sensor 00779 if not any(l_rows_touching) and not any(r_rows_touching): 00780 00781 #just return that there aren't any front elements touching 00782 rospy.loginfo("no front elements touching") 00783 return (0, 0, 0, 0) 00784 00785 00786 #check if we need to move along the x-axis (forward) 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 #check if we need to move along the z-axis (sideways) 00806 #center touching on at least one finger 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 #need to move in +z direction 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 #need to move in -z direction 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 #contact on both sides in z-direction 00819 else: 00820 rospy.loginfo("contacts not all on one side, no need to adjust in z-direction") 00821 00822 00823 #check if we need to rotate the hand about the x-axis 00824 #need to rotate about -x 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 #need to rotate about +x 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 ##pretty-print scipy matrix to string 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 ##check the gripper opening to see if it's within our goal gripper opening range 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 ##lift while checking for slip (assumes we already closed around the object) 00862 #gripper_translation is a desired GripperTranslation for the lift 00863 #slip servoing should continue after the lift (for moving the object through free space), until the gripper opens or closes 00864 #returns 1 if the object is still in the hand at the end of the lift, 0 otherwise 00865 def lift_carefully(self, gripper_translation, min_gripper_opening = .0021, max_gripper_opening = .1, hardness_gain = 0.060): 00866 00867 #convert the gripper_translation to a desired PoseStamped for the wrist 00868 base_link_translation = change_vector3_stamped_frame(self.cm.tf_listener, gripper_translation.direction, 'base_link') 00869 00870 #add it to the current pose of the wrist 00871 start_wrist_pose = self.cm.get_current_wrist_pose_stamped() 00872 00873 #if using the slip controller, first open and close to determine how hard to grasp the object, 00874 #and start the slip servo controller going (blocks until open and close is done) 00875 if self.using_slip_detection: 00876 self.cm.start_gripper_grab(hardness_gain) 00877 00878 self.broadcast_phase(ManipulationPhase.LIFTING) 00879 00880 #now move to the final desired position 00881 desired_wrist_pose = copy.deepcopy(start_wrist_pose) 00882 00883 #temporary, while deepcopy of rospy.Time is broken 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 #check to see if the object is still in the hand 00891 success = self.check_gripper_opening(min_gripper_opening, max_gripper_opening) 00892 00893 return success 00894 00895 00896 ##shake the object by rotating the wrist roll and flex joints 00897 #and the arm pan joint at a particular set of arm angles 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 #move to standard arm angles for this arm 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 #allow time for the object to fall out 00915 self.broadcast_phase(ManipulationPhase.HOLDING_OBJECT) 00916 time.sleep(4) 00917 00918 #check if the object is still in the gripper; if not, return failed 00919 success = self.check_gripper_opening(min_gripper_opening, max_gripper_opening) 00920 if not success: 00921 return 0 00922 00923 #rotate wrist roll joint (+,-,+,-) 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 #allow time for the object to fall out 00939 time.sleep(4) 00940 00941 #check if the object is still in the gripper; if not, return failed 00942 success = self.check_gripper_opening(min_gripper_opening, max_gripper_opening) 00943 if not success: 00944 return 0 00945 00946 #rotate wrist flex joint 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 #allow time for the object to fall out 00961 time.sleep(4) 00962 00963 #check if the object is still in the gripper; if not, return failed 00964 success = self.check_gripper_opening(min_gripper_opening, max_gripper_opening) 00965 if not success: 00966 return 0 00967 00968 #rotate arm pan joint 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 #allow time for the object to fall out 00987 time.sleep(4) 00988 00989 #check if the object is still in the gripper; if not, return failed 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 ##place while checking for the gripper to report that the object has hit the table 00998 #final_place_pose is a PoseStamped for the gripper in its desired place pose 00999 #place_overshoot is the distance in m to go past the final_place_pose 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 #check if there's still something in the gripper to place 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 #get the current pose of the wrist 01012 (start_wrist_pos, start_wrist_rot) = self.cm.return_cartesian_pose() 01013 01014 #set the actual goal to just past the final place goal by place_overshoot 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 #change the Cartesian control parameters to be more gentle 01023 self.cm.cart_params.set_params_to_gentle(set_tip_frame = 0, set_params_on_server = 1) 01024 01025 #tell the gripper controller to go into place mode 01026 self.check_preempt() 01027 if self.using_slip_detection: 01028 self.cm.start_gripper_event_detector(blocking = 0) 01029 01030 #move to the actual goal using the Cartesian controllers 01031 self.move_cartesian_step(overshoot_pose, blocking = 0) 01032 01033 #look for the trigger saying that the object hit the table 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 #check the state of the gripper place action 01041 if self.using_slip_detection: 01042 state = self.cm.get_gripper_event_detector_state() 01043 01044 #action finished (trigger seen) 01045 if state not in [GoalStatus.ACTIVE, GoalStatus.PENDING]: 01046 rospy.loginfo("place carefully saw the trigger, stopping the arm") 01047 break 01048 01049 #check if the arm angles have changed too much 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 #timed out 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 #freeze the arm as soon as the trigger happens, or after the timeout 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 #open the gripper 01070 self.broadcast_phase(ManipulationPhase.OPENING) 01071 self.cm.command_gripper(.1, -1, 1) 01072 01073 #change the controller params back 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 ##close with a force of self.close_force, and record that the fingertips need a long-reset 01080 #unless we're using the slip controller, in which case open and close to estimate stiffness 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 ##open the gripper, wait for the fingertips to release, and reset the thresholds if we're not touching anything 01093 def open_and_reset_fingertips(self, reset = 0): 01094 01095 #open the gripper 01096 rospy.loginfo("opening gripper") 01097 self.cm.command_gripper(.1, -1, 1) 01098 01099 self.check_preempt() 01100 01101 #if we pressed hard when closing, have to wait awhile for the fingertip sensors to release 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 #assume the fingertips are not touching anything and reset the thresholds 01113 self.pressure_listener.set_thresholds(short = 1) 01114 01115 self.check_preempt() 01116 01117 01118 ##check to see if the fingers are colliding with the table in gripper_pose according to the collision model 01119 #defaults to the current gripper pose 01120 def check_fingers_in_table(self, gripper_pose = None): 01121 01122 #if we don't have tip contact, we're probably not touching the table 01123 (l_regions_touching, r_regions_touching) = self.pressure_listener.regions_touching() 01124 #print "l_regions_touching:", l_regions_touching 01125 #print "r_regions_touching:", r_regions_touching 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 # ordered_collision_operations = OrderedCollisionOperations([collision_oper1, collision_oper2, collision_oper3]) 01143 # (pos, error_code) = self.cm.ik_utilities.run_ik(gripper_pose, current_angles, self.cm.ik_utilities.link_name, \ 01144 # collision_aware = 1, \ 01145 # ordered_collision_operations = ordered_collision_operations) 01146 # if error_code == "IK_LINK_IN_COLLISION": 01147 # rospy.loginfo("fingers are near the table") 01148 # return 1 01149 # rospy.loginfo("fingers not near the table") 01150 return 0 01151 01152 ##adjust the grasp in an attempt to center the object according to the contacts, if we appear to be in a marginal grasp 01153 #assumes the fingers start closed around the object 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 #check if the gripper opening is acceptable; if not, give up 01159 if not self.check_gripper_opening(min_gripper_opening, max_gripper_opening): 01160 return ("gripper opening unacceptable", grasp_pose) 01161 01162 #check if the contacts need adjusting, and if so, in which directions 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 #try to adjust the grasp a maximum of num_tries times 01166 tries = 0 01167 current_grasp_pose = grasp_pose 01168 self._closed_hard = 0 01169 while(1): 01170 01171 self.check_preempt() 01172 01173 #if you're out of adjust tries, but there are front contacts, just close hard and see if the grasp is okay anyway 01174 if tries >= num_tries: 01175 01176 #if front_touching: 01177 rospy.loginfo("ran out of tries while adjusting the grasp, trying to close hard anyway") 01178 self.close_hard() 01179 01180 #check if the gripper opening is acceptable and return 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 #else: 01185 # return ("ran out of tries", current_grasp_pose) 01186 01187 rospy.loginfo("adjust_grasp tries: "+str(tries)) 01188 01189 #adjust the hand position in the appropriate directions (don't bother adjusting just rotation) 01190 if adjust_x or adjust_z: 01191 01192 self.broadcast_phase(ManipulationPhase.ADJUSTING_GRASP) 01193 01194 #find the new, adjusted grasp pose 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 #open the gripper to adjust the grasp 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 #update the current grasp pose 01215 current_grasp_pose = new_grasp_pose 01216 01217 #move the hand to the new grasp pose 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 #close the gripper compliantly 01225 (l_readings, r_readings) = self.compliant_close() 01226 01227 self.check_preempt() 01228 if self._photo: 01229 keypause() 01230 01231 #update the new grasp pose to the current position 01232 current_grasp_pose = self.cm.get_current_wrist_pose_stamped() 01233 01234 #check if the gripper opening is acceptable; if not, give up 01235 if not self.check_gripper_opening(min_gripper_opening, max_gripper_opening): 01236 return ("gripper opening unacceptable", current_grasp_pose) 01237 01238 #get the new fingertip sensor readings and check if the new contacts are okay 01239 (front_touching, adjust_x, adjust_z, adjust_rot) = self.check_if_contacts_need_adjusting(min_contact_row, l_readings, r_readings) 01240 01241 #is the grasp okay now? (ignore a possible rotation adjustment) 01242 grasp_okay = 1 01243 if not front_touching or adjust_x or adjust_z: 01244 grasp_okay = 0 01245 01246 #grasp is okay, close hard 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 #check if the gripper opening is acceptable; if not, give up 01257 if not self.check_gripper_opening(min_gripper_opening, max_gripper_opening): 01258 return ("gripper opening unacceptable", current_grasp_pose) 01259 01260 #get the new fingertip sensor readings and check if the new contacts are okay 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 #if the grasp is still fine, stop 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 ##do a reactive approach followed by a compliant close if the approach is successful 01278 #then move forward more and try again along the approach if the gripper opening is not as expected 01279 #succeeds if the gripper is closed with an opening between min_gripper_opening and max_gripper_opening 01280 #tries the approach-and-grasp grasp_num_tries times (including the first approach) 01281 #forward step is how far to move along the approach direction (past the goal) for the next approach-and-grasp try 01282 #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) 01283 #possible return values: 0=success, 1=ran out of grasp tries, 2=ran out of approach tries, 3=aborted 01284 #approach_pose, grasp_pose, joint_path, side_step, back_step, approach_num_tries, goal_pos_thres are inputs to the approach 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 #if approach_pose is not specified, use the current pose of the hand 01294 if approach_pose == None: 01295 approach_pose = self.cm.get_current_wrist_pose_stamped() 01296 01297 #reset the fingertip thresholds and palm sensor state (assumes we aren't touching anything right now) 01298 self.pressure_listener.set_thresholds() 01299 #self.palm_listener.reset_state() 01300 01301 self.check_preempt() 01302 01303 #if approach and/or grasp are not in base_link frame, convert them to base_link frame 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 #compute (unit) approach direction in base_link frame 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 #try the approach, unless the reactive approach is disabled (approach_num_tries is 0) 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 #quit if the approach ran out of tries (couldn't get around the object) (disabled; just close anyway) 01336 #if approach_result == self.reactive_approach_result_dict["ran out of tries"]: 01337 # return self.reactive_grasp_result_dict["ran out of approach tries"] 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 #reactive approach is disabled; just move to the goal 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 #if both fingers were touching, or we got to the grasp goal, or we're touching the table, try to close the fingers 01350 rospy.loginfo("starting compliant_close") 01351 (l_readings, r_readings) = self.compliant_close() 01352 01353 #update the new goal pose to the current position 01354 current_goal_pose = self.cm.get_current_wrist_pose_stamped() 01355 01356 #check if the object is centered/adjust incrementally if necessary 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 #if the grasp looks good, declare success and stop 01363 if adjust_result == "grasp okay": 01364 return self.reactive_grasp_result_dict["success"] 01365 01366 #if the gripper opening is unacceptable, but the palm sensors are touching something, also declare success and stop 01367 if self.palm_touching(): 01368 return self.reactive_grasp_result_dict["success"] 01369 01370 #if the gripper opening is unacceptable or there were no front contacts, and there are no palm contacts, take a major step forward and try approaching again until we run out of tries 01371 num_tries += 1 01372 if num_tries < grasp_num_tries: 01373 01374 #move the goal forward along the approach direction by forward_step 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 #print the new goal 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 #open the gripper back up 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 #sample code for using the above functions directly 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 #top approach/grasp 01421 #table_height = .55 #simulated table 01422 #table_height = .7366 #table in middle of green room 01423 table_height = .7239 #round table 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] #from the top 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 #side approach/grasp 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 #compute approach vector 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 #update currentgoal to the new current position 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 #update currentgoal to the new current position 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 #update currentgoal to the new current position 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 # while not cm.check_joint_trajectory_done(): 01515 # time.sleep(.1) 01516 # print "done" 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 #check that the approach is reachable, find the arm angle trajectory 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 #trajectory bad, use the Cartesian controllers 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 #compute (unit) approach direction in base_link frame 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 #trajectory good, run the reactive approach 01550 else: 01551 if c == 'ra': 01552 #compute (unit) approach direction in base_link frame 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 #update currentgoal to the new current position 01561 (pos, rot) = cm.return_cartesian_pose() 01562 currentgoal = pos+rot 01563 01564 #if reactive grasp was successful, lift 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 #update currentgoal to the new current position 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 #cm.command_gripper(0, 100.0, 1) 01581 elif c == 'o': 01582 print "opening gripper" 01583 rg.open_and_reset_fingertips(reset = 1) 01584 #cm.command_gripper(.1, -1, 1) 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