run_experiments.py

Go to the documentation of this file.

#!/usr/bin/python
"""test client for pr2_gripper_reactive_approach_server
requires pr2_gripper_reactive_approach_server.py r (right arm) to be running
"""

from __future__ import division
import roslib
roslib.load_manifest('pr2_gripper_stereo')
import rospy
import math
import scipy
from geometry_msgs.msg import PoseStamped, PointStamped, \
    QuaternionStamped, Pose, Point, Quaternion
from object_manipulation_msgs.msg import ReactiveGraspAction, \
    ReactiveGraspGoal, ReactiveLiftAction, ReactiveLiftGoal, \
    GripperTranslation, ReactivePlaceAction, ReactivePlaceGoal
import actionlib
from trajectory_msgs.msg import JointTrajectory
from pr2_gripper_reactive_approach import controller_manager
from std_srvs.srv import Empty
from object_manipulator.convert_functions import *
import tabletop_collision_map_processing.collision_map_interface as collision_map_interface
from tabletop_object_detector.srv import TabletopDetection, TabletopDetectionRequest
from tabletop_object_detector.msg import TabletopDetectionResult
import object_manipulator.draw_functions
from object_manipulation_msgs.srv import FindClusterBoundingBox, FindClusterBoundingBoxRequest

#call reactive_grasp to do a reactive grasp using the fingertip sensors
#(backs up and move to the side if the tip contacts on the way to the grasp,
#does a compliant grasp if it gets there, tries the approach and grasp 
#several times if the gripper opening isn't within bounds)
def call_reactive_grasp(ac, grasp_pose, trajectory):

    if trajectory == None:
        trajectory = JointTrajectory()

    goal = ReactiveGraspGoal()
    goal.final_grasp_pose = grasp_pose
    goal.trajectory = trajectory
    goal.collision_support_surface_name = "table"

    ac.send_goal(goal)
    ac.wait_for_result()
    result = ac.get_result()
    print "reactive grasp result:", result

    return result.manipulation_result


#call reactive_grasp to do a reactive approach using the fingertip sensors
#(backs up and move to the side if the tip contacts on the way to the grasp)
def call_reactive_approach(ac, grasp_pose, trajectory):

    if trajectory == None:
        trajectory = JointTrajectory()

    goal = ReactiveGraspGoal()
    goal.final_grasp_pose = grasp_pose
    goal.trajectory = trajectory
    goal.collision_support_surface_name = "table"

    ac.send_goal(goal)
    ac.wait_for_result()
    result = ac.get_result()
    print "reactive approach result:", result

    return result.manipulation_result


#call reactive lift (starts up slip servoing if using the slip controllers, 
#otherwise just a Cartesian move)
def call_reactive_lift(ac, lift):

    goal = ReactiveLiftGoal()
    goal.lift = lift

    ac.send_goal(goal)
    ac.wait_for_result()
    result = ac.get_result()
    print "reactive lift result:", result

    return result


#call reactive place (uses the slip controllers to detect when the object
#hits the table, and opens the gripper)
def call_reactive_place(ac, place_pose):

    goal = ReactivePlaceGoal()
    goal.final_place_pose = place_pose

    ac.send_goal(goal)
    ac.wait_for_result()
    result = ac.get_result()
    print "reactive place result:", result

    return result


def do_adjust():
    
      print "adjusting pregrasp position"

      #do a tabletop detection and find the farthest cluster point along farthest_point_axis
      if approach_type == 'top':
          farthest_point_axis = [0,0,1]
      else:
          farthest_point_axis = [0,-1,0] 
      box_poses, box_dims = call_tabletop_detection(detect_srv, bounding_box_srv)
      if box_poses == []:
          rospy.logerr("No clusters returned!")
          return
      farthest_point = find_farthest_box(box_poses, box_dims, farthest_point_axis)
      if farthest_point == None:
          rospy.logerr("No farthest point returned!")
          return

      #draw the farthest_point
      draw_sphere_mat = scipy.matrix(scipy.identity(4))
      draw_sphere_mat[0:3, 3] = scipy.matrix(farthest_point).T
      draw_functions.draw_rviz_sphere(draw_sphere_mat, .0075, frame = 'base_link')

      #set the pregrasp between-fingers point to be the farthest_point + 7 cm 
      if approach_type == 'top':
          approachpos = [farthest_point[0], farthest_point[1], farthest_point[2] + (.185+.09)]
          pregrasp_pose = create_pose_stamped(approachpos+approachquat)
      else:
          sideapproachpos = [farthest_point[0], farthest_point[1] - (.185+.09), sideapproachpos[2]]
          pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat)

      #draw the new pregrasp pose
      draw_functions.draw_rviz_axes(pose_to_mat(pregrasp_pose.pose), 'base_link')
      
     

