Ejemplo n.º 1
0
def goToMouth(robotConfig = None,
             binNum = 0, 
             isExecute = True,
             withPause = False):
    ## robotConfig: current time robot configuration
    joint_topic = '/joint_states'
    
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
            
    # plan store
    plans = []
       
    ## initial variable and tcp definitions
    # set tcp
    l2 = 0.47  
    tip_hand_transform = [0, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5)
    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    tcpPosHome = tcpPos
    # set home orientation
    gripperOri = [0, 0.7071, 0, 0.7071]
    
    # move to bin mouth
    distFromShelf = 0.15
    mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
    mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
    targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z]
    q_initial = robotConfig
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print '[goToMouth] move to bin mouth successful'
        plan.visualize()
        plans.append(plan)
        if isExecute:
            pauseFunc(withPause)
            plan.execute()
    else:
        print '[goToMouth] move to bin mouth fail'
        return None
        
    qf = plan.q_traj[-1]
    
    ## open gripper fully
    openGripper()
    
    return plan
Ejemplo n.º 2
0
def scoop(objPose=[1.95, 0.25, 1.4],
          objId=0,
          binNum=3,
          robotConfig=[0, 0, 0, 0, 0, 0],
          shelfPosition=[1.91, 0, -0.50],
          forceThreshold=1,
          isExecute=False):
    ## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ. This is a list, not a matrix
    ## objId: object identifier
    ## robotConfig: current time robot configuration. List of joint angles, in radians
    ## shelfPosition: shelf position in world frame. This is a list.
    ## force threshold: amount of force needed to say we have a grasp

    ## initialize listener rospy
    ## Copy pasta hasta la pasta
    rospy.init_node('listener', anonymous=True)
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    ##hasta la copy la patio
    ## stop pasta

    # shelf variables
    pretensionDelta = 0.00  #how far we push nail down
    lipHeight = 0.035  #height of the lip of the bin

    # plan store
    plans = []  #list of plans generated each plan is an object
    ## get object position (world frame)
    objPosition = getObjCOM(objPose, objId)  #

    ## move gripper to object com outside bin along world y direction and
    ## move the gripper over the lip of the bin
    # set tcp. this is the XYZ roll-pitch-yaw of middle of the wrist with respect to link 6
    l2 = 0.45  #how far tcp is away from link 6
    tip_hand_transform = [
        0, 0, l2, 0, 0, 0
    ]  #something used for planning in drake, I don't get it
    # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    rospy.sleep(0.1)  #.1 second delay so broadcasting works. syncing with ROS
    #broadcasting transformation between tcp and link 6 on ROS
    br.sendTransform(tuple(tip_hand_transform[0:3]),
                     tfm.quaternion_from_euler(*tip_hand_transform[3:6]),
                     rospy.Time.now(), 'tip', "link_6")
    rospy.sleep(0.1)
    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
                                          'map', listener)
    #tcp position as a list
    tcpPos = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]
    tcpPosHome = tcpPos
    # set scoop orientation (rotate wrist)
    scoopOrientation = [0, 0.7071, 0, 0.7071]

    # set first target to move gripper in front of the object and adjust height to middle of bin
    distFromShelf = 0.05
    wristWidth = 0.0725  # this is actually half the wrist width
    (binMouth, binFloorHeight) = getBinMouthAndFloor(distFromShelf, binNum)
    pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map',
                                          listener)
    binMouth = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]
    targetPosition = [
        binMouth[0], objPosition[1], binMouth[2]
    ]  #align the position of the tcp relative to the object before the scoop
    frontOfObjectPtOutOfBin = targetPosition
    q_initial = robotConfig
    #q0 is initial configuration
    #target_tip_pos is where we want the tcp to be
    #target_tip_ori is the tip orientation
    #tip hand transform is see above
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=scoopOrientation,
                 tip_hand_transform=tip_hand_transform)
    plan = planner.plan()
    s = plan.success()  #true if successful
    if s:
        print 'move to COM in y successful'
        print 'tcp at:'
        print(targetPosition)
        plan.visualize()  #display in arvix
        plans.append(plan)
        if isExecute:
            plan.execute()  #this executes the plan for reals
    else:
        print 'move to COM in y fail'
        #after this point, it's just a sequence of points for the plan
    qf = plan.q_traj[-1]  #this is the final configuration of the object
    # set second target, go over the lip of the bin
    interiorLipBin = [0, 0.37,
                      0]  # define over the lip distance in shelf frame
    interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map',
                                              listener)
    deltaX = np.add(-targetPosition[0], interiorLipBin.pose.position.x)
    targetPosition = np.add(targetPosition, [deltaX, 0, 0])
    q_initial = qf
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=scoopOrientation,
                 tip_hand_transform=tip_hand_transform)
    plan = planner.plan()
    s = plan.success()
    if s:
        print 'move to inside the lip success'
        print 'tcp at:'
        print(targetPosition)
        plans.append(plan)
        plan.visualize()
        if isExecute:
            plan.execute()
    else:
        print 'move to inside the lip fail'
    qf = plan.q_traj[-1]

    ## open gripper fully
    openGripper()

    ## push spatula against base of bin (pre-tension)
    binFloorHeight = coordinateFrameTransform([0, 0, binFloorHeight], 'shelf',
                                              'map', listener)
    q_initial = qf
    deltaH = targetPosition[
        2] - binFloorHeight.pose.position.z - pretensionDelta - lipHeight
    targetPosition = np.add(targetPosition, [0, 0, -deltaH + wristWidth])
    scoopOrientation = [0, 0.7071 + 0.11 / 2, 0, 0.7071 - 0.11 / 2]
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=scoopOrientation,
                 tip_hand_transform=tip_hand_transform)
    plan = planner.plan()
    s = plan.success()
    if s:
        print 'push finger against floor of bin success'
        print 'tcp at:'
        print(targetPosition)
        plans.append(plan)
        plan.visualize()
        if isExecute:
            plan.execute()
    else:
        print 'push finger against floor of bin fail'
    qf = plan.q_traj[-1]

    ## perform bin length stroke to middle
    q_initial = qf
    targetPosition = np.add(targetPosition, [0.20, 0, -lipHeight])
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=scoopOrientation,
                 tip_hand_transform=tip_hand_transform)
    plan = planner.plan()
    s = plan.success()
    if s:
        print 'stroke middle of bin success'
        print 'tcp at:'
        print(targetPosition)
        plans.append(plan)
        plan.visualize()
        if isExecute:
            plan.execute()
    else:
        print 'stroke middle of bin fail'
    qf = plan.q_traj[-1]
    plans.append(plan)

    ## perform bin length stroke to end
    q_initial = qf
    targetPosition = np.add(targetPosition, [0.20, 0, 0])
    scoopOrientation = [0, 0.7071 + 0.11 / 4, 0, 0.7071 - 0.11 / 4]
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=scoopOrientation,
                 tip_hand_transform=tip_hand_transform)
    plan = planner.plan()
    s = plan.success()
    if s:
        print 'stroke middle to end of bin success'
        print 'tcp at:'
        print(targetPosition)
        plans.append(plan)
        plan.visualize()
        if isExecute:
            plan.execute()
    else:
        print 'stroke middle to end of bin fail'
    qf = plan.q_traj[-1]
    plans.append(plan)

    ## close gripper
    closeGripper(forceThreshold)

    ## retreat
    #pdb.set_trace() #TRACEEEEE
    #this does each plan backwards so that the hand can leave the bin
    #VERY IMPORTANT
    for numOfPlan in range(0, len(plans)):
        plans[len(plans) - numOfPlan - 1].visualizeBackward()
    if isExecute:
        for numOfPlan in range(0, len(plans)):
            plans[len(plans) - numOfPlan - 1].executeBackward()
Ejemplo n.º 3
0
def manual_cal_1(binNum=0, strPos=None):

    if strPos == None:
        print 'No desired position entered'
        return

    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    joint_topic = '/joint_states'

    # set tcp
    l1 = 0.06345
    l2 = 0.400
    tip_hand_transform = [
        l1, 0, l2, 0, 0, 0
    ]  # to be updated when we have a hand design finalized

    # broadcast frame attached to tcp
    rospy.sleep(0.1)
    br.sendTransform(tuple(tip_hand_transform[0:3]),
                     tfm.quaternion_from_euler(*tip_hand_transform[3:6]),
                     rospy.Time.now(), 'tip', "link_6")
    rospy.sleep(0.1)

    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
                                          'map', listener)
    tcpPos = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]
    tcpPosHome = tcpPos
    orientation = [0.0, 0.7071, 0.0,
                   0.7071]  # set topple orientation (rotate wrist)

    rospy.sleep(0.1)
    br.sendTransform(tuple(tip_hand_transform[0:3]),
                     tfm.quaternion_from_euler(*tip_hand_transform[3:6]),
                     rospy.Time.now(), 'tip', "link_6")
    rospy.sleep(0.1)

    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
                                          'map', listener)
    tcpPos = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]

    print 'TCP is at:', tcpPos

    distFromShelf = 0.05
    mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(
        distFromShelf, binNum)
    mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map',
                                        listener)
    mouthBin = [
        mouthBin.pose.position.x, mouthBin.pose.position.y,
        mouthBin.pose.position.z
    ]

    # define modes here:

    # go home
    if strPos == 'home':
        print 'planning to go home'
        plan = Plan()
        plan.q_traj = [[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745]]
        plan.visualize()
        raw_input('execute?')
        plan.execute()
        print 'done going home'

    # go mouth
    if strPos == 'mouth':
        print 'planning to go to mouth'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = mouthBin
        planner = IK(q0=None,
                     target_tip_pos=targetPt,
                     target_tip_ori=Orientation,
                     tip_hand_transform=tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.visualize()
        s = plan.success()
        if s:
            raw_input('execute?')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to mouth'

    # go mid back bin
    if strPos == 'shelf':
        print 'planning to go to mid back of the bin'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = [mouthBin[0], mouthBin[1], mouthBin[2]]
        planner = IK(q0=None,
                     target_tip_pos=targetPt,
                     target_tip_ori=Orientation,
                     tip_hand_transform=tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.visualize()
        s = plan.success()
        if s:
            raw_input('execute?')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to mid back of the bin'
Ejemplo n.º 4
0
def scoop(objPose = [1.95,0.25,1.4], objId = 0, binNum=3, robotConfig=[0,0,0,0,0,0], shelfPosition = [1.91,0,-0.50], forceThreshold = 1, isExecute = False):
    ## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ. This is a list, not a matrix
    ## objId: object identifier
    ## robotConfig: current time robot configuration. List of joint angles, in radians
    ## shelfPosition: shelf position in world frame. This is a list.
    ## force threshold: amount of force needed to say we have a grasp

    ## initialize listener rospy
    ## Copy pasta hasta la pasta
    rospy.init_node('listener', anonymous=True)
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    ##hasta la copy la patio
    ## stop pasta
    
    # shelf variables
    pretensionDelta = 0.00 #how far we push nail down
    lipHeight = 0.035 #height of the lip of the bin
    
    # plan store
    plans = [] #list of plans generated each plan is an object
    ## get object position (world frame)
    objPosition = getObjCOM(objPose, objId) #
        
    ## move gripper to object com outside bin along world y direction and
    ## move the gripper over the lip of the bin    
    # set tcp. this is the XYZ roll-pitch-yaw of middle of the wrist with respect to link 6
    l2 = 0.45  #how far tcp is away from link 6
    tip_hand_transform = [0, 0, l2, 0,0,0] #something used for planning in drake, I don't get it
    # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    rospy.sleep(0.1) #.1 second delay so broadcasting works. syncing with ROS
    #broadcasting transformation between tcp and link 6 on ROS
    br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
    rospy.sleep(0.1)
    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener) 
    #tcp position as a list
    tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    tcpPosHome = tcpPos
    # set scoop orientation (rotate wrist)
    scoopOrientation = [0, 0.7071, 0, 0.7071]
    
    # set first target to move gripper in front of the object and adjust height to middle of bin
    distFromShelf = 0.05
    wristWidth = 0.0725 # this is actually half the wrist width
    (binMouth, binFloorHeight) = getBinMouthAndFloor(distFromShelf, binNum)
    pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map', listener)
    binMouth=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    targetPosition = [binMouth[0], objPosition[1], binMouth[2]] #align the position of the tcp relative to the object before the scoop
    frontOfObjectPtOutOfBin = targetPosition
    q_initial = robotConfig
    #q0 is initial configuration
    #target_tip_pos is where we want the tcp to be
    #target_tip_ori is the tip orientation
    #tip hand transform is see above
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform)
    plan = planner.plan()
    s = plan.success() #true if successful
    if s:
        print 'move to COM in y successful'
        print 'tcp at:'
        print(targetPosition)
        plan.visualize() #display in arvix
        plans.append(plan)
        if isExecute:
            plan.execute() #this executes the plan for reals
    else:
        print 'move to COM in y fail'
        #after this point, it's just a sequence of points for the plan
    qf = plan.q_traj[-1] #this is the final configuration of the object
    # set second target, go over the lip of the bin
    interiorLipBin = [0,0.37,0] # define over the lip distance in shelf frame
    interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener)
    deltaX = np.add(-targetPosition[0],interiorLipBin.pose.position.x)
    targetPosition = np.add(targetPosition, [deltaX,0,0])
    q_initial = qf
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform)
    plan = planner.plan()
    s = plan.success()
    if s:
        print 'move to inside the lip success'
        print 'tcp at:'
        print(targetPosition)
        plans.append(plan)
        plan.visualize()
        if isExecute:
            plan.execute()
    else:
        print 'move to inside the lip fail'
    qf = plan.q_traj[-1]
    
    ## open gripper fully
    openGripper()
    
    ## push spatula against base of bin (pre-tension)
    binFloorHeight = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener)
    q_initial = qf
    deltaH = targetPosition[2] - binFloorHeight.pose.position.z - pretensionDelta - lipHeight
    targetPosition = np.add(targetPosition, [0,0,-deltaH+wristWidth])
    scoopOrientation = [0, 0.7071+0.11/2, 0, 0.7071-0.11/2]
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform)
    plan = planner.plan()
    s = plan.success()
    if s:
        print 'push finger against floor of bin success'
        print 'tcp at:'
        print(targetPosition)
        plans.append(plan)
        plan.visualize()
        if isExecute:
            plan.execute()
    else:
        print 'push finger against floor of bin fail'
    qf = plan.q_traj[-1]
    
    ## perform bin length stroke to middle
    q_initial = qf
    targetPosition = np.add(targetPosition, [0.20,0,-lipHeight])
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform)
    plan = planner.plan()
    s = plan.success()
    if s:
        print 'stroke middle of bin success'
        print 'tcp at:'
        print(targetPosition)
        plans.append(plan)
        plan.visualize()
        if isExecute:
            plan.execute()
    else:
        print 'stroke middle of bin fail'
    qf = plan.q_traj[-1]
    plans.append(plan)
    
    ## perform bin length stroke to end
    q_initial = qf
    targetPosition = np.add(targetPosition, [0.20,0,0])
    scoopOrientation = [0, 0.7071+0.11/4, 0, 0.7071-0.11/4]
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform)
    plan = planner.plan()
    s = plan.success()
    if s:
        print 'stroke middle to end of bin success'
        print 'tcp at:'
        print(targetPosition)
        plans.append(plan)
        plan.visualize()
        if isExecute:
            plan.execute()
    else:
        print 'stroke middle to end of bin fail'
    qf = plan.q_traj[-1]
    plans.append(plan)
    
    ## close gripper
    closeGripper(forceThreshold)
    
    ## retreat
    #pdb.set_trace() #TRACEEEEE
    #this does each plan backwards so that the hand can leave the bin
    #VERY IMPORTANT
    for numOfPlan in range(0, len(plans)):
        plans[len(plans)-numOfPlan-1].visualizeBackward()
    if isExecute:
        for numOfPlan in range(0, len(plans)):
            plans[len(plans)-numOfPlan-1].executeBackward()
Ejemplo n.º 5
0
def scoop(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1],
          binNum=3,
          objId=0,
          bin_contents=None,
          robotConfig=None,
          shelfPosition=[1.9116, -0.012498, -0.4971],
          forceThreshold=1,
          isExecute=True,
          withPause=True,
          withVisualize=False):
    ## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
    ## objId: object identifier
    ## robotConfig: current time robot configuration
    ## shelfPosition: shelf position in world frame
    ## force threshold: amount fo force needed to say we have a grasp

    setSpeedByName(speedName='faster')

    joint_topic = '/joint_states'
    planSuccess = True
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)

    # shelf variables
    if binNum < 3:
        pretensionDelta = 0.03
    if binNum > 2 and binNum < 6:
        pretensionDelta = 0.009
    if binNum > 5 and binNum < 9:
        pretensionDelta = 0.009
    if binNum > 8:
        pretensionDelta = 0.03
    #pretensionDelta = 0.00
    lipHeight = 0.025

    # plan store
    plans = []
    ## get object position (world frame)
    objPosition = getObjCOM(objPose[0:3], objId)

    ## move gripper to object com outside bin along world y direction and
    ## move the gripper over the lip of the bin
    # set tcp
    tcpXOffset = 0.018
    l2 = 0.47
    tip_hand_transform = [
        tcpXOffset, 0, l2, 0, 0, 0
    ]  # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    pubFrame(br,
             pose=tip_hand_transform,
             frame_id='tip',
             parent_frame_id='link_6',
             npub=5)
    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
                                          'map', listener)
    tcpPos = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]
    tcpPosHome = tcpPos
    # set scoop orientation (rotate wrist)
    scoopOrientation = [0, 0.7071, 0, 0.7071]

    # set first target to move gripper in front of the object and adjust height to middle of bin
    distFromShelf = 0.05
    wristWidth = 0.0725  # this is actually half the wrist width
    (binMouth, binFloorHeight) = getBinMouthAndFloor(distFromShelf, binNum)
    pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map',
                                          listener)
    binMouth = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]
    verticalOffsetLip = 0.00  # we need this so we don't damage the sucker
    wH = 0.075
    targetPosition = [
        binMouth[0], objPosition[1],
        binMouth[2] + tcpXOffset - wH + lipHeight - pretensionDelta
    ]

    ## check to make sure we are inside the bin and not colliding with sides:
    minHeight, maxHeight, leftWall, rightWall = BinBBDims(binNum)
    leftWall = coordinateFrameTransform([leftWall, 0, 0], 'shelf', 'map',
                                        listener)
    leftWall = leftWall.pose.position.y
    rightWall = coordinateFrameTransform([rightWall, 0, 0], 'shelf', 'map',
                                         listener)
    rightWall = rightWall.pose.position.y

    interiorLipBin = [0, 0.40,
                      0]  # define over the lip distance in shelf frame
    interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map',
                                              listener)
    stepOverLip = interiorLipBin.pose.position.x
    fStroke = 0.20
    sStroke = 0.19
    binLengthSafety = 0.015

    if targetPosition[1] + 0.04 > leftWall:
        interiorLipBin = [0, 0.36,
                          0]  # define over the lip distance in shelf frame
        interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf',
                                                  'map', listener)
        stepOverLip = interiorLipBin.pose.position.x
        fStroke = 0.17

    if targetPosition[1] - 0.04 < leftWall:
        interiorLipBin = [0, 0.36,
                          0]  # define over the lip distance in shelf frame
        interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf',
                                                  'map', listener)
        stepOverLip = interiorLipBin.pose.position.x
        fStroke = 0.17

    if targetPosition[1] + binLengthSafety > leftWall:
        targetPosition[1] = leftWall

    if targetPosition[1] - binLengthSafety < rightWall:
        targetPosition[1] = rightWall

    frontOfObjectPtOutOfBin = targetPosition
    q_initial = robotConfig
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=scoopOrientation,
                 tip_hand_transform=tip_hand_transform,
                 joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()  # Plan 0
    if s:
        print '[Scoop] move to COM in y successful'
        #~ print '[Scoop] tcp at:'
        #~ print(targetPosition)
        visualizeFunc(withVisualize, plan)
        plans.append(plan)
    else:
        print '[Scoop] move to COM in y fail'
        return (False, False)

    qf = plan.q_traj[-1]

    ## push spatula against base of bin (pre-tension)
    binFloorHeight = coordinateFrameTransform([0, 0, binFloorHeight], 'shelf',
                                              'map', listener)
    q_initial = qf
    percentTilt = 0.1  # 10 percent
    scoopOrientation = [
        0, 0.7071 * (1 + percentTilt), 0, 0.7071 * (1 - percentTilt)
    ]
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=scoopOrientation,
                 tip_hand_transform=tip_hand_transform,
                 joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()  # Plan 1
    if s:
        print '[Scoop] reorient the hand'
        #~ print '[Scoop] tcp at:'
        #~ print(targetPosition)
        plans.append(plan)
        visualizeFunc(withVisualize, plan)
    else:
        print '[Scoop] reorient the hand fail'
        return (False, False)
    qf = plan.q_traj[-1]

    # set second target, go over the lip of the bin
    deltaX = np.add(-targetPosition[0], stepOverLip)
    targetPosition = np.add(targetPosition, [deltaX, 0, 0])
    q_initial = qf
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=scoopOrientation,
                 tip_hand_transform=tip_hand_transform,
                 joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()  # Plan 2
    if s:
        print '[Scoop] move to inside the lip success'
        #~ print '[Scoop] tcp at:'
        #~ print(targetPosition)
        plans.append(plan)
        visualizeFunc(withVisualize, plan)
    else:
        print '[Scoop] move to inside the lip fail'
        return (False, False)
    qf = plan.q_traj[-1]

    ## perform bin length stroke to middle
    q_initial = qf

    targetPosition = np.add(targetPosition, [fStroke, 0, -lipHeight])
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=scoopOrientation,
                 tip_hand_transform=tip_hand_transform,
                 joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()  # Plan 3
    if s:
        print '[Scoop] stroke middle of bin success'
        #~ print '[Scoop] tcp at:'
        #~ print(targetPosition)
        plans.append(plan)
        visualizeFunc(withVisualize, plan)
    else:
        print '[Scoop] stroke middle of bin fail'
        return (False, False)
    qf = plan.q_traj[-1]
    ## perform bin length stroke to end
    q_initial = qf
    targetPosition = np.add(targetPosition, [sStroke, 0, 0])
    scoopOrientation = [0, 0.7071 + 0.11 / 4, 0, 0.7071 - 0.11 / 4]
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=scoopOrientation,
                 tip_hand_transform=tip_hand_transform,
                 joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()  # Plan 4
    if s:
        print '[Scoop] stroke middle to end of bin success'
        #~ print '[Scoop] tcp at:'
        #~ print(targetPosition)
        plans.append(plan)
        visualizeFunc(withVisualize, plan)
    else:
        print '[Scoop] stroke middle to end of bin fail'
        return (False, False)

    qf = plan.q_traj[-1]

    ## close gripper
    #~ closeGripper(forceThreshold)

    closeGripper(forceThreshold)
    execution_possible = True
    ## execute
    isNotInCollision = True
    for numOfPlan in range(0, len(plans)):
        plans[numOfPlan].visualize()
        if isExecute:
            if numOfPlan == 3:
                openGripper()
            pauseFunc(withPause)
            isNotInCollision = plans[numOfPlan].execute()
            if numOfPlan == 3:
                closeGripper(forceThreshold)
                plans[numOfPlan].executeBackward()
                openGripper()
                plans[numOfPlan].execute()

            if not isNotInCollision:
                planFailNum = numOfPlan
                print '[Scoop] collision detected'
                break
            if numOfPlan == 4:
                closeGripper(forceThreshold)
                rospy.sleep(0.5)
                while True:
                    APCrobotjoints = ROS_Wait_For_Msg(
                        joint_topic, sensor_msgs.msg.JointState).getmsg()
                    q0 = APCrobotjoints.position

                    if len(q0) == 2 or len(q0) == 8:
                        q0 = q0[-2:]  # get last 2
                        break

                gripper_q0 = np.fabs(q0)
                drop_thick = 0.000001  # finger gap =0.002m = .5 mm
                if gripper_q0[0] < drop_thick:
                    print '[Scoop] ***************'
                    print '[Scoop] Could not grasp'
                    print '[Scoop] ***************'
                    execution_possible = False
                else:
                    print '[Scoop] ***************'
                    print '[Scoop] Grasp Successful'
                    print '[Scoop] ***************'
                    execution_possible = True

    if not isNotInCollision:
        for numOfPlan in range(0, len(planFailNum)):
            plans[planFailNum - numOfPlan].executeBackward()

    ## retreat

    for numOfPlan in range(0, len(plans)):
        if withVisualize:
            plans[len(plans) - numOfPlan - 1].visualizeBackward()
        if isExecute:
            pauseFunc(withPause)
            plans[len(plans) - numOfPlan - 1].executeBackward()

    return True, execution_possible
