def suction_side(objPose = [1.95,1.25,1.4,0,0,0,1],
            binNum=4,
            objId = 0,
            bin_contents = None,
            robotConfig = None, 
            shelfPosition = [1.9019,0.00030975,-0.503], 
            forceThreshold = 1, 
            isExecute = True,
            withPause = 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
    joint_topic = '/joint_states'  
    ## initialize listener rospy
    listener = tf.TransformListener()
    #rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    
    # shelf variables
    pretensionDelta = 0.00
    lipHeight = 0.035
    
    #tcp_offset_variables
    
    

    
    # plan store
    plans = []
    ## get object position (world frame)
    objPosition = getObjCOM(objPose[0:3], objId)
    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]
    vertical_index=np.argmax(np.fabs(obj_pose_orient[2,0:3]))
    
    object_dim=get_obj_dim(objId)
    vertical_dim=object_dim[vertical_index]
    
    
    s1=np.fabs(obj_pose_orient[1,0])*object_dim[0]
    s2=np.fabs(obj_pose_orient[1,1])*object_dim[1]
    s3=np.fabs(obj_pose_orient[1,2])*object_dim[2]
    s4=np.fabs(obj_pose_orient[1,vertical_index])*object_dim[vertical_index]
    
    horizontal_dim=s1+s2+s3-s4

    hand_gap=0
    gripper.close()
        
    while True:
        APCrobotjoints = ROS_Wait_For_Msg(joint_topic, sensor_msgs.msg.JointState).getmsg() 
        q0 = APCrobotjoints.position
        if len(q0) < 6:
            continue
        else:
            break
    
        
    ## move gripper to object com outside bin along world y direction and
    ## move the gripper over the lip of the bin    
    # set tcp
    vert_offset=.035
    l2 = 0.43  #change this to edit where you think the cup is
    l2 =.44
    
    tip_hand_transform = [-vert_offset, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    # for i in range(5):
        # 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='tip', 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]
    
    
    #this may cause issues!!!
    tcpPosHome = tcpPos
    
    
    # set scoop orientation (rotate wrist)
    
    
    
    
    
    distFromShelf = 0.05
    wristWidth = 0.0725 # this is actually half the wrist width
    (binMouth,bin_height,bin_width) = getBinMouth(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]
    
    
    
    object_depth=objPosition[0]-binMouth[0]
    
    
    finger_length=.23     
    finger_width=.08
    
    up_offset=.05
    down_offset=.01
    cup_to_spatula=.08
    hand_width=.15
    max_gripper_width=.110
    hand_top_offset=hand_width/2+vert_offset+.015
    hand_bot_offset=hand_width/2-vert_offset+.015
    
    up_down_adjust=.025
    
    side_offset=.03
    
    binFloorHeight=binMouth[2]-bin_height/2
    binCeilHeight=binMouth[2]+bin_height/2
    
    
    min_height=binFloorHeight+lipHeight+finger_width/2+down_offset
    desired_height=objPosition[2]
    max_height=binCeilHeight-lipHeight-finger_width/2-down_offset-up_down_adjust
    
    target_height=desired_height
    if target_height<min_height:
        target_height=min_height
    if target_height>max_height:
        target_height=max_height
    
    bin_sideways=binMouth[1]
    sidepos=objPosition[1]
    
    horz_offset=0.01
    
    #this determines bin sides based on object
    #(binSmall,binLarge)=getSides(objPosition[1],listener)
    (binSmall,binLarge)=getSides(binNum,listener)
    binRight=binSmall
    binLeft=binLarge
    
 
    if sidepos>bin_sideways:
        #this stuff is what happens if the object is to the left
        print '[SucSide] Object is to the left'
        
        #turn the suction cup so that it is sideways left
        scoopOrientation = [.5,-.5,.5,-.5]
        side_waypoint1a=binRight+cup_to_spatula+.01
        side_waypoint1b=sidepos-horizontal_dim/2-2*horz_offset
        if side_waypoint1a>side_waypoint1b:
            side_waypoint1=side_waypoint1a
            print '[SucSide] side waypoint A'
        else:
            side_waypoint1=side_waypoint1b
            print '[SucSide] side waypoint B'
            
        #side_waypoint2=sidepos+horizontal_dim-side_offset
        side_waypoint2=binLeft-horizontal_dim-horz_offset
        
        if side_waypoint2<side_waypoint1:
            if side_waypoint2<side_waypoint1a:
                print '[SucSide] Not enought gap'
                return (False, False)
            else:
                side_waypoint1=side_waypoint2
        

        
        
    else:
        #this stuff is what happens if the object is to the right
        print '[SucSide] Object is to the right'
        
        #turn the suction cup so that it is sideways right
        scoopOrientation = [0.5,.5,.5,.5]
        side_waypoint1a=binLeft-cup_to_spatula-.01
        side_waypoint1b=sidepos+horizontal_dim/2+2*horz_offset
        if side_waypoint1a<side_waypoint1b:
            side_waypoint1=side_waypoint1a
            print '[SucSide] side waypoint A'
        else:
            side_waypoint1=side_waypoint1b
            print '[SucSide] side waypoint B'
        
        #side_waypoint2=sidepos-horizontal_dim+side_offset
        side_waypoint2=binRight+horizontal_dim+horz_offset
        
        if side_waypoint2>side_waypoint1:
            
            if side_waypoint2>side_waypoint1a:
                print '[SucSide] Not enought gap'
                return (False, False)
            else:
                side_waypoint1=side_waypoint2
        


        
    targetPositionList=[    
    [binMouth[0]-.15, side_waypoint1, target_height+up_down_adjust],
    [objPosition[0], side_waypoint1, target_height+up_down_adjust],
    [objPosition[0], side_waypoint2, target_height+up_down_adjust],
    [objPosition[0], side_waypoint2, target_height]]
    
    
    qf=q0
    
    
    for tp_index in range(0, len(targetPositionList)):
    
        targetPosition = targetPositionList[tp_index]
        frontOfObjectPtOutOfBin = targetPosition
        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()
        if s:
            print '[SucSide] move to COM in y successful'
            print '[SucSide] tcp at:', targetPosition
            plan.visualize(hand_param=hand_gap)
            plans.append(plan)
            #if isExecute:
            #    pauseFunc(withPause)
            #    plan.execute()
        else:
            print '[SucSide] move to COM in y fail'
            return (False, False)
            
        #print plan.q_traj
        qf = plan.q_traj[-1]
        print '[SucSide] qf:', qf
    
    #set robot speed
    setSpeedByName(speedName = 'faster')
    for numOfPlan in range(0, len(plans)):
        if numOfPlan >=len(plans)-2:
            setSpeedByName(speedName = 'fast')
        if isExecute:
            plans[numOfPlan].visualize(hand_param=hand_gap)
            pauseFunc(withPause)
            plans[numOfPlan].execute()
            
    
    
    suction.start()
    
    final_hand_gap=110
    
    gripper.set_force(12)
    gripper.grasp(move_pos=final_hand_gap)
    gripper.close()
    
    hand_gap=final_hand_gap
    print '[SucSide] hand_gap:', hand_gap
    
    continue_suction=suction_items(objId)
    
    if continue_suction:
        print '[SucSide] object is of type that suction side will try to remove from bin'
    else:
        print '[SucSide] object is to big to remove from bin'
        
    ## retreat
    for numOfPlan in range(0, len(plans)-1):
        plans[len(plans)-numOfPlan-1].visualizeBackward(hand_param=hand_gap)
        if isExecute:
            pauseFunc(withPause)
            plans[len(plans)-numOfPlan-1].executeBackward()
            if not continue_suction:
                suction.stop()
    
    rospy.sleep(3)
                
    print '[SucSide] Is suction in contact still? Lets see:'
    print '[SucSide] suction.check(): ', suction.check()
    print '[SucSide] suction.check(): ', suction.check()
    
    if suction.check():
        #set robot speed
        print '[SucSide] got item. continuing. suction'
        setSpeedByName(speedName = 'fast')
        return (True, True)
    else:
        print '[SucSide] did not get item. Stopping suction'
        suction.stop()
        return (True, False)
Beispiel #2
0
            shelf_gap=AdjustedX_list[i+1]-AdjustedX_list[i]
            shelfXVec=[AdjustedX_list[i+1],0,0]
            
            newShelfXVec=coordinateFrameTransform(shelfXVec, 'shelf', 'map', listener)
            
            return (newShelfXVec.pose.position.y-shelf_gap,newShelfXVec.pose.position.y)
    return 'failed'
    
   

    
    
if __name__=='__main__':
    rospy.init_node('listener', anonymous=True)
    
    suction.stop()
    
    posesrv = rospy.ServiceProxy('/pose_service', GetPose)   # should move service name out
    rospy.sleep(0.5)
    data = posesrv('','')
    pos_x= data.pa.object_list[0].pose.position.x
    pos_y= data.pa.object_list[0].pose.position.y
    pos_z= data.pa.object_list[0].pose.position.z
    orient_x= data.pa.object_list[0].pose.orientation.x
    orient_y= data.pa.object_list[0].pose.orientation.y
    orient_z= data.pa.object_list[0].pose.orientation.z
    orient_w= data.pa.object_list[0].pose.orientation.w
    #objPose =[1.55620419979, .281148612499, 1.14214038849,0,0,0,0]
    
    flush_grasp(
        objPose = [pos_x,pos_y,pos_z,orient_x,orient_y,orient_z,orient_w],
    def try_suction(target_x,target_y,qf,num_iter):

        
        object_depth=target_x-binMouth[0]
        sidepos=min(max(target_y,small_limit),large_limit)
        
        if object_depth<finger_length:
            print 'shallow suction'
            h1=binCeilHeight-lipHeight-cup_to_spatula
            h2=binFloorHeight+vertical_dim-down_offset
        else:
            print 'deep suction'
            h1=binCeilHeight-lipHeight-hand_top_offset
            h2a=binFloorHeight+vertical_dim-down_offset
            h2b=binFloorHeight+hand_bot_offset+lipHeight
            h2=max(h2a,h2b)

        h2=max(h2,binFloorHeight)
                
      
            
        if h2>h1:
            print 'cant go in'
            return False, False
            
        final_hand_gap=max_gripper_width
        

        targetPositionList=[
        [binMouth[0]-.15, sidepos, h1],
        [target_x, sidepos, h1],
        [target_x, sidepos, h2]]
        
        
        for tp_index in range(0, len(targetPositionList)):
        
            targetPosition = targetPositionList[tp_index]
            planner = IK(q0 = qf, target_tip_pos = targetPosition, target_tip_ori = scoopOrientation,
            tip_hand_transform=tip_hand_transform, joint_topic=joint_topic)
            
            plan = planner.plan()
            s = plan.success()
            print 'Plan number:',tp_index+1
            if s:
                print 'Plan calculated successfully'
                plans.append(plan)

            else:
                print 'Plan calulation failed'
                return (False, False)
                
            qf = plan.q_traj[-1]
            
        
        #set robot speed
        setSpeedByName(speedName = 'faster')
        
        
        hand_gap=0
        gripper.close()
        
        
        for numOfPlan in range(0, len(plans)):
            if isExecute:
                plans[numOfPlan].visualize(hand_param=hand_gap)
                pauseFunc(withPause)
                plans[numOfPlan].execute()
        
        
        suction.start()
        
        gripper.set_force(10)
        gripper.grasp(move_pos=max_gripper_width)
        gripper.close()
        hand_gap=max_gripper_width
        
        print hand_gap
        
        ## retreat
        if num_iter==0:
            plan_offset=2
        else:
            plan_offset=0
        
        for numOfPlan in range(0, len(plans)-plan_offset):
            plans[len(plans)-numOfPlan-1].visualizeBackward(hand_param=hand_gap)
            if isExecute:
                #backward_plan=plans[len(plans)-numOfPlan-1]
                #q_first=backward_plan[0]
                #q_last=backward_plan[len(backward_plan)-1]
                
                #if(abs(q_first[5]-q_last[5])>math.pi/2):
                #    print 'something weird is going on with joint 5 rotation'
                #    break
                
                pauseFunc(withPause)
                plans[len(plans)-numOfPlan-1].executeBackward()
            
                
                
                

        time.sleep(4)
        my_return=suction.check() or suction_override(objId)
        
        if my_return==True:
            #set robot speed
            setSpeedByName(speedName = 'fast')
            return (True, True)
        else:
            suction.stop()
            return (True, False)
Beispiel #4
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 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 suction_down(objPose=[1.95, 1.25, 1.4, 0, 0, 0, 1],
                 binNum=4,
                 objId=0,
                 bin_contents=None,
                 robotConfig=None,
                 shelfPosition=[1.9019, 0.00030975, -0.503],
                 forceThreshold=1,
                 isExecute=True,
                 withPause=True):
    ## 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
    joint_topic = '/joint_states'
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)

    # shelf variables
    pretensionDelta = 0.00
    lipHeight = 0.035

    ## get object position (world frame)
    objPosition = getObjCOM(objPose[0:3], objId)
    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]
    vertical_index = np.argmax(np.fabs(obj_pose_orient[2, 0:3]))

    object_dim = get_obj_dim(objId)
    object_dim = adjust_obj_dim(objId, object_dim)
    vertical_dim = object_dim[vertical_index]

    while True:
        APCrobotjoints = ROS_Wait_For_Msg(joint_topic,
                                          sensor_msgs.msg.JointState).getmsg()
        q0 = APCrobotjoints.position
        if len(q0) >= 6:
            q0 = q0[
                0:
                6]  # take first 6, because in virtual environmet there will be additional 2 hand joint
            break

    ## move gripper to object com outside bin along world y direction and
    ## move the gripper over the lip of the bin
    # set tcp
    vert_offset = .035
    l2 = .44

    tip_hand_transform = [
        -vert_offset, 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
    ]

    #this may cause issues!!!
    tcpPosHome = tcpPos

    # set scoop orientation (rotate wrist)
    scoopOrientation = [0.7071, 0, 0.7071, 0]

    distFromShelf = 0.05
    wristWidth = 0.0725  # this is actually half the wrist width
    (binMouth, bin_height, bin_width) = getBinMouth(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
    ]

    hand_gap = 0
    gripper.close()

    finger_length = .23
    finger_width = .08

    up_offset = .05
    down_offset = -.025
    cup_to_spatula = .08
    hand_width = .15
    max_gripper_width = 110
    hand_top_offset = hand_width / 2 + vert_offset + .01
    hand_bot_offset = hand_width / 2 - vert_offset

    #this determines bin stuff based on bin input
    binFloorHeight = binMouth[2] - bin_height / 2
    binCeilHeight = binMouth[2] + bin_height / 2

    #this determines bin sides based on object
    #(binSmall,binLarge)=getSides(objPosition[1],listener)
    (binSmall, binLarge) = getSides(binNum, listener)

    horz_offset = 0
    small_limit = binSmall + finger_width / 2 + horz_offset
    large_limit = binLarge - finger_width / 2 - horz_offset

    plans = []
    plan = Plan()

    if objPosition[1] < 0:
        link6_angle = (q0[5] - math.pi)
        possible_start_config = [
            0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539,
            0.09102420595, 3.33888627518 - 2 * math.pi
        ]
    else:
        link6_angle = (q0[5] + math.pi)
        possible_start_config = [
            0.007996209289, -0.6193283503, 0.5283758664, -0.1974229539,
            0.09102420595, 3.33888627518
        ]

    #start_config=possible_start_config
    start_config = [q0[0], q0[1], q0[2], q0[3], q0[4], link6_angle]
    planner = IKJoint(q0=q0, target_joint_pos=start_config)
    plan = planner.plan()
    #plan.q_traj=[q0,start_config]

    plans.append(plan)

    qf = plan.q_traj[-1]

    def get_motion_param(target_x, target_y):
        object_depth = target_x - binMouth[0]
        sidepos = min(max(target_y, small_limit), large_limit)

        if object_depth < finger_length:
            print '[SucDown] shallow suction'
            h1 = binCeilHeight - lipHeight - cup_to_spatula
            h2 = binFloorHeight + vertical_dim - down_offset
        else:
            print '[SucDown] deep suction, quitting'
            h1 = binCeilHeight - lipHeight - hand_top_offset
            h2a = binFloorHeight + vertical_dim - down_offset
            h2b = binFloorHeight + hand_bot_offset + lipHeight
            h2 = max(h2a, h2b)
            return False, h1, h2, sidepos

        h2 = max(h2, binFloorHeight)

        if h2 > h1:
            print '[SucDown] cant go in'
            return False, h1, h2, sidepos
        return True, h1, h2, sidepos

    def generate_plan(targetPositionList, plans, qf):
        for tp_index in range(0, len(targetPositionList)):

            targetPosition = targetPositionList[tp_index]
            planner = IK(q0=qf,
                         target_tip_pos=targetPosition,
                         target_tip_ori=scoopOrientation,
                         tip_hand_transform=tip_hand_transform,
                         joint_topic=joint_topic)

            plan = planner.plan()
            s = plan.success()
            print '[SucDown] Plan number:', tp_index + 1
            if s:
                print '[SucDown] Plan calculated successfully'
                plans.append(plan)

            else:
                print '[SucDown] Plan calulation failed'
                return (False, plans, qf)

            qf = plan.q_traj[-1]
        return (True, plans, qf)

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

    def execute_backward(plans, hand_gap, plan_offset):
        for numOfPlan in range(0, len(plans) - plan_offset):
            plans[len(plans) - numOfPlan -
                  1].visualizeBackward(hand_param=hand_gap)
            if isExecute:
                pauseFunc(withPause)
                plans[len(plans) - numOfPlan - 1].executeBackward()

    def try_suction(target_x, target_y, plans, qf, num_iter):

        (continue_val, h1, h2, sidepos) = get_motion_param(target_x, target_y)

        if continue_val == False:
            return False, False

        final_hand_gap = max_gripper_width

        targetPositionList = [[target_x, sidepos, h1], [target_x, sidepos, h2]]

        (continue_val, plans,
         throwaway) = generate_plan(targetPositionList, plans, qf)

        if continue_val == False:
            return False, False

        #set robot speed
        setSpeedByName(speedName='faster')

        hand_gap = 0
        gripper.close()

        execute_forward(plans, hand_gap)

        #suction.start()

        gripper.set_force(10)
        gripper.grasp(move_pos=max_gripper_width)
        gripper.close()
        hand_gap = max_gripper_width

        print '[SucDown] hand_gap:', hand_gap

        execute_backward(plans, hand_gap, 0)

        #time.sleep(4)
        my_return = suction.check() or suction_override(objId)

        if my_return == True:
            #set robot speed
            setSpeedByName(speedName='fast')
            return (True, True)
        else:
            #suction.stop()
            return (True, False)

    target_offset = get_test_offset(objId)
    target_x_list = [
        objPosition[0], objPosition[0] - target_offset, objPosition[0],
        objPosition[0], objPosition[0] + target_offset
    ]
    target_y_list = [
        objPosition[1], objPosition[1], objPosition[1] + target_offset,
        objPosition[1] - target_offset, objPosition[1]
    ]

    count = 0

    suction_succeed = False

    overall_plan_succeed = False

    (continue_val, h1, h2,
     sidepos) = get_motion_param(target_x=target_x_list[0],
                                 target_y=target_y_list[0])

    if continue_val == False:
        return False, False

    #targetPositionList=[
    #[binMouth[0]-.15, sidepos, h1],
    #[binMouth[0]-.15, sidepos, h1],
    #[target_x_list[0], sidepos, h1]]

    targetPositionListA = [[binMouth[0] - .15, sidepos, h1],
                           [binMouth[0] - .15, sidepos, h1]]

    targetPositionListB = [[target_x_list[0], sidepos, h1]]

    (continue_val, plans1, qf) = generate_plan(targetPositionListA, plans, qf)

    if continue_val == False:
        return False, False

    (continue_val, plans2, qf) = generate_plan(targetPositionListB, [], qf)

    if continue_val == False:
        return False, False

    setSpeedByName(speedName='yolo')
    execute_forward(plans1, 0)

    setSpeedByName(speedName='faster')
    execute_forward(plans2, 0)

    suction.start()
    #time.sleep(6)

    #plans_store=plans
    plans = []

    for count in range(0, len(target_x_list)):

        (plan_succeed,
         suction_succeed) = try_suction(target_x=target_x_list[count],
                                        target_y=target_y_list[count],
                                        plans=plans,
                                        qf=qf,
                                        num_iter=count)

        if suction_succeed:
            overall_plan_succeed = True
            break
            #return (plan_succeed,suction_succeed)

        if plan_succeed == True:
            overall_plan_succeed = True

        while True:
            APCrobotjoints = ROS_Wait_For_Msg(
                joint_topic, sensor_msgs.msg.JointState).getmsg()
            qf = APCrobotjoints.position
            if len(qf) < 6:
                continue
            else:
                break
        print '[SucDown] qf:', hand_gap

        plans = []
        count = count + 1

    execute_backward(plans2, 0, 0)

    if not suction_succeed:
        suction.stop()

    return (overall_plan_succeed, suction_succeed)
