run_experiments.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 """test client for pr2_gripper_reactive_approach_server
00003 requires pr2_gripper_reactive_approach_server.py r (right arm) to be running
00004 """
00005 
00006 from __future__ import division
00007 import roslib
00008 roslib.load_manifest('pr2_gripper_stereo')
00009 import rospy
00010 import math
00011 import scipy
00012 from geometry_msgs.msg import PoseStamped, PointStamped, \
00013     QuaternionStamped, Pose, Point, Quaternion
00014 from object_manipulation_msgs.msg import ReactiveGraspAction, \
00015     ReactiveGraspGoal, ReactiveLiftAction, ReactiveLiftGoal, \
00016     GripperTranslation, ReactivePlaceAction, ReactivePlaceGoal
00017 import actionlib
00018 from trajectory_msgs.msg import JointTrajectory
00019 from pr2_gripper_reactive_approach import controller_manager
00020 from std_srvs.srv import Empty
00021 from object_manipulator.convert_functions import *
00022 import tabletop_collision_map_processing.collision_map_interface as collision_map_interface
00023 from tabletop_object_detector.srv import TabletopDetection, TabletopDetectionRequest
00024 from tabletop_object_detector.msg import TabletopDetectionResult
00025 import object_manipulator.draw_functions
00026 from object_manipulation_msgs.srv import FindClusterBoundingBox, FindClusterBoundingBoxRequest
00027 
00028 #call reactive_grasp to do a reactive grasp using the fingertip sensors
00029 #(backs up and move to the side if the tip contacts on the way to the grasp,
00030 #does a compliant grasp if it gets there, tries the approach and grasp 
00031 #several times if the gripper opening isn't within bounds)
00032 def call_reactive_grasp(ac, grasp_pose, trajectory):
00033 
00034     if trajectory == None:
00035         trajectory = JointTrajectory()
00036 
00037     goal = ReactiveGraspGoal()
00038     goal.final_grasp_pose = grasp_pose
00039     goal.trajectory = trajectory
00040     goal.collision_support_surface_name = "table"
00041 
00042     ac.send_goal(goal)
00043     ac.wait_for_result()
00044     result = ac.get_result()
00045     print "reactive grasp result:", result
00046 
00047     return result.manipulation_result
00048 
00049 
00050 #call reactive_grasp to do a reactive approach using the fingertip sensors
00051 #(backs up and move to the side if the tip contacts on the way to the grasp)
00052 def call_reactive_approach(ac, grasp_pose, trajectory):
00053 
00054     if trajectory == None:
00055         trajectory = JointTrajectory()
00056 
00057     goal = ReactiveGraspGoal()
00058     goal.final_grasp_pose = grasp_pose
00059     goal.trajectory = trajectory
00060     goal.collision_support_surface_name = "table"
00061 
00062     ac.send_goal(goal)
00063     ac.wait_for_result()
00064     result = ac.get_result()
00065     print "reactive approach result:", result
00066 
00067     return result.manipulation_result
00068 
00069 
00070 #call reactive lift (starts up slip servoing if using the slip controllers, 
00071 #otherwise just a Cartesian move)
00072 def call_reactive_lift(ac, lift):
00073 
00074     goal = ReactiveLiftGoal()
00075     goal.lift = lift
00076 
00077     ac.send_goal(goal)
00078     ac.wait_for_result()
00079     result = ac.get_result()
00080     print "reactive lift result:", result
00081 
00082     return result
00083 
00084 
00085 #call reactive place (uses the slip controllers to detect when the object
00086 #hits the table, and opens the gripper)
00087 def call_reactive_place(ac, place_pose):
00088 
00089     goal = ReactivePlaceGoal()
00090     goal.final_place_pose = place_pose
00091 
00092     ac.send_goal(goal)
00093     ac.wait_for_result()
00094     result = ac.get_result()
00095     print "reactive place result:", result
00096 
00097     return result
00098 
00099 
00100 def do_adjust():
00101     
00102       print "adjusting pregrasp position"
00103 
00104       #do a tabletop detection and find the farthest cluster point along farthest_point_axis
00105       if approach_type == 'top':
00106           farthest_point_axis = [0,0,1]
00107       else:
00108           farthest_point_axis = [0,-1,0] 
00109       box_poses, box_dims = call_tabletop_detection(detect_srv, bounding_box_srv)
00110       if box_poses == []:
00111           rospy.logerr("No clusters returned!")
00112           return
00113       farthest_point = find_farthest_box(box_poses, box_dims, farthest_point_axis)
00114       if farthest_point == None:
00115           rospy.logerr("No farthest point returned!")
00116           return
00117 
00118       #draw the farthest_point
00119       draw_sphere_mat = scipy.matrix(scipy.identity(4))
00120       draw_sphere_mat[0:3, 3] = scipy.matrix(farthest_point).T
00121       draw_functions.draw_rviz_sphere(draw_sphere_mat, .0075, frame = 'base_link')
00122 
00123       #set the pregrasp between-fingers point to be the farthest_point + 7 cm 
00124       if approach_type == 'top':
00125           approachpos = [farthest_point[0], farthest_point[1], farthest_point[2] + (.185+.09)]
00126           pregrasp_pose = create_pose_stamped(approachpos+approachquat)
00127       else:
00128           sideapproachpos = [farthest_point[0], farthest_point[1] - (.185+.09), sideapproachpos[2]]
00129           pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat)
00130 
00131       #draw the new pregrasp pose
00132       draw_functions.draw_rviz_axes(pose_to_mat(pregrasp_pose.pose), 'base_link')
00133       
00134      
00135 
00136 #pause for input
00137 def keypause():
00138     print "press enter to continue"
00139     input = raw_input()
00140     return input
00141 
00142 
00143 #move to a Cartesian pose goal
00144 def move_cartesian_step(cm, pose, timeout = 10.0,
00145                         settling_time = 3.0, blocking = 0):
00146     if type(pose) == list:
00147         pose = create_pose_stamped(pose, 'base_link')
00148         cm.move_cartesian(pose, blocking = blocking, \
00149                    pos_thres = .0025, rot_thres = .05, \
00150                    timeout = rospy.Duration(timeout), \
00151                    settling_time = rospy.Duration(settling_time))
00152 
00153 
00154 #used to call compliant close or grasp adjustment
00155 def call_empty_service(service_name, serv):
00156     try:
00157         serv()
00158     except rospy.ServiceException, e:
00159         rospy.logerr("error when calling %s,%s"%(service_name,e))
00160         return 0
00161     return 1
00162 
00163 
00164 #return the current pos and rot of the wrist for the right arm 
00165 #as a 7-list (pos, quaternion rot)
00166 def return_current_pose_as_list(cm):
00167     (pos, rot) = cm.return_cartesian_pose()
00168     return pos+rot
00169 
00170 
00171 ##convert a relative vector in frame to a pose in the base_link frame
00172 #if start_pose is not specified, uses current pose of the wrist
00173 def return_rel_pose(cm, vector, frame, start_pose = None):
00174 
00175     #if start_pose is not specified, use the current Cartesian pose of the wrist
00176     if start_pose == None:
00177         (start_trans, start_rot) = cm.return_cartesian_pose('base_link')
00178     else:
00179         start_pose.header.stamp = rospy.Time(0)
00180         (start_trans, start_rot) = pose_stamped_to_lists(cm.tf_listener, start_pose, 'base_link')
00181 
00182     #convert the vector in frame to base_link frame
00183     if frame != 'base_link':
00184         vector3_stamped = create_vector3_stamped(vector, frame)
00185         base_link_vector = vector3_stamped_to_list(cm.tf_listener, vector3_stamped, 'base_link')    
00186     else:
00187         base_link_vector = vector
00188 
00189     #add the desired vector to the position
00190     new_trans = [x+y for (x,y) in zip(start_trans, base_link_vector)]
00191 
00192     #create a new poseStamped
00193     pose_stamped = create_pose_stamped(new_trans+start_rot)
00194 
00195     return pose_stamped
00196 
00197 
00198 #try to move to a set of joint angles using move_arm, ignoring current collisions if necessary, and if that fails, move open-loop
00199 def try_hard_to_move_joint(cm, col_map_interface, trajectory, max_tries = 5, use_open_loop = 1):
00200     reset_collision_map = 0
00201     for angles in trajectory:
00202         success = 0
00203         for tries in range(max_tries):
00204             rospy.loginfo("try_hard_to_move_joint try number: %d"%tries)
00205             error_code = cm.move_arm_joint(angles, blocking = 1)
00206 
00207             if error_code == 1:
00208                 rospy.loginfo("move arm reported success")
00209                 success = 1
00210                 break
00211             elif error_code == 0:
00212                 if not reset_collision_map:
00213                     rospy.loginfo("problem with collision map!  Resetting collision map and taking a new one")
00214                     col_map_interface.reset_collision_map()
00215                     col_map_interface.take_static_map()
00216                     reset_collision_map = 1
00217                 else:
00218                     rospy.loginfo("resetting collision map didn't work, move arm is still stuck!")
00219                     break
00220             rospy.loginfo("move arm reports failure with error code %d"%error_code)
00221         if not success and use_open_loop:
00222             rospy.loginfo("moving open-loop to desired joint angles")
00223             cm.command_joint_trajectory([angles], blocking = 1)
00224 
00225 
00226 #move the arm to the side
00227 def move_arm_to_side(cm, col_map_interface):
00228     arm_above_and_to_side_angles = [-0.968, 0.729, -0.554, -1.891, -1.786, -1.127, 0.501]
00229     arm_to_side_angles = [-2.135, 0.803, -1.732, -1.905, -2.369, -1.680, 1.398]
00230     try_hard_to_move_joint(cm, col_map_interface, [arm_above_and_to_side_angles, \
00231                                      arm_to_side_angles], use_open_loop = 1)
00232 
00233 
00234 #move the arm from the side to the approach
00235 def side_to_approach_joint(cm, col_map_interface): 
00236     arm_above_and_to_side_angles = [-0.968, 0.729, -0.554, -1.891, -1.786, -1.127, 0.501]
00237     # [-0.309647, 0.346556, -1.203788, -1.641639, -1.907762, -1.941233, -0.357047]
00238     try_hard_to_move_joint(cm, col_map_interface, 
00239                            [arm_above_and_to_side_angles],
00240                            use_open_loop = 1)
00241 
00242 ##call find_cluster_bounding_box to get the bounding box for a cluster
00243 def call_find_cluster_bounding_box(bounding_box_srv, cluster):
00244 
00245     req = FindClusterBoundingBoxRequest()
00246     req.cluster = cluster
00247     try:
00248         res = bounding_box_srv(req)
00249     except rospy.ServiceException, e:
00250         rospy.logerr("error when calling find_cluster_bounding_box: %s"%e)  
00251         return (None, [])
00252 
00253     return (res.pose, [res.box_dims.x, res.box_dims.y, res.box_dims.z])
00254 
00255 
00256 #call tabletop object detection to detect table/objects
00257 def call_tabletop_detection(detect_srv, bounding_box_srv):
00258 
00259     rospy.loginfo("calling tabletop detection")
00260 
00261     det_req = TabletopDetectionRequest()
00262     det_req.return_clusters = 1
00263     det_req.return_models = 0
00264     det_req.num_models = 0
00265 
00266     #call tabletop detection, get a detection result
00267     for try_num in range(3):
00268         try:
00269             det_res = detect_srv(det_req)
00270         except rospy.ServiceException, e:
00271             rospy.logerr("error when calling tabletop_detection: %s"%e)
00272             return ([], [])
00273         if det_res.detection.result == det_res.detection.SUCCESS:
00274             rospy.loginfo("tabletop detection reports success")
00275             break
00276         else:
00277             rospy.logerr("tabletop detection failed with error:"+str(det_res.detection.result))
00278     else:
00279         rospy.logerr("tabletop detection failed too many times.  Returning.")
00280         return ([], [])
00281     
00282     #find bounding boxes for found clusters (should be in base_link frame)
00283     box_poses = []
00284     box_dims = []
00285     for cluster in det_res.detection.clusters:
00286         (pose, dims) = call_find_cluster_bounding_box(bounding_box_srv, cluster)
00287         box_poses.append(pose)
00288         box_dims.append(dims)
00289     return (box_poses, box_dims)
00290 
00291 
00292 #find the position of the farthest corner of any cluster bounding box along axis in base_link frame
00293 def find_farthest_box(box_poses, box_dims, axis):
00294     axis_mat = scipy.matrix([axis[0], axis[1], axis[2], 0])
00295 
00296     farthest_box_points = []
00297     farthest_dists = []
00298     for (box_pose, dims) in zip(box_poses, box_dims):
00299 
00300         transform = pose_to_mat(box_pose.pose)
00301         ranges = [[-dims[0]/2, -dims[1]/2, -dims[2]/2], [dims[0]/2, dims[1]/2, dims[2]/2]]
00302         corners = [scipy.matrix([ranges[xind][0], ranges[yind][1], ranges[zind][2], 1]).T for \
00303                        (xind, yind, zind) in scipy.ndindex(2,2,2)] 
00304         corners = scipy.concatenate(corners, axis=1)
00305         transformed_corners = transform*corners #4xn
00306         #print "transformed_corners:", transformed_corners
00307 
00308         dists = axis_mat * transformed_corners
00309         max_dist = dists.max()
00310         #print "max_dist:", max_dist
00311         ind = scipy.argmax(dists)
00312         #print "ind:", ind
00313         #print "dists:", dists
00314         farthest_dists.append(max_dist)
00315         farthest_point = list(scipy.array(transformed_corners[0:3, ind].T)[0])
00316         #print "farthest_point:", farthest_point
00317         farthest_box_points.append(farthest_point)
00318 
00319     farthest_ind = scipy.argmax(scipy.array(farthest_dists))
00320     farthest_point = farthest_box_points[farthest_ind]
00321     print "farthest_box_points:", farthest_box_points
00322     print "farthest_dists:", farthest_dists
00323     print "overall farthest_ind:", farthest_ind
00324     print "overall farthest_point:", farthest_point
00325     return farthest_point
00326 
00327 
00328 #run the test
00329 if __name__ == "__main__":
00330 
00331     rospy.init_node('test_reactive_grasp_server', anonymous = True)
00332 
00333     use_slip_detection = rospy.get_param('/reactive_grasp_node_right/use_slip_controller', 0)
00334     use_slip_controller = rospy.get_param('/reactive_grasp_node_right/use_slip_detection', 0)
00335     rospy.loginfo("use_slip_controller:"+str(use_slip_controller))
00336     rospy.loginfo("use_slip_detection:"+str(use_slip_detection))
00337 
00338     rospy.loginfo("waiting for compliant_close and grasp_adjustment services")
00339     rospy.wait_for_service("r_reactive_grasp/compliant_close")
00340     rospy.wait_for_service("r_reactive_grasp/grasp_adjustment")
00341     rospy.wait_for_service("object_detection")
00342     rospy.wait_for_service('/find_cluster_bounding_box')
00343     rospy.loginfo("service found")
00344 
00345     rg_ac = actionlib.SimpleActionClient("reactive_grasp/right", \
00346                                              ReactiveGraspAction)
00347     ra_ac = actionlib.SimpleActionClient("reactive_approach/right", \
00348                                              ReactiveGraspAction)
00349     rl_ac = actionlib.SimpleActionClient("reactive_lift/right", \
00350                                              ReactiveLiftAction)
00351     rp_ac = actionlib.SimpleActionClient("reactive_place/right", \
00352                                              ReactivePlaceAction)
00353     cc_srv = rospy.ServiceProxy("r_reactive_grasp/compliant_close", Empty)
00354     ga_srv = rospy.ServiceProxy("r_reactive_grasp/grasp_adjustment", Empty)
00355     detect_srv = rospy.ServiceProxy("object_detection", TabletopDetection)
00356     bounding_box_srv = rospy.ServiceProxy('/find_cluster_bounding_box', FindClusterBoundingBox)
00357 
00358     rospy.loginfo("waiting for reactive grasp action")
00359     rg_ac.wait_for_server()
00360     rospy.loginfo("reactive grasp action found")
00361 
00362     rospy.loginfo("waiting for reactive approach action")
00363     ra_ac.wait_for_server()
00364     rospy.loginfo("reactive approach action found")
00365 
00366     rospy.loginfo("waiting for reactive lift action")
00367     rl_ac.wait_for_server()
00368     rospy.loginfo("reactive lift action found")
00369 
00370     rospy.loginfo("waiting for reactive place action")
00371     rp_ac.wait_for_server()
00372     rospy.loginfo("reactive place action found")
00373 
00374     cm = controller_manager.ControllerManager('r', \
00375                              using_slip_controller = use_slip_controller, \
00376                              using_slip_detection = use_slip_detection)
00377 
00378     #collision map interface for using move_arm
00379     col_map_interface = collision_map_interface.CollisionMapInterface()
00380 
00381     #draw_functions object for drawing stuff in rviz
00382     draw_functions = object_manipulator.draw_functions.DrawFunctions('grasp_markers')
00383 
00384     #joint names for the right arm
00385     joint_names = ["r_shoulder_pan_joint",
00386                    "r_shoulder_lift_joint",
00387                    "r_upper_arm_roll_joint",
00388                    "r_elbow_flex_joint",
00389                    "r_forearm_roll_joint",
00390                    "r_wrist_flex_joint",
00391                    "r_wrist_roll_joint"]
00392 
00393     #reactive approach from above 
00394     #table_height = .55  #simulated table
00395     table_height = .7239 #round table
00396     tip_dist_to_table = .18
00397     wrist_height = table_height + tip_dist_to_table + .02
00398     approachpos = [.52, -.05, wrist_height+.1]
00399     approachquat = [0.5, 0.5, -0.5, 0.5]  #from the top
00400     sideangles = [-0.447, -0.297, -2.229, -0.719, 0.734, -1.489, -1.787]
00401     side_tip_dist_to_table = .06 
00402     sideapproachpos = [.50, -.3, table_height-.035+side_tip_dist_to_table]
00403     sideapproachquat = [.707, .707, 0, 0]
00404     current_goal = approachpos[:] + approachquat[:]
00405     small_step = .02
00406     approach_type = 'top'
00407 
00408     while(not rospy.is_shutdown()):
00409 
00410         #update current goal to current pose
00411         current_goal = return_current_pose_as_list(cm)
00412 
00413         #pregrasp_pose = create_pose_stamped(current_goal)
00414         if approach_type == 'top':
00415             pregrasp_pose = create_pose_stamped(approachpos+approachquat)
00416         else:
00417             pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat)
00418 
00419         print "enter:"
00420         print "side to switch to side approach, top to switch to top approach"
00421         print "appi to go to the approach position with interpolated IK"
00422         print "appc to go to the approach position with Cartesian controllers"
00423         print "appm to go to the approach position with move_arm"
00424         print "appj to move from the side to the top approach with joint angles"
00425         print "ms to move the arm to the side"
00426         print "rg to grasp reactively, ra to just approach reactively, gl to grasp and lift"
00427         print "cc to do a compliant close"
00428         print "rl to do a reactive lift"
00429         print "rp to do a reactive place"
00430         print "ga to do a grasp adjustment"
00431         print "adj to adjust the pre-grasp position (arms should be at side)"
00432         print "c to close, o to open"
00433         print "u to go up 10 cm, d to go down 10 cm"
00434         print "small steps: us to go up, ds to go down, r to go right, l to go left,"
00435         print "     a to go away from the robot, t to go toward the robot"
00436         print "j to go to a set of nearish-side-grasp angles"
00437         print "reset to reset the collision map"
00438         print "map to take a static collision map"
00439         print "s to stop"
00440         c = keypause()
00441         if c == 'side':
00442             approach_type = 'side'
00443         elif c == 'top':
00444             approach_type = 'top'
00445         elif c == 'appi':
00446             #Go to the approach position using the interpolated 
00447             #IK controllers (collision-aware)
00448             cm.command_interpolated_ik(pregrasp_pose)
00449         elif c == 'appc':
00450             #Go to the approach position using the Cartesian controllers
00451             cm.command_cartesian(pregrasp_pose)
00452         elif c == 'appm':
00453             #Go to the approach position using move_arm
00454             for tries in range(3):
00455                 error_code = cm.move_arm_pose(pregrasp_pose, blocking = 1)
00456                 if error_code == 1:
00457                     print "move_arm reported success"
00458                     break
00459 
00460         elif c == 'appj':
00461             side_to_approach_joint(cm, col_map_interface)
00462         elif c == 'ms':
00463             move_arm_to_side(cm, col_map_interface)
00464         elif c == 'j':
00465             print "moving to near-side-grasp angles"
00466             cm.command_joint(sideangles)
00467         elif c == 'adj':
00468     
00469             print "adjusting pregrasp position"
00470 
00471             #do a tabletop detection and find the farthest cluster point along farthest_point_axis
00472             if approach_type == 'top':
00473                 farthest_point_axis = [0,0,1]
00474             else:
00475                 farthest_point_axis = [0,-1,0] 
00476             box_poses, box_dims = call_tabletop_detection(detect_srv, bounding_box_srv)
00477             if box_poses == []:
00478                 rospy.logerr("No clusters returned!")
00479                 continue
00480             farthest_point = find_farthest_box(box_poses, box_dims, farthest_point_axis)
00481             if farthest_point == None:
00482                 rospy.logerr("No farthest point returned!")
00483                 continue
00484 
00485             #draw the farthest_point
00486             draw_sphere_mat = scipy.matrix(scipy.identity(4))
00487             draw_sphere_mat[0:3, 3] = scipy.matrix(farthest_point).T
00488             draw_functions.draw_rviz_sphere(draw_sphere_mat, .0075, frame = 'base_link')
00489 
00490             #set the pregrasp between-fingers point to be the farthest_point + 7 cm 
00491             if approach_type == 'top':
00492                 approachpos = [farthest_point[0], farthest_point[1], farthest_point[2] + (.185+.09)]
00493                 pregrasp_pose = create_pose_stamped(approachpos+approachquat)
00494             else:
00495                 sideapproachpos = [farthest_point[0], farthest_point[1] - (.185+.09), sideapproachpos[2]]
00496                 pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat)
00497 
00498             #draw the new pregrasp pose
00499             draw_functions.draw_rviz_axes(pose_to_mat(pregrasp_pose.pose), 'base_link')
00500       
00501      
00502 
00503         elif c == 'res':
00504             print "restoring original approach positions"
00505             approachpos = [.52, -.05, wrist_height+.1]
00506             sideapproachpos = [.50, -.3, table_height-.035+side_tip_dist_to_table]
00507 
00508         elif c == 'rg' or c == 'ra' or c == 'gl':
00509 
00510             #set the goal to be 10 cm along the current gripper x-axis
00511             grasp_pose = return_rel_pose(cm, [.1, 0, 0], 'r_wrist_roll_link')
00512 
00513             # grasp_goal = current_goal[:]
00514             # grasp_goal[2] -= .1
00515             # grasp_pose = create_pose_stamped(grasp_goal)
00516 
00517             start_angles = [0.]*7
00518             trajectory = None
00519             if c == 'rg' or c == 'gl':
00520                 print "calling reactive grasp"
00521                 result = call_reactive_grasp(rg_ac, grasp_pose, trajectory)
00522             else:
00523                 print "calling reactive approach"
00524                 result = call_reactive_approach(ra_ac, grasp_pose, trajectory)
00525             print "result:", result
00526 
00527             if c == 'gl':
00528                 if result.value == 1:
00529                     print "calling reactive lift"
00530                     lift = GripperTranslation()
00531                     lift.direction = create_vector3_stamped([0,0,1])
00532                     lift.desired_distance = .1
00533                     lift.min_distance = 0.
00534                     result = call_reactive_lift(rl_ac, lift)
00535                     move_arm_to_side(cm, col_map_interface)
00536                     cm.command_gripper(.087, -1)
00537                     '''
00538                     print "adjusting pregrasp position"
00539 
00540                     #do a tabletop detection and find the farthest cluster point along farthest_point_axis
00541                     if approach_type == 'top':
00542                         farthest_point_axis = [0,0,1]
00543                     else:
00544                         farthest_point_axis = [0,-1,0] 
00545                     box_poses, box_dims = call_tabletop_detection(detect_srv, bounding_box_srv)
00546                     if box_poses == []:
00547                         rospy.logerr("No clusters returned!")
00548                         continue
00549                     farthest_point = find_farthest_box(box_poses, box_dims, farthest_point_axis)
00550                     if farthest_point == None:
00551                         rospy.logerr("No farthest point returned!")
00552                         continue
00553 
00554                     #draw the farthest_point
00555                     draw_sphere_mat = scipy.matrix(scipy.identity(4))
00556                     draw_sphere_mat[0:3, 3] = scipy.matrix(farthest_point).T
00557                     draw_functions.draw_rviz_sphere(draw_sphere_mat, .0075, frame = 'base_link')
00558 
00559                     #set the pregrasp between-fingers point to be the farthest_point + 7 cm 
00560                     if approach_type == 'top':
00561                         approachpos = [farthest_point[0], farthest_point[1], farthest_point[2] + (.185+.09)]
00562                         pregrasp_pose = create_pose_stamped(approachpos+approachquat)
00563                     else:
00564                         sideapproachpos = [farthest_point[0], farthest_point[1] - (.185+.09), sideapproachpos[2]]
00565                         pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat)
00566 
00567                     #draw the new pregrasp pose
00568                     draw_functions.draw_rviz_axes(pose_to_mat(pregrasp_pose.pose), 'base_link')
00569                     side_to_approach_joint(cm, col_map_interface)
00570                     cm.command_cartesian(pregrasp_pose)
00571                     '''
00572                 else:
00573                     print "grasp didn't return success, opening gripper"
00574                     cm.command_gripper(.087, -1)
00575 
00576         elif c == 'rl':
00577             print "calling reactive lift"
00578             lift = GripperTranslation()
00579             lift.direction = create_vector3_stamped([0,0,1])
00580             lift.desired_distance = .1
00581             lift.min_distance = 0.
00582             result = call_reactive_lift(rl_ac, lift)
00583 
00584         elif c == 'rp':
00585             print "calling reactive place"
00586             place_goal = current_goal[:]
00587             place_goal[2] -= .1
00588             place_pose = create_pose_stamped(place_goal)
00589             result = call_reactive_place(rp_ac, place_pose)
00590 
00591         elif c == 'cc':
00592             print "calling compliant close"
00593             call_empty_service("compliant close", cc_srv)
00594         elif c == 'ga':
00595             print "calling grasp adjustment"
00596             call_empty_service("grasp adjustment", ga_srv)
00597         elif c == 'c':
00598             print "closing gripper"
00599             cm.command_gripper(0, 40.0)
00600         elif c == 'o':
00601             print "opening gripper"
00602             cm.command_gripper(.087, -1)
00603         elif c == 'u':
00604             print "going up"
00605             current_goal[2] += .1
00606             move_cartesian_step(cm, current_goal, blocking = 1)
00607         elif c == 'us':
00608             print "going up a small amount"
00609             current_goal[2] += small_step
00610             move_cartesian_step(cm, current_goal, blocking = 1)
00611         elif c == 'd':
00612             print "going down"
00613             current_goal[2] -= .05
00614             move_cartesian_step(cm, current_goal, blocking = 1)
00615         elif c == 'ds':
00616             print "going down a small amount"
00617             current_goal[2] -= small_step
00618             move_cartesian_step(cm, current_goal, blocking = 1)
00619         elif c == 'r':
00620             print "moving right"
00621             current_goal[1] -= small_step
00622             move_cartesian_step(cm, current_goal, blocking = 1)
00623         elif c == 'l':
00624             print "moving left"
00625             current_goal[1] += small_step
00626             move_cartesian_step(cm, current_goal, blocking = 1)
00627         elif c == 'a':
00628             print "moving away from robot"
00629             current_goal[0] += small_step
00630             move_cartesian_step(cm, current_goal, blocking = 1)
00631         elif c == 't':
00632             print "moving toward the robot"
00633             current_goal[0] -= small_step
00634             move_cartesian_step(cm, current_goal, blocking = 1)
00635         elif c == 'reset':
00636             col_map_interface.reset_collision_map()
00637         elif c == 'map':
00638             col_map_interface.take_static_map()
00639         elif c == 's':
00640             print "quitting"
00641             break
00642 
00643         #update current goal to current pose
00644         current_goal = return_current_pose_as_list(cm)
00645 
00646 


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