Ejemplo n.º 6
0
def goToMouth(robotConfig=None, binNum=0, isExecute=True, withPause=False):
    ## robotConfig: current time robot configuration
    joint_topic = '/joint_states'

    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)

    # plan store
    plans = []

    ## initial variable and tcp definitions
    # set tcp
    l2 = 0.47
    tip_hand_transform = [
        0, 0, l2, 0, 0, 0
    ]  # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    pubFrame(br,
             pose=tip_hand_transform,
             frame_id='tip',
             parent_frame_id='link_6',
             npub=5)
    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
                                          'map', listener)
    tcpPos = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]
    tcpPosHome = tcpPos
    # set home orientation
    gripperOri = [0, 0.7071, 0, 0.7071]

    # move to bin mouth
    distFromShelf = 0.15
    mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
    mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
    targetPosition = [
        mouthPt.pose.position.x, mouthPt.pose.position.y,
        mouthPt.pose.position.z
    ]
    q_initial = robotConfig
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=gripperOri,
                 tip_hand_transform=tip_hand_transform,
                 joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print '[goToMouth] move to bin mouth successful'
        plan.visualize()
        plans.append(plan)
        if isExecute:
            pauseFunc(withPause)
            plan.execute()
    else:
        print '[goToMouth] move to bin mouth fail'
        return None

    qf = plan.q_traj[-1]

    ## open gripper fully
    openGripper()

    return plan
def manual_cal_1(binNum = 0, strPos = None):
    
    if strPos  == None:
        print 'No desired position entered'
        return
        
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    joint_topic = '/joint_states'
    
    # set tcp
    l1 = 0.06345
    l2 = 0.400  
    tip_hand_transform = [l1, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
    
    # broadcast frame attached to tcp
    rospy.sleep(0.1)
    br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
    rospy.sleep(0.1)
    
    # get position of the tcp in world frame
    pose_world  = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos      =[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    tcpPosHome  = tcpPos
    orientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist)
    
    rospy.sleep(0.1)
    br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
    rospy.sleep(0.1)
    
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    
    print 'TCP is at:', tcpPos
    
    distFromShelf = 0.05
    mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(distFromShelf, binNum)
    mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map', listener)
    mouthBin = [mouthBin.pose.position.x, mouthBin.pose.position.y, mouthBin.pose.position.z]
    
    # define modes here:
    
    # go home
    if strPos  == 'home':
        print 'planning to go home'
        plan = Plan()
        plan.q_traj = [[0, -0.3959, 0.58466, 0.03,   0.1152, -0.1745]]
        plan.visualize()
        raw_input('execute?')
        plan.execute()
        print 'done going home'
    
    # go mouth
    if strPos  == 'mouth':
        print 'planning to go to mouth'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = mouthBin
        planner = IK(q0 = None, target_tip_pos = targetPt, 
                     target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform, 
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.visualize()
        s = plan.success()
        if s:
            raw_input('execute?')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to mouth'
    
    # go mid back bin
    if strPos  == 'shelf':
        print 'planning to go to mid back of the bin'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = [mouthBin[0], mouthBin[1], mouthBin[2]] 
        planner = IK(q0 = None, target_tip_pos = targetPt, 
                     target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform, 
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.visualize()
        s = plan.success()
        if s:
            raw_input('execute?')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to mid back of the bin'
def main(argv=None):
    rospy.init_node('calib', anonymous=True)
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    
    filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final_apc.json'
    robot_touch_pts = readFromJson(filename)
    
    binNums = [0,2,3,5,6,8,9,11]
    nbin = len(binNums)
    diff_sum = 0
    # shelf_pose/x: 1.92421
    # shelf_pose/y: -0.0124050312627
    # shelf_pose/z: -0.513553368384
    ###########################################
    print 'floor of the shelves'
    for i in binNums:
        pose = coordinateFrameTransform(robot_touch_pts[i][4], '/map', '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i)  # a bin at row i
        diff = binFloorHeight - touch_pt[2]  # examine difference in z
        print 'bin',i,'(expected - robot_touch)=', diff
        diff_sum += diff
    print 'avg(diff)=', diff_sum/nbin
    offset_x = diff_sum/nbin
    print 'Please move the shelf in z by', offset_x
    print '\twhich is', -0.513553368384 - offset_x
    
    ###########################################
    diff_sum_left = 0
    print 'left wall of the shelves'
    for i in binNums:
        pose = coordinateFrameTransform(robot_touch_pts[i][1], '/map', '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (leftWall, rightWall) = getBinSideWalls(i)  # a bin at row i
        diff = leftWall - touch_pt[0]  # examine difference in x
        print 'left wall: bin',i,'(expected - robot_touch)=', diff
        diff_sum_left += diff
    print 'avg(diff) left wall=', diff_sum_left/nbin
    
    
    ###########################################
    diff_sum_right = 0
    print 'right wall of the shelves'
    for i in binNums:
        pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map', '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (leftWall, rightWall) = getBinSideWalls(i)  # a bin at row i
        diff = rightWall - touch_pt[0]  # examine difference in x
        print 'right wall: bin',i,'(expected - robot_touch)=', diff
        diff_sum_right += diff
    print 'avg(diff) right wall=', diff_sum_right/nbin
    
    offset_y = (diff_sum_left/nbin+diff_sum_right/nbin) / 2
    print 'Please move the shelf in y by', offset_y
    print '\twhich is', 0.0124050312627 - offset_y
    
    ###########################################
    diff_sum = 0
    print 'lip to floor of the shelves'
    for i in binNums:
        pose = coordinateFrameTransform(robot_touch_pts[i][0], '/map', '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i)  # a bin at row i
        diff = binFloorHeight - touch_pt[2]  # examine difference in z
        print 'row',i,'(bin floor - robot_touch)=', diff
        diff_sum += diff
    print 'avg(diff)=', diff_sum/nbin

    offset_y = (diff_sum_left/nbin+diff_sum_right/nbin) / 2
    print 'Please move the shelf in y by', offset_y
    print '\twhich is', 0.0124050312627 - offset_y



    ##########################################  front
    diff_sum_front = 0
    print 'front of the shelves'
    for i in binNums:
        pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map', '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (leftWall, rightWall) = getBinSideWalls(i)  # a bin at row i
        diff = rightWall - touch_pt[1]  # examine difference in y
        print 'right front wall: bin',i,'(expected - robot_touch)=', diff
        diff_sum_front += diff
    print 'avg(diff) right wall=', diff_sum_front/nbin
    offset_x = diff_sum_front/nbin
    print 'Please move the shelf in x by', offset_x
    print '\twhich is', 1.92421 - offset_x  #- 0.87/2 
Ejemplo n.º 9
0
def push_back(objPose=[1.95,0.25,1.4,0,0,0,1],
            binNum=4,
            objId = 'crayola_64_ct',
            bin_contents = ['paper_mate_12_count_mirado_black_warrior','crayola_64_ct','expo_dry_erase_board_eraser'],
            robotConfig = None,
            grasp_range_lim=0.11,
            fing_width=0.06,
            isExecute = True,
            withPause = True):
    
    #~ *****************************************************************
    obj_dim=get_obj_dim(objId)
    obj_dim=np.array(obj_dim)
    #~ *****************************************************************

    # # DEFINE HAND WIDTH#########################
    hand_width=0.186
    
    #~ *****************************************************************
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    
    (shelf_position, shelf_quaternion) = lookupTransform("map", "shelf", listener)
    # print  shelf_position, shelf_quaternion 
    
    #~ *****************************************************************

    # Convert xyx quat to tranformation matrix for Shelf frame
    #~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7])

    shelf_pose_tfm_list=matrix_from_xyzquat(shelf_position,shelf_quaternion)
    shelf_pose_tfm=np.array(shelf_pose_tfm_list)

    shelf_pose_orient=shelf_pose_tfm[0:3,0:3]
    #~ print '[PushBack] shelf_pose_orient'
    #~ print shelf_pose_orient
    
    shelf_pose_pos=shelf_pose_tfm[0:3,3]
    #~ print '[PushBack] shelf_pose_pos'
    #~ print shelf_pose_pos
        
    #Normalized axes of the shelf frame
    shelf_X=shelf_pose_orient[:,0]/la.norm(shelf_pose_orient[:,0])
    shelf_Y=shelf_pose_orient[:,1]/la.norm(shelf_pose_orient[:,1])
    shelf_Z=shelf_pose_orient[:,2]/la.norm(shelf_pose_orient[:,2])
    
    #~ *****************************************************************
    
    # Convert xyx quat to tranformaation matrix for Object frame
    
    obj_pose_tfm_list=matrix_from_xyzquat(objPose[0:3], objPose[3:7])
    obj_pose_tfm=np.array(obj_pose_tfm_list)
    
    obj_pose_orient=obj_pose_tfm[0:3,0:3]
    obj_pose_pos=obj_pose_tfm[0:3,3]
    #~ print '[PushBack] obj_pose_orient'
    #~ print obj_pose_orient
    
    #Normalized axes of the object frame
    obj_X=obj_pose_orient[:,0]/la.norm(obj_pose_orient[:,0])
    #~ print '[PushBack] obj_X'
    #~ print obj_X
    
    obj_Y=obj_pose_orient[:,1]/la.norm(obj_pose_orient[:,1])
    #~ print '[PushBack] obj_Y'
    #~ print obj_Y
    
    obj_Z=obj_pose_orient[:,2]/la.norm(obj_pose_orient[:,2])
    #~ print '[PushBack] obj_Z'
    #~ print obj_Z
    
    #Normalized object frame
    obj_pose_orient_norm=np.vstack((obj_X,obj_Y,obj_Z))
    obj_pose_orient_norm=obj_pose_orient_norm.transpose()
    #~ print '[PushBack] obj_pose_orient_norm'
    #~ print obj_pose_orient_norm
    
    #~ *****************************************************************
    
    # We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin)
    # Find projection of object axes on Y axis of the shelf frame
    
    proj_vecY=np.dot(shelf_Y,obj_pose_orient_norm)
    #~ print '[PushBack] proj'
    #~ print proj_vecY
    
    max_proj_valY,hand_norm_dir=np.max(np.fabs(proj_vecY)), np.argmax(np.fabs(proj_vecY))
    
    if proj_vecY[hand_norm_dir]>0:
        hand_norm_vec=-obj_pose_orient_norm[:,hand_norm_dir]
    else:
        hand_norm_vec=obj_pose_orient_norm[:,hand_norm_dir]
        
    #~ print '[PushBack] hand_norm_vec'
    #~ print hand_norm_vec
    
    #Find angle of the edge of the object 
    Cos_angle_made_with_shelf_Y=max_proj_valY/(la.norm(shelf_Y)*la.norm(hand_norm_vec));
                        
    angle_to_shelfY=np.arccos(Cos_angle_made_with_shelf_Y)*180/np.pi
    
    #~ print'angle_to_shelfY'
    #~ print angle_to_shelfY
    #Find object dimension along hand normal axis
            
    obj_dim_along_hand_norm=obj_dim[hand_norm_dir]
    # print '[PushBack] dim along hand norm'
    # print obj_dim_along_hand_norm
    #~ *****************************************************************
    
    #Find projection of object axes on X axis of the shelf frame    
    #To find out which object frame is lying closer to the X axis of the shelf
        
    proj_vecX=np.dot(shelf_X,obj_pose_orient_norm)
        
    max_proj_valX,fing_axis_dir=np.max(np.fabs(proj_vecX)), np.argmax(np.fabs(proj_vecX))
    
    if proj_vecX[fing_axis_dir]>0:
        fing_axis_vec=obj_pose_orient_norm[:,fing_axis_dir]
    else:
        fing_axis_vec=-obj_pose_orient_norm[:,fing_axis_dir]
        
    #~ print '[PushBack] fing_axis_vec'
    #~ print fing_axis_vec
        
    #Find object dimension along the finger axis
    obj_dim_along_fingAxis=obj_dim[fing_axis_dir]
    # print '[PushBack] obj_dim_along_fingAxis'
    # print obj_dim_along_fingAxis
    
    #~ *****************************************************************
    
    #Find projection of object axes on Z axis of the shelf frame    
    #To find out which object frame is lying closer to the Z axis of the shelf
        
    proj_vecZ=np.dot(shelf_Z,obj_pose_orient_norm)

    max_proj_valZ,Zaxis_dir=np.max(np.fabs(proj_vecZ)), np.argmax(np.fabs(proj_vecZ))

    Zaxis_vec=obj_pose_orient_norm[:,Zaxis_dir]
        
    #~ print '[PushBack] Zaxis_vec'
    #~ print Zaxis_vec
    #Find object dimension along the finger axis, shelf Z
    obj_dim_along_ZAxis=obj_dim[Zaxis_dir]
    
    hand_Y=np.cross(hand_norm_vec,fing_axis_vec)
    
    diag_dist=la.norm(np.array([obj_dim_along_hand_norm,obj_dim_along_fingAxis]))
    # print'req obj diag dist=', diag_dist
    
    #~ *****************************************************************
    # Find the maximum distance to which we will push
    
    other_diag=np.zeros(len(bin_contents))
    
    for num_bin_contents in range(0, len(bin_contents)):
        if bin_contents[num_bin_contents] != objId:
            temp_obj_dim=get_obj_dim(objId)
            other_diag[num_bin_contents]=la.norm(np.array([temp_obj_dim[0],temp_obj_dim[1]]))
    
    max_diag,max_obj_ID=np.max(np.fabs(other_diag)), np.argmax(np.fabs(other_diag))
    # print '[PushBack] max_obj_ID',max_diag
    # print '[PushBack] max_obj_ID', bin_contents[max_obj_ID]
    #*******************************************************************
    
    bin_inner_cnstr=get_bin_inner_cnstr()
    
    #*******************************************************************
    #Check if we can do left push
    diag_factor=0.75
    
    obj_left_edge=obj_pose_pos[1]+diag_factor*diag_dist/2.0
    
    binLeftWall=bin_inner_cnstr[binNum][1]
    binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener)
    
    binRightWall=bin_inner_cnstr[binNum][0]
    binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener)
    
    left_gap=binLeftWall_world.pose.position.y-obj_left_edge
    # print '[PushBack] left_gap=', left_gap
    
    fing_clearance=0.010 # It's like 5 mm on both sides of finger
    
    bin_width=binLeftWall_world.pose.position.y-binRightWall_world.pose.position.y
    
    tolerance_in_push=0.020
    if left_gap+diag_dist/2.0-tolerance_in_push>bin_width:
        print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin'
        return (False,False)
            
    if left_gap > fing_width+fing_clearance:
        left_push=True
        print '[PushBack] left push is possible'
    else:
        left_push=False
        print '[PushBack] left push is NOT possible'
        
    # check if we can do right push 
    
    obj_right_edge=obj_pose_pos[1]-diag_factor*diag_dist/2.0
    
    right_gap=obj_right_edge-binRightWall_world.pose.position.y
    # print '[PushBack] right_gap', right_gap
    fing_clearance=0.010 # It's like 5 mm on both sides of finger
    
    if right_gap+diag_dist/2.0-tolerance_in_push>bin_width:
        print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin'
        return (False,False)
        
    if right_gap > fing_width+fing_clearance:
        right_push=True
        print '[PushBack] right push is possible'
    else:
        right_push=False
        print '[PushBack] right push is NOT possible'
    #*******************************************************************
    #Find Push Points for left push
    bin_face_pos,binFloorHeight=getBinMouthAndFloor(0.0, binNum)
    bin_face_pos_world = coordinateFrameTransform(bin_face_pos, 'shelf', 'map', listener)
    
    tcp_Z_off=0.005 #lower down from center of the bin
    push_tcp_Z_shelfFrame=((bin_inner_cnstr[binNum][4]+bin_inner_cnstr[binNum][5])/2.0)-tcp_Z_off
    
    push_tcp_Z_WorldFrame = coordinateFrameTransform([0,0,push_tcp_Z_shelfFrame], 'shelf', 'map', listener)
    
    push_tcp_Z=push_tcp_Z_WorldFrame.pose.position.z
    
    push_side_off=1.0
    if left_push:
        
        left_pushPt_X=bin_face_pos_world.pose.position.x+0.05
        left_pushPt_Y=binLeftWall_world.pose.position.y-push_side_off*left_gap
        # left_Y_lim=binLeftWall_world.pose.position.y-
        left_pushPt_Z=deepcopy(push_tcp_Z)
        
        left_push_Pt=np.array([left_pushPt_X,left_pushPt_Y,left_pushPt_Z])
        print '[PushBack] expected left push clearance=',binLeftWall_world.pose.position.y-left_pushPt_Y-fing_width/2.0
    #Find Push Points for right push
    
    if right_push:
        
        right_pushPt_X=bin_face_pos_world.pose.position.x+0.05
        right_pushPt_Y=binRightWall_world.pose.position.y+push_side_off*right_gap
        right_pushPt_Z=deepcopy(push_tcp_Z)
        
        
        right_push_Pt=np.array([right_pushPt_X,right_pushPt_Y,right_pushPt_Z])        
        
        print '[PushBack] expected right push clearance=',right_pushPt_Y-binRightWall_world.pose.position.y-fing_width/2.0
    #Execute push trajectories
    
    #~ *************************************************************
    # set spatual down orientation (rotate wrist)
    spatula_down_orient = [0, 0.7071, 0, 0.7071]
    
    push_orient=spatula_down_orient
    
    push_hand_opening=0.100
    
    joint_topic = '/joint_states'
    #~ *************************************************************
    
    #Execure left push trajectory
    
    if left_push:
        # plan store
        left_plans = []
        
        # set tcp (same as that set in generate dictionary)
        l1=0.0
        l2=0.0
        l3 = 0.47  
        tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized
        # broadcast frame attached to tcp
        pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5)
        
        #~ *************************************************************
        
        # GO TO MOUTH OF THE BIN
        
        q_initial = robotConfig
        # move to bin mouth
        distFromShelf = 0.1
        mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
        mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
        targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z]
        gripperOri=[0, 0.7071, 0, 0.7071]
        
        pubFrame(br, pose = targetPosition+gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5)

        planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
        plan = planner.plan()
        plan.setSpeedByName('faster')
        s = plan.success()
        if s:
            print '[PushBack] move to bin mouth in push-back code successful (left push)'
            left_plans.append(plan) # Plan 0
        else:
            print '[PushBack] move to bin mouth in push-back code fail (left push)'
            return (False,False)
        
        qf = plan.q_traj[-1]
    
        q_initial = qf
        #################### CLOSE THE GRIPPER GRASP #################
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        left_plans.append(grasp_plan)
        #~ *************************************************************
        
        ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######

        # set push_tcp
        push_l1=0.0
        push_l2=0.0
        
        push_l3 = 0.47  
        push_tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized
        # broadcast frame attached to grasp_tcp
        pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10)
        
        distFromShelf = -0.015
        InbinPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
        InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
        prepush_targetPosition = [InbinPt.pose.position.x, left_push_Pt[1] , InbinPt.pose.position.z] #matching world_Y of first push pt
        
        pubFrame(br, pose=prepush_targetPosition+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10)

        planner = IK(q0 = q_initial, target_tip_pos = prepush_targetPosition, target_tip_ori = push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic)
        plan = planner.plan()
        s = plan.success()
        plan.setSpeedByName('fast')
        s = plan.success()
        if s:
            print '[PushBack] move inside bin in push orient successful'
            #~ print '[PushBack] tcp at:'
            #~ print(pregrasp_targetPosition)
            #~ plan.visualize()
            left_plans.append(plan) # Plan 1
            #~ if isExecute:
                #~ pauseFunc(withPause)
                #~ plan.execute()
        else:
            print '[PushBack] move inside bin in push orient fail'
            return (False,False)
        
        qf = plan.q_traj[-1]
    
        q_initial = qf
        
        #~ *************************************************************
        
        ############## OPEN THE GRIPPER To PUSH###############
        
        # Open the gripper to dim calculated for pushing
        print '[PushBack] hand opening to'
        print push_hand_opening*1000.0
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
        left_plans.append(grasp_plan)
        
        #~ *************************************************************
        
        ############  Execute the push trajectory ##############
        
        back_bin_X=bin_face_pos_world.pose.position.x+0.43
        push_stop_worldX=back_bin_X-max_diag-0.010

        left_pushStop_Pt_pos=deepcopy(left_push_Pt)
        left_pushStop_Pt_pos[0]=push_stop_worldX
        
        #Push until the end
    
        planner = IK(q0 = q_initial, target_tip_pos = left_pushStop_Pt_pos, target_tip_ori = push_orient, 
             joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform)
        plan = planner.plan()
        plan.setSpeedByName('fast')
        s = plan.success()
        if s:
            print '[PushBack] start point push traj successful'
            left_plans.append(plan) # Plan 1
        else:
            print '[PushBack] start point push traj fail'
            return (False,False)
        
        qf = plan.q_traj[-1]
        q_initial = qf
        
        #################### CLOSE THE GRIPPER GRASP #################
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        left_plans.append(grasp_plan)
        
        #~ *************************************************************
        # #################### EXECUTE FORWARD #################### 
        
        for numOfPlan in range(0, len(left_plans)):
            if isExecute:
                left_plans[numOfPlan].visualize()
                pauseFunc(withPause)
                left_plans[numOfPlan].execute()
        
        # #################### RETREAT #################### 
        
        for numOfPlan in range(0, len(left_plans)):
            if isExecute:
                left_plans[numOfPlan].visualizeBackward()
                pauseFunc(withPause)
                left_plans[len(left_plans)-numOfPlan-1].executeBackward()
        
    #*******************************************************************
    
    if right_push: 
        # plan store
        right_plans = []
        
        # set tcp (same as that set in generate dictionary)
        l1=0.0
        l2=0.0
        l3 = 0.47  
        tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized
        # broadcast frame attached to tcp
        pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5)
        
        #~ *************************************************************
        
        # GO TO MOUTH OF THE BIN
        
        q_initial = robotConfig
        # move to bin mouth
        distFromShelf = 0.1
        mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
        mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
        targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z]
        gripperOri=[0, 0.7071, 0, 0.7071]
        
        pubFrame(br, pose = targetPosition+gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5)

        planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
        plan = planner.plan()
        plan.setSpeedByName('faster')
        s = plan.success()
        if s:
            print '[PushBack] move to bin mouth in push-back code successful (right push)'
            right_plans.append(plan) # Plan 0
        else:
            print '[PushBack] move to bin mouth in push-back code fail (right push)'
            return (False,False)
        
        qf = plan.q_traj[-1]
    
        q_initial = qf
        #################### CLOSE THE GRIPPER GRASP #################
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        right_plans.append(grasp_plan)
        #~ *************************************************************
        
        ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######

        # set push_tcp
        push_l1=0.0
        push_l2=0.0
        
        push_l3 = 0.47  
        push_tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized
        # broadcast frame attached to grasp_tcp
        pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10)
        
        distFromShelf = -0.015
        InbinPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
        InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
        prepush_targetPosition = [InbinPt.pose.position.x, right_push_Pt[1] , InbinPt.pose.position.z] #matching world_Y of first push pt
        
        pubFrame(br, pose=prepush_targetPosition+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10)

        planner = IK(q0 = q_initial, target_tip_pos = prepush_targetPosition, target_tip_ori = push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic)
        plan = planner.plan()
        s = plan.success()
        plan.setSpeedByName('fast')
        s = plan.success()
        if s:
            print '[PushBack] move inside bin in push orient successful'
            #~ print '[PushBack] tcp at:'
            #~ print(pregrasp_targetPosition)
            #~ plan.visualize()
            right_plans.append(plan) # Plan 1
            #~ if isExecute:
                #~ pauseFunc(withPause)
                #~ plan.execute()
        else:
            print '[PushBack] move inside bin in push orient fail'
            return (False,False)
        
        qf = plan.q_traj[-1]
    
        q_initial = qf
        
        #~ *************************************************************
        
        ############## OPEN THE GRIPPER To PUSH###############
        
        # Open the gripper to dim calculated for pushing
        print '[PushBack] hand opening to'
        print push_hand_opening*1000.0
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
        right_plans.append(grasp_plan)
        
        #~ *************************************************************
        
        ############  Execute the push trajectory ##############
        
        back_bin_X=bin_face_pos_world.pose.position.x+0.42
        push_stop_worldX=back_bin_X-max_diag-0.010

        right_pushStop_Pt_pos=deepcopy(right_push_Pt)
        right_pushStop_Pt_pos[0]=push_stop_worldX
        
        #Push until the end
    
        planner = IK(q0 = q_initial, target_tip_pos = right_pushStop_Pt_pos, target_tip_ori = push_orient, 
             joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform)
        plan = planner.plan()
        plan.setSpeedByName('fast')
        s = plan.success()
        if s:
            print '[PushBack] start point push traj successful'
            right_plans.append(plan) # Plan 1
        else:
            print '[PushBack] start point push traj fail'
            return (False,False)
        
        qf = plan.q_traj[-1]
        q_initial = qf
        
        #################### CLOSE THE GRIPPER GRASP #################
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        right_plans.append(grasp_plan)
        
        #~ *************************************************************
        # #################### EXECUTE FORWARD #################### 
        
        for numOfPlan in range(0, len(right_plans)):
            if isExecute:
                right_plans[numOfPlan].visualize()
                pauseFunc(withPause)
                right_plans[numOfPlan].execute()
        
        # #################### RETREAT #################### 
        
        for numOfPlan in range(0, len(right_plans)):
            if isExecute:
                right_plans[numOfPlan].visualizeBackward()
                pauseFunc(withPause)
                right_plans[len(right_plans)-numOfPlan-1].executeBackward()
    
    if right_push or left_push:
        push_possible=True
    else:
        push_possible=False
        
    return(push_possible, False)
