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()
Example #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()
Example #3
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
Example #4
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
Example #5
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
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