Ejemplo n.º 1
0
def gentle_move(dir_index, q_initial, pos, gripperOri_list, tip_hand_transform,
                listener, point_counter):
    plans = []
    axis_list = [[0, 1, 0], [-1, 0, 0], [0, -1, 0], [1, 0, 0], [0, 0, -1],
                 [0, 0, 1]]
    axis = axis_list[dir_index]
    if point_counter == 4:
        bin_counter = 0
    elif point_counter == 9:
        bin_counter = 1
    elif point_counter == 14:
        bin_counter = 2
    elif point_counter == 19:
        bin_counter = 3

    for i in range(0, 10000):
        for i in range(0, 3):
            pos[i] = pos[i] + 0.002 * axis[i]
        with Timer('generatePlan'):
            plan, qf, plan_possible = generatePlan(q_initial, pos,
                                                   gripperOri_list,
                                                   tip_hand_transform,
                                                   'fastest')
            if plan_possible:
                plans.append(plan)
                q_initial = qf
            else:
                return '[IK] Error'
        #~execute plan
        with Timer('executePlanForward'):
            executePlanForward(plans, False, False)
        value = raw_input(
            "Enter: Move 2mm closer to wall, s: Stop robot and record value ")
        #save data if s
        if value == 's':
            fpath = os.environ[
                'CODE_BASE'] + '/catkin_ws/src/apc_config/storage_corners.yaml'
            stream = open(fpath, 'r')
            data = yaml.load(stream)
            tcp_pose = get_tcp_pose(listener)
            if dir_index == 4:
                terms = ['z']
            else:
                terms = ['x', 'y']
            for i in range(0, len(terms)):
                if dir_index == 4:
                    param_name = '/bin' + str(bin_counter) + '/' + terms[i]
                    value_tmp = tcp_pose[2] - rospy.get_param(
                        '/gripper/spatula_tip_to_tcp_dist')
                else:
                    param_name = '/corner' + str(
                        point_counter) + '/' + terms[i]
                    value_tmp = tcp_pose[i]
                value = value_tmp.astype(type('float', (float, ), {}))
                data[param_name] = value
            with open(fpath, 'w') as yaml_file:
                yaml_file.write(yaml.dump(data, default_flow_style=False))
            break
        else:
            plans = []
Ejemplo n.º 2
0
def release_safe(listener, isExecute=True, withPause=False):
    plans = []
    #Initialize parameters
    spatula_tip_to_tcp_dist = rospy.get_param(
        "/gripper/spatula_tip_to_tcp_dist")
    l1 = 0.0
    l2 = 0.0
    l3 = spatula_tip_to_tcp_dist
    tip_hand_transform = [l1, l2, l3, 0, 0, 0]
    bin_pose = ik.helper.get_params_yaml('bin' + str(0) + '_pose')
    UpdistFromBinFast = 0.15 - 0.1
    #Initial configuration of robot
    q_initial = ik.helper.get_joints()
    targetPose = ik.helper.get_tcp_pose(listener,
                                        tcp_offset=spatula_tip_to_tcp_dist)
    #Desired new safe position above binNum
    targetPose[2] = bin_pose[2] + UpdistFromBinFast
    #################
    ## Build plans ##
    #################
    #~1. Open gripper
    grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % 0.11)
    plans.append(grasp_plan)
    #~2. Open spatula
    grasp_plan = EvalPlan('spatula.open()')
    plans.append(grasp_plan)
    #~3. Move up
    plan, qf, plan_possible = generatePlan(q_initial,
                                           targetPose[0:3],
                                           targetPose[3:7],
                                           tip_hand_transform,
                                           'superSaiyan',
                                           plan_name='go_safe_bin')
    if plan_possible:
        plans.append(plan)
        q_initial = qf
    else:
        print('[Release Safe] Plans failed:')

    #execute plan_name
    if isExecute and plan_possible:
        executePlanForward(plans, withPause)