Ejemplo n.º 10
0
def main(argv=None):
    rospy.init_node('calib', anonymous=True)
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    
    filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib.json'
    robot_touch_floor_pts = readFromJson(filename)
    num_rows = 4
    diff_sum = 0
    print 'floor of the shelves'
    for i in range(num_rows):
        pose = coordinateFrameTransform(robot_touch_floor_pts[i], '/map', '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i*3)  # a bin at row i
        diff = binFloorHeight - touch_pt[2]  # examine difference in z
        print 'row',i,'(expected - robot_touch)=', diff
        diff_sum += diff
    print 'avg(diff)=', diff_sum/num_rows
    
    filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_left_side.json'
    robot_touch_left_pts = readFromJson(filename)
    num_rows = 4
    diff_sum_left = 0
    print 'left wall of the shelves'
    for i in range(num_rows):
        pose = coordinateFrameTransform(robot_touch_left_pts[i], '/map', '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (leftWall, rightWall) = getBinSideWalls(i*3)  # a bin at row i
        diff = leftWall - touch_pt[0]  # examine difference in x
        print 'left wall: row',i,'(expected - robot_touch)=', diff
        diff_sum_left += diff
    print 'avg(diff) left wall=', diff_sum_left/num_rows
    
    filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_right_side.json'
    robot_touch_right_pts = readFromJson(filename)
    num_rows = 4
    diff_sum_right = 0
    print 'right wall of the shelves'
    for i in range(num_rows):
        pose = coordinateFrameTransform(robot_touch_right_pts[i], '/map', '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (leftWall, rightWall) = getBinSideWalls(i*3)  # a bin at row i
        diff = rightWall - touch_pt[0]  # examine difference in x
        print 'right wall: row',i,'(expected - robot_touch)=', diff
        diff_sum_right += diff
    print 'avg(diff) right wall=', diff_sum_right/num_rows
    
    filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_right.json'
    robot_touch_lip_pts = readFromJson(filename)
    num_rows = 4
    diff_sum = 0
    print 'lip to floor of the shelves'
    for i in range(num_rows):
        pose = coordinateFrameTransform(robot_touch_lip_pts[i], '/map', '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (mouthposition, binFloorHeight) = getBinMouthAndFloor(0, i*3)  # a bin at row i
        diff = binFloorHeight - touch_pt[2]  # examine difference in z
        print 'row',i,'(bin floor - robot_touch)=', diff
        diff_sum += diff
    print 'avg(diff)=', diff_sum/num_rows
Ejemplo n.º 11
0
def topple(objPose = [1.55,0.25,1.1,0,0,0,0],
            binNum=0,
            objId = 0, 
            bin_contents = None,
            robotConfig = None,
            shelfPosition = [1.9116,-0.012498,-0.4971],
            forceThreshold = 1,
            binDepthReach = 0.40,
            isExecute = True,
            withPause = True,
            withVisualize = True):
    ## objPose: world position and orientation frame attached to com of object
    ## objId: object identifier
    ## robotConfig: current time robot configuration
    ## shelfPosition: shelf position in world frame
    ## force threshold: amount fo force needed to say we have a grasp
    planSuccess = True
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    
    setSpeedByName(speedName = 'faster')
    joint_topic = '/joint_states'
        
    # shelf variables
    pretensionDelta = 0.00
    lipHeight = 0.035
    
    temp_bias = 0.00 # TO-DO remove this:
    
    # plan store
    plans = []
    
    ## get object position (world frame)
    objPosition = getObjCOM(objPose[0:3], objId)
        
    ## move gripper to object com outside bin along world y direction and
    ## move the gripper over the lip of the bin    
    # set tcp
    l1 = 0.018
    l2 = 0.470  
    tip_hand_transform = [l1, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
    
    # broadcast frame attached to tcp
    pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5)
    
    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    tcpPosHome = tcpPos
    toppleOrientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist)
    closeGripper(forceThreshold) # if gripper open close it
    
    # set first target to move gripper in front of the object and adjust height to middle of bin
    distFromShelf   = 0.05
    wristWidth      = 0.0725 # this is actually half the wrist width
    (binMouth, binFloorHeight)  = getBinMouthAndFloor(distFromShelf, binNum)
    pose_world                  = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map', listener)
    binFloorHeight              = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener)
    binFloorHeight              = binFloorHeight.pose.position.z
    binMouth                    = [pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    
    # set offset past COM
    # offset is 1/4 of object's height + half finger width
    # note: (COM height - bin floor height) is half of the object's height
    fingerWidth     = 0.06
    offsetFinger    = fingerWidth/2
    targetHeight    = (objPosition[2] - binFloorHeight)*2 # to be changed to be from an object
    
    offsetEffect = targetHeight*3/4

    offset = offsetEffect
    
    vertToppleHeight = binFloorHeight+offset-temp_bias
    
    ## bounding box constraint
    minHeight, maxHeight, leftWall, rightWall = BinBBDims(binNum) # shelf frame
    # trying to topple an object that is too tall?
    maxHeight = coordinateFrameTransform([0,0,maxHeight], 'shelf', 'map', listener)
    
    #pdb.set_trace()
    
    targetPosition = [binMouth[0], objPosition[1], vertToppleHeight]
    binDepthReach = 0.3 
    
    binHeightSaftey = 0.01
    if vertToppleHeight + binHeightSaftey > maxHeight.pose.position.z:
        fingerLength = 0.26
        binDepth = 0.42
        binDepthReach = binDepth - fingerLength + 0.08
        toppleHeightWhenTall = binMouth[2] + 0.03
        targetPosition = [binMouth[0], objPosition[1], toppleHeightWhenTall]
        print '[Topple] Object too tall, topple will not go all the way in'
    
    # trying to topple an object that is too short?
    minHeight = coordinateFrameTransform([0,0,minHeight], 'shelf', 'map', listener)
    
    closeGripper(forceThreshold)
    
    if vertToppleHeight - binHeightSaftey < minHeight.pose.position.z:
        print '[Topple] Object too short, resetting topple height'
        toppleHeightWhenShort = minHeight.pose.position.z + binHeightSaftey 
        targetPosition = [binMouth[0], objPosition[1], toppleHeightWhenShort]
        gripper.move(80,100)
    
    # too far left or right?
    leftWall = coordinateFrameTransform([leftWall,0,0], 'shelf', 'map', listener)
    leftWall = leftWall.pose.position.y
    rightWall = coordinateFrameTransform([rightWall,0,0], 'shelf', 'map', listener)
    rightWall = rightWall.pose.position.y
    
    binLengthSafety = 0.02
    horizontalSaftey = 0.01
    if targetPosition[1] + binLengthSafety > leftWall:
        targetPosition[1] = leftWall - horizontalSaftey
    
    if targetPosition[1] - binLengthSafety < rightWall:
        targetPosition[1] = rightWall + horizontalSaftey

    frontOfObjectPtOutOfBin = targetPosition
    q_initial = robotConfig
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    
    effect = 'topple'
    if s and effect == 'topple':
        print '[Topple] move to COM in y and above COM in z successful'
    elif s and effect == 'slide':
        print '[Topple] move to COM in y and below COM in z successful'
    else:
        print '[Topple] move to COM in y and below COM in z successful' #Default is slide, made three cases for flexibility
    
    if s:
        visualizeFunc(withVisualize, plan)
        plans.append(plan)
    else:
        print '[Topple] move to COM in y and above COM in z fail'
        planSuccess = False
        
    qf = plan.q_traj[-1]
        
    ## perform bin length stroke to end
    q_initial = qf
    targetPosition = np.add(targetPosition, [binDepthReach,0,0])
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print '[Topple] stroke to end of bin success'
        visualizeFunc(withVisualize, plan)
        plans.append(plan)
    else:
        print '[Topple] stroke to end of bin fail'
        planSuccess = False
    qf = plan.q_traj[-1]
    
    ## close gripper
    closeGripper(forceThreshold)
    
    ## retreat
    # go back along stroke
    q_initial = qf
    targetPosition = frontOfObjectPtOutOfBin
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print '[Topple] stroke to bin start success'
        visualizeFunc(withVisualize, plan)
        plans.append(plan)
    else:
        print '[Topple] stroke to bin start fail'
        planSuccess = False
    qf = plan.q_traj[-1]
    
    # go back to initial tcp position
    q_initial = qf
    targetPosition = frontOfObjectPtOutOfBin
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print '[Topple] return to mouth success'
        visualizeFunc(withVisualize, plan)
        plans.append(plan)
    else:
        print '[Topple] return to mouth fail'
        planSuccess = False
    
    if not planSuccess:
        return False, False
    
    ## execute
    #~ for numOfPlan in range(0, len(plans)):
        #~ plans[numOfPlan].visualize()
        #~ if isExecute:
            #~ pauseFunc(withPause)
            #~ plans[numOfPlan].execute()
    
    ## execute
    isNotInCollision = True
    for numOfPlan in range(0, len(plans)):
        plans[numOfPlan].visualize()
        if isExecute:
            pauseFunc(withPause)
            isNotInCollision = plans[numOfPlan].execute()
            if not isNotInCollision:
                print '[Topple] collision detected'
                planFailNum = numOfPlan
                break
                
    if not isNotInCollision:
        for numOfPlan in range(0, len(planFailNum)):
            plans[planFailNum-numOfPlan].executeBackward()
            
    ## retreat
    
    for numOfPlan in range(0, len(plans)):
        if withVisualize:
            plans[len(plans)-numOfPlan-1].visualizeBackward()
        if isExecute:
            pauseFunc(withPause)
            plans[len(plans)-numOfPlan-1].executeBackward()
            
    return True, False
Ejemplo n.º 12
0
def main(argv=None):
    rospy.init_node('calib', anonymous=True)
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)

    filename = os.environ[
        'APC_BASE'] + '/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final_apc.json'
    robot_touch_pts = readFromJson(filename)

    binNums = [0, 2, 3, 5, 6, 8, 9, 11]
    nbin = len(binNums)
    diff_sum = 0
    # shelf_pose/x: 1.92421
    # shelf_pose/y: -0.0124050312627
    # shelf_pose/z: -0.513553368384
    ###########################################
    print 'floor of the shelves'
    for i in binNums:
        pose = coordinateFrameTransform(robot_touch_pts[i][4], '/map',
                                        '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (mouthposition,
         binFloorHeight) = getBinMouthAndFloor(0, i)  # a bin at row i
        diff = binFloorHeight - touch_pt[2]  # examine difference in z
        print 'bin', i, '(expected - robot_touch)=', diff
        diff_sum += diff
    print 'avg(diff)=', diff_sum / nbin
    offset_x = diff_sum / nbin
    print 'Please move the shelf in z by', offset_x
    print '\twhich is', -0.513553368384 - offset_x

    ###########################################
    diff_sum_left = 0
    print 'left wall of the shelves'
    for i in binNums:
        pose = coordinateFrameTransform(robot_touch_pts[i][1], '/map',
                                        '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (leftWall, rightWall) = getBinSideWalls(i)  # a bin at row i
        diff = leftWall - touch_pt[0]  # examine difference in x
        print 'left wall: bin', i, '(expected - robot_touch)=', diff
        diff_sum_left += diff
    print 'avg(diff) left wall=', diff_sum_left / nbin

    ###########################################
    diff_sum_right = 0
    print 'right wall of the shelves'
    for i in binNums:
        pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map',
                                        '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (leftWall, rightWall) = getBinSideWalls(i)  # a bin at row i
        diff = rightWall - touch_pt[0]  # examine difference in x
        print 'right wall: bin', i, '(expected - robot_touch)=', diff
        diff_sum_right += diff
    print 'avg(diff) right wall=', diff_sum_right / nbin

    offset_y = (diff_sum_left / nbin + diff_sum_right / nbin) / 2
    print 'Please move the shelf in y by', offset_y
    print '\twhich is', 0.0124050312627 - offset_y

    ###########################################
    diff_sum = 0
    print 'lip to floor of the shelves'
    for i in binNums:
        pose = coordinateFrameTransform(robot_touch_pts[i][0], '/map',
                                        '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (mouthposition,
         binFloorHeight) = getBinMouthAndFloor(0, i)  # a bin at row i
        diff = binFloorHeight - touch_pt[2]  # examine difference in z
        print 'row', i, '(bin floor - robot_touch)=', diff
        diff_sum += diff
    print 'avg(diff)=', diff_sum / nbin

    offset_y = (diff_sum_left / nbin + diff_sum_right / nbin) / 2
    print 'Please move the shelf in y by', offset_y
    print '\twhich is', 0.0124050312627 - offset_y

    ##########################################  front
    diff_sum_front = 0
    print 'front of the shelves'
    for i in binNums:
        pose = coordinateFrameTransform(robot_touch_pts[i][2], '/map',
                                        '/shelf', listener)
        touch_pt = pose2list(pose.pose)
        (leftWall, rightWall) = getBinSideWalls(i)  # a bin at row i
        diff = rightWall - touch_pt[1]  # examine difference in y
        print 'right front wall: bin', i, '(expected - robot_touch)=', diff
        diff_sum_front += diff
    print 'avg(diff) right wall=', diff_sum_front / nbin
    offset_x = diff_sum_front / nbin
    print 'Please move the shelf in x by', offset_x
    print '\twhich is', 1.92421 - offset_x  #- 0.87/2
def manual_cal_2(binNum = 0, endBinNum = 11):
    
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    joint_topic = '/joint_states'
    
    # set tcp
    l1 = 0.06345
    l2 = 0.400  
    tip_hand_transform = [l1, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
    
    moveDistHorizontal = 0.09 #we could do 1 more cm (without the lip we could move about 4cm more)
    moveDistVertical = 0.08 #we could do 1 more cm
    
    #set Speed
    ik.ik.setSpeedByName('faster')
    
    # broadcast frame attached to tcp
    #~ rospy.sleep(0.1)
    #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
    #~ rospy.sleep(0.1)
    
    # get position of the tcp in world frame
    pose_world  = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos      =[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    tcpPosHome  = tcpPos
    orientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist)
    
    #~ rospy.sleep(0.1)
    #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
    #~ rospy.sleep(0.1)
    
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    
    print 'TCP is at:', tcpPos
    
    print 'planning to go home'
    
    # home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
    # planner = IKJoint(target_joint_pos=home_joint_pose)
    # plan = planner.plan()
    # plan.visualize()
    raw_input('execute?')
    # plan.execute()
    goToHome.goToHome()
    print 'done going home'
    
    calibBins = []
    for x in range(binNum, endBinNum+1):
        calibPts = []
        
        if x == 1 or x == 4 or x == 7 or x == 10:
            calibBins.append([])
            continue
        
        distFromShelf = 0.05
        mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(distFromShelf, x)
        mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map', listener)
        mouthBin = [mouthBin.pose.position.x, mouthBin.pose.position.y, mouthBin.pose.position.z]
        
        epsilon = 0.016
        distFromShelf = distFromShelf - epsilon
               
        # go to mouth of bin
        print 'planning to go to mouth but a little out'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = mouthBin
        planner = IK(q0 = None, target_tip_pos = targetPt, 
                     target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform, 
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.visualize()
        s = plan.success()
        if s:
            raw_input('execute?')
            ik.ik.setSpeedByName('fast')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to mouth'
        
        # go to the mouth of the bin but actually inside
        print 'planning to go to start position'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]] 
        planner = IK(q0 = None, target_tip_pos = targetPt, 
                     target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform, 
                     joint_topic=joint_topic)
        plan = planner.plan()
        s = plan.success()
        if s:
            plan.visualize()
            raw_input('execute?')
            ik.ik.setSpeedByName('fast')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to start position'
        
        
        
        OrientationList = []
        targetPtList = []
        captionList = []
        ntouch = 5
        
        # go down
        captionList.append('go down')
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]-moveDistVertical] 
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)
        
        # go left
        captionList.append('go left')
        Orientation = [0.5, 0.5, 0.5, 0.5]
        targetPt = [mouthBin[0]+distFromShelf, mouthBin[1]+moveDistHorizontal, mouthBin[2]] 
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)
        
        # go right
        captionList.append('go right')
        Orientation = [0.5, -0.5, 0.5, -0.5]
        targetPt = [mouthBin[0]+distFromShelf, mouthBin[1]-moveDistHorizontal, mouthBin[2]] 
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)
        
        # go up
        captionList.append('go up')
        Orientation = [0.7071, 0, 0.7071, 0]
        targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]+moveDistVertical] 
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)
        
        # go to mid back of the bin
        captionList.append('go mid back')
        Orientation = [0, 0.7071+0.11/4, 0, 0.7071-0.11/4]
        targetPt = [mouthBin[0]+0.30, mouthBin[1], mouthBin[2]-0.08] 
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)
        
        for touchInd in range(ntouch):
            print captionList[touchInd]
            targetPt = targetPtList[touchInd]
            Orientation = OrientationList[touchInd]
            
            planner = IK(q0 = None, target_tip_pos = targetPt, 
                         target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform, 
                         joint_topic=joint_topic)
            plan = planner.plan()
            s = plan.success()
            if s:
                plan.visualize()
                raw_input('execute?')
                ik.ik.setSpeedByName('fast')
                plan.execute()
            if not s:
                print 'could not find plan'
            print 'done going down, TELEOP TIME:'
            raw_input('when done with teleop hit enter: ')
            #~ rospy.sleep(0.1)
            #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
            #~ rospy.sleep(0.1)
            t = listener.getLatestCommonTime('calib', 'map')
            (calibPoseTrans, calibPoseRot) = listener.lookupTransform('map','calib',t)
            calibPose = list(calibPoseTrans) + list(calibPoseRot)
            #~ calibPose = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
            #~ calibPtsTemp = [calibPose.pose.position.x,calibPose.pose.position.y,calibPose.pose.position.z]
            calibPts.append(calibPose)
            raw_input('done recording position, hit enter to go back')
            
            if s:
                plan.executeBackward()
            else:
                print 'Please teleop back'
                raw_input('done?')
                
                
        
        calibBins.append(calibPts)
        
        filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final.json'
        with open(filename, 'w') as outfile:
            json.dump(calibBins, outfile)
        
        print 'planning to go home'
        home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
        planner = IKJoint(target_joint_pos=home_joint_pose)
        plan = planner.plan()
        plan.visualize()
        raw_input('execute?')
        ik.ik.setSpeedByName('faster')
        plan.execute()
        print 'done going home'
    
        
    print 'dumped to file ', filename