def suction_side(objPose=[1.95, 1.25, 1.4, 0, 0, 0, 1],
                 binNum=4,
                 objId=0,
                 bin_contents=None,
                 robotConfig=None,
                 shelfPosition=[1.9019, 0.00030975, -0.503],
                 forceThreshold=1,
                 isExecute=True,
                 withPause=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
    joint_topic = '/joint_states'
    ## initialize listener rospy
    listener = tf.TransformListener()
    #rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)

    # shelf variables
    pretensionDelta = 0.00
    lipHeight = 0.035

    #tcp_offset_variables

    # plan store
    plans = []
    ## get object position (world frame)
    objPosition = getObjCOM(objPose[0:3], objId)
    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]
    vertical_index = np.argmax(np.fabs(obj_pose_orient[2, 0:3]))

    object_dim = get_obj_dim(objId)
    vertical_dim = object_dim[vertical_index]

    s1 = np.fabs(obj_pose_orient[1, 0]) * object_dim[0]
    s2 = np.fabs(obj_pose_orient[1, 1]) * object_dim[1]
    s3 = np.fabs(obj_pose_orient[1, 2]) * object_dim[2]
    s4 = np.fabs(obj_pose_orient[1,
                                 vertical_index]) * object_dim[vertical_index]

    horizontal_dim = s1 + s2 + s3 - s4

    hand_gap = 0
    gripper.close()

    while True:
        APCrobotjoints = ROS_Wait_For_Msg(joint_topic,
                                          sensor_msgs.msg.JointState).getmsg()
        q0 = APCrobotjoints.position
        if len(q0) < 6:
            continue
        else:
            break

    ## move gripper to object com outside bin along world y direction and
    ## move the gripper over the lip of the bin
    # set tcp
    vert_offset = .035
    l2 = 0.43  #change this to edit where you think the cup is
    l2 = .44

    tip_hand_transform = [
        -vert_offset, 0, l2, 0, 0, 0
    ]  # to be updated when we have a hand design finalized
    # broadcast frame attached to tcp
    # for i in range(5):
    # 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='tip',
             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
    ]

    #this may cause issues!!!
    tcpPosHome = tcpPos

    # set scoop orientation (rotate wrist)

    distFromShelf = 0.05
    wristWidth = 0.0725  # this is actually half the wrist width
    (binMouth, bin_height, bin_width) = getBinMouth(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
    ]

    object_depth = objPosition[0] - binMouth[0]

    finger_length = .23
    finger_width = .08

    up_offset = .05
    down_offset = .01
    cup_to_spatula = .08
    hand_width = .15
    max_gripper_width = .110
    hand_top_offset = hand_width / 2 + vert_offset + .015
    hand_bot_offset = hand_width / 2 - vert_offset + .015

    up_down_adjust = .025

    side_offset = .03

    binFloorHeight = binMouth[2] - bin_height / 2
    binCeilHeight = binMouth[2] + bin_height / 2

    min_height = binFloorHeight + lipHeight + finger_width / 2 + down_offset
    desired_height = objPosition[2]
    max_height = binCeilHeight - lipHeight - finger_width / 2 - down_offset - up_down_adjust

    target_height = desired_height
    if target_height < min_height:
        target_height = min_height
    if target_height > max_height:
        target_height = max_height

    bin_sideways = binMouth[1]
    sidepos = objPosition[1]

    horz_offset = 0.01

    #this determines bin sides based on object
    #(binSmall,binLarge)=getSides(objPosition[1],listener)
    (binSmall, binLarge) = getSides(binNum, listener)
    binRight = binSmall
    binLeft = binLarge

    if sidepos > bin_sideways:
        #this stuff is what happens if the object is to the left
        print '[SucSide] Object is to the left'

        #turn the suction cup so that it is sideways left
        scoopOrientation = [.5, -.5, .5, -.5]
        side_waypoint1a = binRight + cup_to_spatula + .01
        side_waypoint1b = sidepos - horizontal_dim / 2 - 2 * horz_offset
        if side_waypoint1a > side_waypoint1b:
            side_waypoint1 = side_waypoint1a
            print '[SucSide] side waypoint A'
        else:
            side_waypoint1 = side_waypoint1b
            print '[SucSide] side waypoint B'

        #side_waypoint2=sidepos+horizontal_dim-side_offset
        side_waypoint2 = binLeft - horizontal_dim - horz_offset

        if side_waypoint2 < side_waypoint1:
            if side_waypoint2 < side_waypoint1a:
                print '[SucSide] Not enought gap'
                return (False, False)
            else:
                side_waypoint1 = side_waypoint2

    else:
        #this stuff is what happens if the object is to the right
        print '[SucSide] Object is to the right'

        #turn the suction cup so that it is sideways right
        scoopOrientation = [0.5, .5, .5, .5]
        side_waypoint1a = binLeft - cup_to_spatula - .01
        side_waypoint1b = sidepos + horizontal_dim / 2 + 2 * horz_offset
        if side_waypoint1a < side_waypoint1b:
            side_waypoint1 = side_waypoint1a
            print '[SucSide] side waypoint A'
        else:
            side_waypoint1 = side_waypoint1b
            print '[SucSide] side waypoint B'

        #side_waypoint2=sidepos-horizontal_dim+side_offset
        side_waypoint2 = binRight + horizontal_dim + horz_offset

        if side_waypoint2 > side_waypoint1:

            if side_waypoint2 > side_waypoint1a:
                print '[SucSide] Not enought gap'
                return (False, False)
            else:
                side_waypoint1 = side_waypoint2

    targetPositionList = [
        [binMouth[0] - .15, side_waypoint1, target_height + up_down_adjust],
        [objPosition[0], side_waypoint1, target_height + up_down_adjust],
        [objPosition[0], side_waypoint2, target_height + up_down_adjust],
        [objPosition[0], side_waypoint2, target_height]
    ]

    qf = q0

    for tp_index in range(0, len(targetPositionList)):

        targetPosition = targetPositionList[tp_index]
        frontOfObjectPtOutOfBin = targetPosition
        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()
        if s:
            print '[SucSide] move to COM in y successful'
            print '[SucSide] tcp at:', targetPosition
            plan.visualize(hand_param=hand_gap)
            plans.append(plan)
            #if isExecute:
            #    pauseFunc(withPause)
            #    plan.execute()
        else:
            print '[SucSide] move to COM in y fail'
            return (False, False)

        #print plan.q_traj
        qf = plan.q_traj[-1]
        print '[SucSide] qf:', qf

    #set robot speed
    setSpeedByName(speedName='faster')
    for numOfPlan in range(0, len(plans)):
        if numOfPlan >= len(plans) - 2:
            setSpeedByName(speedName='fast')
        if isExecute:
            plans[numOfPlan].visualize(hand_param=hand_gap)
            pauseFunc(withPause)
            plans[numOfPlan].execute()

    suction.start()

    final_hand_gap = 110

    gripper.set_force(12)
    gripper.grasp(move_pos=final_hand_gap)
    gripper.close()

    hand_gap = final_hand_gap
    print '[SucSide] hand_gap:', hand_gap

    continue_suction = suction_items(objId)

    if continue_suction:
        print '[SucSide] object is of type that suction side will try to remove from bin'
    else:
        print '[SucSide] object is to big to remove from bin'

    ## retreat
    for numOfPlan in range(0, len(plans) - 1):
        plans[len(plans) - numOfPlan -
              1].visualizeBackward(hand_param=hand_gap)
        if isExecute:
            pauseFunc(withPause)
            plans[len(plans) - numOfPlan - 1].executeBackward()
            if not continue_suction:
                suction.stop()

    rospy.sleep(3)

    print '[SucSide] Is suction in contact still? Lets see:'
    print '[SucSide] suction.check(): ', suction.check()
    print '[SucSide] suction.check(): ', suction.check()

    if suction.check():
        #set robot speed
        print '[SucSide] got item. continuing. suction'
        setSpeedByName(speedName='fast')
        return (True, True)
    else:
        print '[SucSide] did not get item. Stopping suction'
        suction.stop()
        return (True, False)