#pause for input
def keypause():
    print "press enter to continue"
    input = raw_input()
    return input


#move to a Cartesian pose goal
def move_cartesian_step(cm, pose, timeout = 10.0,
                        settling_time = 3.0, blocking = 0):
    if type(pose) == list:
        pose = create_pose_stamped(pose, 'base_link')
        cm.move_cartesian(pose, blocking = blocking, \
                   pos_thres = .0025, rot_thres = .05, \
                   timeout = rospy.Duration(timeout), \
                   settling_time = rospy.Duration(settling_time))


#used to call compliant close or grasp adjustment
def call_empty_service(service_name, serv):
    try:
        serv()
    except rospy.ServiceException, e:
        rospy.logerr("error when calling %s,%s"%(service_name,e))
        return 0
    return 1


#return the current pos and rot of the wrist for the right arm 
#as a 7-list (pos, quaternion rot)
def return_current_pose_as_list(cm):
    (pos, rot) = cm.return_cartesian_pose()
    return pos+rot


##convert a relative vector in frame to a pose in the base_link frame
#if start_pose is not specified, uses current pose of the wrist
def return_rel_pose(cm, vector, frame, start_pose = None):

    #if start_pose is not specified, use the current Cartesian pose of the wrist
    if start_pose == None:
        (start_trans, start_rot) = cm.return_cartesian_pose('base_link')
    else:
        start_pose.header.stamp = rospy.Time(0)
        (start_trans, start_rot) = pose_stamped_to_lists(cm.tf_listener, start_pose, 'base_link')

    #convert the vector in frame to base_link frame
    if frame != 'base_link':
        vector3_stamped = create_vector3_stamped(vector, frame)
        base_link_vector = vector3_stamped_to_list(cm.tf_listener, vector3_stamped, 'base_link')    
    else:
        base_link_vector = vector

    #add the desired vector to the position
    new_trans = [x+y for (x,y) in zip(start_trans, base_link_vector)]

    #create a new poseStamped
    pose_stamped = create_pose_stamped(new_trans+start_rot)

    return pose_stamped


#try to move to a set of joint angles using move_arm, ignoring current collisions if necessary, and if that fails, move open-loop
def try_hard_to_move_joint(cm, col_map_interface, trajectory, max_tries = 5, use_open_loop = 1):
    reset_collision_map = 0
    for angles in trajectory:
        success = 0
        for tries in range(max_tries):
            rospy.loginfo("try_hard_to_move_joint try number: %d"%tries)
            error_code = cm.move_arm_joint(angles, blocking = 1)

            if error_code == 1:
                rospy.loginfo("move arm reported success")
                success = 1
                break
            elif error_code == 0:
                if not reset_collision_map:
                    rospy.loginfo("problem with collision map!  Resetting collision map and taking a new one")
                    col_map_interface.reset_collision_map()
                    col_map_interface.take_static_map()
                    reset_collision_map = 1
                else:
                    rospy.loginfo("resetting collision map didn't work, move arm is still stuck!")
                    break
            rospy.loginfo("move arm reports failure with error code %d"%error_code)
        if not success and use_open_loop:
            rospy.loginfo("moving open-loop to desired joint angles")
            cm.command_joint_trajectory([angles], blocking = 1)


#move the arm to the side
def move_arm_to_side(cm, col_map_interface):
    arm_above_and_to_side_angles = [-0.968, 0.729, -0.554, -1.891, -1.786, -1.127, 0.501]
    arm_to_side_angles = [-2.135, 0.803, -1.732, -1.905, -2.369, -1.680, 1.398]
    try_hard_to_move_joint(cm, col_map_interface, [arm_above_and_to_side_angles, \
                                     arm_to_side_angles], use_open_loop = 1)