Ejemplo n.º 14
0
def push_rotate(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1],
                binNum=4,
                objId='crayola_64_ct',
                bin_contents=None,
                robotConfig=None,
                grasp_range_lim=0.11,
                fing_width=0.06,
                isExecute=True,
                withPause=True):

    #~ *****************************************************************
    obj_dim = get_obj_dim(objId)
    obj_dim = np.array(obj_dim)
    #~ *****************************************************************

    # # DEFINE HAND WIDTH#########################
    hand_width = 0.186

    #~ *****************************************************************
    ## initialize listener rospy
    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()
    rospy.sleep(0.2)

    (shelf_position,
     shelf_quaternion) = lookupTransform("map", "shelf", listener)
    # print  shelf_position, shelf_quaternion

    #~ *****************************************************************

    # Convert xyx quat to tranformation matrix for Shelf frame
    #~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7])

    shelf_pose_tfm_list = matrix_from_xyzquat(shelf_position, shelf_quaternion)
    shelf_pose_tfm = np.array(shelf_pose_tfm_list)

    shelf_pose_orient = shelf_pose_tfm[0:3, 0:3]
    #~ print 'shelf_pose_orient'
    #~ print shelf_pose_orient

    shelf_pose_pos = shelf_pose_tfm[0:3, 3]
    #~ print 'shelf_pose_pos'
    #~ print shelf_pose_pos

    #Normalized axes of the shelf frame
    shelf_X = shelf_pose_orient[:, 0] / la.norm(shelf_pose_orient[:, 0])
    shelf_Y = shelf_pose_orient[:, 1] / la.norm(shelf_pose_orient[:, 1])
    shelf_Z = shelf_pose_orient[:, 2] / la.norm(shelf_pose_orient[:, 2])

    #~ *****************************************************************

    # Convert xyx quat to tranformaation matrix for Object frame

    obj_pose_tfm_list = matrix_from_xyzquat(objPose[0:3], objPose[3:7])
    obj_pose_tfm = np.array(obj_pose_tfm_list)

    obj_pose_orient = obj_pose_tfm[0:3, 0:3]
    obj_pose_pos = obj_pose_tfm[0:3, 3]
    #~ print 'obj_pose_orient'
    #~ print obj_pose_orient

    #Normalized axes of the object frame
    obj_X = obj_pose_orient[:, 0] / la.norm(obj_pose_orient[:, 0])
    #~ print 'obj_X'
    #~ print obj_X

    obj_Y = obj_pose_orient[:, 1] / la.norm(obj_pose_orient[:, 1])
    #~ print 'obj_Y'
    #~ print obj_Y

    obj_Z = obj_pose_orient[:, 2] / la.norm(obj_pose_orient[:, 2])
    #~ print 'obj_Z'
    #~ print obj_Z

    #Normalized object frame
    obj_pose_orient_norm = np.vstack((obj_X, obj_Y, obj_Z))
    obj_pose_orient_norm = obj_pose_orient_norm.transpose()
    #~ print 'obj_pose_orient_norm'
    #~ print obj_pose_orient_norm

    #~ *****************************************************************

    # We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin)
    # Find projection of object axes on Y axis of the shelf frame

    proj_vecY = np.dot(shelf_Y, obj_pose_orient_norm)
    #~ print 'proj'
    #~ print proj_vecY

    max_proj_valY, hand_norm_dir = np.max(np.fabs(proj_vecY)), np.argmax(
        np.fabs(proj_vecY))

    if proj_vecY[hand_norm_dir] > 0:
        hand_norm_vec = -obj_pose_orient_norm[:, hand_norm_dir]
    else:
        hand_norm_vec = obj_pose_orient_norm[:, hand_norm_dir]

    #~ print 'hand_norm_vec'
    #~ print hand_norm_vec

    #Find angle of the edge of the object
    Cos_angle_made_with_shelf_Y = max_proj_valY / (la.norm(shelf_Y) *
                                                   la.norm(hand_norm_vec))

    angle_to_shelfY = np.arccos(Cos_angle_made_with_shelf_Y) * 180.0 / np.pi

    #~ print'angle_to_shelfY'
    #~ print angle_to_shelfY
    #Find object dimension along hand normal axis

    obj_dim_along_hand_norm = obj_dim[hand_norm_dir]
    print '[PushRotate] dim along hand norm', obj_dim_along_hand_norm
    #~ *****************************************************************

    #Find projection of object axes on X axis of the shelf frame
    #To find out which object frame is lying closer to the X axis of the shelf

    proj_vecX = np.dot(shelf_X, obj_pose_orient_norm)

    max_proj_valX, fing_axis_dir = np.max(np.fabs(proj_vecX)), np.argmax(
        np.fabs(proj_vecX))

    if proj_vecX[fing_axis_dir] > 0:
        fing_axis_vec = obj_pose_orient_norm[:, fing_axis_dir]
    else:
        fing_axis_vec = -obj_pose_orient_norm[:, fing_axis_dir]

    #~ print 'fing_axis_vec'
    #~ print fing_axis_vec

    #Find object dimension along the finger axis
    obj_dim_along_fingAxis = obj_dim[fing_axis_dir]
    print '[PushRotate] obj_dim_along_fingAxis:', obj_dim_along_fingAxis

    #~ *****************************************************************

    #Find projection of object axes on Z axis of the shelf frame
    #To find out which object frame is lying closer to the Z axis of the shelf

    proj_vecZ = np.dot(shelf_Z, obj_pose_orient_norm)

    max_proj_valZ, Zaxis_dir = np.max(np.fabs(proj_vecZ)), np.argmax(
        np.fabs(proj_vecZ))

    Zaxis_vec = obj_pose_orient_norm[:, Zaxis_dir]

    #~ print 'Zaxis_vec'
    #~ print Zaxis_vec
    #Find object dimension along the finger axis, shelf Z
    obj_dim_along_ZAxis = obj_dim[Zaxis_dir]

    hand_Y = np.cross(hand_norm_vec, fing_axis_vec)

    #~ *****************************************************************
    ####################  DECISION STRATEGIES #########################

    bin_inner_cnstr = get_bin_inner_cnstr()

    proj_handNorm_ShelfX = np.dot(hand_norm_vec, shelf_X)

    right_push = False
    left_push = False

    # print 'obj_dim_along_hand_norm', obj_dim_along_hand_norm
    # print 'obj_dim_along_fingAxis', obj_dim_along_fingAxis
    #
    # print 'proj_handNorm_ShelfX'
    # print proj_handNorm_ShelfX

    if obj_dim_along_hand_norm > obj_dim_along_fingAxis:
        #~ print 'proj_vecY'
        #~ print proj_vecY
        #~ print proj_vecY[hand_norm_dir]
        #~
        #~ print 'proj_vecX'
        #~ print proj_vecX
        #~ print proj_vecX[fing_axis_dir]

        #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0:
        #~ #Do left push
        #~ Ypush_mult=1
        #~ print 'Left Push'
        #~ else:
        #~ #Do right push
        #~ Ypush_mult=-1
        #~ print 'right Push'
        if proj_handNorm_ShelfX > 0:
            #Do left push
            Ypush_mult = 1
            left_push = True
            print '[PushRotate] Left Push'
        else:
            #Do right push
            Ypush_mult = -1
            print '[PushRotate] right Push'
            right_push = True
        push_offset = (
            obj_dim_along_hand_norm / 2.0
        )  #(Instead pf pushing on edge we will push 5 mm inside)

    else:

        #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0:
        #~ #Do right push
        #~ Ypush_mult=-1
        #~ print 'right Push'
        #~ else:
        #~ #Do left push
        #~ Ypush_mult=1
        #~ print 'Left Push'
        if proj_handNorm_ShelfX > 0:
            #Do right push
            Ypush_mult = -1
            print '[PushRotate] right Push'
            right_push = True
        else:
            #Do left push
            Ypush_mult = 1
            print '[PushRotate] Left Push'
            left_push = True
        push_offset = (obj_dim_along_fingAxis / 2.0)

    num_intm_points = 5
    push_x_intm = np.zeros(num_intm_points + 1)
    push_y_intm = np.zeros(num_intm_points + 1)

    backWall_shelf = bin_inner_cnstr[binNum][1]
    backWall_world = coordinateFrameTransform([0, backWall_shelf, 0], 'shelf',
                                              'map', listener)

    back_check = backWall_world.pose.position.x - obj_dim_along_hand_norm / 2.0

    binRightWall = bin_inner_cnstr[binNum][0]
    binLeftWall = bin_inner_cnstr[binNum][1]

    binRightWall_world = coordinateFrameTransform([binRightWall, 0, 0],
                                                  'shelf', 'map', listener)
    binLeftWall_world = coordinateFrameTransform([binLeftWall, 0, 0], 'shelf',
                                                 'map', listener)

    if left_push:
        sideWall_check = binRightWall_world.pose.position.y + (
            fing_width / 2.0) + (obj_dim_along_fingAxis)

    if right_push:
        sideWall_check = binLeftWall_world.pose.position.y - (
            fing_width / 2.0) - (obj_dim_along_fingAxis)

    for i in range(num_intm_points + 1):
        push_x_intm[i] = obj_pose_pos[0] + (push_offset) * np.sin(
            ((90 / num_intm_points) * i * np.pi) / 180.0)

        if push_x_intm[i] > back_check:
            push_x_intm[i] = back_check

        push_y_intm[i] = obj_pose_pos[1] + (Ypush_mult * push_offset) * np.cos(
            ((90 / num_intm_points) * i * np.pi) / 180.0)

        if left_push:
            if push_y_intm[i] < sideWall_check:
                push_y_intm[i] = sideWall_check
                print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper'

        if right_push:
            if push_y_intm[i] > sideWall_check:
                push_y_intm[i] = sideWall_check
                print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper'

    push_x_intm = np.array(push_x_intm)

    #~ print'push_x_intm'
    #~ print push_x_intm

    push_y_intm = np.array(push_y_intm)

    #*******************************************************************
    #Move the hand to the center of the bin and open the hand based on the height of the object

    bin_mid_pos, binFloorHeight = getBinMouthAndFloor(0.0, binNum)
    binFloorHeight_world = coordinateFrameTransform([0, 0, binFloorHeight],
                                                    'shelf', 'map', listener)

    bin_mid_pos_world = coordinateFrameTransform(bin_mid_pos, 'shelf', 'map',
                                                 listener)

    #*******************************************************************
    tcp_Z_off = 0.005
    push_tcp_Z_shelfFrame = (
        (bin_inner_cnstr[binNum][4] + bin_inner_cnstr[binNum][5]) /
        2.0) - tcp_Z_off

    push_tcp_Z_WorldFrame = coordinateFrameTransform(
        [0, 0, push_tcp_Z_shelfFrame], 'shelf', 'map', listener)

    push_tcp_Z = push_tcp_Z_WorldFrame.pose.position.z

    push_z_intm = (push_tcp_Z) * np.ones(push_x_intm.size)

    push_series_pos = np.vstack((push_x_intm, push_y_intm, push_z_intm))
    push_series_pos = push_series_pos.transpose()

    #~ print 'push_series_pos'
    #~ print push_series_pos

    #~ print push_series_pos[0,:]
    #~ print push_series_pos[-1,:]

    push_possible = True
    #******************************************************************
    fing_push_pt = 0.2 * obj_dim_along_ZAxis + binFloorHeight_world.pose.position.z

    blade_tip_TCP_off = 0.038  # dist between finger inner end and spatual edge

    push_hand_opening = 2 * (push_tcp_Z - blade_tip_TCP_off - fing_push_pt)

    if push_hand_opening > grasp_range_lim:
        push_hand_opening = grasp_range_lim

    # Spatula finger should not hit the lip of the bin while pushing

    lip_Z_shelf = bin_inner_cnstr[binNum][4]
    lip_Z_world = coordinateFrameTransform([0, 0, lip_Z_shelf], 'shelf', 'map',
                                           listener)

    #~ lip_off=.025
    #~ bin_lip_Z=binFloorHeight_world.pose.position.z+lip_off

    max_allowed_hand_opening_out = 2 * (push_tcp_Z -
                                        lip_Z_world.pose.position.z)
    max_allowed_hand_opening = max_allowed_hand_opening_out - 0.036  # 0.036 is diff between inside and outside of the finger/finger mount surface

    if push_hand_opening > max_allowed_hand_opening:
        print '[PushRotate] reducing hand opening bcaz it may hit the lip of the bin'
        push_hand_opening = max_allowed_hand_opening

    #~ print 'push_hand_opening'
    #~ print push_hand_opening

    #~ #hand should not hit the side walls
    #~ binRightWall,binLeftWall=find_shelf_walls(binNum)
    #~
    #~ binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener)
    #~ binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener)

    side_wall_clearance = 0.0

    print '[PushRotate] binRightWall_world.pose.position.y', binRightWall_world.pose.position.y
    print '[PushRotate] right_hand_edge=', (push_y_intm[0] - fing_width -
                                            side_wall_clearance)

    print '[PushRotate] binLeftWall_world.pose.position.y', binLeftWall_world.pose.position.y
    print '[PushRotate] left_hand_edge=', (push_y_intm[0] + fing_width +
                                           side_wall_clearance)

    if push_y_intm[
            0] - fing_width - side_wall_clearance < binRightWall_world.pose.position.y:
        print '[PushRotate] Hand will hit the right wall, Push rotate not possible'
        push_possible = False
    if push_y_intm[
            0] + fing_width + side_wall_clearance > binLeftWall_world.pose.position.y:
        print '[PushRotate] Hand will hit the left wall, push rotate not possible'
        push_possible = False

    #~ *************************************************************
    # set spatual down orientation (rotate wrist)
    spatula_down_orient = [0, 0.7071, 0, 0.7071]

    push_orient = deepcopy(spatula_down_orient)

    #~ *************************************************************

    if push_possible == True:
        #Now we know where we wan to move TCP
        #Let's do robot motion planning now

        joint_topic = '/joint_states'

        # plan store
        plans = []

        # set tcp (same as that set in generate dictionary)
        l1 = 0.0
        l2 = 0.0
        l3 = 0.47
        tip_hand_transform = [
            l1, l2, l3, 0, 0, 0
        ]  # to be updated when we have a hand design finalized
        # broadcast frame attached to tcp
        pubFrame(br,
                 pose=tip_hand_transform,
                 frame_id='tip',
                 parent_frame_id='link_6',
                 npub=5)

        #~ *************************************************************

        # GO TO MOUTH OF THE BIN

        q_initial = robotConfig
        # move to bin mouth
        distFromShelf = 0.1
        mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
        mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
        targetPosition = [
            mouthPt.pose.position.x, mouthPt.pose.position.y,
            mouthPt.pose.position.z
        ]
        gripperOri = [0, 0.7071, 0, 0.7071]

        pubFrame(br,
                 pose=targetPosition + gripperOri,
                 frame_id='target_pose',
                 parent_frame_id='link_6',
                 npub=5)

        planner = IK(q0=q_initial,
                     target_tip_pos=targetPosition,
                     target_tip_ori=gripperOri,
                     tip_hand_transform=tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.setSpeedByName('faster')
        s = plan.success()
        if s:
            print '[PushRotate] move to bin mouth in push-rotate code successful'
            #plan.visualize()
            plans.append(plan)  # Plan 0
            #if isExecute:
            #    pauseFunc(withPause)
            #   plan.execute()
        else:
            print '[PushRotate] move to bin mouth in push-rotate code fail'
            return False

        qf = plan.q_traj[-1]

        q_initial = qf
        #################### CLOSE THE GRIPPER GRASP #################
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        plans.append(grasp_plan)
        #~ moveGripper(0.1,100)
        #~ *************************************************************

        ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######

        # set push_tcp
        push_l1 = 0.0  #0.035
        push_l2 = -Ypush_mult * (
            fing_width / 2.0
        )  # This should depend on the righ or left push conditions

        push_l3 = 0.47
        push_tip_hand_transform = [
            l1, l2, l3, 0, 0, 0
        ]  # to be updated when we have a hand design finalized
        # broadcast frame attached to grasp_tcp
        pubFrame(br,
                 pose=push_tip_hand_transform,
                 frame_id='push_tip',
                 parent_frame_id='link_6',
                 npub=10)

        #~ q_initial = robotConfig
        # move just inside the bin with push orientation and open the hand suitable to push

        distFromShelf = -0.01
        InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
        InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
        prepush_targetPosition = [
            InbinPt.pose.position.x, push_series_pos[0, 1],
            InbinPt.pose.position.z
        ]  #matching world_Y of first push pt

        print '[PushRotate] prepush_targetPosition=', prepush_targetPosition

        pubFrame(br,
                 pose=prepush_targetPosition + push_orient,
                 frame_id='target_pose',
                 parent_frame_id='map',
                 npub=10)

        planner = IK(q0=q_initial,
                     target_tip_pos=prepush_targetPosition,
                     target_tip_ori=push_orient,
                     tip_hand_transform=push_tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.setSpeedByName('slow')
        s = plan.success()
        if s:
            print '[PushRotate] move inside bin in push orient successful'
            #~ print 'tcp at:'
            #~ print(pregrasp_targetPosition)
            #~ plan.visualize()
            plans.append(plan)  # Plan 1
            #~ if isExecute:
            #~ pauseFunc(withPause)
            #~ plan.execute()
        else:
            print '[PushRotate] move inside bin in push orient fail'
            return False

        qf = plan.q_traj[-1]

        q_initial = qf

        #~ *************************************************************

        ############## OPEN THE GRIPPER To PUSH ###############

        # Open the gripper to dim calculated for pushing
        print '[PushRotate] hand opening to'
        print push_hand_opening * 1000.0
        #~ moveGripper(push_hand_opening,100)
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
        plans.append(grasp_plan)

        #~ *************************************************************

        ############  Execute the push trajectory ##############
        #~ push_rotate_traj = Plan()
        #~ push_rotate_traj.q_traj = qf

        for i in range(0, push_x_intm.size):
            pushpt_pos = push_series_pos[i, :].transpose()
            pubFrame(br,
                     pose=pushpt_pos.tolist() + push_orient,
                     frame_id='target_pose',
                     parent_frame_id='map',
                     npub=10)

            #~ pdb.set_trace()
            # planner = IK(q0 = q_initial, target_tip_pos = graspPT_pose_pos, target_tip_ori = push_orient,
            # joint_topic=joint_topic, tip_hand_transform=tip_hand_transform, straightness = 0.3, inframebb = inframebb)
            planner = IK(q0=q_initial,
                         target_tip_pos=pushpt_pos,
                         target_tip_ori=push_orient,
                         joint_topic=joint_topic,
                         tip_hand_transform=push_tip_hand_transform)
            plan = planner.plan()

            plan.setSpeedByName('slow')
            s = plan.success()
            if s:
                print '[PushRotate] point inside push traj successful'
                #~ print 'tcp at:'
                #~ print(pregrasp_targetPosition)
                #~ plan.visualize()
                plans.append(plan)  # Plan 1
                #~ if isExecute:
                #~ pauseFunc(withPause)
                #~ plan.execute()
            else:
                print '[PushRotate] point inside push traj fail'
                return False

            qf = plan.q_traj[-1]
            q_initial = qf
        #~ *************************************************************

        #~ if isExecute:
        #~ pauseFunc(withPause)
        #~ push_rotate_traj.execute()

        #################### CLOSE THE GRIPPER GRASP #################
        #~ moveGripper(0.0,100)
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        plans.append(grasp_plan)
        #~ *************************************************************

        # #################### EXECUTE FORWARD ####################

        for numOfPlan in range(0, len(plans)):
            if isExecute:
                plans[numOfPlan].visualize()
                pauseFunc(withPause)
                plans[numOfPlan].execute()

        # #################### RETREAT ####################

        for numOfPlan in range(0, len(plans)):
            if isExecute:
                plans[len(plans) - numOfPlan - 1].visualizeBackward()
                pauseFunc(withPause)
                plans[len(plans) - numOfPlan - 1].executeBackward()

    #*******************************************************************

        print '[PushRotate] push_successful'

    return (push_possible, False)
Ejemplo n.º 15
0
def goToBin(binNum = 0,
            robotConfig = None,
            objectiveBinPos = [1.2,0,0.6], 
            isExecute = True,
            withPause = False,
            withSuction = False,
            counter = 0):
    ## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
    ## objId: object identifier
    ## robotConfig: current time robot configuration
    ## shelfPosition: shelf position in world frame
    ## force threshold: amount fo force needed to say we have a grasp
    planSuctionSuccess = False
    planGraspSuccess = False
    
    joint_topic = '/joint_states'
    
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
            
    # plan store
    plans = []
       
    ## initial variable and tcp definitions
    # set tcp
    l2 = 0.47  
    tip_hand_transform = [0, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    # rospy.sleep(0.1)
    # br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
    # rospy.sleep(0.1)
    
    pubFrame(br, pose=tip_hand_transform, frame_id='target_pose', parent_frame_id='map', npub=1)
    
    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    tcpPosHome = tcpPos
    # set scoop orientation (rotate wrist)
    if not withSuction:
        gripperOri = [0, 0.7071, 0, 0.7071]
    if withSuction:
        print '[goToBin] with suck-down primitive, defining new end orientation of gripper'
        gripperOri = [pose_world.pose.orientation.x,pose_world.pose.orientation.y,pose_world.pose.orientation.z,pose_world.pose.orientation.w]
    
    # move back 10 cms to avoid object with shelf collision
    distAwayFromBin = 0.1
    targetPosition = [tcpPos[0]-distAwayFromBin, tcpPos[1], tcpPos[2]]
    q_initial = robotConfig
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print '[goToBin] move away from bin by %d cm successful' % distAwayFromBin
        print '[goToBin] tcp at:', targetPosition
        plan.visualize()
        plans.append(plan)
        if isExecute:
            pauseFunc(withPause)
            plan.execute()
    else:
        print '[goToBin] move away from bin fail'
        return False
        
    qf = plan.q_traj[-1]
    
    if binNum > 8:
        #plan = Plan()
        #plan.q_traj = [[0, -0.3959, 0.58466, 0.03,   0.1152, -0.1745]]  # should use IKJoint
        planner = IKJoint(q0 = qf, target_joint_pos=[0, -0.3959, 0.58466, 0.03,   0.1152, -0.1745])
        plan = planner.plan()
        
        print '[goToBin] going home because risky to go straight to objective bin'
        plan.visualize()
        qf = plan.q_traj[-1]
        if isExecute:
            pauseFunc(withPause)
            plan.execute()
            
    # move above objective bin, approx
    use_JointPos = False
    if use_JointPos:
        planner = IKJoint(q0 = qf, target_joint_pos=[0, -0.2953, 0.4462, 0,   0.8292, 1.5707])
        plan = planner.plan()
        #plan = Plan()
        #plan.q_traj = [[0, -0.2953, 0.4462, 0,   0.8292, 1.5707]]  # should use IKJoint
        s = True
    else:
        if not withSuction:
            gripperOri = [0.7071,0.7071,0,0]
            locationAboveBinCOM = [0,0,0.15] # define over the lip distance in shelf frame
            targetPosition = [objectiveBinPos[0]+locationAboveBinCOM[0],objectiveBinPos[1]+locationAboveBinCOM[1],objectiveBinPos[2]+locationAboveBinCOM[2]]
            q_initial = qf
            planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
            plan = planner.plan()
            s = plan.success()
            if s:
                print '[goToBin] move to above bin success'
                planSuctionSuccess = True
                plans.append(plan)
                plan.visualize()
                qf = plan.q_traj[-1]
                if isExecute:
                    pauseFunc(withPause)
                    plan.execute()
            else:
                return False
                    
        if withSuction:
            bin_xyz, baseHeight = getBinMouthAndFloor(0.20, 10)
            bin_xyz = coordinateFrameTransform(bin_xyz, 'shelf', 'map', listener)
            targetPosition = [bin_xyz.pose.position.x,bin_xyz.pose.position.y,bin_xyz.pose.position.z]
            q_initial = qf
            planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
            plan = planner.plan()
            s = plan.success()
            print '[goToBin] setting joints for suction'
            while True:
                APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg() 
                q0 = APCrobotjoints.position
                if len(q0) < 6:
                    continue
                else:
                    break
            if q0[5]<0:
                stackAngle = -(counter-1)*1.6*3.1415/180+9*3.1415/180
                possible_start_config=[stackAngle, 0.610, 1.0277, 0.0, -1.4834, 3.1415-2*math.pi]
            else:
                stackAngle = -(counter-1)*1.6*3.1415/180+9*3.1415/180
                possible_start_config=[stackAngle, 0.610, 1.0277, 0.0, -1.4834, 3.1415]
                
            # plan_n = Plan()
            # plan_n.q_traj=[possible_start_config]
            planner = IKJoint(target_joint_pos=possible_start_config)
            plan_n = planner.plan()
            plan_n.visualize()
            qf = plan_n.q_traj[-1]
            if isExecute:
                pauseFunc(withPause)
                plan_n.execute()
    
    
    ## go down and release object
    if not withSuction:
        q_initial = qf
        objectiveBinPos[1] = (counter-1)*0.036-0.2
        targetPosition = [objectiveBinPos[0],objectiveBinPos[1],objectiveBinPos[2]]
        planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
        plan = planner.plan()
        s = plan.success()
        if s:
            print '[goToBin] go to xyz of bin with vertical bias success'
            plans.append(plan)
            plan.visualize()
            if isExecute:
                pauseFunc(withPause)
                plan.execute()
        else:
            print '[goToBin] go to xyz of bin with vertical bias fail'
        qf = plan.q_traj[-1]
        openGripper()
    
    ## open gripper fully
    # rospy.sleep(0.5)
    # openGripper()
    rospy.sleep(0.5)
    suction.stop()
    return True
Ejemplo n.º 16
0
def manual_cal_2(binNum=0, endBinNum=11):

    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    joint_topic = '/joint_states'

    # set tcp
    l1 = 0.06345
    l2 = 0.400
    tip_hand_transform = [
        l1, 0, l2, 0, 0, 0
    ]  # to be updated when we have a hand design finalized

    moveDistHorizontal = 0.09  #we could do 1 more cm (without the lip we could move about 4cm more)
    moveDistVertical = 0.08  #we could do 1 more cm

    #set Speed
    ik.ik.setSpeedByName('faster')

    # broadcast frame attached to tcp
    #~ rospy.sleep(0.1)
    #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
    #~ rospy.sleep(0.1)

    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
                                          'map', listener)
    tcpPos = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]
    tcpPosHome = tcpPos
    orientation = [0.0, 0.7071, 0.0,
                   0.7071]  # set topple orientation (rotate wrist)

    #~ rospy.sleep(0.1)
    #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
    #~ rospy.sleep(0.1)

    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
                                          'map', listener)
    tcpPos = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]

    print 'TCP is at:', tcpPos

    print 'planning to go home'

    # home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
    # planner = IKJoint(target_joint_pos=home_joint_pose)
    # plan = planner.plan()
    # plan.visualize()
    raw_input('execute?')
    # plan.execute()
    goToHome.goToHome()
    print 'done going home'

    calibBins = []
    for x in range(binNum, endBinNum + 1):
        calibPts = []

        if x == 1 or x == 4 or x == 7 or x == 10:
            calibBins.append([])
            continue

        distFromShelf = 0.05
        mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(
            distFromShelf, x)
        mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map',
                                            listener)
        mouthBin = [
            mouthBin.pose.position.x, mouthBin.pose.position.y,
            mouthBin.pose.position.z
        ]

        epsilon = 0.016
        distFromShelf = distFromShelf - epsilon

        # go to mouth of bin
        print 'planning to go to mouth but a little out'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = mouthBin
        planner = IK(q0=None,
                     target_tip_pos=targetPt,
                     target_tip_ori=Orientation,
                     tip_hand_transform=tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.visualize()
        s = plan.success()
        if s:
            raw_input('execute?')
            ik.ik.setSpeedByName('fast')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to mouth'

        # go to the mouth of the bin but actually inside
        print 'planning to go to start position'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = [mouthBin[0] + distFromShelf, mouthBin[1], mouthBin[2]]
        planner = IK(q0=None,
                     target_tip_pos=targetPt,
                     target_tip_ori=Orientation,
                     tip_hand_transform=tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        s = plan.success()
        if s:
            plan.visualize()
            raw_input('execute?')
            ik.ik.setSpeedByName('fast')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to start position'

        OrientationList = []
        targetPtList = []
        captionList = []
        ntouch = 5

        # go down
        captionList.append('go down')
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = [
            mouthBin[0] + distFromShelf, mouthBin[1],
            mouthBin[2] - moveDistVertical
        ]
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)

        # go left
        captionList.append('go left')
        Orientation = [0.5, 0.5, 0.5, 0.5]
        targetPt = [
            mouthBin[0] + distFromShelf, mouthBin[1] + moveDistHorizontal,
            mouthBin[2]
        ]
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)

        # go right
        captionList.append('go right')
        Orientation = [0.5, -0.5, 0.5, -0.5]
        targetPt = [
            mouthBin[0] + distFromShelf, mouthBin[1] - moveDistHorizontal,
            mouthBin[2]
        ]
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)

        # go up
        captionList.append('go up')
        Orientation = [0.7071, 0, 0.7071, 0]
        targetPt = [
            mouthBin[0] + distFromShelf, mouthBin[1],
            mouthBin[2] + moveDistVertical
        ]
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)

        # go to mid back of the bin
        captionList.append('go mid back')
        Orientation = [0, 0.7071 + 0.11 / 4, 0, 0.7071 - 0.11 / 4]
        targetPt = [mouthBin[0] + 0.30, mouthBin[1], mouthBin[2] - 0.08]
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)

        for touchInd in range(ntouch):
            print captionList[touchInd]
            targetPt = targetPtList[touchInd]
            Orientation = OrientationList[touchInd]

            planner = IK(q0=None,
                         target_tip_pos=targetPt,
                         target_tip_ori=Orientation,
                         tip_hand_transform=tip_hand_transform,
                         joint_topic=joint_topic)
            plan = planner.plan()
            s = plan.success()
            if s:
                plan.visualize()
                raw_input('execute?')
                ik.ik.setSpeedByName('fast')
                plan.execute()
            if not s:
                print 'could not find plan'
            print 'done going down, TELEOP TIME:'
            raw_input('when done with teleop hit enter: ')
            #~ rospy.sleep(0.1)
            #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
            #~ rospy.sleep(0.1)
            t = listener.getLatestCommonTime('calib', 'map')
            (calibPoseTrans,
             calibPoseRot) = listener.lookupTransform('map', 'calib', t)
            calibPose = list(calibPoseTrans) + list(calibPoseRot)
            #~ calibPose = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
            #~ calibPtsTemp = [calibPose.pose.position.x,calibPose.pose.position.y,calibPose.pose.position.z]
            calibPts.append(calibPose)
            raw_input('done recording position, hit enter to go back')

            if s:
                plan.executeBackward()
            else:
                print 'Please teleop back'
                raw_input('done?')

        calibBins.append(calibPts)

        filename = os.environ[
            'APC_BASE'] + '/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final.json'
        with open(filename, 'w') as outfile:
            json.dump(calibBins, outfile)

        print 'planning to go home'
        home_joint_pose = [
            -0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715,
            0.1061231983, -0.1045031463
        ]
        planner = IKJoint(target_joint_pos=home_joint_pose)
        plan = planner.plan()
        plan.visualize()
        raw_input('execute?')
        ik.ik.setSpeedByName('faster')
        plan.execute()
        print 'done going home'

    print 'dumped to file ', filename