Ejemplo n.º 3
0
def grasp(objInput,
          listener,
          br,
          isExecute=True,
          objId='expo_eraser',
          binId=0,
          flag=0,
          withPause=False,
          rel_pose=None,
          BoxBody=None,
          place_pose=None,
          viz_pub=None,
          is_drop=True,
          update_command=None,
          recorder=None):

    #########################################################
    # fhogan and nikhilcd
    # Description:
    #~2017 picking primitive
    #
    #~Usage
    #~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None):
    #
    #~Parameters:
    #
    #~Inputs:
    # 1) objInput: Input from vision or RViz (list of 12, or 7)
    # 2) listener: Input from vision or RViz (list of 12, or 7)
    # 3) br: Input from vision or RViz (list of 12, or 7)
    # 4) isExecute: execute the robot motion if possible or not? (Bool)
    # 5) objId: name of object (str)
    # 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1)
    # 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location)
    # 8) withPause: Pause between robot motions (Bool)
    # 9) rel_pose: object pose relative to link_6
    # 10) BoxBody: Bounding box points resolved in link 6 frame
    # 11) place_pose: desired position of bounding box
    # 12) viz_pub: desired position of bounding box
    #
    #~Output:
    # Dictionary of following keys:
    # 1) grasp_possible: Does the object fit in the hand? (True/False)
    # 2) plan_possible: Has the plan succeeded? (True/False)
    # 3) execution_possible: Is there anything in the grasp? (True/False)
    # 4) gripper_opening: gripper opening after grasp attempt (in meters)
    # 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion)
    # 6) gelsight_data: [] , dummy for now
    #########################################################

    #rospy.logdebug(msg, *args)
    #rospy.loginfo(msg, *args)
    #rospy.logwarn(msg, *args)
    #rospy.logerr(msg, *args)
    #rospy.logfatal(msg, *args)

    ###############################
    ## Pring input variables for ##
    ###############################
    rospy.logdebug('[Picking] Starting primitive with flag: %d' % flag)
    rospy.logdebug('[Picking] objInput %s', objInput)
    rospy.logdebug('[Picking] objId %s', objId)
    rospy.logdebug('[Picking] flag %s', flag)
    rospy.logdebug('[Picking] withPause %s', withPause)
    rospy.logdebug('[Picking] rel_pose %s', rel_pose)
    rospy.logdebug('[Picking] BoxBody %s', BoxBody)
    rospy.logdebug('[Picking] place_pose %s', place_pose)

    ########################
    ## Initialize values ##
    ########################
    q_initial = ik.helper.get_joints()
    bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
    spatula_tip_to_tcp_dist = rospy.get_param(
        "/gripper/spatula_tip_to_tcp_dist")
    delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
    vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
    plans = []
    plans_grasping = []
    plans_grasping2 = []
    plans_guarded = []
    plans_placing = []
    graspPose = []
    plan_possible = False
    execution_possible = False
    gripper_opening = 0.0
    grasp_possible = True
    gelsight_data = []
    collision = False
    final_object_pose = None
    rospy.set_param('is_record', False)
    rospy.set_param('is_contact', False)
    weightSensor = ws_prob.WeightSensor()

    liftoff_pub = rospy.Publisher('/liftoff_time',
                                  std_msgs.msg.String,
                                  queue_size=10,
                                  latch=False)

    def compose_output():
        return {
            'collision': collision,
            'grasp_possible': grasp_possible,
            'plan_possible': plan_possible,
            'execution_possible': execution_possible,
            'gripper_opening': gripper_opening,
            'graspPose': graspPose,
            'gelsight_data': gelsight_data,
            'final_object_pose': final_object_pose
        }

    #########################
    ## Constant parameters ##
    #########################
    UpdistFromBinFast = 0.15  # Margin above object during "move to a location"
    UpdistFromBinGuarded = 0.05  # Margin above object during "move to a location"
    gripperOriHome = [0, 1, 0, 0]
    l1 = 0.0
    l2 = 0.0
    l3 = spatula_tip_to_tcp_dist
    tip_hand_transform = [l1, l2, l3, 0, 0, 0]
    vision_pos[2] = 0.17786528

    ###############################
    ## Picking primitive flag: 0 ##
    ###############################
    if flag == 0:
        #        ik.visualize_helper.visualize_grasping_proposals(viz_pub, np.asarray([objInput]),  listener, br, False)
        #~Define reference frames
        world_X, world_Y, world_Z, tote_X, tote_Y, tote_Z, tote_pose_pos = ik.helper.reference_frames(
            listener=listener, br=br)

        #~get grasp pose and gripper opening from vision
        if len(objInput) == 12:
            graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_12(
                objInput)
        elif len(objInput) == 7:
            graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_7(
                objInput, objId, listener, br)

        #~build gripper orientation matrix 3x3
        hand_orient_norm = np.vstack([hand_X, hand_Y, hand_Z])
        hand_orient_norm = hand_orient_norm.transpose()

        #~Grasp relocation script
        hand_orient_quat = ik.helper.mat2quat(hand_orient_norm)
        euler = tf.transformations.euler_from_quaternion(hand_orient_quat)
        theta = euler[2] - np.pi
        colFreePose = collisionFree(graspPos,
                                    binId=binId,
                                    listener=listener,
                                    br=br,
                                    finger_opening=grasp_width,
                                    safety_margin=0.00,
                                    theta=theta)
        graspPos[0:2] = colFreePose[0:2]

        #################################
        ## GENERATE PLANS OF PRIMITIVE ##
        #################################.
        #~ Start from home position
        #        if isExecute:
        #            q_initial = collision_free_placing(binId, listener, isSuction = False, with_object = False, hand_orientation=[0,1,0,0], projected_target_position = bin_pose[0:3], isReturn = True)
        #            scorpion.back()

        #~0. Move to a location above the bin, Rotate gripper to grasp orientation
        pregrasp_targetPosition = graspPos
        pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinFast
        plan, qf, plan_possible = generatePlan(q_initial,
                                               pregrasp_targetPosition,
                                               hand_orient_quat,
                                               tip_hand_transform,
                                               'superSaiyan',
                                               plan_name='go_safe_bin')
        if plan_possible:
            plans.append(plan)
            q_initial = qf
        else:
            return compose_output()

        #~1. Move to to surface of bin
        pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinGuarded
        plan, qf, plan_possible = generatePlan(q_initial,
                                               pregrasp_targetPosition,
                                               hand_orient_quat,
                                               tip_hand_transform,
                                               'superSaiyan',
                                               plan_name='go_top_bin')
        if plan_possible:
            plans.append(plan)
            q_initial = qf
        else:
            return compose_output()

    #~2. Open gripper
        grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % grasp_width)
        plans.append(grasp_plan)

        #~ 3. Open spatula
        grasp_plan = EvalPlan('spatula.open()')
        plans.append(grasp_plan)

        grasp_targetPosition = deepcopy(graspPos)
        #        rospy.loginfo('[Picking] Grasp Target %s', grasp_targetPosition)

        #~4. sleep
        sleep_plan = EvalPlan('rospy.sleep(0.2)')
        plans.append(sleep_plan)

        #~5. perform guarded move down
        grasp_targetPosition[2] = bin_pose[2] - rospy.get_param(
            '/tote/height') + 0.000  #~frank hack for safety
        if binId == 0:
            grasp_targetPosition[2] = grasp_targetPosition[2] + 0.005

        plan, qf, plan_possible = generatePlan(q_initial,
                                               grasp_targetPosition,
                                               hand_orient_quat,
                                               tip_hand_transform,
                                               'faster',
                                               guard_on=WeightGuard(
                                                   binId, threshold=100),
                                               plan_name='guarded_pick')
        if plan_possible:
            plans_guarded.append(plan)
            q_initial = qf
        else:
            return compose_output()

        #~7. Close spatula
        grasp_plan = EvalPlan('spatula.close()')
        plans_guarded.append(grasp_plan)

        #~8. Close gripper
        grasp_plan = EvalPlan('helper.graspinGripper(%f,%f)' % (800, 50))
        plans_guarded.append(grasp_plan)

        #~9. sleep
        sleep_plan = EvalPlan('rospy.sleep(0.2)')
        plans_guarded.append(sleep_plan)

        #~10. Move to a location above the bin
        plan, qf, plan_possible = generatePlan(q_initial,
                                               vision_pos,
                                               delta_vision_pose[3:7],
                                               tip_hand_transform,
                                               'fastest',
                                               plan_name='retrieve_object')
        if plan_possible:
            plans_grasping.append(plan)
            q_initial = qf
        else:
            return compose_output()

        ################
        ## EXECUTION ###
        ################

        if isExecute and plan_possible:

            executePlanForward(plans, withPause)

            ###We start recording now
            lasers.start(binId)
            recorder.start_recording(
                action='grasping',
                action_id=str(
                    datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
                tote_num=binId,
                frame_rate_ratio=5,
                image_size=-1)

            #Execute guarded move
            rospy.set_param('is_record', True)
            executePlanForward(plans_guarded, withPause)
            rospy.set_param('is_record', False)

            #Publish liftoff time
            liftoff_msgs = std_msgs.msg.String()
            liftoff_msgs.data = 'Liftoff (Robot command)'
            liftoff_pub.publish(liftoff_msgs)

            #Execute non-guarded grasp plan move
            executePlanForward(plans_grasping, withPause)

            ###We stop recording
            recorder.stop_recording(save_action=True)
            lasers.stop(binId)

        #~Check if picking success
        low_threshold = 0.0035
        high_threshold = 0.015
        if isExecute and plan_possible:
            rospy.sleep(2)
            gripper_opening = gripper.getGripperopening()
            if gripper_opening > high_threshold:
                rospy.loginfo('[Picking] ***************')
                rospy.loginfo(
                    '[Picking] Pick Successful (Gripper Opening Test)')
                rospy.loginfo('[Picking] ***************')
                execution_possible = True
            else:
                rospy.loginfo('[Picking] ***************')
                rospy.loginfo(
                    '[Picking] Pick Inconclusive (Gripper Opening Test)')
                rospy.loginfo('[Picking] ***************')
                execution_possible = None

        elif not isExecute:
            execution_possible = plan_possible

        return compose_output()

    #############
    ## Placing ##
    #############
    elif flag == 2:
        #if primitive called without placing arguments, go to center of bin
        if rel_pose == None:
            drop_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
            drop_pose[3:7] = gripperOriHome
        else:
            drop_pose = ik.helper.drop_pose_transform(binId, rel_pose, BoxBody,
                                                      place_pose, viz_pub,
                                                      listener, br)

        #~ Adjust height according to weigth
        if is_drop:
            drop_offset = 0.15
        else:
            drop_offset = 0.002
        #~set drop pose target z position to be bottom of bin
        drop_pose[2] = bin_pose[2] - rospy.get_param(
            'tote/height') + drop_offset

        #~Predrop: go to top middle bin surface fast without guarded move
        predrop_pos = np.array(drop_pose[0:3])
        predrop_pos[2] = bin_pose[2] + 0.05

        ######################
        ##  Generate plans ##
        ######################
        #~move safe to placing bin number
        rospy.logdebug(
            '[Picking] Collision free placing to binId %s and position %s',
            binId, predrop_pos)
        #        if isExecute:
        #            q_initial = collision_free_placing(binId, listener, obj_ID=objId, BoxBody=BoxBody, isSuction = False,with_object = True, hand_orientation=drop_pose[3:7],projected_target_position = predrop_pos, isReturn = True)
        #            if update_command is not None:
        #                update_command.execute()

        #~0.go up to avoid collision with tote walls
        current_pose = ik.helper.get_tcp_pose(listener, tcp_offset=l3)
        current_pose[2] = current_pose[2] + 0.15
        plan, qf, plan_possible = generatePlan(q_initial,
                                               current_pose[0:3],
                                               current_pose[3:7],
                                               tip_hand_transform,
                                               'faster',
                                               plan_name='go_up')
        if plan_possible:
            plans.append(plan)
            q_initial = qf
        else:
            return compose_output()

        #~0.1.fast motion to bin surface high
        predrop_pos_high = predrop_pos
        predrop_pos_high[2] = current_pose[2]
        plan, qf, plan_possible = generatePlan(q_initial,
                                               predrop_pos_high,
                                               drop_pose[3:7],
                                               tip_hand_transform,
                                               'faster',
                                               plan_name='go_bin_surface_high')
        if plan_possible:
            plans.append(plan)
            q_initial = qf
        else:
            return compose_output()

        #~1.fast motion to bin surface
        predrop_pos[2] = bin_pose[2] + 0.05
        plan, qf, plan_possible = generatePlan(q_initial,
                                               predrop_pos,
                                               drop_pose[3:7],
                                               tip_hand_transform,
                                               'faster',
                                               plan_name='go_bin_surface')
        if plan_possible:
            plans.append(plan)
            q_initial = qf
        else:
            return compose_output()

        #~2. Guarded move down slow
        plan, qf, plan_possible = generatePlan(q_initial,
                                               drop_pose[0:3],
                                               drop_pose[3:7],
                                               tip_hand_transform,
                                               'fast',
                                               guard_on=WeightGuard(binId),
                                               backwards_speed='superSaiyan',
                                               plan_name='guarded_drop')
        if plan_possible:
            plans_placing.append(plan)
            q_initial = qf
        else:
            return compose_output()

        #~3. open gripper
        grasp_plan = EvalPlan('helper.moveGripper(0.110, 200)')
        plans_placing.append(grasp_plan)

        #~4. open spatula
        grasp_plan = EvalPlan('spatula.open()')
        plans_placing.append(grasp_plan)

        #~5. go to predrop_pos
        plan, qf, plan_possible = generatePlan(q_initial,
                                               predrop_pos[0:3],
                                               drop_pose[3:7],
                                               tip_hand_transform,
                                               'superSaiyan',
                                               plan_name='predrop_pos')
        if plan_possible:
            plans_placing.append(plan)
            q_initial = qf
        else:
            return compose_output()

        #~6. go to final pose
        final_pos = bin_pose[0:3]
        final_pos[2] = bin_pose[2] + 0.3
        plan, qf, plan_possible = generatePlan(q_initial,
                                               final_pos,
                                               drop_pose[3:7],
                                               tip_hand_transform,
                                               'superSaiyan',
                                               plan_name='final_pose')
        if plan_possible:
            plans_placing.append(plan)
            q_initial = qf
        else:
            return compose_output()

        #~ Execute plans
        if plan_possible:
            if isExecute:
                executePlanForward(plans, withPause)

                #We start recording now
                lasers.start(binId)
                recorder.start_recording(
                    action='placing',
                    action_id=str(
                        datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
                    tote_num=binId,
                    frame_rate_ratio=5,
                    image_size=-1)

                rospy.set_param('is_record', True)
                executePlanForward(plans_placing, withPause)
                rospy.set_param('is_record', False)

                recorder.stop_recording(save_action=True)
                lasers.stop(binId)

            execution_possible = True
            return compose_output()
Ejemplo n.º 4
0
def calibrate_gelsight(isExecute=True, withPause=False):
    arc_ori = np.array([0, 1, 0, 0])
    ori_1 = np.array([-0.70710678118654757, 0.70710678118654757, 0, 0])
    ori_2 = np.array([0.70710678118654757, 0.70710678118654757, 0, 0])
    #initialize Parameters
    tip_hand_transform = [0, 0, 0, 0, 0, 0]
    #read initial robot poses
    q_initial = ik.helper.get_joints()
    #go to arc_1
    # goToHome.goToARC()
    plans_list = []
    pose_list = []
    #arc position (high)
    pose_list.append(np.array([1.0, 0., 0.792, 0, 1, 0, 0]))
    pose_list.append(
        np.array([.642, .72005, .95168, .69934, .70796, -0.06744, .07189]))
    pose_list.append(
        np.array([.642, .72005, .75559, .69934, .70796, -0.06744, .07189]))
    pose_list.append(
        np.array([.56577, .72005, .75559, .69934, .70796, -0.06744, .07189]))
    pose_list.append(
        np.array([.48138, .72005, .75559, .69934, .70796, -0.06744, .07189]))
    pose_list.append(
        np.array([.48138, .72005, .95168, .69934, .70796, -0.06744, .07189]))
    pose_list.append(np.array([1.0, 0., 0.792, 0, 1, 0, 0]))
    speed_list = [
        'superSaiyan', 'superSaiyan', 'slow', 'slow', 'slow', 'slow',
        'superSaiyan'
    ]
    #gripper hand_commands
    # is_gripper_list = [0,0,0,0,0,0,0]
    is_gripper_list = [0, 0, 1, 1, 1, 0, 0]
    #~1. Open gripper
    grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % 0.11)
    plans_list.append(grasp_plan)

    #2. generate robot motions
    for i in range(len(pose_list)):
        print ' speed_list[i]', speed_list[i]
        plan, qf, plan_possible = generatePlan(q_initial,
                                               pose_list[i][0:3],
                                               pose_list[i][3:7],
                                               tip_hand_transform,
                                               speed_list[i],
                                               plan_name=str(i))
        if plan_possible:
            plans_list.append(plan)
            q_initial = qf
        else:
            print('[Release Safe] Plans failed:')
        if is_gripper_list[i]:
            #sleep
            sleep_plan = EvalPlan('rospy.sleep(1.0)')
            plans_list.append(sleep_plan)
            #close gripper
            grasp_plan = EvalPlan('helper.graspinGripper(%f,%f)' % (800, 30))
            plans_list.append(grasp_plan)
            #sleep
            sleep_plan = EvalPlan('rospy.sleep(1.5)')
            plans_list.append(sleep_plan)
            #capture image

            gelsight_plan = EvalPlan('helper.capture_gelsight()')
            plans_list.append(gelsight_plan)
            #sleep
            sleep_plan = EvalPlan('rospy.sleep(.5)')
            plans_list.append(sleep_plan)
            #open gripper
            grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % 0.11)
            plans_list.append(grasp_plan)
            #sleep
            sleep_plan = EvalPlan('rospy.sleep(2.)')
            plans_list.append(sleep_plan)

    #execute plan_name
    if isExecute and plan_possible:
        executePlanForward(plans_list, withPause)