#move the arm from the side to the approach
def side_to_approach_joint(cm, col_map_interface): 
    arm_above_and_to_side_angles = [-0.968, 0.729, -0.554, -1.891, -1.786, -1.127, 0.501]
    # [-0.309647, 0.346556, -1.203788, -1.641639, -1.907762, -1.941233, -0.357047]
    try_hard_to_move_joint(cm, col_map_interface, 
                           [arm_above_and_to_side_angles],
                           use_open_loop = 1)

##call find_cluster_bounding_box to get the bounding box for a cluster
def call_find_cluster_bounding_box(bounding_box_srv, cluster):

    req = FindClusterBoundingBoxRequest()
    req.cluster = cluster
    try:
        res = bounding_box_srv(req)
    except rospy.ServiceException, e:
        rospy.logerr("error when calling find_cluster_bounding_box: %s"%e)  
        return (None, [])

    return (res.pose, [res.box_dims.x, res.box_dims.y, res.box_dims.z])


#call tabletop object detection to detect table/objects
def call_tabletop_detection(detect_srv, bounding_box_srv):

    rospy.loginfo("calling tabletop detection")

    det_req = TabletopDetectionRequest()
    det_req.return_clusters = 1
    det_req.return_models = 0
    det_req.num_models = 0

    #call tabletop detection, get a detection result
    for try_num in range(3):
        try:
            det_res = detect_srv(det_req)
        except rospy.ServiceException, e:
            rospy.logerr("error when calling tabletop_detection: %s"%e)
            return ([], [])
        if det_res.detection.result == det_res.detection.SUCCESS:
            rospy.loginfo("tabletop detection reports success")
            break
        else:
            rospy.logerr("tabletop detection failed with error:"+str(det_res.detection.result))
    else:
        rospy.logerr("tabletop detection failed too many times.  Returning.")
        return ([], [])
    
    #find bounding boxes for found clusters (should be in base_link frame)
    box_poses = []
    box_dims = []
    for cluster in det_res.detection.clusters:
        (pose, dims) = call_find_cluster_bounding_box(bounding_box_srv, cluster)
        box_poses.append(pose)
        box_dims.append(dims)
    return (box_poses, box_dims)


#find the position of the farthest corner of any cluster bounding box along axis in base_link frame
def find_farthest_box(box_poses, box_dims, axis):
    axis_mat = scipy.matrix([axis[0], axis[1], axis[2], 0])

    farthest_box_points = []
    farthest_dists = []
    for (box_pose, dims) in zip(box_poses, box_dims):

        transform = pose_to_mat(box_pose.pose)
        ranges = [[-dims[0]/2, -dims[1]/2, -dims[2]/2], [dims[0]/2, dims[1]/2, dims[2]/2]]
        corners = [scipy.matrix([ranges[xind][0], ranges[yind][1], ranges[zind][2], 1]).T for \
                       (xind, yind, zind) in scipy.ndindex(2,2,2)] 
        corners = scipy.concatenate(corners, axis=1)
        transformed_corners = transform*corners #4xn
        #print "transformed_corners:", transformed_corners

        dists = axis_mat * transformed_corners
        max_dist = dists.max()
        #print "max_dist:", max_dist
        ind = scipy.argmax(dists)
        #print "ind:", ind
        #print "dists:", dists
        farthest_dists.append(max_dist)
        farthest_point = list(scipy.array(transformed_corners[0:3, ind].T)[0])
        #print "farthest_point:", farthest_point
        farthest_box_points.append(farthest_point)

    farthest_ind = scipy.argmax(scipy.array(farthest_dists))
    farthest_point = farthest_box_points[farthest_ind]
    print "farthest_box_points:", farthest_box_points
    print "farthest_dists:", farthest_dists
    print "overall farthest_ind:", farthest_ind
    print "overall farthest_point:", farthest_point
    return farthest_point