Ejemplo n.º 17
0
def goToBin(binNum=0,
            robotConfig=None,
            objectiveBinPos=[1.2, 0, 0.6],
            isExecute=True,
            withPause=False,
            withSuction=False,
            counter=0):
    ## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
    ## objId: object identifier
    ## robotConfig: current time robot configuration
    ## shelfPosition: shelf position in world frame
    ## force threshold: amount fo force needed to say we have a grasp
    planSuctionSuccess = False
    planGraspSuccess = False

    joint_topic = '/joint_states'

    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)

    # plan store
    plans = []

    ## initial variable and tcp definitions
    # set tcp
    l2 = 0.47
    tip_hand_transform = [
        0, 0, l2, 0, 0, 0
    ]  # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    # rospy.sleep(0.1)
    # br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'tip', "link_6")
    # rospy.sleep(0.1)

    pubFrame(br,
             pose=tip_hand_transform,
             frame_id='target_pose',
             parent_frame_id='map',
             npub=1)

    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
                                          'map', listener)
    tcpPos = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]
    tcpPosHome = tcpPos
    # set scoop orientation (rotate wrist)
    if not withSuction:
        gripperOri = [0, 0.7071, 0, 0.7071]
    if withSuction:
        print '[goToBin] with suck-down primitive, defining new end orientation of gripper'
        gripperOri = [
            pose_world.pose.orientation.x, pose_world.pose.orientation.y,
            pose_world.pose.orientation.z, pose_world.pose.orientation.w
        ]

    # move back 10 cms to avoid object with shelf collision
    distAwayFromBin = 0.1
    targetPosition = [tcpPos[0] - distAwayFromBin, tcpPos[1], tcpPos[2]]
    q_initial = robotConfig
    planner = IK(q0=q_initial,
                 target_tip_pos=targetPosition,
                 target_tip_ori=gripperOri,
                 tip_hand_transform=tip_hand_transform,
                 joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print '[goToBin] move away from bin by %d cm successful' % distAwayFromBin
        print '[goToBin] tcp at:', targetPosition
        plan.visualize()
        plans.append(plan)
        if isExecute:
            pauseFunc(withPause)
            plan.execute()
    else:
        print '[goToBin] move away from bin fail'
        return False

    qf = plan.q_traj[-1]

    if binNum > 8:
        #plan = Plan()
        #plan.q_traj = [[0, -0.3959, 0.58466, 0.03,   0.1152, -0.1745]]  # should use IKJoint
        planner = IKJoint(
            q0=qf,
            target_joint_pos=[0, -0.3959, 0.58466, 0.03, 0.1152, -0.1745])
        plan = planner.plan()

        print '[goToBin] going home because risky to go straight to objective bin'
        plan.visualize()
        qf = plan.q_traj[-1]
        if isExecute:
            pauseFunc(withPause)
            plan.execute()

    # move above objective bin, approx
    use_JointPos = False
    if use_JointPos:
        planner = IKJoint(
            q0=qf, target_joint_pos=[0, -0.2953, 0.4462, 0, 0.8292, 1.5707])
        plan = planner.plan()
        #plan = Plan()
        #plan.q_traj = [[0, -0.2953, 0.4462, 0,   0.8292, 1.5707]]  # should use IKJoint
        s = True
    else:
        if not withSuction:
            gripperOri = [0.7071, 0.7071, 0, 0]
            locationAboveBinCOM = [
                0, 0, 0.15
            ]  # define over the lip distance in shelf frame
            targetPosition = [
                objectiveBinPos[0] + locationAboveBinCOM[0],
                objectiveBinPos[1] + locationAboveBinCOM[1],
                objectiveBinPos[2] + locationAboveBinCOM[2]
            ]
            q_initial = qf
            planner = IK(q0=q_initial,
                         target_tip_pos=targetPosition,
                         target_tip_ori=gripperOri,
                         tip_hand_transform=tip_hand_transform,
                         joint_topic=joint_topic)
            plan = planner.plan()
            s = plan.success()
            if s:
                print '[goToBin] move to above bin success'
                planSuctionSuccess = True
                plans.append(plan)
                plan.visualize()
                qf = plan.q_traj[-1]
                if isExecute:
                    pauseFunc(withPause)
                    plan.execute()
            else:
                return False

        if withSuction:
            bin_xyz, baseHeight = getBinMouthAndFloor(0.20, 10)
            bin_xyz = coordinateFrameTransform(bin_xyz, 'shelf', 'map',
                                               listener)
            targetPosition = [
                bin_xyz.pose.position.x, bin_xyz.pose.position.y,
                bin_xyz.pose.position.z
            ]
            q_initial = qf
            planner = IK(q0=q_initial,
                         target_tip_pos=targetPosition,
                         target_tip_ori=gripperOri,
                         tip_hand_transform=tip_hand_transform,
                         joint_topic=joint_topic)
            plan = planner.plan()
            s = plan.success()
            print '[goToBin] setting joints for suction'
            while True:
                APCrobotjoints = ROS_Wait_For_Msg(
                    joint_topic, sensor_msgs.msg.JointState).getmsg()
                q0 = APCrobotjoints.position
                if len(q0) < 6:
                    continue
                else:
                    break
            if q0[5] < 0:
                stackAngle = -(counter -
                               1) * 1.6 * 3.1415 / 180 + 9 * 3.1415 / 180
                possible_start_config = [
                    stackAngle, 0.610, 1.0277, 0.0, -1.4834,
                    3.1415 - 2 * math.pi
                ]
            else:
                stackAngle = -(counter -
                               1) * 1.6 * 3.1415 / 180 + 9 * 3.1415 / 180
                possible_start_config = [
                    stackAngle, 0.610, 1.0277, 0.0, -1.4834, 3.1415
                ]

            # plan_n = Plan()
            # plan_n.q_traj=[possible_start_config]
            planner = IKJoint(target_joint_pos=possible_start_config)
            plan_n = planner.plan()
            plan_n.visualize()
            qf = plan_n.q_traj[-1]
            if isExecute:
                pauseFunc(withPause)
                plan_n.execute()

    ## go down and release object
    if not withSuction:
        q_initial = qf
        objectiveBinPos[1] = (counter - 1) * 0.036 - 0.2
        targetPosition = [
            objectiveBinPos[0], objectiveBinPos[1], objectiveBinPos[2]
        ]
        planner = IK(q0=q_initial,
                     target_tip_pos=targetPosition,
                     target_tip_ori=gripperOri,
                     tip_hand_transform=tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        s = plan.success()
        if s:
            print '[goToBin] go to xyz of bin with vertical bias success'
            plans.append(plan)
            plan.visualize()
            if isExecute:
                pauseFunc(withPause)
                plan.execute()
        else:
            print '[goToBin] go to xyz of bin with vertical bias fail'
        qf = plan.q_traj[-1]
        openGripper()

    ## open gripper fully
    # rospy.sleep(0.5)
    # openGripper()
    rospy.sleep(0.5)
    suction.stop()
    return True
def push_rotate(objPose=[1.95,0.25,1.4,0,0,0,1],
            binNum=4,
            objId = 'crayola_64_ct',
            bin_contents = None, 
            robotConfig = None,
            grasp_range_lim=0.11,
            fing_width=0.06,
            isExecute = True,
            withPause = True):
    
    #~ *****************************************************************
    obj_dim=get_obj_dim(objId)
    obj_dim=np.array(obj_dim)
    #~ *****************************************************************

    # # DEFINE HAND WIDTH#########################
    hand_width=0.186
    
    #~ *****************************************************************
    ## initialize listener rospy
    listener = tf.TransformListener()
    br = tf.TransformBroadcaster()
    rospy.sleep(0.2)
    
    (shelf_position, shelf_quaternion) = lookupTransform("map", "shelf", listener)
    # print  shelf_position, shelf_quaternion 
    
    #~ *****************************************************************

    # Convert xyx quat to tranformation matrix for Shelf frame
    #~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7])

    shelf_pose_tfm_list=matrix_from_xyzquat(shelf_position,shelf_quaternion)
    shelf_pose_tfm=np.array(shelf_pose_tfm_list)

    shelf_pose_orient=shelf_pose_tfm[0:3,0:3]
    #~ print 'shelf_pose_orient'
    #~ print shelf_pose_orient
    
    shelf_pose_pos=shelf_pose_tfm[0:3,3]
    #~ print 'shelf_pose_pos'
    #~ print shelf_pose_pos
        
    #Normalized axes of the shelf frame
    shelf_X=shelf_pose_orient[:,0]/la.norm(shelf_pose_orient[:,0])
    shelf_Y=shelf_pose_orient[:,1]/la.norm(shelf_pose_orient[:,1])
    shelf_Z=shelf_pose_orient[:,2]/la.norm(shelf_pose_orient[:,2])
    
    #~ *****************************************************************
    
    # Convert xyx quat to tranformaation matrix for Object frame
    
    obj_pose_tfm_list=matrix_from_xyzquat(objPose[0:3], objPose[3:7])
    obj_pose_tfm=np.array(obj_pose_tfm_list)
    
    obj_pose_orient=obj_pose_tfm[0:3,0:3]
    obj_pose_pos=obj_pose_tfm[0:3,3]
    #~ print 'obj_pose_orient'
    #~ print obj_pose_orient
    
    #Normalized axes of the object frame
    obj_X=obj_pose_orient[:,0]/la.norm(obj_pose_orient[:,0])
    #~ print 'obj_X'
    #~ print obj_X
    
    obj_Y=obj_pose_orient[:,1]/la.norm(obj_pose_orient[:,1])
    #~ print 'obj_Y'
    #~ print obj_Y
    
    obj_Z=obj_pose_orient[:,2]/la.norm(obj_pose_orient[:,2])
    #~ print 'obj_Z'
    #~ print obj_Z
    
    #Normalized object frame
    obj_pose_orient_norm=np.vstack((obj_X,obj_Y,obj_Z))
    obj_pose_orient_norm=obj_pose_orient_norm.transpose()
    #~ print 'obj_pose_orient_norm'
    #~ print obj_pose_orient_norm
    
    #~ *****************************************************************
    
    # We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin)
    # Find projection of object axes on Y axis of the shelf frame
    
    proj_vecY=np.dot(shelf_Y,obj_pose_orient_norm)
    #~ print 'proj'
    #~ print proj_vecY
    
    max_proj_valY,hand_norm_dir=np.max(np.fabs(proj_vecY)), np.argmax(np.fabs(proj_vecY))
    
    if proj_vecY[hand_norm_dir]>0:
        hand_norm_vec=-obj_pose_orient_norm[:,hand_norm_dir]
    else:
        hand_norm_vec=obj_pose_orient_norm[:,hand_norm_dir]
        
    #~ print 'hand_norm_vec'
    #~ print hand_norm_vec
    
    #Find angle of the edge of the object 
    Cos_angle_made_with_shelf_Y=max_proj_valY/(la.norm(shelf_Y)*la.norm(hand_norm_vec));
    
    angle_to_shelfY=np.arccos(Cos_angle_made_with_shelf_Y)*180.0/np.pi
    
    #~ print'angle_to_shelfY'
    #~ print angle_to_shelfY
    #Find object dimension along hand normal axis
    
    obj_dim_along_hand_norm=obj_dim[hand_norm_dir]
    print '[PushRotate] dim along hand norm', obj_dim_along_hand_norm
    #~ *****************************************************************
    
    #Find projection of object axes on X axis of the shelf frame    
    #To find out which object frame is lying closer to the X axis of the shelf
        
    proj_vecX=np.dot(shelf_X,obj_pose_orient_norm)
        
    max_proj_valX,fing_axis_dir=np.max(np.fabs(proj_vecX)), np.argmax(np.fabs(proj_vecX))
    
    if proj_vecX[fing_axis_dir]>0:
        fing_axis_vec=obj_pose_orient_norm[:,fing_axis_dir]
    else:
        fing_axis_vec=-obj_pose_orient_norm[:,fing_axis_dir]
        
    #~ print 'fing_axis_vec'
    #~ print fing_axis_vec
        
    #Find object dimension along the finger axis
    obj_dim_along_fingAxis=obj_dim[fing_axis_dir]
    print '[PushRotate] obj_dim_along_fingAxis:', obj_dim_along_fingAxis
    
    #~ *****************************************************************
    
    #Find projection of object axes on Z axis of the shelf frame    
    #To find out which object frame is lying closer to the Z axis of the shelf
        
    proj_vecZ=np.dot(shelf_Z,obj_pose_orient_norm)

    max_proj_valZ,Zaxis_dir=np.max(np.fabs(proj_vecZ)), np.argmax(np.fabs(proj_vecZ))

    Zaxis_vec=obj_pose_orient_norm[:,Zaxis_dir]
        
    #~ print 'Zaxis_vec'
    #~ print Zaxis_vec
    #Find object dimension along the finger axis, shelf Z
    obj_dim_along_ZAxis=obj_dim[Zaxis_dir]
    
    hand_Y=np.cross(hand_norm_vec,fing_axis_vec)
    
    #~ *****************************************************************
    ####################  DECISION STRATEGIES #########################
    
    bin_inner_cnstr=get_bin_inner_cnstr()
    
    proj_handNorm_ShelfX=np.dot(hand_norm_vec,shelf_X)
    
    right_push=False
    left_push=False
    
    # print 'obj_dim_along_hand_norm', obj_dim_along_hand_norm
    # print 'obj_dim_along_fingAxis', obj_dim_along_fingAxis
    # 
    # print 'proj_handNorm_ShelfX'
    # print proj_handNorm_ShelfX
    
    if obj_dim_along_hand_norm>obj_dim_along_fingAxis:
        #~ print 'proj_vecY'
        #~ print proj_vecY
        #~ print proj_vecY[hand_norm_dir]
        #~ 
        #~ print 'proj_vecX'
        #~ print proj_vecX
        #~ print proj_vecX[fing_axis_dir]
        
        #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0:
            #~ #Do left push
            #~ Ypush_mult=1
            #~ print 'Left Push'
        #~ else:
            #~ #Do right push
            #~ Ypush_mult=-1
            #~ print 'right Push'
        if proj_handNorm_ShelfX>0:
            #Do left push
            Ypush_mult=1
            left_push=True
            print '[PushRotate] Left Push'
        else:
            #Do right push
            Ypush_mult=-1
            print '[PushRotate] right Push'
            right_push=True
        push_offset=(obj_dim_along_hand_norm/2.0) #(Instead pf pushing on edge we will push 5 mm inside)
            
    else:
        
        #~ if proj_vecY[hand_norm_dir]*proj_vecX[fing_axis_dir]<0:
            #~ #Do right push
            #~ Ypush_mult=-1
            #~ print 'right Push'
        #~ else:
            #~ #Do left push
            #~ Ypush_mult=1
            #~ print 'Left Push'
        if proj_handNorm_ShelfX>0:
            #Do right push
            Ypush_mult=-1
            print '[PushRotate] right Push'
            right_push=True
        else:
            #Do left push
            Ypush_mult=1
            print '[PushRotate] Left Push'
            left_push=True
        push_offset=(obj_dim_along_fingAxis/2.0)
    
    num_intm_points=5
    push_x_intm=np.zeros(num_intm_points+1)
    push_y_intm=np.zeros(num_intm_points+1)
    
    backWall_shelf=bin_inner_cnstr[binNum][1]
    backWall_world = coordinateFrameTransform([0,backWall_shelf,0], 'shelf', 'map', listener)
    
    back_check=backWall_world.pose.position.x-obj_dim_along_hand_norm/2.0
    
    
    binRightWall=bin_inner_cnstr[binNum][0]
    binLeftWall=bin_inner_cnstr[binNum][1]
        
    binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener)
    binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener)
    
    if left_push:
        sideWall_check=binRightWall_world.pose.position.y+(fing_width/2.0)+(obj_dim_along_fingAxis)
    
    if right_push:
        sideWall_check=binLeftWall_world.pose.position.y-(fing_width/2.0)-(obj_dim_along_fingAxis)
    
    for i in range(num_intm_points+1):
        push_x_intm[i]=obj_pose_pos[0]+(push_offset)*np.sin(((90/num_intm_points)*i*np.pi)/180.0)
        
        if push_x_intm[i]>back_check:
            push_x_intm[i]=back_check
        
        push_y_intm[i]=obj_pose_pos[1]+(Ypush_mult*push_offset)*np.cos(((90/num_intm_points)*i*np.pi)/180.0)
        
        if left_push:
            if push_y_intm[i]<sideWall_check:
                push_y_intm[i]=sideWall_check
                print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper'
        
        if right_push:
            if push_y_intm[i]>sideWall_check:
                push_y_intm[i]=sideWall_check
                print '[PushRotate] push Y reduced, I do not want to crush the obj and gripper'
    
    push_x_intm=np.array(push_x_intm)
    
    #~ print'push_x_intm'
    #~ print push_x_intm
    
    push_y_intm=np.array(push_y_intm)
    
    #*******************************************************************
    #Move the hand to the center of the bin and open the hand based on the height of the object
    
    bin_mid_pos,binFloorHeight=getBinMouthAndFloor(0.0, binNum)
    binFloorHeight_world = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener)
        
    bin_mid_pos_world = coordinateFrameTransform(bin_mid_pos, 'shelf', 'map', listener)
    
    #*******************************************************************
    tcp_Z_off=0.005
    push_tcp_Z_shelfFrame=((bin_inner_cnstr[binNum][4]+bin_inner_cnstr[binNum][5])/2.0)-tcp_Z_off
    
    push_tcp_Z_WorldFrame = coordinateFrameTransform([0,0,push_tcp_Z_shelfFrame], 'shelf', 'map', listener)
    
    push_tcp_Z=push_tcp_Z_WorldFrame.pose.position.z
    
    push_z_intm=(push_tcp_Z)*np.ones(push_x_intm.size)
    
    push_series_pos=np.vstack((push_x_intm,push_y_intm,push_z_intm))
    push_series_pos=push_series_pos.transpose()
    
    #~ print 'push_series_pos'
    #~ print push_series_pos
    
    #~ print push_series_pos[0,:]
    #~ print push_series_pos[-1,:]
    
    push_possible=True
    #******************************************************************
    fing_push_pt=0.2*obj_dim_along_ZAxis+binFloorHeight_world.pose.position.z
    
    blade_tip_TCP_off=0.038 # dist between finger inner end and spatual edge
    
    push_hand_opening=2*(push_tcp_Z-blade_tip_TCP_off-fing_push_pt)
    
    if push_hand_opening>grasp_range_lim:
        push_hand_opening=grasp_range_lim
    
        
    # Spatula finger should not hit the lip of the bin while pushing
    
    lip_Z_shelf=bin_inner_cnstr[binNum][4]
    lip_Z_world=coordinateFrameTransform([0,0,lip_Z_shelf], 'shelf', 'map', listener)
        
    #~ lip_off=.025
    #~ bin_lip_Z=binFloorHeight_world.pose.position.z+lip_off
    
    max_allowed_hand_opening_out=2*(push_tcp_Z-lip_Z_world.pose.position.z)
    max_allowed_hand_opening=max_allowed_hand_opening_out-0.036 # 0.036 is diff between inside and outside of the finger/finger mount surface
    
    if push_hand_opening>max_allowed_hand_opening:
        print '[PushRotate] reducing hand opening bcaz it may hit the lip of the bin'
        push_hand_opening=max_allowed_hand_opening
    
    
    #~ print 'push_hand_opening'
    #~ print push_hand_opening
        
    #~ #hand should not hit the side walls 
    #~ binRightWall,binLeftWall=find_shelf_walls(binNum)
    #~ 
    #~ binRightWall_world = coordinateFrameTransform([binRightWall,0,0], 'shelf', 'map', listener)
    #~ binLeftWall_world = coordinateFrameTransform([binLeftWall,0,0], 'shelf', 'map', listener)
    
    side_wall_clearance=0.0
    
    print '[PushRotate] binRightWall_world.pose.position.y', binRightWall_world.pose.position.y
    print '[PushRotate] right_hand_edge=', (push_y_intm[0]-fing_width-side_wall_clearance)
    
    print '[PushRotate] binLeftWall_world.pose.position.y', binLeftWall_world.pose.position.y
    print '[PushRotate] left_hand_edge=', (push_y_intm[0]+fing_width+side_wall_clearance)
    
    if push_y_intm[0]-fing_width-side_wall_clearance<binRightWall_world.pose.position.y:
        print '[PushRotate] Hand will hit the right wall, Push rotate not possible'
        push_possible=False
    if push_y_intm[0]+fing_width+side_wall_clearance>binLeftWall_world.pose.position.y:
        print '[PushRotate] Hand will hit the left wall, push rotate not possible'
        push_possible=False

    #~ *************************************************************
    # set spatual down orientation (rotate wrist)
    spatula_down_orient = [0, 0.7071, 0, 0.7071]
    
    push_orient=deepcopy(spatula_down_orient)
        
    #~ *************************************************************
        
    if push_possible==True:
        #Now we know where we wan to move TCP
        #Let's do robot motion planning now
        
        joint_topic = '/joint_states'
            
        # plan store
        plans = []
        
        # set tcp (same as that set in generate dictionary)
        l1=0.0
        l2=0.0
        l3 = 0.47
        tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized
        # broadcast frame attached to tcp
        pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5)
        
        #~ *************************************************************
        
        # GO TO MOUTH OF THE BIN
        
        q_initial = robotConfig
        # move to bin mouth
        distFromShelf = 0.1
        mouthPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
        mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
        targetPosition = [mouthPt.pose.position.x, mouthPt.pose.position.y, mouthPt.pose.position.z]
        gripperOri=[0, 0.7071, 0, 0.7071]
        
        pubFrame(br, pose = targetPosition+gripperOri, frame_id='target_pose', parent_frame_id='link_6', npub=5)

        planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = gripperOri, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
        plan = planner.plan()
        plan.setSpeedByName('faster')
        s = plan.success()
        if s:
            print '[PushRotate] move to bin mouth in push-rotate code successful'
            #plan.visualize()
            plans.append(plan) # Plan 0
            #if isExecute:
            #    pauseFunc(withPause)
             #   plan.execute()
        else:
            print '[PushRotate] move to bin mouth in push-rotate code fail'
            return False
        
        qf = plan.q_traj[-1]
    
        q_initial = qf
        #################### CLOSE THE GRIPPER GRASP #################
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        plans.append(grasp_plan)
        #~ moveGripper(0.1,100)
        #~ *************************************************************
        
        ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######

        # set push_tcp
        push_l1=0.0 #0.035
        push_l2=-Ypush_mult*(fing_width/2.0) # This should depend on the righ or left push conditions
        
        push_l3 = 0.47  
        push_tip_hand_transform = [l1, l2, l3, 0,0,0] # to be updated when we have a hand design finalized
        # broadcast frame attached to grasp_tcp
        pubFrame(br, pose=push_tip_hand_transform, frame_id='push_tip', parent_frame_id='link_6', npub=10)
                
        #~ q_initial = robotConfig
        # move just inside the bin with push orientation and open the hand suitable to push
        
        distFromShelf = -0.01
        InbinPt,temp = getBinMouthAndFloor(distFromShelf, binNum)
        InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
        prepush_targetPosition = [InbinPt.pose.position.x, push_series_pos[0,1] , InbinPt.pose.position.z] #matching world_Y of first push pt
        
        print '[PushRotate] prepush_targetPosition=', prepush_targetPosition
        
        pubFrame(br, pose=prepush_targetPosition+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10)

        planner = IK(q0 = q_initial, target_tip_pos = prepush_targetPosition, target_tip_ori = push_orient, tip_hand_transform=push_tip_hand_transform, joint_topic=joint_topic)
        plan = planner.plan()
        plan.setSpeedByName('slow')
        s = plan.success()
        if s:
            print '[PushRotate] move inside bin in push orient successful'
            #~ print 'tcp at:'
            #~ print(pregrasp_targetPosition)
            #~ plan.visualize()
            plans.append(plan) # Plan 1
            #~ if isExecute:
                #~ pauseFunc(withPause)
                #~ plan.execute()
        else:
            print '[PushRotate] move inside bin in push orient fail'
            return False
        
        qf = plan.q_traj[-1]
    
        q_initial = qf
        
        #~ *************************************************************
        
        ############## OPEN THE GRIPPER To PUSH ###############
        
        # Open the gripper to dim calculated for pushing
        print '[PushRotate] hand opening to'
        print push_hand_opening*1000.0
        #~ moveGripper(push_hand_opening,100)
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
        plans.append(grasp_plan)
        
        #~ *************************************************************
        
        ############  Execute the push trajectory ##############
        #~ push_rotate_traj = Plan()
        #~ push_rotate_traj.q_traj = qf
        
        for i in range(0,push_x_intm.size):
            pushpt_pos=push_series_pos[i,:].transpose()
            pubFrame(br, pose=pushpt_pos.tolist()+push_orient, frame_id='target_pose', parent_frame_id='map', npub=10)
                
            #~ pdb.set_trace()
            # planner = IK(q0 = q_initial, target_tip_pos = graspPT_pose_pos, target_tip_ori = push_orient, 
                 # joint_topic=joint_topic, tip_hand_transform=tip_hand_transform, straightness = 0.3, inframebb = inframebb)
            planner = IK(q0 = q_initial, target_tip_pos = pushpt_pos, target_tip_ori = push_orient, 
                 joint_topic=joint_topic, tip_hand_transform=push_tip_hand_transform)
            plan = planner.plan()
            
            plan.setSpeedByName('slow')
            s = plan.success()
            if s:
                print '[PushRotate] point inside push traj successful'
                #~ print 'tcp at:'
                #~ print(pregrasp_targetPosition)
                #~ plan.visualize()
                plans.append(plan) # Plan 1
                #~ if isExecute:
                    #~ pauseFunc(withPause)
                    #~ plan.execute()
            else:
                print '[PushRotate] point inside push traj fail'
                return False
            
            qf = plan.q_traj[-1]
            q_initial = qf
        #~ *************************************************************
        
        #~ if isExecute:
            #~ pauseFunc(withPause)
            #~ push_rotate_traj.execute()
            
        #################### CLOSE THE GRIPPER GRASP #################
        #~ moveGripper(0.0,100)
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        plans.append(grasp_plan)
        #~ *************************************************************
        
        # #################### EXECUTE FORWARD #################### 
        
        for numOfPlan in range(0, len(plans)):
            if isExecute:
                plans[numOfPlan].visualize()
                pauseFunc(withPause)
                plans[numOfPlan].execute()
        
        # #################### RETREAT #################### 
        
        for numOfPlan in range(0, len(plans)):
            if isExecute:
                plans[len(plans)-numOfPlan-1].visualizeBackward()
                pauseFunc(withPause)
                plans[len(plans)-numOfPlan-1].executeBackward()
        
    #*******************************************************************
        
        print '[PushRotate] push_successful'
    
    return (push_possible,False)