Ejemplo n.º 5
0
def unit_test_centers():
    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()
    #~ ~Build hand transform
    l1 = 0.0
    l2 = 0.0
    l3 = rospy.get_param("/gripper/spatula_tip_to_tcp_dist")
    tip_hand_transform = [l1, l2, l3, 0, 0, 0]
    gripperOriHome = [0, 1, 0, 0]
    q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
    plans = []
    withPause = True
    #~generate plans
    #loop through sections (bins)
    plans = []
    goToHome.prepGripperPicking()
    goToHome.goToARC()

    for i in range(0, 9):
        binId = i
        collision_free_placing(binId, listener, isSuction=False)
        #~READ robot position
        rospy.sleep(.3)
        tcpPose = get_tcp_pose(listener, tcp_offset=l3)
        tcpPose[2] = rospy.get_param(
            '/bin' + str(i) +
            '_pose/z')  #~- rospy.get_param('/bin'+str(i)+'/height')
        rospy.sleep(.3)
        q_initial = get_joints()
        rospy.sleep(1.0)
        ##move to edge of structure
        map_bin_id_to_ws_id = {
            0: 0,
            1: 1,
            2: 2,
            3: 3,
            4: 0,
            5: 0,
            6: 4,
            7: 4,
            8: 5
        }
        plan, qf, plan_possible = generatePlan(q_initial, tcpPose[0:3],
                                               gripperOriHome,
                                               tip_hand_transform, 'fast')
        if plan_possible:
            plans.append(plan)
            q_initial = qf
        else:
            return '[IK] Error'
        ##move to edge of structure
        map_bin_id_to_ws_id = {
            0: 0,
            1: 1,
            2: 2,
            3: 3,
            4: 0,
            5: 0,
            6: 4,
            7: 4,
            8: 5
        }
        plan, qf, plan_possible = generatePlan(
            q_initial,
            tcpPose[0:3] - np.array([0, 0, 0.2]),
            gripperOriHome,
            tip_hand_transform,
            'fast',
            guard_on=WeightGuard(map_bin_id_to_ws_id[binId], threshold=100))
        if plan_possible:
            plans.append(plan)
            q_initial = qf
        else:
            return '[IK] Error'
        executePlanForward(plans, False)
        if binId > 5:
            collision_free_placing(binId, listener, isSuction=False)
        plans = []
