reactive_grasp.py
Go to the documentation of this file.
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 


pr2_gripper_reactive_approach
Author(s): Kaijen Hsiao
autogenerated on Mon Oct 6 2014 12:27:12