Ejemplo n.º 19
0
def push(objPose = [1.55,0.25,1.1,0,0,0,0],
            binNum=0,
            objId = 0, 
            bin_contents = None, 
            robotConfig=None,
            shelfPosition = [1.9116,-0.012498,-0.4971],
            forceThreshold = 1,
            binDepthReach = 0.40,
            isExecute = True,
            withPause = False):
    ## objPose: world position and orientation frame attached to com of object
    ## objId: object identifier
    ## robotConfig: current time robot configuration
    ## shelfPosition: shelf position in world frame
    ## force threshold: amount fo force needed to say we have a grasp

    ## initialize listener rospy
    rospy.init_node('listener', anonymous=True)
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    
    joint_topic = '/joint_states'
        
    # shelf variables
    pretensionDelta = 0.00
    lipHeight = 0.035
    ## get object position (world frame)
    objPosition = getObjCOM(objPose[0:3], objId)
        
    ## move gripper to object com outside bin along world y direction and
    ## move the gripper over the lip of the bin    
    # set tcp
    l2 = 0.47  
    tip_hand_transform = [0, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5)
    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    tcpPosHome = tcpPos
    # set topple orientation (rotate wrist)
    toppleOrientation = [0.0, 0.7071, 0.0, 0.7071] #[0, 0.7071, 0, 0.7071]
    
    # if gripper open close it
    closeGripper(forceThreshold)
    
    # set first target to move gripper in front of the object and adjust height to middle of bin
    distFromShelf = 0.05
    wristWidth = 0.0725 # this is actually half the wrist width
    (binMouth, binFloorHeight) = getBinMouthAndFloor(distFromShelf, binNum)
    pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map', listener)
    binFloorHeight = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener)
    binFloorHeight = binFloorHeight.pose.position.z
    binMouth=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    
    # set offset past COM
    # offset is 1/4 of object's height + half finger width
    # note: (COM height - bin floor height) is half of the object's height
    #pdb.set_trace() #TRACEEEEE
    fingerWidth = 0.06
    offsetFinger = fingerWidth/2
    targetHeight = (objPosition[2] - binFloorHeight)*2
    effect = 'slide'
    if effect == 'topple':
        offsetEffect = targetHeight*3/4
    elif effect == 'slide':
        offsetEffect = targetHeight*1/4
    else:
        offsetEffect = targetHeight*1/4 #Default is slide, made three cases for flexibility
    
    offset = offsetFinger + offsetEffect
    targetPosition = [binMouth[0], objPosition[1], binFloorHeight+offset]
    frontOfObjectPtOutOfBin = targetPosition
    q_initial = robotConfig
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    
    if s and effect == 'topple':
        print 'move to COM in y and above COM in z successful'
    elif s and effect == 'slide':
        print 'move to COM in y and below COM in z successful'
    else:
        print 'move to COM in y and below COM in z successful' #Default is slide, made three cases for flexibility
    
    if s:
        print 'tcp at:'
        print(targetPosition)
        plan.visualize()
        if isExecute:
            pauseFunc(withPause)
            plan.execute()
    else:
        print 'move to COM in y and above COM in z fail'
        
    qf = plan.q_traj[-1]
    # set second target, go over the lip of the bin
    #interiorLipBin = [0,0.39,0] # define over the lip distance in shelf frame
    #interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener)
    #deltaX = np.add(-targetPosition[0],interiorLipBin.pose.position.x)
    #targetPosition = np.add(targetPosition, [deltaX,0,0])
    #q_initial = qf
    #planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    #plan = planner.plan()
    #s = plan.success()
    #if s:
    #    print 'move to inside the lip success'
    #    print 'tcp at:'
    #    print(targetPosition)
    #    plan.visualize()
    #    if isExecute:
    #        pauseFunc(withPause)
    #        plan.execute()
    #else:
    #    print 'move to inside the lip fail'
    #qf = plan.q_traj[-1]
    
    ## close gripper fully (Do we wish to topple with gripper slightly open?)
    closeGripper(forceThreshold)
    
    ## perform bin length stroke to end
    q_initial = qf
    targetPosition = np.add(targetPosition, [binDepthReach,0,0])
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print 'stroke to end of bin success'
        print 'tcp at:'
        print(targetPosition)
        plan.visualize()
        if isExecute:
            pauseFunc(withPause)
            plan.execute()
    else:
        print 'stroke to end of bin fail'
    qf = plan.q_traj[-1]
    
    ## close gripper
    closeGripper(forceThreshold)
    
    ## retreat
    # go back along stroke
    q_initial = qf
    targetPosition = frontOfObjectPtOutOfBin
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print 'stroke to bin start success'
        print 'tcp at:'
        print(targetPosition)
        plan.visualize()
        if isExecute:
            pauseFunc(withPause)
            plan.execute()
    else:
        print 'stroke to bin start fail'
    qf = plan.q_traj[-1]
    
    # go back to initial tcp position
    q_initial = qf
    targetPosition = frontOfObjectPtOutOfBin
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = toppleOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()
    if s:
        print 'return to mouth success'
        plan.visualize()
        if isExecute:
            pauseFunc(withPause)
            plan.execute()
    else:
        print 'return to mouth fail'
    
    return False