#run the test
if __name__ == "__main__":

    rospy.init_node('test_reactive_grasp_server', anonymous = True)

    use_slip_detection = rospy.get_param('/reactive_grasp_node_right/use_slip_controller', 0)
    use_slip_controller = rospy.get_param('/reactive_grasp_node_right/use_slip_detection', 0)
    rospy.loginfo("use_slip_controller:"+str(use_slip_controller))
    rospy.loginfo("use_slip_detection:"+str(use_slip_detection))

    rospy.loginfo("waiting for compliant_close and grasp_adjustment services")
    rospy.wait_for_service("r_reactive_grasp/compliant_close")
    rospy.wait_for_service("r_reactive_grasp/grasp_adjustment")
    rospy.wait_for_service("object_detection")
    rospy.wait_for_service('/find_cluster_bounding_box')
    rospy.loginfo("service found")

    rg_ac = actionlib.SimpleActionClient("reactive_grasp/right", \
                                             ReactiveGraspAction)
    ra_ac = actionlib.SimpleActionClient("reactive_approach/right", \
                                             ReactiveGraspAction)
    rl_ac = actionlib.SimpleActionClient("reactive_lift/right", \
                                             ReactiveLiftAction)
    rp_ac = actionlib.SimpleActionClient("reactive_place/right", \
                                             ReactivePlaceAction)
    cc_srv = rospy.ServiceProxy("r_reactive_grasp/compliant_close", Empty)
    ga_srv = rospy.ServiceProxy("r_reactive_grasp/grasp_adjustment", Empty)
    detect_srv = rospy.ServiceProxy("object_detection", TabletopDetection)
    bounding_box_srv = rospy.ServiceProxy('/find_cluster_bounding_box', FindClusterBoundingBox)

    rospy.loginfo("waiting for reactive grasp action")
    rg_ac.wait_for_server()
    rospy.loginfo("reactive grasp action found")

    rospy.loginfo("waiting for reactive approach action")
    ra_ac.wait_for_server()
    rospy.loginfo("reactive approach action found")

    rospy.loginfo("waiting for reactive lift action")
    rl_ac.wait_for_server()
    rospy.loginfo("reactive lift action found")

    rospy.loginfo("waiting for reactive place action")
    rp_ac.wait_for_server()
    rospy.loginfo("reactive place action found")

    cm = controller_manager.ControllerManager('r', \
                             using_slip_controller = use_slip_controller, \
                             using_slip_detection = use_slip_detection)

    #collision map interface for using move_arm
    col_map_interface = collision_map_interface.CollisionMapInterface()

    #draw_functions object for drawing stuff in rviz
    draw_functions = object_manipulator.draw_functions.DrawFunctions('grasp_markers')

    #joint names for the right arm
    joint_names = ["r_shoulder_pan_joint",
                   "r_shoulder_lift_joint",
                   "r_upper_arm_roll_joint",
                   "r_elbow_flex_joint",
                   "r_forearm_roll_joint",
                   "r_wrist_flex_joint",
                   "r_wrist_roll_joint"]

    #reactive approach from above 
    #table_height = .55  #simulated table
    table_height = .7239 #round table
    tip_dist_to_table = .18
    wrist_height = table_height + tip_dist_to_table + .02
    approachpos = [.52, -.05, wrist_height+.1]
    approachquat = [0.5, 0.5, -0.5, 0.5]  #from the top
    sideangles = [-0.447, -0.297, -2.229, -0.719, 0.734, -1.489, -1.787]
    side_tip_dist_to_table = .06 
    sideapproachpos = [.50, -.3, table_height-.035+side_tip_dist_to_table]
    sideapproachquat = [.707, .707, 0, 0]
    current_goal = approachpos[:] + approachquat[:]
    small_step = .02
    approach_type = 'top'

    while(not rospy.is_shutdown()):

        #update current goal to current pose
        current_goal = return_current_pose_as_list(cm)

        #pregrasp_pose = create_pose_stamped(current_goal)
        if approach_type == 'top':
            pregrasp_pose = create_pose_stamped(approachpos+approachquat)
        else:
            pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat)

        print "enter:"
        print "side to switch to side approach, top to switch to top approach"
        print "appi to go to the approach position with interpolated IK"
        print "appc to go to the approach position with Cartesian controllers"
        print "appm to go to the approach position with move_arm"
        print "appj to move from the side to the top approach with joint angles"
        print "ms to move the arm to the side"
        print "rg to grasp reactively, ra to just approach reactively, gl to grasp and lift"
        print "cc to do a compliant close"
        print "rl to do a reactive lift"
        print "rp to do a reactive place"
        print "ga to do a grasp adjustment"
        print "adj to adjust the pre-grasp position (arms should be at side)"
        print "c to close, o to open"
        print "u to go up 10 cm, d to go down 10 cm"
        print "small steps: us to go up, ds to go down, r to go right, l to go left,"
        print "     a to go away from the robot, t to go toward the robot"
        print "j to go to a set of nearish-side-grasp angles"
        print "reset to reset the collision map"
        print "map to take a static collision map"
        print "s to stop"
        c = keypause()
        if c == 'side':
            approach_type = 'side'
        elif c == 'top':
            approach_type = 'top'
        elif c == 'appi':
            #Go to the approach position using the interpolated 
            #IK controllers (collision-aware)
            cm.command_interpolated_ik(pregrasp_pose)
        elif c == 'appc':
            #Go to the approach position using the Cartesian controllers
            cm.command_cartesian(pregrasp_pose)
        elif c == 'appm':
            #Go to the approach position using move_arm
            for tries in range(3):
                error_code = cm.move_arm_pose(pregrasp_pose, blocking = 1)
                if error_code == 1:
                    print "move_arm reported success"
                    break

        elif c == 'appj':
            side_to_approach_joint(cm, col_map_interface)
        elif c == 'ms':
            move_arm_to_side(cm, col_map_interface)
        elif c == 'j':
            print "moving to near-side-grasp angles"
            cm.command_joint(sideangles)
        elif c == 'adj':
    
            print "adjusting pregrasp position"

            #do a tabletop detection and find the farthest cluster point along farthest_point_axis
            if approach_type == 'top':
                farthest_point_axis = [0,0,1]
            else:
                farthest_point_axis = [0,-1,0] 
            box_poses, box_dims = call_tabletop_detection(detect_srv, bounding_box_srv)
            if box_poses == []:
                rospy.logerr("No clusters returned!")
                continue
            farthest_point = find_farthest_box(box_poses, box_dims, farthest_point_axis)
            if farthest_point == None:
                rospy.logerr("No farthest point returned!")
                continue

            #draw the farthest_point
            draw_sphere_mat = scipy.matrix(scipy.identity(4))
            draw_sphere_mat[0:3, 3] = scipy.matrix(farthest_point).T
            draw_functions.draw_rviz_sphere(draw_sphere_mat, .0075, frame = 'base_link')

            #set the pregrasp between-fingers point to be the farthest_point + 7 cm 
            if approach_type == 'top':
                approachpos = [farthest_point[0], farthest_point[1], farthest_point[2] + (.185+.09)]
                pregrasp_pose = create_pose_stamped(approachpos+approachquat)
            else:
                sideapproachpos = [farthest_point[0], farthest_point[1] - (.185+.09), sideapproachpos[2]]
                pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat)

            #draw the new pregrasp pose
            draw_functions.draw_rviz_axes(pose_to_mat(pregrasp_pose.pose), 'base_link')
      
     

        elif c == 'res':
            print "restoring original approach positions"
            approachpos = [.52, -.05, wrist_height+.1]
            sideapproachpos = [.50, -.3, table_height-.035+side_tip_dist_to_table]

        elif c == 'rg' or c == 'ra' or c == 'gl':

            #set the goal to be 10 cm along the current gripper x-axis
            grasp_pose = return_rel_pose(cm, [.1, 0, 0], 'r_wrist_roll_link')

            # grasp_goal = current_goal[:]
            # grasp_goal[2] -= .1
            # grasp_pose = create_pose_stamped(grasp_goal)

            start_angles = [0.]*7
            trajectory = None
            if c == 'rg' or c == 'gl':
                print "calling reactive grasp"
                result = call_reactive_grasp(rg_ac, grasp_pose, trajectory)
            else:
                print "calling reactive approach"
                result = call_reactive_approach(ra_ac, grasp_pose, trajectory)
            print "result:", result

            if c == 'gl':
                if result.value == 1:
                    print "calling reactive lift"
                    lift = GripperTranslation()
                    lift.direction = create_vector3_stamped([0,0,1])
                    lift.desired_distance = .1
                    lift.min_distance = 0.
                    result = call_reactive_lift(rl_ac, lift)
                    move_arm_to_side(cm, col_map_interface)
                    cm.command_gripper(.087, -1)
                    '''
                    print "adjusting pregrasp position"

                    #do a tabletop detection and find the farthest cluster point along farthest_point_axis
                    if approach_type == 'top':
                        farthest_point_axis = [0,0,1]
                    else:
                        farthest_point_axis = [0,-1,0] 
                    box_poses, box_dims = call_tabletop_detection(detect_srv, bounding_box_srv)
                    if box_poses == []:
                        rospy.logerr("No clusters returned!")
                        continue
                    farthest_point = find_farthest_box(box_poses, box_dims, farthest_point_axis)
                    if farthest_point == None:
                        rospy.logerr("No farthest point returned!")
                        continue

                    #draw the farthest_point
                    draw_sphere_mat = scipy.matrix(scipy.identity(4))
                    draw_sphere_mat[0:3, 3] = scipy.matrix(farthest_point).T
                    draw_functions.draw_rviz_sphere(draw_sphere_mat, .0075, frame = 'base_link')

                    #set the pregrasp between-fingers point to be the farthest_point + 7 cm 
                    if approach_type == 'top':
                        approachpos = [farthest_point[0], farthest_point[1], farthest_point[2] + (.185+.09)]
                        pregrasp_pose = create_pose_stamped(approachpos+approachquat)
                    else:
                        sideapproachpos = [farthest_point[0], farthest_point[1] - (.185+.09), sideapproachpos[2]]
                        pregrasp_pose = create_pose_stamped(sideapproachpos+sideapproachquat)

                    #draw the new pregrasp pose
                    draw_functions.draw_rviz_axes(pose_to_mat(pregrasp_pose.pose), 'base_link')
                    side_to_approach_joint(cm, col_map_interface)
                    cm.command_cartesian(pregrasp_pose)
                    '''
                else:
                    print "grasp didn't return success, opening gripper"
                    cm.command_gripper(.087, -1)

        elif c == 'rl':
            print "calling reactive lift"
            lift = GripperTranslation()
            lift.direction = create_vector3_stamped([0,0,1])
            lift.desired_distance = .1
            lift.min_distance = 0.
            result = call_reactive_lift(rl_ac, lift)

        elif c == 'rp':
            print "calling reactive place"
            place_goal = current_goal[:]
            place_goal[2] -= .1
            place_pose = create_pose_stamped(place_goal)
            result = call_reactive_place(rp_ac, place_pose)

        elif c == 'cc':
            print "calling compliant close"
            call_empty_service("compliant close", cc_srv)
        elif c == 'ga':
            print "calling grasp adjustment"
            call_empty_service("grasp adjustment", ga_srv)
        elif c == 'c':
            print "closing gripper"
            cm.command_gripper(0, 40.0)
        elif c == 'o':
            print "opening gripper"
            cm.command_gripper(.087, -1)
        elif c == 'u':
            print "going up"
            current_goal[2] += .1
            move_cartesian_step(cm, current_goal, blocking = 1)
        elif c == 'us':
            print "going up a small amount"
            current_goal[2] += small_step
            move_cartesian_step(cm, current_goal, blocking = 1)
        elif c == 'd':
            print "going down"
            current_goal[2] -= .05
            move_cartesian_step(cm, current_goal, blocking = 1)
        elif c == 'ds':
            print "going down a small amount"
            current_goal[2] -= small_step
            move_cartesian_step(cm, current_goal, blocking = 1)
        elif c == 'r':
            print "moving right"
            current_goal[1] -= small_step
            move_cartesian_step(cm, current_goal, blocking = 1)
        elif c == 'l':
            print "moving left"
            current_goal[1] += small_step
            move_cartesian_step(cm, current_goal, blocking = 1)
        elif c == 'a':
            print "moving away from robot"
            current_goal[0] += small_step
            move_cartesian_step(cm, current_goal, blocking = 1)
        elif c == 't':
            print "moving toward the robot"
            current_goal[0] -= small_step
            move_cartesian_step(cm, current_goal, blocking = 1)
        elif c == 'reset':
            col_map_interface.reset_collision_map()
        elif c == 'map':
            col_map_interface.take_static_map()
        elif c == 's':
            print "quitting"
            break

        #update current goal to current pose
        current_goal = return_current_pose_as_list(cm)