Ejemplo n.º 6
0
def unit_test_collision(flag=0):
    #~ ~Build hand transform
    l1 = 0.0
    l2 = 0.0
    l3 = rospy.get_param("/gripper/calibration_to_tcp_dist")
    tip_hand_transform = [l1, l2, l3, 0, 0, 0]
    gripperOriHome = [0, 1, 0, 0]
    q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
    plans = []
    withPause = True
    #~generate plans
    #loop through sections (bins)
    counter = 0
    section_points_list = [[0, 1, 2, 3], [0, 1, 2, 3], [0, 1, 2, 3], [0, 2, 3]]
    #~bin corners
    for i in range(0, 4):
        #~get corner points from yaml
        points_warped_list, points_list = get_bin_corners(i)
        points_warped = np.asarray(points_warped_list)
        points = np.asarray(points_list)
        box1 = points
        box2 = points_warped
        ####################
        ## middle point ##
        ###################
        point_middle_list = get_bin_middle(i)
        point_middle = np.asarray(point_middle_list[0])
        #~transform to warped frame
        point_middle_warped = rect2quad(point_middle, box1, box2)
        #~ point_middle_warped = quad2quad(point_middle, box1, box2)
        #~ point_middle_warped = interp2d(point_middle, box1, box2)
        target_middle = np.hstack((point_middle_warped, 0.05))
        ####################
        ## corner points ##
        ###################
        for term in section_points_list[i]:
            ##move to middle
            plan, qf, plan_possible = generatePlan(q_initial, target_middle,
                                                   gripperOriHome,
                                                   tip_hand_transform,
                                                   'fastest')
            counter += 1
            if plan_possible:
                plans.append(plan)
                q_initial = qf
            else:
                return '[IK] Error'
            #~ target_position2d_warped = np.array(points_warped_list[term])
            target_position2d = np.array(points_list[term])
            #~convert to warped frame
            target_position2d = rect2quad(target_position2d, box1, box2)
            #~ target_position2d = quad2quad(target_position2d,box1,box2)
            #~ target_position2d = interp2d(target_position2d,box1,box2)
            target_position = np.hstack((target_position2d, 0.05))
            ##move to edge of structure
            target_middle2 = np.array(
                [target_middle[0], target_position[1], target_middle[2]])
            ##move to middle edge
            plan, qf, plan_possible = generatePlan(q_initial, target_middle2,
                                                   gripperOriHome,
                                                   tip_hand_transform,
                                                   'fastest')
            counter += 1
            if plan_possible:
                plans.append(plan)
                q_initial = qf
            else:
                return '[IK] Error'
            #~generate plan
            plan, qf, plan_possible = generatePlan(q_initial, target_position,
                                                   gripperOriHome,
                                                   tip_hand_transform,
                                                   'fastest')
            counter += 1
            if plan_possible:
                plans.append(plan)
                q_initial = qf
            else:
                return '[IK] Error'
            ##move to edge of structure
            plan, qf, plan_possible = generatePlan(q_initial, target_middle2,
                                                   gripperOriHome,
                                                   tip_hand_transform,
                                                   'fastest')
            counter += 1
            if plan_possible:
                plans.append(plan)
                q_initial = qf
            else:
                return '[IK] Error'

    #~Execute plans
    goToARC()
    executePlanForward(plans, withPause)
Ejemplo n.º 7
0
def calibrate_storage():
    #################################################
    #~ Generate Plans
    gripperOri = []
    gripperOri.append([-1., 0., 0., 0.])
    gripperOri.append([-0.70710678118654757, -0.70710678118654757, 0., 0.])
    gripperOri.append([0., -1., 0., 0.])
    gripperOri.append([-0.70710678118654757, 0.70710678118654757, 0., 0.])
    #~build total list plan
    gripperOri_list = []
    ori_list = [
        0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0,
        0, 0, 1, 2, 3, 0, 0, 0
    ]
    for i in range(0, len(ori_list)):
        gripperOri_list.append(gripperOri[ori_list[i]])
    #~positions
    pos = []
    #~bin0
    bin_pose = get_params_yaml('bin0_pose')
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([1.00 + .12, -0.46 + 0.19647, 0.41])
    pos.append([0.75 + .12, bin_pose[1], 0.41])
    pos.append([1. + .12, -0.697281360626 + 0.19647, 0.41])
    pos.append([1.19 + .12, bin_pose[1], 0.41])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append(
        [1.00278019905 + .12, -0.763054132462 + 0.19647, 0.447774887085 + .05])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin1
    bin_pose = get_params_yaml('bin1_pose')
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([1.00 + .12, -0.0943171977997 + 0.19647, 0.41])
    pos.append([0.75 + .12, bin_pose[1], 0.41])
    pos.append([1. + .12, -0.330679833889 + 0.19647, 0.41])
    pos.append([1.19 + .12, bin_pose[1], 0.41])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([
        0.961158931255 + .12, -0.0221027284861 + 0.19647, 0.447779148817 + .05
    ])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin2
    bin_pose = get_params_yaml('bin2_pose')
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([1.00 + .12, 0.307756274939 + 0.19647, 0.41])
    pos.append([0.8 + .12, bin_pose[1], 0.41])
    pos.append([1 + .12, 0.0618322640657 + 0.19647, 0.41])
    pos.append([1.18 + .12, bin_pose[1], 0.41])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append(
        [0.940047621727 + .12, 0.353745698929 + 0.19647, 0.447779148817 + .05])
    pos.append([
        bin_pose[0] + .12, bin_pose[1],
        0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])

    ####################
    ## Generate Plans ##
    ####################
    q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
    tip_hand_transform = [0., 0., 0., 0., 0., 0.]
    dir_list = [
        None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None,
        0, 1, 2, 3, None
    ]
    point_counter = 0
    #~generate plans
    plans = []
    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()
    rospy.sleep(0.5)
    goToHome.goToARC()
    #~ goToHome.prepGripperPicking()

    for i in range(0, len(pos)):
        plan, qf, plan_possible = generatePlan(q_initial, pos[i],
                                               gripperOri_list[i],
                                               tip_hand_transform, 'fastest')
        if plan_possible:
            plans.append(plan)
            q_initial = qf
        else:
            return '[IK] Error'
        #~execute plan
        executePlanForward(plans, True)
        plans = []
        if dir_list[i] is not None:
            gentle_move(dir_list[i], q_initial, pos[i], gripperOri_list[i],
                        tip_hand_transform, listener, point_counter)
            point_counter += 1