Ejemplo n.º 20
0
def push_back(objPose=[1.95, 0.25, 1.4, 0, 0, 0, 1],
              binNum=4,
              objId='crayola_64_ct',
              bin_contents=[
                  'paper_mate_12_count_mirado_black_warrior', 'crayola_64_ct',
                  'expo_dry_erase_board_eraser'
              ],
              robotConfig=None,
              grasp_range_lim=0.11,
              fing_width=0.06,
              isExecute=True,
              withPause=True):

    #~ *****************************************************************
    obj_dim = get_obj_dim(objId)
    obj_dim = np.array(obj_dim)
    #~ *****************************************************************

    # # DEFINE HAND WIDTH#########################
    hand_width = 0.186

    #~ *****************************************************************
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)

    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)

    (shelf_position,
     shelf_quaternion) = lookupTransform("map", "shelf", listener)
    # print  shelf_position, shelf_quaternion

    #~ *****************************************************************

    # Convert xyx quat to tranformation matrix for Shelf frame
    #~ shelf_pose_tfm_list=matrix_from_xyzquat(shelfPose[0:3], shelfPose[3:7])

    shelf_pose_tfm_list = matrix_from_xyzquat(shelf_position, shelf_quaternion)
    shelf_pose_tfm = np.array(shelf_pose_tfm_list)

    shelf_pose_orient = shelf_pose_tfm[0:3, 0:3]
    #~ print '[PushBack] shelf_pose_orient'
    #~ print shelf_pose_orient

    shelf_pose_pos = shelf_pose_tfm[0:3, 3]
    #~ print '[PushBack] shelf_pose_pos'
    #~ print shelf_pose_pos

    #Normalized axes of the shelf frame
    shelf_X = shelf_pose_orient[:, 0] / la.norm(shelf_pose_orient[:, 0])
    shelf_Y = shelf_pose_orient[:, 1] / la.norm(shelf_pose_orient[:, 1])
    shelf_Z = shelf_pose_orient[:, 2] / la.norm(shelf_pose_orient[:, 2])

    #~ *****************************************************************

    # Convert xyx quat to tranformaation matrix for Object frame

    obj_pose_tfm_list = matrix_from_xyzquat(objPose[0:3], objPose[3:7])
    obj_pose_tfm = np.array(obj_pose_tfm_list)

    obj_pose_orient = obj_pose_tfm[0:3, 0:3]
    obj_pose_pos = obj_pose_tfm[0:3, 3]
    #~ print '[PushBack] obj_pose_orient'
    #~ print obj_pose_orient

    #Normalized axes of the object frame
    obj_X = obj_pose_orient[:, 0] / la.norm(obj_pose_orient[:, 0])
    #~ print '[PushBack] obj_X'
    #~ print obj_X

    obj_Y = obj_pose_orient[:, 1] / la.norm(obj_pose_orient[:, 1])
    #~ print '[PushBack] obj_Y'
    #~ print obj_Y

    obj_Z = obj_pose_orient[:, 2] / la.norm(obj_pose_orient[:, 2])
    #~ print '[PushBack] obj_Z'
    #~ print obj_Z

    #Normalized object frame
    obj_pose_orient_norm = np.vstack((obj_X, obj_Y, obj_Z))
    obj_pose_orient_norm = obj_pose_orient_norm.transpose()
    #~ print '[PushBack] obj_pose_orient_norm'
    #~ print obj_pose_orient_norm

    #~ *****************************************************************

    # We will assume that hand normal tries to move along object dimension along the Y axis of the shelf (along the lenght of the bin)
    # Find projection of object axes on Y axis of the shelf frame

    proj_vecY = np.dot(shelf_Y, obj_pose_orient_norm)
    #~ print '[PushBack] proj'
    #~ print proj_vecY

    max_proj_valY, hand_norm_dir = np.max(np.fabs(proj_vecY)), np.argmax(
        np.fabs(proj_vecY))

    if proj_vecY[hand_norm_dir] > 0:
        hand_norm_vec = -obj_pose_orient_norm[:, hand_norm_dir]
    else:
        hand_norm_vec = obj_pose_orient_norm[:, hand_norm_dir]

    #~ print '[PushBack] hand_norm_vec'
    #~ print hand_norm_vec

    #Find angle of the edge of the object
    Cos_angle_made_with_shelf_Y = max_proj_valY / (la.norm(shelf_Y) *
                                                   la.norm(hand_norm_vec))

    angle_to_shelfY = np.arccos(Cos_angle_made_with_shelf_Y) * 180 / np.pi

    #~ print'angle_to_shelfY'
    #~ print angle_to_shelfY
    #Find object dimension along hand normal axis

    obj_dim_along_hand_norm = obj_dim[hand_norm_dir]
    # print '[PushBack] dim along hand norm'
    # print obj_dim_along_hand_norm
    #~ *****************************************************************

    #Find projection of object axes on X axis of the shelf frame
    #To find out which object frame is lying closer to the X axis of the shelf

    proj_vecX = np.dot(shelf_X, obj_pose_orient_norm)

    max_proj_valX, fing_axis_dir = np.max(np.fabs(proj_vecX)), np.argmax(
        np.fabs(proj_vecX))

    if proj_vecX[fing_axis_dir] > 0:
        fing_axis_vec = obj_pose_orient_norm[:, fing_axis_dir]
    else:
        fing_axis_vec = -obj_pose_orient_norm[:, fing_axis_dir]

    #~ print '[PushBack] fing_axis_vec'
    #~ print fing_axis_vec

    #Find object dimension along the finger axis
    obj_dim_along_fingAxis = obj_dim[fing_axis_dir]
    # print '[PushBack] obj_dim_along_fingAxis'
    # print obj_dim_along_fingAxis

    #~ *****************************************************************

    #Find projection of object axes on Z axis of the shelf frame
    #To find out which object frame is lying closer to the Z axis of the shelf

    proj_vecZ = np.dot(shelf_Z, obj_pose_orient_norm)

    max_proj_valZ, Zaxis_dir = np.max(np.fabs(proj_vecZ)), np.argmax(
        np.fabs(proj_vecZ))

    Zaxis_vec = obj_pose_orient_norm[:, Zaxis_dir]

    #~ print '[PushBack] Zaxis_vec'
    #~ print Zaxis_vec
    #Find object dimension along the finger axis, shelf Z
    obj_dim_along_ZAxis = obj_dim[Zaxis_dir]

    hand_Y = np.cross(hand_norm_vec, fing_axis_vec)

    diag_dist = la.norm(
        np.array([obj_dim_along_hand_norm, obj_dim_along_fingAxis]))
    # print'req obj diag dist=', diag_dist

    #~ *****************************************************************
    # Find the maximum distance to which we will push

    other_diag = np.zeros(len(bin_contents))

    for num_bin_contents in range(0, len(bin_contents)):
        if bin_contents[num_bin_contents] != objId:
            temp_obj_dim = get_obj_dim(objId)
            other_diag[num_bin_contents] = la.norm(
                np.array([temp_obj_dim[0], temp_obj_dim[1]]))

    max_diag, max_obj_ID = np.max(np.fabs(other_diag)), np.argmax(
        np.fabs(other_diag))
    # print '[PushBack] max_obj_ID',max_diag
    # print '[PushBack] max_obj_ID', bin_contents[max_obj_ID]
    #*******************************************************************

    bin_inner_cnstr = get_bin_inner_cnstr()

    #*******************************************************************
    #Check if we can do left push
    diag_factor = 0.75

    obj_left_edge = obj_pose_pos[1] + diag_factor * diag_dist / 2.0

    binLeftWall = bin_inner_cnstr[binNum][1]
    binLeftWall_world = coordinateFrameTransform([binLeftWall, 0, 0], 'shelf',
                                                 'map', listener)

    binRightWall = bin_inner_cnstr[binNum][0]
    binRightWall_world = coordinateFrameTransform([binRightWall, 0, 0],
                                                  'shelf', 'map', listener)

    left_gap = binLeftWall_world.pose.position.y - obj_left_edge
    # print '[PushBack] left_gap=', left_gap

    fing_clearance = 0.010  # It's like 5 mm on both sides of finger

    bin_width = binLeftWall_world.pose.position.y - binRightWall_world.pose.position.y

    tolerance_in_push = 0.020
    if left_gap + diag_dist / 2.0 - tolerance_in_push > bin_width:
        print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin'
        return (False, False)

    if left_gap > fing_width + fing_clearance:
        left_push = True
        print '[PushBack] left push is possible'
    else:
        left_push = False
        print '[PushBack] left push is NOT possible'

    # check if we can do right push

    obj_right_edge = obj_pose_pos[1] - diag_factor * diag_dist / 2.0

    right_gap = obj_right_edge - binRightWall_world.pose.position.y
    # print '[PushBack] right_gap', right_gap
    fing_clearance = 0.010  # It's like 5 mm on both sides of finger

    if right_gap + diag_dist / 2.0 - tolerance_in_push > bin_width:
        print '[PushBack] looks like vision has messed up. the object does not seem to be in the appropriate bin'
        return (False, False)

    if right_gap > fing_width + fing_clearance:
        right_push = True
        print '[PushBack] right push is possible'
    else:
        right_push = False
        print '[PushBack] right push is NOT possible'
    #*******************************************************************
    #Find Push Points for left push
    bin_face_pos, binFloorHeight = getBinMouthAndFloor(0.0, binNum)
    bin_face_pos_world = coordinateFrameTransform(bin_face_pos, 'shelf', 'map',
                                                  listener)

    tcp_Z_off = 0.005  #lower down from center of the bin
    push_tcp_Z_shelfFrame = (
        (bin_inner_cnstr[binNum][4] + bin_inner_cnstr[binNum][5]) /
        2.0) - tcp_Z_off

    push_tcp_Z_WorldFrame = coordinateFrameTransform(
        [0, 0, push_tcp_Z_shelfFrame], 'shelf', 'map', listener)

    push_tcp_Z = push_tcp_Z_WorldFrame.pose.position.z

    push_side_off = 1.0
    if left_push:

        left_pushPt_X = bin_face_pos_world.pose.position.x + 0.05
        left_pushPt_Y = binLeftWall_world.pose.position.y - push_side_off * left_gap
        # left_Y_lim=binLeftWall_world.pose.position.y-
        left_pushPt_Z = deepcopy(push_tcp_Z)

        left_push_Pt = np.array([left_pushPt_X, left_pushPt_Y, left_pushPt_Z])
        print '[PushBack] expected left push clearance=', binLeftWall_world.pose.position.y - left_pushPt_Y - fing_width / 2.0
    #Find Push Points for right push

    if right_push:

        right_pushPt_X = bin_face_pos_world.pose.position.x + 0.05
        right_pushPt_Y = binRightWall_world.pose.position.y + push_side_off * right_gap
        right_pushPt_Z = deepcopy(push_tcp_Z)

        right_push_Pt = np.array(
            [right_pushPt_X, right_pushPt_Y, right_pushPt_Z])

        print '[PushBack] expected right push clearance=', right_pushPt_Y - binRightWall_world.pose.position.y - fing_width / 2.0
    #Execute push trajectories

    #~ *************************************************************
    # set spatual down orientation (rotate wrist)
    spatula_down_orient = [0, 0.7071, 0, 0.7071]

    push_orient = spatula_down_orient

    push_hand_opening = 0.100

    joint_topic = '/joint_states'
    #~ *************************************************************

    #Execure left push trajectory

    if left_push:
        # plan store
        left_plans = []

        # set tcp (same as that set in generate dictionary)
        l1 = 0.0
        l2 = 0.0
        l3 = 0.47
        tip_hand_transform = [
            l1, l2, l3, 0, 0, 0
        ]  # to be updated when we have a hand design finalized
        # broadcast frame attached to tcp
        pubFrame(br,
                 pose=tip_hand_transform,
                 frame_id='tip',
                 parent_frame_id='link_6',
                 npub=5)

        #~ *************************************************************

        # GO TO MOUTH OF THE BIN

        q_initial = robotConfig
        # move to bin mouth
        distFromShelf = 0.1
        mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
        mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
        targetPosition = [
            mouthPt.pose.position.x, mouthPt.pose.position.y,
            mouthPt.pose.position.z
        ]
        gripperOri = [0, 0.7071, 0, 0.7071]

        pubFrame(br,
                 pose=targetPosition + gripperOri,
                 frame_id='target_pose',
                 parent_frame_id='link_6',
                 npub=5)

        planner = IK(q0=q_initial,
                     target_tip_pos=targetPosition,
                     target_tip_ori=gripperOri,
                     tip_hand_transform=tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.setSpeedByName('faster')
        s = plan.success()
        if s:
            print '[PushBack] move to bin mouth in push-back code successful (left push)'
            left_plans.append(plan)  # Plan 0
        else:
            print '[PushBack] move to bin mouth in push-back code fail (left push)'
            return (False, False)

        qf = plan.q_traj[-1]

        q_initial = qf
        #################### CLOSE THE GRIPPER GRASP #################
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        left_plans.append(grasp_plan)
        #~ *************************************************************

        ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######

        # set push_tcp
        push_l1 = 0.0
        push_l2 = 0.0

        push_l3 = 0.47
        push_tip_hand_transform = [
            l1, l2, l3, 0, 0, 0
        ]  # to be updated when we have a hand design finalized
        # broadcast frame attached to grasp_tcp
        pubFrame(br,
                 pose=push_tip_hand_transform,
                 frame_id='push_tip',
                 parent_frame_id='link_6',
                 npub=10)

        distFromShelf = -0.015
        InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
        InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
        prepush_targetPosition = [
            InbinPt.pose.position.x, left_push_Pt[1], InbinPt.pose.position.z
        ]  #matching world_Y of first push pt

        pubFrame(br,
                 pose=prepush_targetPosition + push_orient,
                 frame_id='target_pose',
                 parent_frame_id='map',
                 npub=10)

        planner = IK(q0=q_initial,
                     target_tip_pos=prepush_targetPosition,
                     target_tip_ori=push_orient,
                     tip_hand_transform=push_tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        s = plan.success()
        plan.setSpeedByName('fast')
        s = plan.success()
        if s:
            print '[PushBack] move inside bin in push orient successful'
            #~ print '[PushBack] tcp at:'
            #~ print(pregrasp_targetPosition)
            #~ plan.visualize()
            left_plans.append(plan)  # Plan 1
            #~ if isExecute:
            #~ pauseFunc(withPause)
            #~ plan.execute()
        else:
            print '[PushBack] move inside bin in push orient fail'
            return (False, False)

        qf = plan.q_traj[-1]

        q_initial = qf

        #~ *************************************************************

        ############## OPEN THE GRIPPER To PUSH###############

        # Open the gripper to dim calculated for pushing
        print '[PushBack] hand opening to'
        print push_hand_opening * 1000.0
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
        left_plans.append(grasp_plan)

        #~ *************************************************************

        ############  Execute the push trajectory ##############

        back_bin_X = bin_face_pos_world.pose.position.x + 0.43
        push_stop_worldX = back_bin_X - max_diag - 0.010

        left_pushStop_Pt_pos = deepcopy(left_push_Pt)
        left_pushStop_Pt_pos[0] = push_stop_worldX

        #Push until the end

        planner = IK(q0=q_initial,
                     target_tip_pos=left_pushStop_Pt_pos,
                     target_tip_ori=push_orient,
                     joint_topic=joint_topic,
                     tip_hand_transform=push_tip_hand_transform)
        plan = planner.plan()
        plan.setSpeedByName('fast')
        s = plan.success()
        if s:
            print '[PushBack] start point push traj successful'
            left_plans.append(plan)  # Plan 1
        else:
            print '[PushBack] start point push traj fail'
            return (False, False)

        qf = plan.q_traj[-1]
        q_initial = qf

        #################### CLOSE THE GRIPPER GRASP #################
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        left_plans.append(grasp_plan)

        #~ *************************************************************
        # #################### EXECUTE FORWARD ####################

        for numOfPlan in range(0, len(left_plans)):
            if isExecute:
                left_plans[numOfPlan].visualize()
                pauseFunc(withPause)
                left_plans[numOfPlan].execute()

        # #################### RETREAT ####################

        for numOfPlan in range(0, len(left_plans)):
            if isExecute:
                left_plans[numOfPlan].visualizeBackward()
                pauseFunc(withPause)
                left_plans[len(left_plans) - numOfPlan - 1].executeBackward()

    #*******************************************************************

    if right_push:
        # plan store
        right_plans = []

        # set tcp (same as that set in generate dictionary)
        l1 = 0.0
        l2 = 0.0
        l3 = 0.47
        tip_hand_transform = [
            l1, l2, l3, 0, 0, 0
        ]  # to be updated when we have a hand design finalized
        # broadcast frame attached to tcp
        pubFrame(br,
                 pose=tip_hand_transform,
                 frame_id='tip',
                 parent_frame_id='link_6',
                 npub=5)

        #~ *************************************************************

        # GO TO MOUTH OF THE BIN

        q_initial = robotConfig
        # move to bin mouth
        distFromShelf = 0.1
        mouthPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
        mouthPt = coordinateFrameTransform(mouthPt, 'shelf', 'map', listener)
        targetPosition = [
            mouthPt.pose.position.x, mouthPt.pose.position.y,
            mouthPt.pose.position.z
        ]
        gripperOri = [0, 0.7071, 0, 0.7071]

        pubFrame(br,
                 pose=targetPosition + gripperOri,
                 frame_id='target_pose',
                 parent_frame_id='link_6',
                 npub=5)

        planner = IK(q0=q_initial,
                     target_tip_pos=targetPosition,
                     target_tip_ori=gripperOri,
                     tip_hand_transform=tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.setSpeedByName('faster')
        s = plan.success()
        if s:
            print '[PushBack] move to bin mouth in push-back code successful (right push)'
            right_plans.append(plan)  # Plan 0
        else:
            print '[PushBack] move to bin mouth in push-back code fail (right push)'
            return (False, False)

        qf = plan.q_traj[-1]

        q_initial = qf
        #################### CLOSE THE GRIPPER GRASP #################
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        right_plans.append(grasp_plan)
        #~ *************************************************************

        ###### MOVE THE ROBOT INSIDE THE BIN WITH PUSH ORIENTATION ######

        # set push_tcp
        push_l1 = 0.0
        push_l2 = 0.0

        push_l3 = 0.47
        push_tip_hand_transform = [
            l1, l2, l3, 0, 0, 0
        ]  # to be updated when we have a hand design finalized
        # broadcast frame attached to grasp_tcp
        pubFrame(br,
                 pose=push_tip_hand_transform,
                 frame_id='push_tip',
                 parent_frame_id='link_6',
                 npub=10)

        distFromShelf = -0.015
        InbinPt, temp = getBinMouthAndFloor(distFromShelf, binNum)
        InbinPt = coordinateFrameTransform(InbinPt, 'shelf', 'map', listener)
        prepush_targetPosition = [
            InbinPt.pose.position.x, right_push_Pt[1], InbinPt.pose.position.z
        ]  #matching world_Y of first push pt

        pubFrame(br,
                 pose=prepush_targetPosition + push_orient,
                 frame_id='target_pose',
                 parent_frame_id='map',
                 npub=10)

        planner = IK(q0=q_initial,
                     target_tip_pos=prepush_targetPosition,
                     target_tip_ori=push_orient,
                     tip_hand_transform=push_tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        s = plan.success()
        plan.setSpeedByName('fast')
        s = plan.success()
        if s:
            print '[PushBack] move inside bin in push orient successful'
            #~ print '[PushBack] tcp at:'
            #~ print(pregrasp_targetPosition)
            #~ plan.visualize()
            right_plans.append(plan)  # Plan 1
            #~ if isExecute:
            #~ pauseFunc(withPause)
            #~ plan.execute()
        else:
            print '[PushBack] move inside bin in push orient fail'
            return (False, False)

        qf = plan.q_traj[-1]

        q_initial = qf

        #~ *************************************************************

        ############## OPEN THE GRIPPER To PUSH###############

        # Open the gripper to dim calculated for pushing
        print '[PushBack] hand opening to'
        print push_hand_opening * 1000.0
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (push_hand_opening))
        right_plans.append(grasp_plan)

        #~ *************************************************************

        ############  Execute the push trajectory ##############

        back_bin_X = bin_face_pos_world.pose.position.x + 0.42
        push_stop_worldX = back_bin_X - max_diag - 0.010

        right_pushStop_Pt_pos = deepcopy(right_push_Pt)
        right_pushStop_Pt_pos[0] = push_stop_worldX

        #Push until the end

        planner = IK(q0=q_initial,
                     target_tip_pos=right_pushStop_Pt_pos,
                     target_tip_ori=push_orient,
                     joint_topic=joint_topic,
                     tip_hand_transform=push_tip_hand_transform)
        plan = planner.plan()
        plan.setSpeedByName('fast')
        s = plan.success()
        if s:
            print '[PushBack] start point push traj successful'
            right_plans.append(plan)  # Plan 1
        else:
            print '[PushBack] start point push traj fail'
            return (False, False)

        qf = plan.q_traj[-1]
        q_initial = qf

        #################### CLOSE THE GRIPPER GRASP #################
        grasp_plan = EvalPlan('moveGripper(%f, 100)' % (0.0))
        right_plans.append(grasp_plan)

        #~ *************************************************************
        # #################### EXECUTE FORWARD ####################

        for numOfPlan in range(0, len(right_plans)):
            if isExecute:
                right_plans[numOfPlan].visualize()
                pauseFunc(withPause)
                right_plans[numOfPlan].execute()

        # #################### RETREAT ####################

        for numOfPlan in range(0, len(right_plans)):
            if isExecute:
                right_plans[numOfPlan].visualizeBackward()
                pauseFunc(withPause)
                right_plans[len(right_plans) - numOfPlan - 1].executeBackward()

    if right_push or left_push:
        push_possible = True
    else:
        push_possible = False

    return (push_possible, False)
Ejemplo n.º 21
0
def scoop(objPose = [1.95,0.25,1.4,0,0,0,1],
            binNum=3,
            objId = 0,
            bin_contents = None,
            robotConfig = None, 
            shelfPosition = [1.9116,-0.012498,-0.4971], 
            forceThreshold = 1, 
            isExecute = True,
            withPause = True,
            withVisualize = False):
    ## objPose: world position and orientation frame attached to com of object in quaternion form. XYZ
    ## objId: object identifier
    ## robotConfig: current time robot configuration
    ## shelfPosition: shelf position in world frame
    ## force threshold: amount fo force needed to say we have a grasp
    
    setSpeedByName(speedName = 'faster')
    
    joint_topic = '/joint_states' 
    planSuccess = True
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    
    # shelf variables
    if binNum <3:
        pretensionDelta = 0.03
    if binNum > 2 and binNum < 6:
        pretensionDelta = 0.009
    if binNum > 5 and binNum < 9:
        pretensionDelta = 0.009
    if binNum > 8:
        pretensionDelta = 0.03
    #pretensionDelta = 0.00
    lipHeight = 0.025
    
    # plan store
    plans = []
    ## get object position (world frame)
    objPosition = getObjCOM(objPose[0:3], objId)
        
    ## move gripper to object com outside bin along world y direction and
    ## move the gripper over the lip of the bin    
    # set tcp
    tcpXOffset = 0.018
    l2 = 0.47  
    tip_hand_transform = [tcpXOffset, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    pubFrame(br, pose=tip_hand_transform, frame_id='tip', parent_frame_id='link_6', npub=5)
    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    tcpPosHome = tcpPos
    # set scoop orientation (rotate wrist)
    scoopOrientation = [0, 0.7071, 0, 0.7071]
    
    
    # set first target to move gripper in front of the object and adjust height to middle of bin
    distFromShelf = 0.05
    wristWidth = 0.0725 # this is actually half the wrist width
    (binMouth, binFloorHeight) = getBinMouthAndFloor(distFromShelf, binNum)
    pose_world = coordinateFrameTransform(binMouth[0:3], 'shelf', 'map', listener)
    binMouth=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    verticalOffsetLip = 0.00 # we need this so we don't damage the sucker
    wH = 0.075
    targetPosition = [binMouth[0], objPosition[1], binMouth[2]+tcpXOffset-wH+lipHeight-pretensionDelta]
    
    ## check to make sure we are inside the bin and not colliding with sides:
    minHeight, maxHeight, leftWall, rightWall = BinBBDims(binNum)
    leftWall = coordinateFrameTransform([leftWall,0,0], 'shelf', 'map', listener)
    leftWall = leftWall.pose.position.y
    rightWall = coordinateFrameTransform([rightWall,0,0], 'shelf', 'map', listener)
    rightWall = rightWall.pose.position.y
    
    interiorLipBin = [0,0.40,0] # define over the lip distance in shelf frame
    interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener)
    stepOverLip = interiorLipBin.pose.position.x
    fStroke = 0.20
    sStroke = 0.19
    binLengthSafety = 0.015
    
    if targetPosition[1] + 0.04 > leftWall:
        interiorLipBin = [0,0.36,0] # define over the lip distance in shelf frame
        interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener)
        stepOverLip = interiorLipBin.pose.position.x
        fStroke = 0.17
    
    if targetPosition[1] - 0.04 < leftWall:
        interiorLipBin = [0,0.36,0] # define over the lip distance in shelf frame
        interiorLipBin = coordinateFrameTransform(interiorLipBin, 'shelf', 'map', listener)
        stepOverLip = interiorLipBin.pose.position.x
        fStroke = 0.17 
        
    if targetPosition[1] + binLengthSafety > leftWall:
        targetPosition[1] = leftWall 
    
    if targetPosition[1] - binLengthSafety < rightWall:
        targetPosition[1] = rightWall
    
    frontOfObjectPtOutOfBin = targetPosition
    q_initial = robotConfig
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()                                    # Plan 0
    if s:
        print '[Scoop] move to COM in y successful'
        #~ print '[Scoop] tcp at:'
        #~ print(targetPosition)
        visualizeFunc(withVisualize, plan)
        plans.append(plan)
    else:
        print '[Scoop] move to COM in y fail'
        return (False, False)
        
    qf = plan.q_traj[-1]
    
    ## push spatula against base of bin (pre-tension)
    binFloorHeight = coordinateFrameTransform([0,0,binFloorHeight], 'shelf', 'map', listener)
    q_initial = qf
    percentTilt = 0.1 # 10 percent 
    scoopOrientation = [0, 0.7071*(1+percentTilt), 0, 0.7071*(1-percentTilt)]
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()                                    # Plan 1
    if s:
        print '[Scoop] reorient the hand'
        #~ print '[Scoop] tcp at:'
        #~ print(targetPosition)
        plans.append(plan)
        visualizeFunc(withVisualize, plan)
    else:
        print '[Scoop] reorient the hand fail'
        return (False, False)
    qf = plan.q_traj[-1]
    
    # set second target, go over the lip of the bin
    deltaX = np.add(-targetPosition[0],stepOverLip)
    targetPosition = np.add(targetPosition, [deltaX,0,0])
    q_initial = qf
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()                                    # Plan 2
    if s:
        print '[Scoop] move to inside the lip success'
        #~ print '[Scoop] tcp at:'
        #~ print(targetPosition)
        plans.append(plan)
        visualizeFunc(withVisualize, plan)
    else:
        print '[Scoop] move to inside the lip fail'
        return (False, False)
    qf = plan.q_traj[-1]
    
    ## perform bin length stroke to middle
    q_initial = qf
    
    targetPosition = np.add(targetPosition, [fStroke,0,-lipHeight])
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()                                    # Plan 3
    if s:
        print '[Scoop] stroke middle of bin success'
        #~ print '[Scoop] tcp at:'
        #~ print(targetPosition)
        plans.append(plan)
        visualizeFunc(withVisualize, plan)
    else:
        print '[Scoop] stroke middle of bin fail'
        return (False, False)
    qf = plan.q_traj[-1]    
    ## perform bin length stroke to end
    q_initial = qf
    targetPosition = np.add(targetPosition, [sStroke,0,0])
    scoopOrientation = [0, 0.7071+0.11/4, 0, 0.7071-0.11/4]
    planner = IK(q0 = q_initial, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation, tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
    plan = planner.plan()
    s = plan.success()                                    # Plan 4
    if s:
        print '[Scoop] stroke middle to end of bin success'
        #~ print '[Scoop] tcp at:'
        #~ print(targetPosition)
        plans.append(plan)
        visualizeFunc(withVisualize, plan)
    else:
        print '[Scoop] stroke middle to end of bin fail'
        return (False, False)
        
    qf = plan.q_traj[-1]
    
    ## close gripper
    #~ closeGripper(forceThreshold)
    
    
    closeGripper(forceThreshold)
    execution_possible = True
    ## execute
    isNotInCollision = True
    for numOfPlan in range(0, len(plans)):
        plans[numOfPlan].visualize()
        if isExecute:
            if numOfPlan == 3:
                openGripper()
            pauseFunc(withPause)
            isNotInCollision = plans[numOfPlan].execute()
            if numOfPlan == 3:
                closeGripper(forceThreshold)
                plans[numOfPlan].executeBackward()
                openGripper()
                plans[numOfPlan].execute()
            
            if not isNotInCollision:          
                planFailNum = numOfPlan       
                print '[Scoop] collision detected'
                break                         
            if numOfPlan == 4:
                closeGripper(forceThreshold)
                rospy.sleep(0.5)
                while True:
                    APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg() 
                    q0 = APCrobotjoints.position
                    
                    if len(q0) == 2 or len(q0) == 8:
                        q0 = q0[-2:]    # get last 2
                        break
                    
                gripper_q0=np.fabs(q0)
                drop_thick=0.000001 # finger gap =0.002m = .5 mm
                if gripper_q0[0] < drop_thick:
                    print '[Scoop] ***************'
                    print '[Scoop] Could not grasp'
                    print '[Scoop] ***************'
                    execution_possible = False
                else:
                    print '[Scoop] ***************'
                    print '[Scoop] Grasp Successful'
                    print '[Scoop] ***************'
                    execution_possible = True
                
    if not isNotInCollision:
        for numOfPlan in range(0, len(planFailNum)):
            plans[planFailNum-numOfPlan].executeBackward()
            
    ## retreat
    
    for numOfPlan in range(0, len(plans)):
        if withVisualize:
            plans[len(plans)-numOfPlan-1].visualizeBackward()
        if isExecute:
            pauseFunc(withPause)
            plans[len(plans)-numOfPlan-1].executeBackward()
            
    return True, execution_possible