reactive_grasp_server.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('sr_move_arm')
00004 import rospy
00005 
00006 import sys
00007 
00008 from object_manipulation_msgs.msg import ReactiveGraspAction, ReactiveGraspResult, \
00009     ManipulationResult, GripperTranslation, ReactiveLiftAction, ReactiveLiftResult, \
00010     ReactivePlaceAction, ReactivePlaceResult, ReactiveGraspFeedback, \
00011     ReactiveLiftFeedback, ReactivePlaceFeedback, ManipulationPhase
00012 import actionlib
00013 from object_manipulator.convert_functions import *
00014 import reactive_grasp
00015 
00016 from actionlib_msgs.msg import GoalStatus
00017 
00018 from std_srvs.srv import Empty, EmptyResponse
00019 
00020 ##preempt exception
00021 class Preempted(Exception): pass
00022 
00023 class ReactiveGrasperWithPreempt(reactive_grasp.ReactiveGrasper):
00024     """
00025     reactive grasp class with overloaded preempt
00026     """
00027     def __init__(self, rg_as, lift_as, approach_as, place_as, which_arm='r', slip_detection=False ):
00028         reactive_grasp.ReactiveGrasper.__init__(self, which_arm, slip_detection)
00029 
00030         #add the action servers so we can check for preempts and send feedback
00031         self._rg_as = rg_as
00032         self._lift_as = lift_as
00033         self._approach_as = approach_as
00034         self._place_as = place_as
00035 
00036         self.rg_state = "off"
00037 
00038         self.pause_for_recording = 0
00039 
00040     ##overloaded to add action preempts
00041     def check_preempt(self):
00042 
00043         if self.rg_state == "rg" and self._rg_as.is_preempt_requested() or \
00044                 self.rg_state == "lift" and self._lift_as.is_preempt_requested() or \
00045                 self.rg_state == "approach" and self._approach_as.is_preempt_requested() or \
00046                 self.rg_state == "place" and self._place_as.is_preempt_requested():
00047 
00048             rospy.loginfo("preempt seen, state=%s"%self.rg_state)
00049 
00050             raise Preempted
00051 
00052     ##overloaded to add feedback broadcasts
00053     def broadcast_phase(self, phase, send_feedback = 1):
00054         self._phase_pub.publish(phase)
00055 
00056         if self.rg_state == "rg":
00057             feedback = ReactiveGraspFeedback()
00058             feedback.manipulation_phase.phase = phase
00059             self._rg_as.publish_feedback(feedback)
00060         elif self.rg_state == "lift":
00061             feedback = ReactiveLiftFeedback()
00062             feedback.manipulation_phase.phase = phase
00063             self._lift_as.publish_feedback(feedback)
00064         elif self.rg_state == "place":
00065             feedback = ReactivePlaceFeedback()
00066             feedback.manipulation_phase.phase = phase
00067             self._place_as.publish_feedback(feedback)
00068 
00069         #if recording in the grasp playpen, pause at MOVING_TO_GRASP or PLACING
00070         #for the recorder to finish broadcasting
00071         if self.pause_for_recording and (phase == ManipulationPhase.MOVING_TO_GRASP \
00072                 or phase == ManipulationPhase.PLACING):
00073             time.sleep(3)
00074 
00075 class ReactiveGraspActionServer(object):
00076     _grasp_result = ReactiveGraspResult()
00077     _lift_result = ReactiveLiftResult()
00078     _place_result = ReactivePlaceResult()
00079 
00080     def __init__(self, which_arm):
00081         """
00082         Class for the reactive grasp actions and services.
00083 
00084         @which_arm is 'r' or 'l'
00085         """
00086         self.which_arm = which_arm
00087 
00088         self._node_name = which_arm+'_reactive_grasp'
00089         if which_arm == 'r':
00090             self._action_name = 'reactive_grasp/right'
00091             self._lift_action_name = 'reactive_lift/right'
00092             self._approach_action_name = 'reactive_approach/right'
00093             self._place_action_name = 'reactive_place/right'
00094         else:
00095             self._action_name = 'reactive_grasp/left'
00096             self._lift_action_name = 'reactive_lift/left'
00097             self._approach_action_name = 'reactive_approach/left'
00098             self._place_action_name = 'reactive_place/left'
00099 
00100         #params for reactive approach/grasp
00101         self.side_step = rospy.get_param("~side_step", .015)
00102         self.back_step = rospy.get_param("~back_step", .03)
00103         self.forward_step = rospy.get_param("~forward_step", 0)  #select based on top/side
00104         self.approach_num_tries = rospy.get_param("~approach_num_tries", 5)
00105         self.goal_pos_thres = rospy.get_param("~goal_pos_thres", .01)
00106 
00107         #params for reactive grasp/grasp adjustment
00108         self.grasp_adjust_x_step = rospy.get_param("~grasp_adjust_x_step", .02)
00109         self.grasp_adjust_z_step = rospy.get_param("~grasp_adjust_z_step", .015)
00110         self.grasp_adjust_num_tries = rospy.get_param("~grasp_adjust_num_tries", 3)
00111 
00112         #params for reactive grasp
00113         self.grasp_num_tries = rospy.get_param("~grasp_num_tries", 2)
00114         self.close_force = rospy.get_param("~close_force", 100)
00115         self.use_slip_detection = rospy.get_param("~use_slip_detection", 0)
00116         rospy.loginfo("reactive_grasp_server: using_slip_detection:"+str(self.use_slip_detection))
00117 
00118         #params for reactive place
00119         self.place_overshoot = rospy.get_param("~place_overshoot", .01)
00120         rospy.loginfo("place_overshoot: %f"%self.place_overshoot)
00121 
00122         #start action servers for reactive grasp, lift, and approach
00123         self._as = actionlib.SimpleActionServer(self._action_name, ReactiveGraspAction, \
00124                                                     execute_cb=self.reactive_grasp_cb)
00125         self._lift_as = actionlib.SimpleActionServer(self._lift_action_name, ReactiveLiftAction, \
00126                                                          execute_cb=self.reactive_lift_cb)
00127         self._approach_as = actionlib.SimpleActionServer(self._approach_action_name, ReactiveGraspAction, \
00128                                                              execute_cb = self.reactive_approach_cb)
00129         self._place_as = actionlib.SimpleActionServer(self._place_action_name, ReactivePlaceAction, \
00130                                                           execute_cb = self.reactive_place_cb)
00131 
00132         #advertise services for compliant grasp and grasp adjustment
00133         rospy.Service(self._node_name+'/compliant_close', \
00134                           Empty, self.compliant_close_callback)
00135         rospy.Service(self._node_name+'/grasp_adjustment', \
00136                           Empty, self.grasp_adjustment_callback)
00137 
00138         self.rg = ReactiveGrasperWithPreempt(self._as, self._lift_as, self._approach_as, self._place_as, which_arm, self.use_slip_detection )
00139         rospy.loginfo("Successfully launched reactive grasper")
00140 
00141 
00142     def reactive_grasp_cb(self, goal):
00143         """
00144         do a reactive grasp using the fingertip sensors
00145         (backs up and moves to the side if a fingertip contacts on the way to the grasp,
00146         closes compliantly, tries approach and grasp again if gripper opening is not within bounds)
00147         """
00148 
00149         rospy.loginfo("got reactive grasp request")
00150         try:
00151             self.rg_state = "rg"
00152 
00153             (trajectory, object_name, table_name, forward_step, pregrasp_posture, grasp_posture) = self.init_reactive_grasp(goal)
00154 
00155             #perform the reactive grasp
00156             #self.cm.switch_to_cartesian_mode()
00157             result = 0
00158             result = self.rg.reactive_grasp(None, goal.final_grasp_pose, trajectory, pregrasp_posture, grasp_posture,
00159                                             self.side_step, self.back_step,
00160                                             self.approach_num_tries, self.goal_pos_thres,
00161                                             self.grasp_num_tries, forward_step,
00162                                             object_name, table_name,
00163                                             self.grasp_adjust_x_step, self.grasp_adjust_z_step, self.grasp_adjust_num_tries)
00164             self.rg.check_preempt()
00165 
00166             #rospy.loginfo("swicthing back to joint controllers")
00167             #self.cm.switch_to_joint_mode()
00168             self.rg_state = "off"
00169 
00170             if result == 0:
00171                 self.rg.broadcast_phase(ManipulationPhase.SUCCEEDED, send_feedback = 0)
00172                 self._grasp_result.manipulation_result.value = ManipulationResult.SUCCESS
00173                 self._as.set_succeeded(self._grasp_result)
00174             else:
00175                 self.rg.broadcast_phase(ManipulationPhase.FAILED, send_feedback = 0)
00176                 self._grasp_result.manipulation_result.value = ManipulationResult.FAILED
00177                 self._as.set_aborted(self._grasp_result)
00178 
00179         except Preempted:
00180             rospy.loginfo("reactive grasp preempted, returning")
00181             self.rg.broadcast_phase(ManipulationPhase.ABORDOED, send_feedback = 0)
00182             self._grasp_result.manipulation_result.value = ManipulationResult.FAILED
00183             self._as.set_preempted(self._grasp_result)
00184 
00185         except reactive_grasp.Aborted:
00186             rospy.loginfo("reactive grasp saw a serious error!  Aborting")
00187             self.rg.broadcast_phase(ManipulationPhase.ABORTED, send_feedback = 0)
00188             self._grasp_result.manipulation_result.value = ManipulationResult.ERROR
00189             self._as.set_aborted(self._grasp_result)
00190 
00191     def init_reactive_grasp(self, goal):
00192         """
00193         pull relevant parts out of the goal for both reactive grasp and approach
00194         """
00195         #pull trajectory angles out of JointTrajectory
00196         if len(goal.trajectory.points) > 0:
00197             trajectory = goal.trajectory
00198         else:
00199             trajectory = None
00200 
00201         object_name = "points"
00202         table_name = goal.collision_support_surface_name
00203 
00204         #if forward_step is 0, adjust the forward_step depending on whether the grasp is a top or side grasp
00205         #if the palm's x-axis's z-component is high enough, it's a top grasp
00206         if self.forward_step == 0:
00207             #transform pose to base_link
00208             forward_step = .03
00209         else:
00210             forward_step = self.forward_step
00211 
00212         return (trajectory, object_name, table_name, forward_step, goal.pre_grasp_posture, goal.grasp_posture)
00213 
00214 
00215 
00216     ##do a reactive approach using the fingertip sensors
00217     #(backs up and moves to the side if a fingertip contacts on the way to the grasp)
00218     def reactive_approach_cb(self, goal):
00219 
00220         rospy.loginfo("got reactive approach request")
00221         try:
00222             self.rg_state = "approach"
00223 
00224             rospy.logingo("Starting the approach")
00225             self.rg.reactive_approach(goal)
00226 
00227             rospy.loginfo("Approach finished")
00228 
00229             self.rg_state = "off"
00230             #self.rg.check_preempt()
00231 
00232             if result == 0:
00233                 rospy.loginfo("Approach succeeded")
00234                 self.rg.broadcast_phase(ManipulationPhase.SUCCEEDED, send_feedback = 0)
00235                 self._grasp_result.manipulation_result.value = ManipulationResult.SUCCESS
00236                 self._approach_as.set_succeeded(self._grasp_result)
00237             else:
00238                 rospy.logwarn("Approach failed")
00239                 self.rg.broadcast_phase(ManipulationPhase.FAILED, send_feedback = 0)
00240                 self._grasp_result.manipulation_result.value = ManipulationResult.FAILED
00241                 self._approach_as.set_aborted(self._grasp_result)
00242 
00243         except Preempted:
00244             rospy.loginfo("reactive grasp preempted, returning")
00245             self.rg.broadcast_phase(ManipulationPhase.ABORTED, send_feedback = 0)
00246             self._grasp_result.manipulation_result.value = ManipulationResult.FAILED
00247             self._approach_as.set_preempted(self._grasp_result)
00248 
00249         except reactive_grasp.Aborted:
00250             rospy.loginfo("reactive grasp saw a serious error!  Aborting")
00251             self.rg.broadcast_phase(ManipulationPhase.ABORTED, send_feedback = 0)
00252             self._grasp_result.manipulation_result.value = ManipulationResult.ERROR
00253             self._approach_as.set_aborted(self._grasp_result)
00254 
00255     ##do a reactive lift using the fingertip sensors (uses slip-servoing to exert just enough force to grasp and lift)
00256     def reactive_lift_cb(self, goal):
00257 
00258         try:
00259             rospy.loginfo("got reactive lift request")
00260 
00261             success = self.rg.reactive_lift(goal)
00262 
00263             self.rg_state = "off"
00264 
00265             if success == 0:
00266                 rospy.loginfo("Reactive lift request ended: SUCCESS" )
00267 
00268                 self.rg.broadcast_phase(ManipulationPhase.SUCCEEDED, send_feedback = 0)
00269                 self._lift_result.manipulation_result.value = ManipulationResult.SUCCESS
00270                 self._lift_as.set_succeeded(self._lift_result)
00271             else:
00272                 rospy.logwarn("Reactive lift request ended: FAILED" )
00273                 self.rg.broadcast_phase(ManipulationPhase.FAILED, send_feedback = 0)
00274                 self._lift_result.manipulation_result.value = ManipulationResult.FAILED
00275                 self._lift_as.set_aborted(self._lift_result)
00276 
00277         except Preempted:
00278             rospy.loginfo("reactive lift preempted, returning")
00279             self.rg.broadcast_phase(ManipulationPhase.ABORTED, send_feedback = 0)
00280             self._lift_result.manipulation_result.value = ManipulationResult.FAILED
00281             self._lift_as.set_preempted(self._lift_result)
00282 
00283         except reactive_grasp.Aborted:
00284             rospy.loginfo("reactive lift saw a serious error!  Aborting")
00285             self.rg.broadcast_phase(ManipulationPhase.ABORTED, send_feedback = 0)
00286             self._lift_result.manipulation_result.value = ManipulationResult.ERROR
00287             self._lift_as.set_aborted(self._lift_result)
00288 
00289 
00290     ##do a reactive place using the fingertip sensors and accelerometer
00291     #(uses slip controller to stop and open when the object hits the table)
00292     #if slip controller is not running, just goes past the goal with the Cartesian controller
00293     def reactive_place_cb(self, goal):
00294 
00295         try:
00296             rospy.loginfo("got reactive place request")
00297 
00298             success = self.rg.reactive_place(goal)
00299 
00300             #place and open when the object hits the table
00301             result = self.rg_state = "place"
00302 
00303             self._place_result.manipulation_result.value = ManipulationResult.SUCCESS
00304             if result == 0:
00305                 self.rg.broadcast_phase(ManipulationPhase.SUCCEEDED, send_feedback = 0)
00306             self._place_as.set_succeeded(self._place_result)
00307 
00308         except Preempted:
00309             rospy.loginfo("reactive place preempted, returning")
00310             self.rg.broadcast_phase(ManipulationPhase.ABORTED, send_feedback = 0)
00311             self._place_result.manipulation_result.value = ManipulationResult.FAILED
00312             self._place_as.set_preempted(self._place_result)
00313 
00314         except reactive_grasp.Aborted:
00315             rospy.loginfo("reactive place saw a serious error!  Aborting")
00316             self.rg.broadcast_phase(ManipulationPhase.ABORTED, send_feedback = 0)
00317             self._place_result.manipulation_result.value = ManipulationResult.ERROR
00318             self._place_as.set_aborted(self._place_result)
00319 
00320     ##do a compliant close using the fingertip sensors (stops at the first detected
00321     #contact and moves the arm so that the contact stays in place)
00322     def compliant_close_callback(self, req):
00323         rospy.loginfo("executing compliant grasp")
00324 
00325         #self.rg.compliant_close()
00326 
00327         return EmptyResponse()
00328 
00329 
00330     ##do a grasp adjustment using the fingertip sensors (tries to move the hand to
00331     #obtain non-tip and non-edge contacts)
00332     def grasp_adjustment_callback(self, req):
00333         rospy.loginfo("executing grasp adjustment--switching to Cartesian controllers")
00334 
00335         rospy.logerr("NOT IMPLEMENTED YET")
00336 
00337         return EmptyResponse()
00338 
00339 
00340 if __name__ == "__main__":
00341 
00342     if len(sys.argv) < 2 or sys.argv[1] != 'r' and sys.argv[1] != 'l':
00343         rospy.logerr("usage: reactive_grasp_server.py which_arm (which_arm is r or l)")
00344         sys.exit(1)
00345 
00346     which_arm = sys.argv[1]
00347 
00348     node_name = which_arm+'_reactive_grasp'
00349     rospy.init_node(node_name, anonymous=True)
00350 
00351     reactive_grasp_services = ReactiveGraspActionServer(which_arm)
00352     rospy.loginfo("Reactive grasp action ready")
00353 
00354     rospy.spin()


sr_move_arm
Author(s): Ugo Cupcic
autogenerated on Fri Jan 3 2014 12:07:38