Ejemplo n.º 8
0
def calibrate_boxes():
    #################################################
    #~ Generate Plans
    gripperOri = []
    gripperOri.append([-1., 0., 0., 0.])
    gripperOri.append([-0.715217411518, -0.715217411518, 0., 0.])
    gripperOri.append([0., -1., 0., 0.])
    gripperOri.append([-0.715217411518, 0.715217411518, 0., 0.])
    #~build total list plan
    gripperOri_list = []
    ori_list = [
        0, 0, 1, 2, 3, 3, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0,
        0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0
    ]
    for i in range(0, len(ori_list)):
        gripperOri_list.append(gripperOri[ori_list[i]])
    #~positions
    pos = []
    #~bin4
    bin_pose = get_params_yaml('bin4_pose')
    pos.append([
        bin_pose[0], bin_pose[1],
        0.15 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([1.1, -0.498443692923, 0.5512911677361 - .15])
    pos.append([1.05, -0.599031984806, 0.5512911677361 - .15])
    pos.append([1.14, -0.70, 0.5512911677361 - .15])
    pos.append([1.2, -0.618239402771, 0.5512911677361 - .15])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.30 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([1.08824729919, -0.808329701424, 0.5512911677361 - 0.05])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin5
    bin_pose = get_params_yaml('bin5_pose')
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([0.79457449913, -0.498443692923, 0.5512911677361 - .15])
    pos.append([0.757445454597, -0.599031984806, 0.5512911677361 - .15])
    pos.append([0.79457449913, -0.70, 0.5512911677361 - .15])
    pos.append([0.889182715416, -0.618239402771, 0.5512911677361 - .15])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([0.79457449913, -0.788900852203, 0.5512911677361 - .05])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin6
    bin_pose = get_params_yaml('bin6_pose')
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([0.09962657094, -0.55, 0.539945185184 + 0.0])
    pos.append([0.0830265730619, -0.6, 0.539945185184 + 0.0])
    pos.append([0.099, -0.68, 0.539945185184 + 0.0])
    pos.append([0.15, -0.6, 0.539945185184 + 0.0])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([0.145160362124, -0.846666872501, 0.65])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin7
    bin_pose = get_params_yaml('bin7_pose')
    pos.append([
        bin_pose[0], bin_pose[1],
        0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([-0.199928298593, -0.5, 0.539945185184 + 0.0])
    pos.append([-0.272910028696, -0.6, 0.539945185184 + 0.0])
    pos.append([-0.195086687803, -0.75, 0.539945185184 + 0.0])
    pos.append([-0.100125610828, -0.6, 0.539945185184 + 0.0])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([-0.203513264656, -0.86, 0.65])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    #~bin8
    bin_pose = get_params_yaml('bin8_pose')
    pos.append([
        bin_pose[0], bin_pose[1],
        0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([-0.09, 0.68, 0.716840863228])
    pos.append([-0.22, 0.62, 0.716840863228])
    pos.append([-0.09, 0.5, 0.716840863228])
    pos.append([0.1, 0.62, 0.716840863228])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])
    pos.append([0.106256209314, 0.78, 0.8])
    pos.append([
        bin_pose[0], bin_pose[1],
        0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
    ])

    ####################
    ## Generate Plans ##
    ####################
    q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
    tip_hand_transform = [0., 0., 0., 0., 0., 0.]
    dir_list = [
        None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None,
        0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1,
        2, 3, None, 4, None
    ]
    point_counter = 0
    #~generate plans
    plans = []
    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()
    rospy.sleep(0.5)
    goToHome.goToARC()
    goToHome.prepGripperPicking()

    ###############################
    ## Unwrap angles example use ##
    ###############################
    #~1) function to unwarp an angle to the range [-pi,pi]
    def unwrap(angle):
        if angle > np.pi:
            angle = np.mod(angle, 2 * np.pi)
            if angle > np.pi:
                angle = angle - 2 * np.pi
        elif angle < -np.pi:
            angle = np.mod(angle, -2 * np.pi)
            if angle < -np.pi:
                angle = angle + 2 * np.pi
        return angle

    for i in range(0, 16):
        plan, qf, plan_possible = generatePlan(q_initial, pos[i],
                                               gripperOri_list[i],
                                               tip_hand_transform, 'fastest')
        if plan_possible:
            #~2) unwrap angle of joint 6 only
            plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
            plans.append(plan)
            #~3) set q_initial (for next plan) with the unwrapped angle joint state
            q_initial = plan.q_traj[-1]
        else:
            return '[IK] Error'
        #~execute plan
        executePlanForward(plans, True)
        plans = []
        if dir_list[i] is not None:
            gentle_move_boxes(dir_list[i], q_initial, pos[i],
                              gripperOri_list[i], tip_hand_transform, listener,
                              point_counter)
            point_counter += 1
    ##################################

    plans_col_free = []
    plans_col_free, q_initial, plan_possible, motion = collision_free_plan(
        q_initial, plans_col_free, [0, 1, 0, 0], False, 6)
    executePlanForward(plans_col_free, True)

    for i in range(16, 32):
        #~ for i in range(0,16):
        plan, qf, plan_possible = generatePlan(q_initial, pos[i],
                                               gripperOri_list[i],
                                               tip_hand_transform, 'fastest')
        if plan_possible:
            plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
            plans.append(plan)
            q_initial = plan.q_traj[-1]
        else:
            return '[IK] Error'
        #~execute plan
        executePlanForward(plans, True)
        plans = []
        if dir_list[i] is not None:
            gentle_move_boxes(dir_list[i], q_initial, pos[i],
                              gripperOri_list[i], tip_hand_transform, listener,
                              point_counter)
            point_counter += 1

    plans_col_free = []
    plans_col_free, q_initial, plan_possible, motion = collision_free_plan(
        q_initial, plans_col_free, [0, 1, 0, 0], False, 8)
    executePlanForward(plans_col_free, True)

    for i in range(32, 40):
        #~ for i in range(16,24):
        plan, qf, plan_possible = generatePlan(q_initial, pos[i],
                                               gripperOri_list[i],
                                               tip_hand_transform, 'fastest')
        if plan_possible:
            plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
            plans.append(plan)
            q_initial = plan.q_traj[-1]
        else:
            return '[IK] Error'
        #~execute plan
        executePlanForward(plans, True)
        plans = []
        if dir_list[i] is not None:
            gentle_move_boxes(dir_list[i], q_initial, pos[i],
                              gripperOri_list[i], tip_hand_transform, listener,
                              point_counter)
            point_counter += 1
Ejemplo n.º 9
0
def place(listener,
          br,
          isExecute=True,
          binId=0,
          withPause=False,
          rel_pose=None,
          BoxBody=None,
          place_pose=None,
          viz_pub=None,
          is_drop=True,
          recorder=None):

    #########################################################
    # fhogan and nikhilcd
    # Description:
    #~2017 picking primitive
    #
    #~Usage
    #~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None):
    #
    #~Parameters:
    #
    #~Inputs:
    # 1) objInput: Input from vision or RViz (list of 12, or 7)
    # 2) listener: Input from vision or RViz (list of 12, or 7)
    # 3) br: Input from vision or RViz (list of 12, or 7)
    # 4) isExecute: execute the robot motion if possible or not? (Bool)
    # 5) objId: name of object (str)
    # 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1)
    # 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location)
    # 8) withPause: Pause between robot motions (Bool)
    # 9) rel_pose: object pose relative to link_6
    # 10) BoxBody: Bounding box points resolved in link 6 frame
    # 11) place_pose: desired position of bounding box
    # 12) viz_pub: desired position of bounding box
    #
    #~Output:
    # Dictionary of following keys:
    # 1) grasp_possible: Does the object fit in the hand? (True/False)
    # 2) plan_possible: Has the plan succeeded? (True/False)
    # 3) execution_possible: Is there anything in the grasp? (True/False)
    # 4) gripper_opening: gripper opening after grasp attempt (in meters)
    # 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion)
    # 6) gelsight_data: [] , dummy for now
    #########################################################

    #rospy.logdebug(msg, *args)
    #rospy.loginfo(msg, *args)
    #rospy.logwarn(msg, *args)
    #rospy.logerr(msg, *args)
    #rospy.logfatal(msg, *args)

    ###############################
    ## Pring input variables for ##
    ###############################
    rospy.logdebug('[Picking] withPause %s', withPause)
    rospy.logdebug('[Picking] rel_pose %s', rel_pose)
    rospy.logdebug('[Picking] BoxBody %s', BoxBody)
    rospy.logdebug('[Picking] place_pose %s', place_pose)

    ########################
    ## Initialize values ##
    ########################
    q_initial = ik.helper.get_joints()
    bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
    spatula_tip_to_tcp_dist = rospy.get_param(
        "/gripper/spatula_tip_to_tcp_dist")
    delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
    vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
    plans = []
    plans_grasping = []
    plans_grasping2 = []
    plans_guarded = []
    plans_guarded2 = []
    plans_placing = []
    plans_placing2 = []
    graspPose = []
    plan_possible = False
    execution_possible = False
    gripper_opening = 0.0
    grasp_possible = True
    gelsight_data = []
    collision = False
    final_object_pose = None
    rospy.set_param('is_record', False)
    rospy.set_param('is_contact', False)

    liftoff_pub = rospy.Publisher('/liftoff_time',
                                  std_msgs.msg.String,
                                  queue_size=10,
                                  latch=False)

    def compose_output():
        return {
            'collision': collision,
            'grasp_possible': grasp_possible,
            'plan_possible': plan_possible,
            'execution_possible': execution_possible,
            'gripper_opening': gripper_opening,
            'graspPose': graspPose,
            'gelsight_data': gelsight_data,
            'final_object_pose': final_object_pose
        }

    #########################
    ## Constant parameters ##
    #########################
    UpdistFromBinFast = 0.15  # Margin above object during "move to a location"
    UpdistFromBinGuarded = 0.05  # Margin above object during "move to a location"
    gripperOriHome = [0, 1, 0, 0]
    l1 = 0.0
    l2 = 0.0
    l3 = spatula_tip_to_tcp_dist
    tip_hand_transform = [l1, l2, l3, 0, 0, 0]
    vision_pos[2] = 0.17786528

    #############
    ## Placing ##
    #############

    #if primitive called without placing arguments, go to center of bin
    if rel_pose == None:
        drop_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
        drop_pose[3:7] = gripperOriHome
    else:
        drop_pose = ik.helper.drop_pose_transform(binId, rel_pose, BoxBody,
                                                  place_pose, viz_pub,
                                                  listener, br)

    #~ Adjust height according to weigth
    if is_drop:
        drop_offset = 0.15
    else:
        drop_offset = 0.025
    #~set drop pose target z position to be bottom of bin
    drop_pose[2] = bin_pose[2] - rospy.get_param('tote/height') + drop_offset

    #~Predrop: go to top middle bin surface fast without guarded move
    predrop_pos = np.array(drop_pose[0:3])
    predrop_pos[2] = bin_pose[2] + 0.05

    ######################
    ##  Generate plans ##
    ######################
    #~move safe to placing bin number
    rospy.logdebug(
        '[Picking] Collision free placing to binId %s and position %s', binId,
        predrop_pos)
    #        if isExecute:
    #            q_initial = collision_free_placing(binId, listener, obj_ID=objId, BoxBody=BoxBody, isSuction = False,with_object = True, hand_orientation=drop_pose[3:7],projected_target_position = predrop_pos, isReturn = True)
    #            if update_command is not None:
    #                update_command.execute()

    #~0.go up to avoid collision with tote walls
    current_pose = ik.helper.get_tcp_pose(listener, tcp_offset=l3)
    current_pose[2] = current_pose[2] + 0.15
    plan, qf, plan_possible = generatePlan(q_initial,
                                           current_pose[0:3],
                                           current_pose[3:7],
                                           tip_hand_transform,
                                           'faster',
                                           plan_name='go_up')
    if plan_possible:
        plans.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~0.1.fast motion to bin surface high
    predrop_pos_high = predrop_pos
    predrop_pos_high[2] = current_pose[2]
    plan, qf, plan_possible = generatePlan(q_initial,
                                           predrop_pos_high,
                                           drop_pose[3:7],
                                           tip_hand_transform,
                                           'faster',
                                           plan_name='go_bin_surface_high')
    if plan_possible:
        plans.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~1.fast motion to bin surface
    predrop_pos[2] = bin_pose[2] + 0.05
    plan, qf, plan_possible = generatePlan(q_initial,
                                           predrop_pos,
                                           drop_pose[3:7],
                                           tip_hand_transform,
                                           'faster',
                                           plan_name='go_bin_surface')
    if plan_possible:
        plans.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~2. Guarded move down slow
    plan, qf, plan_possible = generatePlan(q_initial,
                                           drop_pose[0:3],
                                           drop_pose[3:7],
                                           tip_hand_transform,
                                           'fast',
                                           guard_on=WeightGuard(binId,
                                                                threshold=100),
                                           backwards_speed='superSaiyan',
                                           plan_name='guarded_drop')
    if plan_possible:
        plans_placing.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~3. open gripper
    grasp_plan = EvalPlan('helper.moveGripper(0.110, 200)')
    plans_placing2.append(grasp_plan)

    #~4. open spatula
    grasp_plan = EvalPlan('spatula.open()')
    plans_placing2.append(grasp_plan)

    #~5. go to predrop_pos
    plan, qf, plan_possible = generatePlan(q_initial,
                                           predrop_pos[0:3],
                                           drop_pose[3:7],
                                           tip_hand_transform,
                                           'superSaiyan',
                                           plan_name='predrop_pos')
    if plan_possible:
        plans_placing2.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~6. go to final pose
    final_pos = bin_pose[0:3]
    final_pos[2] = bin_pose[2] + 0.3
    plan, qf, plan_possible = generatePlan(q_initial,
                                           final_pos,
                                           drop_pose[3:7],
                                           tip_hand_transform,
                                           'superSaiyan',
                                           plan_name='final_pose')
    if plan_possible:
        plans_placing2.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~ Execute plans
    if plan_possible:
        if isExecute:
            executePlanForward(plans, withPause)

            #We start recording now
            if recorder is not None:
                lasers.start(binId)
                recorder.start_recording(
                    action='placing',
                    action_id=str(
                        datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
                    tote_num=binId,
                    frame_rate_ratio=5,
                    image_size=-1)

            rospy.set_param('is_record', True)
            executePlanForward(plans_placing, withPause)
            if rospy.get_param('is_contact'):
                ik.helper.move_cart(dz=0.015)
            executePlanForward(plans_placing2, withPause)
            rospy.set_param('is_record', False)
            if recorder is not None:
                recorder.stop_recording(save_action=True)
                lasers.stop(binId)

        execution_possible = True
        return compose_output()
Ejemplo n.º 10
0
def retrieve(listener,
             br,
             isExecute=True,
             objId='expo_eraser',
             binId=0,
             withPause=False,
             viz_pub=None,
             recorder=None,
             ws_object=None,
             isShake=True):

    liftoff_pub = rospy.Publisher('/liftoff_time',
                                  std_msgs.msg.String,
                                  queue_size=10,
                                  latch=False)

    def compose_output():
        return {
            'collision': collision,
            'grasp_possible': grasp_possible,
            'plan_possible': plan_possible,
            'execution_possible': execution_possible,
            'gripper_opening': gripper_opening,
            'graspPose': graspPose,
            'gelsight_data': gelsight_data,
            'final_object_pose': final_object_pose
        }

    q_initial = ik.helper.get_joints()
    bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
    spatula_tip_to_tcp_dist = rospy.get_param(
        "/gripper/spatula_tip_to_tcp_dist")
    l1 = 0.0
    l2 = 0.0
    l3 = spatula_tip_to_tcp_dist
    tip_hand_transform = [l1, l2, l3, 0, 0, 0]
    delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
    delta_vision_pose[3:7] = np.array([0, 1, 0, 0])
    vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
    vision_pos[2] = 0.17786528 - 0.1
    plans = []
    plans_grasping = []
    graspPose = []
    plan_possible = False
    execution_possible = False
    gripper_opening = 0.0
    grasp_possible = True
    gelsight_data = []
    collision = False
    final_object_pose = None
    rospy.set_param('is_record', False)
    rospy.set_param('is_contact', False)

    # ~10. Move to a location above the bin
    plan, qf, plan_possible = generatePlan(q_initial,
                                           vision_pos,
                                           delta_vision_pose[3:7],
                                           tip_hand_transform,
                                           'fastest',
                                           plan_name='retrieve_object')
    if plan_possible:
        plans_grasping.append(plan)
        q_initial = qf
    else:
        return compose_output()

    if isExecute and plan_possible:
        #Publish liftoff time
        liftoff_msgs = std_msgs.msg.String()
        liftoff_msgs.data = 'Liftoff (Robot command)'
        liftoff_pub.publish(liftoff_msgs)

        #Execute non-guarded grasp plan move
        executePlanForward(plans_grasping, withPause)

    #~Check if picking success
    low_threshold = 0.0035
    high_threshold = 0.027
    #shake robot
    rospy.sleep(1.5)
    if isShake and gripper.getGripperopening() > high_threshold and isExecute:
        # We initialize drop listener
        ws_object.start_listening_for_drop(bin_num=binId)

        ik.ik.shake()
        rospy.sleep(2.)

        # We stop drop listener
        ws_object.stop_listening_for_drop()

    ### We stop recording
    if recorder is not None:
        recorder.stop_recording(save_action=True)
        lasers.stop(binId)

    if isExecute and plan_possible:
        rospy.sleep(2)
        gripper_opening = gripper.getGripperopening()
        if gripper_opening > high_threshold:
            rospy.loginfo('[Picking] ***************')
            rospy.loginfo('[Picking] Pick Successful (Gripper Opening Test)')
            rospy.loginfo('[Picking] ***************')
            execution_possible = True  #temporary hack
        else:
            rospy.loginfo('[Picking] ***************')
            rospy.loginfo('[Picking] Pick Inconclusive (Gripper Opening Test)')
            rospy.loginfo('[Picking] ***************')
            execution_possible = None

    elif not isExecute:
        execution_possible = plan_possible

    return compose_output()
Ejemplo n.º 11
0
def grasp(objInput,
          listener,
          br,
          isExecute=True,
          objId='expo_eraser',
          binId=0,
          withPause=False,
          viz_pub=None,
          recorder=None,
          is_regrasp=False,
          open_hand=True):

    #########################################################
    # fhogan and nikhilcd
    # Description:
    #~2017 picking primitive
    #
    #~Usage
    #~ pick(objInput, listener, br, isExecute = True, objId = 'cheezit_big_original', flag = 1, withPause = False, rel_pose=None, BoxBody=None, place_pose=None, viz_pub=None):
    #
    #~Parameters:
    #
    #~Inputs:
    # 1) objInput: Input from vision or RViz (list of 12, or 7)
    # 2) listener: Input from vision or RViz (list of 12, or 7)
    # 3) br: Input from vision or RViz (list of 12, or 7)
    # 4) isExecute: execute the robot motion if possible or not? (Bool)
    # 5) objId: name of object (str)
    # 6) binId: bin in which we need to place the objects. (It is useless when flag 0 and 1)
    # 7) flag: Type of action directed by planning (0:run picking, 1:go to home (camera), 2: drop the object at target location)
    # 8) withPause: Pause between robot motions (Bool)
    # 9) rel_pose: object pose relative to link_6
    # 10) BoxBody: Bounding box points resolved in link 6 frame
    # 11) place_pose: desired position of bounding box
    # 12) viz_pub: desired position of bounding box
    #
    #~Output:
    # Dictionary of following keys:
    # 1) grasp_possible: Does the object fit in the hand? (True/False)
    # 2) plan_possible: Has the plan succeeded? (True/False)
    # 3) execution_possible: Is there anything in the grasp? (True/False)
    # 4) gripper_opening: gripper opening after grasp attempt (in meters)
    # 5) graspPose: pose of the gripper at grasp (7X1 - position and quaternion)
    # 6) gelsight_data: [] , dummy for now
    #########################################################

    #rospy.logdebug(msg, *args)
    #rospy.loginfo(msg, *args)
    #rospy.logwarn(msg, *args)
    #rospy.logerr(msg, *args)
    #rospy.logfatal(msg, *args)

    ###############################
    ## Pring input variables for ##
    ###############################
    rospy.logdebug('[Picking] objInput %s', objInput)
    rospy.logdebug('[Picking] objId %s', objId)
    rospy.logdebug('[Picking] withPause %s', withPause)

    ########################
    ## Initialize values ##
    ########################
    q_initial = ik.helper.get_joints()
    bin_pose = ik.helper.get_params_yaml('bin' + str(binId) + '_pose')
    spatula_tip_to_tcp_dist = rospy.get_param(
        "/gripper/spatula_tip_to_tcp_dist")
    delta_vision_pose = ik.helper.get_params_yaml('vision_pose_picking')
    vision_pos = np.array(bin_pose[0:3]) + np.array(delta_vision_pose[0:3])
    plans = []
    plans_grasping = []
    plans_grasping2 = []
    plans_guarded = []
    plans_guarded2 = []
    plans_placing = []
    plans_placing2 = []
    graspPos = []
    plan_possible = False
    execution_possible = False
    gripper_opening = 0.0
    grasp_possible = True
    gelsight_data = []
    collision = False
    final_object_pose = None
    rospy.set_param('is_record', False)
    rospy.set_param('is_contact', False)

    def compose_output():
        return {
            'collision': collision,
            'grasp_possible': grasp_possible,
            'plan_possible': plan_possible,
            'execution_possible': execution_possible,
            'gripper_opening': gripper_opening,
            'graspPos': graspPos,
            'gelsight_data': gelsight_data,
            'final_object_pose': final_object_pose
        }

    #########################
    ## Constant parameters ##
    #########################
    UpdistFromBinFast = 0.15  # Margin above object during "move to a location"
    UpdistFromBinGuarded = 0.05  # Margin above object during "move to a location"
    gripperOriHome = [0, 1, 0, 0]
    l1 = 0.0
    l2 = 0.0
    l3 = spatula_tip_to_tcp_dist
    tip_hand_transform = [l1, l2, l3, 0, 0, 0]
    vision_pos[2] = 0.17786528

    ###############################
    ## Picking primitive  ##
    ###############################
    #        ik.visualize_helper.visualize_grasping_proposals(viz_pub, np.asarray([objInput]),  listener, br, False)
    #~Define reference frames
    world_X, world_Y, world_Z, tote_X, tote_Y, tote_Z, tote_pose_pos = ik.helper.reference_frames(
        listener=listener, br=br)

    #~get grasp pose and gripper opening from vision
    if len(objInput) == 12:
        graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_12(
            objInput)
        if not is_regrasp:
            graspPos = graspPos - hand_X * 0.015 * 1

    elif len(objInput) == 7:
        graspPos, hand_X, hand_Y, hand_Z, grasp_width = ik.helper.get_picking_params_from_7(
            objInput, objId, listener, br)

    #frank hack for stationary spatula:
    grasp_width = 0.11
    #~build gripper orientation matrix 3x3
    hand_orient_norm = np.vstack([hand_X, hand_Y, hand_Z])
    hand_orient_norm = hand_orient_norm.transpose()

    #~Grasp relocation script
    hand_orient_quat = ik.helper.mat2quat(hand_orient_norm)
    euler = tf.transformations.euler_from_quaternion(hand_orient_quat)
    theta = euler[2] - np.pi
    colFreePose = collisionFree(graspPos,
                                binId=binId,
                                listener=listener,
                                br=br,
                                finger_opening=grasp_width,
                                safety_margin=0.00,
                                theta=theta)
    graspPos[0:2] = colFreePose[0:2]

    #################################
    ## GENERATE PLANS OF PRIMITIVE ##
    #################################.
    #~ Start from home position
    #        if isExecute:
    #            q_initial = collision_free_placing(binId, listener, isSuction = False, with_object = False, hand_orientation=[0,1,0,0], projected_target_position = bin_pose[0:3], isReturn = True)
    #            scorpion.back()

    #~0. Move to a location above the bin, Rotate gripper to grasp orientation
    pregrasp_targetPosition = graspPos
    pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinFast - 0.1
    plan, qf, plan_possible = generatePlan(q_initial,
                                           pregrasp_targetPosition,
                                           hand_orient_quat,
                                           tip_hand_transform,
                                           'superSaiyan',
                                           plan_name='go_safe_bin')
    if plan_possible:
        plans.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~1. Move to to surface of bin
    pregrasp_targetPosition[2] = bin_pose[2] + UpdistFromBinGuarded - 0.1
    plan, qf, plan_possible = generatePlan(q_initial,
                                           pregrasp_targetPosition,
                                           hand_orient_quat,
                                           tip_hand_transform,
                                           'superSaiyan',
                                           plan_name='go_top_bin')
    if plan_possible:
        plans.append(plan)
        q_initial = qf
    else:
        return compose_output()

    if open_hand:
        #~2. Open gripper

        grasp_plan = EvalPlan('helper.moveGripper(%f, 200)' % grasp_width)
        plans.append(grasp_plan)

        #~ 3. Open spatula
        grasp_plan = EvalPlan('spatula.open()')
        plans.append(grasp_plan)

    grasp_targetPosition = deepcopy(graspPos)
    #        rospy.loginfo('[Picking] Grasp Target %s', grasp_targetPosition)

    #~4. sleep
    sleep_plan = EvalPlan('rospy.sleep(0.2)')
    plans.append(sleep_plan)

    #~5. perform guarded move down
    grasp_targetPosition[2] = bin_pose[2] - rospy.get_param(
        '/tote/height') + 0.015  #~frank hack for safety

    plan, qf, plan_possible = generatePlan(q_initial,
                                           grasp_targetPosition,
                                           hand_orient_quat,
                                           tip_hand_transform,
                                           'faster',
                                           guard_on=WeightGuard(binId,
                                                                threshold=140),
                                           plan_name='guarded_pick')
    if plan_possible:
        plans_guarded.append(plan)
        q_initial = qf
    else:
        return compose_output()

    #~7. Close spatula
    grasp_plan = EvalPlan('spatula.close()')
    plans_guarded2.append(grasp_plan)

    #~8. Close gripper
    grasp_plan = EvalPlan('helper.graspinGripper(%f,%f)' % (800, 30))
    plans_guarded2.append(grasp_plan)

    #~9. sleep
    sleep_plan = EvalPlan('rospy.sleep(0.6)')
    plans_guarded2.append(sleep_plan)

    #~10. Move to a location above the bin
    # plan, qf, plan_possible = generatePlan(q_initial, vision_pos, delta_vision_pose[3:7], tip_hand_transform, 'fastest', plan_name = 'retrieve_object')
    # if plan_possible:
    #     plans_grasping.append(plan)
    #     q_initial = qf
    # else:
    #     return compose_output()

    ################
    ## EXECUTION ###
    ################

    if isExecute and plan_possible:

        executePlanForward(plans, withPause)

        ###We start recording now
        if recorder is not None:
            lasers.start(binId)
            recorder.start_recording(
                action='grasping',
                action_id=str(
                    datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")),
                tote_num=binId,
                frame_rate_ratio=5,
                image_size=-1)

        #Execute guarded move
        rospy.set_param('is_record', True)
        executePlanForward(plans_guarded, withPause)
        if rospy.get_param('is_contact'):
            ik.helper.move_cart(dz=0.005)
        executePlanForward(plans_guarded2, withPause)
        rospy.set_param('is_record', False)

    elif not isExecute:
        execution_possible = plan_possible

    return compose_output()