gripper_stereo_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 gripper_stereo_reactive_grasp
00037 #Reactive grasping based on fingertip readings and gripper stereo
00038 
00039 from __future__ import division
00040 import roslib
00041 roslib.load_manifest('pr2_gripper_stereo')
00042 import time
00043 import rospy
00044 import os
00045 import pr2_gripper_reactive_approach.controller_manager as controller_manager
00046 import sys
00047 from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, Vector3
00048 from object_manipulation_msgs.msg import GripperTranslation, ManipulationPhase
00049 import tf
00050 import scipy
00051 import scipy.linalg
00052 import math
00053 import pdb
00054 import pr2_gripper_reactive_approach.hand_sensor_listeners as hand_sensor_listeners
00055 from object_manipulator.convert_functions import *
00056 import copy
00057 from actionlib_msgs.msg import GoalStatus
00058 from pr2_gripper_reactive_approach.reactive_grasp import ReactiveGrasper, Aborted
00059 from pr2_gripper_stereo.srv import QueryGripperStereo, QueryGripperStereoRequest, GripperStereoGraspAdjust, GripperStereoGraspAdjustRequest, GripperStereoGraspAdjustResponse
00060 import object_manipulator.draw_functions as draw_functions
00061 
00062 
00063 ##reactive/guarded movement and grasping with the gripper stereo integrated into the approach
00064 class GripperStereoReactiveGrasper(ReactiveGrasper):
00065 
00066   def __init__(self, cm):
00067 
00068     ReactiveGrasper.__init__(self, cm)
00069 
00070     #client for gripper stereo
00071     rospy.loginfo("waiting for gripper stereo service")
00072     rospy.wait_for_service("/gripper_stereo_grasp_adjust")
00073     rospy.loginfo("gripper service found")
00074     self._gripper_stereo_grasp_adjust_client = rospy.ServiceProxy("/gripper_stereo_grasp_adjust", GripperStereoGraspAdjust)
00075 
00076     self.stereo_adjusted = 0
00077 
00078     #class for drawing poses
00079     self.draw_functions = draw_functions.DrawFunctions('grasp_markers')
00080 
00081 
00082 #   #query the gripper stereo camera to figure out which direction to move (simple version)
00083 #   def query_gripper_stereo_simple(self, dist_to_obj):
00084 #     req = QueryGripperStereoRequest()
00085 #     req.roi_frame = create_pose_stamped([dist_to_obj+.185-.035, 0, 0, 0, 0, 0, 1], self.whicharm+'_wrist_roll_link')
00086 #     #req.roi_box_dims = Vector3(.1, .4, .4)
00087 #     req.roi_box_dims = Vector3(.08, .15, .1)
00088 
00089 #     try:
00090 #       resp = self._gripper_stereo_simple_client(req)
00091 #     except rospy.ServiceException, e:
00092 #       rospy.logerr("error when querying gripper stereo, %s"%e)
00093 #       return None
00094 #     return resp.object_frame
00095 
00096 
00097   #adjust a grasp using the point cloud from the gripper stereo camera 
00098   #returns adjusted grasp pose, result (0=success, 1=failed to adjust, 2=grasp impossible)
00099   def gripper_stereo_grasp_adjust(self, grasp_pose, do_global_search = 1):
00100 
00101     req = GripperStereoGraspAdjustRequest()
00102     req.grasp_pose = grasp_pose
00103     req.roi_dims = Vector3(.1, .3, .3)
00104     req.do_global_search = do_global_search
00105 
00106     try:
00107       resp = self._gripper_stereo_grasp_adjust_client(req)
00108     except rospy.ServiceException, e:
00109       rospy.logerr("error when asking gripper stereo to adjust grasp, %s"%e)
00110       return ([grasp_pose], 1)
00111     return (resp.adjusted_grasp_pose, resp.result)
00112 
00113 
00114   #simple reactive far-approach using the gripper stereo (move to dist away from grasp_pose)
00115   #stop if the fingertip sensors hit something
00116   def gripper_stereo_reactive_approach(self, approach_dir, grasp_pose, stop_at_dist = 0.05):
00117 
00118     left_touching = right_touching = palm_touching = 0
00119     new_goal = grasp_pose
00120     new_approach_dir = approach_dir.copy()
00121 
00122     if self.stereo_adjusted == 1:
00123       rospy.loginfo("gripper stereo adjustment already done")
00124       return (0, 0, 0, grasp_pose, approach_dir, 1)
00125 
00126     #compute the distance of the gripper's current pose from the new_goal along approach_dir
00127     (proj_vect, orthogonal_vect) = self.projected_vect_to_obj(new_goal, approach_dir)
00128     dist_to_obj = scipy.linalg.norm(proj_vect)
00129 
00130     #keep moving toward the object, adjusting the grasp pose with each 1 cm step inwards
00131     #while not rospy.is_shutdown():
00132     
00133     #adjust the grasp in a direction orthogonal to approach_dir
00134     grasp_feasible = True
00135 #    if use_simple:
00136 #      (left_touching, right_touching, palm_touching, new_goal) = self.adjust_using_gripper_stereo_simple(approach_dir, .10, new_goal, dist_to_obj)
00137 #    else:
00138     for i in range(2):
00139       do_global_search = 1
00140       if i > 0:
00141         print "Pausing 1.0 sec..."
00142         rospy.sleep(1.0)
00143         do_global_search = 0
00144       (left_touching, right_touching, palm_touching, new_goal, new_approach_dir, grasp_feasible) = self.adjust_using_gripper_stereo(new_goal, new_approach_dir, do_global_search)
00145 
00146       #return if we hit something or if the grasp is infeasible
00147       if left_touching or right_touching or palm_touching or not grasp_feasible:
00148         break
00149 
00150 #     #compute the distance of the gripper's current pose from the new_goal along approach_dir
00151 #     (proj_vect, orthogonal_vect) = self.projected_vect_to_obj(new_goal, approach_dir)
00152 #     dist_to_obj = scipy.linalg.norm(proj_vect)
00153 
00154 #     #how far to move towards the grasp
00155 #     dist_to_move = dist_to_obj - stop_at_dist
00156 # #       if dist_to_move > .01:
00157 # #         dist_to_move = .01
00158 # #       elif dist_to_move < .005:
00159 # #         break
00160 
00161 #     #move in another step along approach_dir
00162 #     move_in_vect = new_approach_dir * dist_to_move
00163 #     pose = self.return_rel_pose(move_in_vect, 'base_link')
00164 #     (left_touching, right_touching, palm_touching) = self.guarded_move_ik(pose)
00165 
00166 #     #return if we hit something
00167 #     if left_touching or right_touching or palm_touching:
00168 #       return (left_touching, right_touching, palm_touching, new_goal, grasp_feasible)
00169 
00170     self.stereo_adjusted = 1
00171 
00172     return (left_touching, right_touching, palm_touching, new_goal, new_approach_dir, grasp_feasible)
00173 
00174 
00175   #move to dist_to_obj away along the gripper x-axis from the new grasp goal
00176   def adjust_using_gripper_stereo(self, grasp_goal, approach_dir, do_global_search = 1):
00177 
00178     print "grasp_goal: "+str(grasp_goal)
00179 
00180     #try to adjust the grasp up to 3 times (stop if we found one in reach and tried it)
00181     for tries in range(3):
00182       rospy.loginfo("asking gripper stereo to adjust the grasp, try number %d"%tries)
00183 
00184       #where the gripper stereo thinks we should grasp
00185       (new_goal_list, result) = self.gripper_stereo_grasp_adjust(grasp_goal, do_global_search)
00186 
00187       #grasp is impossible!  
00188       if result == GripperStereoGraspAdjustResponse.IMPOSSIBLE:
00189         rospy.loginfo("gripper stereo adjustment returned grasp impossible!")
00190         continue
00191 
00192       if result == GripperStereoGraspAdjustResponse.FAILURE:
00193         rospy.loginfo("gripper stereo adjustment returned failed!")
00194         continue
00195 
00196       #try to reach each goal in turn
00197       for new_goal in new_goal_list:
00198 
00199         print "new_goal: "+str(new_goal)
00200 
00201         #run interpolated IK to see if new_goal is reachable
00202         current_angles = self.cm.get_current_arm_angles()
00203         current_pose = self.cm.get_current_wrist_pose_stamped()
00204         (trajectory, error_codes) = self.cm.ik_utilities.check_cartesian_path(current_pose, new_goal, current_angles, collision_aware = 0)
00205         print "error_codes:", error_codes
00206         if any(error_codes):
00207           rospy.loginfo("can't execute an interpolated IK trajectory to new_goal, moving onto the next")
00208           continue
00209         else:
00210           rospy.loginfo("found interpolated IK trajectory")
00211 
00212         #convert to base link
00213         new_goal = change_pose_stamped_frame(self.cm.tf_listener, new_goal, 'base_link')
00214 
00215         #separate the vector from our current pose to new_goal into projected and orthogonal components
00216         (proj_vect, orthogonal_vect) = self.projected_vect_to_obj(grasp_goal, approach_dir)
00217 
00218         #current distance to the goal pose along approach_dir
00219         dist_to_obj = scipy.linalg.norm(proj_vect)
00220 
00221         #compute the pose dist_to_obj away from new_goal along the gripper x-axis (turns into base_link)
00222         new_goal_mat = pose_to_mat(new_goal.pose)
00223         dist_to_obj_mat = new_goal_mat.copy()
00224         dist_to_obj_mat[0:3, 3] += new_goal_mat[0:3, 0] * -dist_to_obj
00225         dist_to_obj_pose = stamp_pose(mat_to_pose(dist_to_obj_mat), 'base_link')
00226 
00227         #return the new approach dir in base_link frame
00228         new_approach_dir = scipy.array(new_goal_mat[0:3, 0].T)[0]
00229 
00230     #     print "approach_dir: "+pplist(approach_dir)
00231     #     print "proj_vect: "+pplist(proj_vect)
00232     #     print "orthogonal_vect: "+pplist(orthogonal_vect)
00233         print "dist_to_obj: %0.3f"%dist_to_obj
00234         print "dist_to_obj_pose: "+str(dist_to_obj_pose)
00235 
00236         #draw the desired pose
00237         self.draw_functions.draw_rviz_axes(dist_to_obj_mat, 'base_link', id = 0, duration = 30)
00238 
00239         #use IK to move the gripper to that pose
00240         (left_touching, right_touching, palm_touching) = self.guarded_move_ik(dist_to_obj_pose, max_joint_vel = .15)
00241         break
00242 
00243       #didn't find a reachable grasp pose on this try
00244       else:
00245         continue
00246 
00247       #executed a grasp adjustment, stop
00248       break
00249 
00250     #no tries succeeded in finding a reachable grasp pose
00251     else:
00252         return (0, 0, 0, grasp_goal, approach_dir, False)
00253 
00254     return (left_touching, right_touching, palm_touching, new_goal, approach_dir, True)
00255 
00256 
00257   #vector from our current pose origin to the goal origin, separated into projected/orthogonal vectors to approach_dir
00258   def projected_vect_to_obj(self, goal, approach_dir):
00259 
00260     #convert to base_link
00261     bl_goal_frame = change_pose_stamped_frame(self.cm.tf_listener, goal, 'base_link')
00262 
00263     #vector from our current pose to the goal frame
00264     current_pose = self.cm.get_current_wrist_pose_stamped()
00265     goal_frame_pos = scipy.array(get_xyz(bl_goal_frame.pose.position))
00266     current_frame_pos = scipy.array(get_xyz(current_pose.pose.position))
00267     toward_goal_frame = goal_frame_pos - current_frame_pos
00268 
00269     #projection of toward_goal_frame onto approach_dir
00270     proj = self.vect_proj(toward_goal_frame, approach_dir)
00271 
00272     #subtract out the projection to find the orthogonal component
00273     orthogonal_vect = toward_goal_frame - proj
00274 
00275 #     print "bl_goal_frame: "+str(bl_goal_frame)
00276 #     print "current_pose: "+str(current_pose)
00277 #     print "toward_goal_frame "+pplist(toward_goal_frame)
00278 
00279     return (proj, orthogonal_vect)
00280 
00281 
00282 #   #make a small adjustment in the gripper's position in a direction orthogonal to approach_dir, based on the gripper stereo's object frame
00283 #   def adjust_using_gripper_stereo_simple(self, approach_dir, max_dist, grasp_goal, dist_to_obj):
00284     
00285 #     #where the gripper stereo thinks the object is
00286 #     object_frame = self.query_gripper_stereo_simple(dist_to_obj)
00287 
00288 #     #find the vector from our current pose to the object frame projected onto approach_dir
00289 #     (proj_vect, orthogonal_vect) = self.projected_vect_to_obj(object_frame, approach_dir)
00290 
00291 #     #clip the magnitude at max_dist
00292 #     orthogonal_vect_mag = scipy.linalg.norm(orthogonal_vect)
00293 #     if orthogonal_vect_mag > max_dist:
00294 #       orthogonal_vect = orthogonal_vect / orthogonal_vect_mag * max_dist
00295 
00296 #     #move the gripper
00297 #     pose = self.return_rel_pose(orthogonal_vect, 'base_link')
00298 #     (left_touching, right_touching, palm_touching) = self.guarded_move_ik(pose, max_joint_vel = .15)
00299 
00300 #     #update the grasp_goal
00301 #     new_goal = self.return_rel_pose(orthogonal_vect, 'base_link', grasp_goal)
00302 
00303 # #     print "approach_dir: "+pplist(approach_dir)
00304 # #     print "object_frame_pos: "+pplist(object_frame_pos)
00305 # #     print "current_frame_pos: "+pplist(current_frame_pos)
00306 # #     print "toward_object_frame: "+pplist(toward_object_frame)
00307 # #     print "proj: "+pplist(proj)
00308 # #     print "orthogonal_vect: "+pplist(orthogonal_vect)
00309 
00310 #     return (left_touching, right_touching, palm_touching, new_goal)
00311   
00312 
00313   ##reactive approach (stop if unexpected fingertip contact and go around)
00314   #assumes we are already at the approach pose, going to grasp_pose along the direction approach_dir
00315   #grasp_pose should be in base_link frame
00316   #and have found and checked the joint-angle path
00317   #side_step is how far to move right or left when you hit something
00318   #num_tries is how many bump-and-moves to try before quitting
00319   def reactive_approach(self, approach_dir, grasp_pose, joint_path = None, 
00320                         side_step = .015, back_step = .03, \
00321                           num_tries = 10, goal_pos_thres = 0.01):
00322     
00323     rospy.loginfo("running gripper stereo reactive approach")
00324     (left_touching, right_touching, palm_touching, grasp_pose, approach_dir, grasp_feasible) = self.gripper_stereo_reactive_approach(approach_dir, grasp_pose, stop_at_dist = 0.05)
00325 
00326     if not grasp_feasible:
00327       rospy.loginfo("grasp infeasible!")
00328       return self.reactive_approach_result_dict["grasp infeasible"]      
00329 
00330     rospy.loginfo("running normal approach")
00331     rospy.loginfo("new grasp pose: "+str(grasp_pose))
00332     ReactiveGrasper.reactive_approach(self, approach_dir, grasp_pose, joint_path, side_step,
00333                                       back_step, num_tries, goal_pos_thres)
00334 
00335   #overloaded to only do gripper stereo adjust once
00336   def reactive_grasp(self, approach_pose, grasp_pose, joint_path = None, side_step = .015, back_step = .03, \
00337                        approach_num_tries = 10, goal_pos_thres = 0.01, min_gripper_opening = 0.0021, max_gripper_opening = 0.1, \
00338                        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):
00339     
00340     self.stereo_adjusted = 0
00341 
00342     result = ReactiveGrasper.reactive_grasp(self, approach_pose, grasp_pose, joint_path, side_step, back_step, 
00343                                    approach_num_tries, goal_pos_thres, min_gripper_opening, max_gripper_opening,
00344                                    grasp_num_tries, forward_step, min_contact_row, object_name, table_name,
00345                                    grasp_adjust_x_step, grasp_adjust_z_step, grasp_adjust_num_tries)
00346 
00347     return result


pr2_grasp_adjust
Author(s): Adam Leeper
autogenerated on Fri Jan 3 2014 12:00:29