reactive_grasp_server.py
Go to the documentation of this file.
00001 #!/usr/bin/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_server
00037 #Actions and services for reactive grasp, reactive approach, reactive lift, 
00038 #compliant close, and grasp adjustment.
00039 
00040 from __future__ import division
00041 import roslib; roslib.load_manifest('pr2_gripper_reactive_approach')
00042 import rospy
00043 #import pr2_gripper_reactive_approach.reactive_grasp as reactive_grasp
00044 import gripper_stereo_reactive_grasp as reactive_grasp
00045 import pr2_gripper_reactive_approach.controller_manager as controller_manager
00046 import sys
00047 import math
00048 from object_manipulation_msgs.msg import ReactiveGraspAction, ReactiveGraspResult, \
00049     ManipulationResult, GripperTranslation, ReactiveLiftAction, ReactiveLiftResult, \
00050     ReactivePlaceAction, ReactivePlaceResult
00051 from object_manipulator.convert_functions import *
00052 from std_srvs.srv import Empty, EmptyResponse
00053 import actionlib
00054 import pdb
00055 from object_manipulator.convert_functions import *
00056 
00057 ##preempt exception
00058 class Preempted(Exception): pass
00059 
00060 ##reactive grasp class with overloaded preempt
00061 class ReactiveGrasperWithPreempt(reactive_grasp.GripperStereoReactiveGrasper):
00062     
00063     def __init__(self, cm, rg_as, lift_as, approach_as, place_as):
00064         
00065         reactive_grasp.GripperStereoReactiveGrasper.__init__(self, cm)
00066 
00067         #add the action servers so we can check for preempts
00068         self._rg_as = rg_as
00069         self._lift_as = lift_as
00070         self._approach_as = approach_as
00071         self._place_as = place_as
00072 
00073         self.rg_state = "off"
00074 
00075     ##overloaded to add action preempts
00076     def check_preempt(self):
00077         
00078         if self.rg_state == "rg" and self._rg_as.is_preempt_requested() or \
00079                 self.rg_state == "lift" and self._lift_as.is_preempt_requested() or \
00080                 self.rg_state == "approach" and self._approach_as.is_preempt_requested() or \
00081                 self.rg_state == "place" and self._place_as.is_preempt_requested() or \
00082                 self.rg_state == "center" and self._center_as.is_preempt_requested():
00083             
00084             rospy.loginfo("preempt seen, state=%s"%self.rg_state)
00085 
00086             #switch back to joint controllers so we're not left in a bad state
00087             rospy.loginfo("switching back to joint controllers")
00088             self.cm.switch_to_joint_mode()
00089             
00090             raise Preempted
00091             
00092 
00093 ##Class for the reactive grasp actions and services
00094 class ReactiveGraspActionServer(object):
00095     _result = ReactiveGraspResult()
00096     _lift_result = ReactiveLiftResult()
00097     _place_result = ReactivePlaceResult()
00098 
00099     def __init__(self, which_arm): #which_arm is 'r' or 'l'
00100         self.which_arm = which_arm
00101 
00102         self._node_name = which_arm+'_reactive_grasp'
00103         if which_arm == 'r':
00104             self._action_name = 'reactive_grasp/right'
00105             self._lift_action_name = 'reactive_lift/right'
00106             self._approach_action_name = 'reactive_approach/right'
00107             self._place_action_name = 'reactive_place/right'
00108         else:
00109             self._action_name = 'reactive_grasp/left'
00110             self._lift_action_name = 'reactive_lift/left'
00111             self._approach_action_name = 'reactive_approach/left'
00112             self._place_action_name = 'reactive_place/left'
00113 
00114         #params for reactive approach/grasp
00115         self.side_step = rospy.get_param("~side_step", .015)
00116         self.back_step = rospy.get_param("~back_step", .03)
00117         self.approach_num_tries = rospy.get_param("~approach_num_tries", 5)
00118         self.goal_pos_thres = rospy.get_param("~goal_pos_thres", .01)
00119 
00120         #params for reactive grasp/grasp adjustment
00121         self.min_contact_row = rospy.get_param("~min_contact_row", 1)
00122         self.min_gripper_opening = rospy.get_param("~min_gripper_opening", .0021)
00123         self.max_gripper_opening = rospy.get_param("~max_gripper_opening", .1) 
00124         self.grasp_adjust_x_step = rospy.get_param("~grasp_adjust_x_step", .02)
00125         self.grasp_adjust_z_step = rospy.get_param("~grasp_adjust_z_step", .015)
00126         self.grasp_adjust_num_tries = rospy.get_param("~grasp_adjust_num_tries", 3)
00127 
00128         #params for reactive grasp 
00129         self.grasp_num_tries = rospy.get_param("~grasp_num_tries", 2)
00130         self.forward_step = rospy.get_param("~forward_step", 0)  #select based on top/side
00131         self.close_force = rospy.get_param("~close_force", 100)
00132         self.use_slip_controller = rospy.get_param("~use_slip_controller", 0)
00133 
00134         #params for reactive place
00135         self.place_overshoot = rospy.get_param("~place_overshoot", .01)
00136 
00137         rospy.loginfo("reactive_grasp_server: using_slip_controller:"+str(self.use_slip_controller))
00138 
00139         #start action servers for reactive grasp, lift, and approach
00140         self._as = actionlib.SimpleActionServer(self._action_name, ReactiveGraspAction, \
00141                                                     execute_cb=self.reactive_grasp_cb)
00142         self._lift_as = actionlib.SimpleActionServer(self._lift_action_name, ReactiveLiftAction, \
00143                                                          execute_cb=self.reactive_lift_cb)
00144         self._approach_as = actionlib.SimpleActionServer(self._approach_action_name, ReactiveGraspAction, \
00145                                                              execute_cb = self.reactive_approach_cb)
00146         self._place_as = actionlib.SimpleActionServer(self._place_action_name, ReactivePlaceAction, \
00147                                                           execute_cb = self.reactive_place_cb)
00148 
00149         #advertise services for compliant grasp and grasp adjustment
00150         rospy.Service(self._node_name+'/compliant_close', \
00151                         Empty, self.compliant_close_callback)
00152         rospy.Service(self._node_name+'/grasp_adjustment', \
00153                           Empty, self.grasp_adjustment_callback)
00154 
00155         #initialize controler manager and reactive grasper
00156         self.cm = controller_manager.ControllerManager(which_arm, using_slip_controller = self.use_slip_controller)
00157         self.rg = ReactiveGrasperWithPreempt(self.cm, self._as, self._lift_as, self._approach_as, self._place_as)
00158 
00159 
00160     ##do a reactive grasp using the fingertip sensors
00161     #(backs up and moves to the side if a fingertip contacts on the way to the grasp,
00162     #closes compliantly, tries approach and grasp again if gripper opening is not within bounds)
00163     def reactive_grasp_cb(self, goal):
00164 
00165         rospy.loginfo("got reactive grasp request")
00166         try:
00167             self.rg_state = "rg"
00168             (trajectory, object_name, table_name, forward_step) = self.init_reactive_grasp(goal)
00169 
00170             #perform the reactive grasp
00171             self.cm.switch_to_cartesian_mode()
00172             result = self.rg.reactive_grasp(None, goal.final_grasp_pose, trajectory, \
00173                               self.side_step, self.back_step, \
00174                               self.approach_num_tries, self.goal_pos_thres, \
00175                               self.min_gripper_opening, self.max_gripper_opening, \
00176                               self.grasp_num_tries, forward_step, self.min_contact_row, \
00177                               object_name, table_name, \
00178                               self.grasp_adjust_x_step, self.grasp_adjust_z_step, self.grasp_adjust_num_tries)
00179             self.rg.check_preempt()
00180 
00181             rospy.loginfo("switching back to joint controllers")
00182             self.cm.switch_to_joint_mode()
00183             self.rg_state = "off"
00184 
00185             if result == self.rg.reactive_grasp_result_dict["success"]:
00186                 self._result.manipulation_result.value = ManipulationResult.SUCCESS
00187                 self._as.set_succeeded(self._result)
00188             else:
00189                 self._result.manipulation_result.value = ManipulationResult.FAILED
00190                 self._as.set_aborted(self._result)
00191 
00192         except Preempted:
00193             rospy.loginfo("reactive grasp preempted, returning")
00194             self._result.manipulation_result.value = ManipulationResult.FAILED       
00195             self._as.set_preempted(self._result)
00196 
00197         except reactive_grasp.Aborted:
00198             rospy.loginfo("reactive grasp saw a serious error!  Aborting")
00199             self._result.manipulation_result.value = ManipulationResult.ERROR
00200             self._as.set_aborted(self._result)
00201 
00202 
00203     ##pull relevant parts out of the goal for both reactive grasp and approach
00204     def init_reactive_grasp(self, goal):
00205 
00206         self.rg.check_preempt()
00207 
00208         self.rg.pressure_listener.set_thresholds()
00209 
00210         self.rg.check_preempt()
00211 
00212         #check the param server for a new closing force and pass it on
00213         self.close_force = rospy.get_param("close_force", 100)
00214         self.rg.close_force = self.close_force
00215         rospy.loginfo("using a close_force of %d"%self.close_force)
00216 
00217         #pull trajectory angles out of JointTrajectory
00218         if len(goal.trajectory.points) > 0:
00219             trajectory = [goal.trajectory.points[i].positions for i in range(len(goal.trajectory.points))]
00220             print "trajectory:"
00221             for angles in trajectory:
00222                 print pplist(angles)
00223         else:
00224             trajectory = None
00225 
00226         object_name = "points"
00227         table_name = goal.collision_support_surface_name
00228 
00229         #if forward_step is 0, adjust the forward_step depending on whether the grasp is a top or side grasp
00230         #if the palm's x-axis's z-component is high enough, it's a top grasp
00231         if self.forward_step == 0:
00232             forward_step = .06
00233 
00234             #transform pose to base_link
00235             bl_pose = change_pose_stamped_frame(self.rg.cm.tf_listener, goal.final_grasp_pose, 'base_link')
00236             palm_mat = pose_to_mat(bl_pose.pose)  
00237             if palm_mat[2,0] < -math.cos(math.pi/3.):
00238                 rospy.loginfo("top grasp")
00239 
00240                 #if it's a top grasp, lower the forward_step
00241                 forward_step = .03
00242             else:
00243                 rospy.loginfo("side grasp")
00244         else:
00245             forward_step = self.forward_step
00246 
00247         self.rg.check_preempt()
00248 
00249         return (trajectory, object_name, table_name, forward_step)
00250         
00251     ##do a reactive centering using the gripper stereo cameras
00252     #(approaches until object surface is at finger-tip distance, which is near limit of stereo range)
00253     def reactive_center_cb(self, goal):
00254 
00255         rospy.loginfo("got reactive center request")
00256         try:
00257             self.rg_state = "center"
00258             (trajectory, object_name, table_name, forward_step) = self.init_reactive_grasp(goal)
00259 
00260             #compute the approach dir using the current wrist pose
00261             current_wrist_pose = self.rg.cm.get_current_wrist_pose_stamped()
00262             approach_dir = self.rg.compute_approach_dir(current_wrist_pose, goal.final_grasp_pose)
00263 
00264             #perform the reactive center
00265             self.cm.switch_to_cartesian_mode()
00266             result = self.rg.reactive_approach(approach_dir, goal.final_grasp_pose, trajectory, \
00267                               self.side_step, self.back_step, \
00268                               self.approach_num_tries, self.goal_pos_thres)
00269             self.rg_state = "off"
00270             self.rg.check_preempt()
00271 
00272             rospy.loginfo("switching back to joint controllers")
00273             self.cm.switch_to_joint_mode()
00274 
00275             if result == 0:
00276                 self._result.manipulation_result.value = ManipulationResult.SUCCESS
00277                 self._approach_as.set_succeeded(self._result)
00278             else:
00279                 self._result.manipulation_result.value = ManipulationResult.FAILED
00280                 self._approach_as.set_aborted(self._result)
00281 
00282         except Preempted:
00283             rospy.loginfo("reactive grasp preempted, returning")
00284             self._result.manipulation_result.value = ManipulationResult.FAILED       
00285             self._approach_as.set_preempted(self._result)
00286 
00287         except reactive_grasp.Aborted:
00288             rospy.loginfo("reactive grasp saw a serious error!  Aborting")
00289             self._result.manipulation_result.value = ManipulationResult.ERROR
00290             self._approach_as.set_aborted(self._result)
00291 
00292     ##do a reactive approach using the fingertip sensors
00293     #(backs up and moves to the side if a fingertip contacts on the way to the grasp)
00294     def reactive_approach_cb(self, goal):
00295 
00296         rospy.loginfo("got reactive approach request")
00297         self.rg.stereo_adjusted = 0
00298         try:
00299             self.rg_state = "approach"
00300             (trajectory, object_name, table_name, forward_step) = self.init_reactive_grasp(goal)
00301 
00302             #compute the approach dir using the current wrist pose
00303             current_wrist_pose = self.rg.cm.get_current_wrist_pose_stamped()
00304             approach_dir = self.rg.compute_approach_dir(current_wrist_pose, goal.final_grasp_pose)
00305 
00306             #perform the reactive approach
00307             self.cm.switch_to_cartesian_mode()
00308             result = self.rg.reactive_approach(approach_dir, goal.final_grasp_pose, trajectory, \
00309                               self.side_step, self.back_step, \
00310                               self.approach_num_tries, self.goal_pos_thres)
00311             self.rg_state = "off"
00312             self.rg.check_preempt()
00313 
00314             rospy.loginfo("switching back to joint controllers")
00315             self.cm.switch_to_joint_mode()
00316 
00317             if result == self.rg.reactive_approach_result_dict["success"]:
00318                 self._result.manipulation_result.value = ManipulationResult.SUCCESS
00319                 self._approach_as.set_succeeded(self._result)
00320             else:
00321                 self._result.manipulation_result.value = ManipulationResult.FAILED
00322                 self._approach_as.set_aborted(self._result)
00323 
00324         except Preempted:
00325             rospy.loginfo("reactive grasp preempted, returning")
00326             self._result.manipulation_result.value = ManipulationResult.FAILED       
00327             self._approach_as.set_preempted(self._result)
00328 
00329         except reactive_grasp.Aborted:
00330             rospy.loginfo("reactive grasp saw a serious error!  Aborting")
00331             self._result.manipulation_result.value = ManipulationResult.ERROR
00332             self._approach_as.set_aborted(self._result)
00333 
00334 
00335     ##do a reactive lift using the fingertip sensors (uses slip-servoing to exert just enough force to grasp and lift)
00336     def reactive_lift_cb(self, goal):
00337 
00338         try:
00339             rospy.loginfo("got reactive lift request")
00340             self.cm.switch_to_cartesian_mode()
00341 
00342             #lift and check if the object is still within the hand
00343             self.rg_state = "lift"
00344             success = self.rg.lift_carefully(goal.lift, self.min_gripper_opening, self.max_gripper_opening)
00345             self.rg_state = "off"
00346 
00347             rospy.loginfo("switching back to joint controllers")
00348             self.cm.switch_to_joint_mode()
00349             
00350             if success:
00351                 self._lift_result.manipulation_result.value = ManipulationResult.SUCCESS
00352                 self._lift_as.set_succeeded(self._lift_result)
00353             else:
00354                 self._lift_result.manipulation_result.value = ManipulationResult.FAILED
00355                 self._lift_as.set_aborted(self._lift_result)
00356             
00357         except Preempted:
00358             rospy.loginfo("reactive lift preempted, returning")
00359             self._lift_result.manipulation_result.value = ManipulationResult.FAILED       
00360             self._lift_as.set_preempted(self._lift_result)
00361 
00362         except reactive_grasp.Aborted:
00363             rospy.loginfo("reactive lift saw a serious error!  Aborting")
00364             self._lift_result.manipulation_result.value = ManipulationResult.ERROR
00365             self._lift_as.set_aborted(self._lift_result)
00366 
00367 
00368     ##do a reactive place using the fingertip sensors and accelerometer 
00369     #(uses slip controller to stop and open when the object hits the table)
00370     #if slip controller is not running, just goes past the goal with the Cartesian controller
00371     def reactive_place_cb(self, goal):
00372         
00373         try:
00374             rospy.loginfo("got reactive place request")
00375             self.cm.switch_to_cartesian_mode()
00376 
00377             #place and open when the object hits the table
00378             self.rg_state = "place"
00379             self.rg.place_carefully(goal.final_place_pose, self.place_overshoot)
00380 
00381             self._place_result.manipulation_result.value = ManipulationResult.SUCCESS
00382             self._place_as.set_succeeded(self._place_result)
00383 
00384         except Preempted:
00385             rospy.loginfo("reactive place preempted, returning")
00386             self._place_result.manipulation_result.value = ManipulationResult.FAILED
00387             self._place_as.set_preempted(self._place_result)
00388         
00389         except reactive_grasp.Aborted:
00390             rospy.loginfo("reactive place saw a serious error!  Aborting")
00391             self._place_result.manipulation_result.value = ManipulationResult.ERROR
00392             self._place_as.set_aborted(self._place_result)
00393 
00394 
00395     ##do a compliant close using the fingertip sensors (stops at the first detected 
00396     #contact and moves the arm so that the contact stays in place)
00397     def compliant_close_callback(self, req):
00398         rospy.loginfo("executing compliant grasp--switching to Cartesian controllers")
00399         self.cm.switch_to_cartesian_mode()
00400 
00401         self.rg.compliant_close()
00402 
00403         rospy.loginfo("switching back to joint controllers")
00404         self.cm.switch_to_joint_mode()
00405         
00406         return EmptyResponse()
00407     
00408 
00409     ##do a grasp adjustment using the fingertip sensors (tries to move the hand to
00410     #obtain non-tip and non-edge contacts)
00411     def grasp_adjustment_callback(self, req):
00412         rospy.loginfo("executing grasp adjustment--switching to Cartesian controllers")
00413         self.cm.switch_to_cartesian_mode()
00414 
00415         #start by opening the hand, restting the fingertips, and doing a compliant close 
00416         #to get proper fingertip readings
00417         self.rg.open_and_reset_fingertips()
00418         (initial_l_readings, initial_r_readings) = self.rg.compliant_close()
00419 
00420         #get the current gripper pose as the 'desired grasp pose'
00421         current_wrist_pose = self.cm.get_current_wrist_pose_stamped()
00422 
00423         #run grasp adjustment
00424         (adjust_result, current_goal_pose) = self.rg.adjust_grasp(current_wrist_pose, \
00425                     initial_l_readings, initial_r_readings, \
00426                     z_step = self.grasp_adjust_z_step, x_step = self.grasp_adjust_x_step, \
00427                     min_gripper_opening = self.min_gripper_opening, \
00428                     max_gripper_opening = self.max_gripper_opening, \
00429                     min_contact_row = self.min_contact_row, 
00430                     num_tries = self.grasp_adjust_num_tries)
00431 
00432         rospy.loginfo("switching back to joint controllers")
00433         self.cm.switch_to_joint_mode()
00434 
00435         return EmptyResponse()
00436         
00437 
00438 if __name__ == "__main__":
00439     
00440     if len(sys.argv) < 2 or sys.argv[1] != 'r' and sys.argv[1] != 'l':
00441         rospy.logerr("usage: reactive_grasp_server.py which_arm (which_arm is r or l)")
00442         sys.exit(1)
00443 
00444     which_arm = sys.argv[1]
00445 
00446     node_name = which_arm+'_reactive_grasp'
00447     rospy.init_node(node_name, anonymous=True)
00448 
00449     reactive_grasp_services = ReactiveGraspActionServer(which_arm)
00450     rospy.loginfo("Reactive grasp action ready")
00451 
00452     rospy.spin()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends


pr2_grasp_adjust
Author(s): Adam Leeper
autogenerated on Wed Jan 23 2013 15:27:54