Ejemplo n.º 1
0
                         tfm.quaternion_from_euler(*tip_hand_transform[3:6]),
                         rospy.Time.now(), 'tip', "link_5")
        br.sendTransform(tuple(target_pose[0:3]), tuple(target_pose[3:7]),
                         rospy.Time.now(), 'target_pose', "world")

        # user_reply=raw_input('Execute? [(y)es/(n)o]: ')
        # if user_reply == 'y':
        # plan.setSpeedByName('fastest')
        plan.execute()
        raw_input('[NIMA] test realsense now: ')

    rospy.sleep(0.8)  # wait for new pointcloud
    obj_pose = capsen.capsen.detectOneObject(obj_list[i], [obj_list[i]],
                                             i,
                                             mode=1)
    pause()

    # go home
    plan = Plan()
    plan.q_traj = [home_joint_pose]
    plan.execute()

    # if obj_pose is None:
    # print 'Detection failed'
    # continue

    # pubFrame(br, obj_pose, 'obj', 'world')

plan = Plan()
plan.q_traj = [home_joint_pose]
plan.execute()
            
        br.sendTransform(tuple(tip_hand_transform[0:3]), 
                         tfm.quaternion_from_euler(*tip_hand_transform[3:6]), 
                         rospy.Time.now(), 'tip', "link_5")
        br.sendTransform(tuple(target_pose[0:3]), tuple(target_pose[3:7]), 
                         rospy.Time.now(), 'target_pose', "world")

        # user_reply=raw_input('Execute? [(y)es/(n)o]: ')
        # if user_reply == 'y':
            # plan.setSpeedByName('fastest')
        plan.execute()
        raw_input('[NIMA] test realsense now: ')
    
    rospy.sleep(0.8)  # wait for new pointcloud
    obj_pose = capsen.capsen.detectOneObject(obj_list[i], [obj_list[i]], i, mode = 1)
    pause()
    
    # go home
    plan = Plan(); plan.q_traj = [home_joint_pose]; 
    plan.execute()
    
    # if obj_pose is None:
        # print 'Detection failed'
        # continue
        
    # pubFrame(br, obj_pose, 'obj', 'world')

plan = Plan(); plan.q_traj = [home_joint_pose]; 
plan.execute()
    
Ejemplo n.º 3
0
def main(argv=None):
    global br
    if argv is None:
        argv = sys.argv
    
    rospy.init_node('capsen_test', anonymous=True)
    rospy.sleep(0.1)
    init()
    rospy.sleep(0.1)
    
    
    #retObjects, retScore = detectObjects('expo_dry_erase_board_eraser', ['expo_dry_erase_board_eraser', 'elmers_washable_no_run_school_glue'], bin_num = 0, mode = 0)
    # retObjects, retScore = detectObjects('expo_dry_erase_board_eraser', ['expo_dry_erase_board_eraser', 'elmers_washable_no_run_school_glue'], bin_num = 0, mode = 0)
    # pose = pose2list(retObjects[0].pose)
    # pubFrame(br, pose, 'obj', 'map')
    # print 'Objects', retObjects
    # pause()
    
    obj_list = ['mommys_helper_outlet_plugs', 
    'kong_duck_dog_toy',
    'first_years_take_and_toss_straw_cup',
    'champion_copper_plus_spark_plug',
    'mead_index_cards',
    'laugh_out_loud_joke_book',
    'highland_6539_self_stick_notes',
    'elmers_washable_no_run_school_glue',
    'stanley_66_052',
    'genuine_joe_plastic_stir_sticks',
    'safety_works_safety_glasses',
    'munchkin_white_hot_duck_bath_toy'
    # 'crayola_64_ct',
    # 'dr_browns_bottle_brush',
    # 'kyjen_squeakin_eggs_plush_puppies',
    # 'expo_dry_erase_board_eraser',
    # 'cheezit_big_original',
    # 'kong_air_dog_squeakair_tennis_ball',
    # 'safety_works_safety_glasses',
    # 'genuine_joe_plastic_stir_sticks'
    ]
    

    bin_contents_all = [
        [     "mommys_helper_outlet_plugs", "mark_twain_huckleberry_finn"      ],
        [        "feline_greenies_dental_treats", "kong_duck_dog_toy"        ],
        [        "first_years_take_and_toss_straw_cup","kong_sitting_frog_dog_toy"        ],
        [        "paper_mate_12_count_mirado_black_warrior", "champion_copper_plus_spark_plug"        ],
        [        "mead_index_cards", "sharpie_accent_tank_style_highlighters"        ],
        [        "mommys_helper_outlet_plugs", "laugh_out_loud_joke_book"        ],
        [        "kyjen_squeakin_eggs_plush_puppies", "highland_6539_self_stick_notes"        ],
        [        "elmers_washable_no_run_school_glue", "champion_copper_plus_spark_plug"        ],
        [        "crayola_64_ct", "stanley_66_052"        ],
        [        "genuine_joe_plastic_stir_sticks", "expo_dry_erase_board_eraser"        ],
        [        "safety_works_safety_glasses"        ],
        [        "kong_air_dog_squeakair_tennis_ball", "munchkin_white_hot_duck_bath_toy"        ]]
    
    #for i, obj_id in enumerate(obj_list):
    for i in range(5,12):
        pose = detectOneObject(obj_list[i], bin_contents_all[i], i)
        pubFrame(br, pose, 'obj_final', 'map')
        print 'Pose', pose
        pause()
Ejemplo n.º 4
0
def main(argv=None):
    global br
    if argv is None:
        argv = sys.argv

    rospy.init_node('capsen_test', anonymous=True)
    rospy.sleep(0.1)
    init()
    rospy.sleep(0.1)

    #retObjects, retScore = detectObjects('expo_dry_erase_board_eraser', ['expo_dry_erase_board_eraser', 'elmers_washable_no_run_school_glue'], bin_num = 0, mode = 0)
    # retObjects, retScore = detectObjects('expo_dry_erase_board_eraser', ['expo_dry_erase_board_eraser', 'elmers_washable_no_run_school_glue'], bin_num = 0, mode = 0)
    # pose = pose2list(retObjects[0].pose)
    # pubFrame(br, pose, 'obj', 'map')
    # print 'Objects', retObjects
    # pause()

    obj_list = [
        'mommys_helper_outlet_plugs', 'kong_duck_dog_toy',
        'first_years_take_and_toss_straw_cup',
        'champion_copper_plus_spark_plug', 'mead_index_cards',
        'laugh_out_loud_joke_book', 'highland_6539_self_stick_notes',
        'elmers_washable_no_run_school_glue', 'stanley_66_052',
        'genuine_joe_plastic_stir_sticks', 'safety_works_safety_glasses',
        'munchkin_white_hot_duck_bath_toy'
        # 'crayola_64_ct',
        # 'dr_browns_bottle_brush',
        # 'kyjen_squeakin_eggs_plush_puppies',
        # 'expo_dry_erase_board_eraser',
        # 'cheezit_big_original',
        # 'kong_air_dog_squeakair_tennis_ball',
        # 'safety_works_safety_glasses',
        # 'genuine_joe_plastic_stir_sticks'
    ]

    bin_contents_all = [
        ["mommys_helper_outlet_plugs", "mark_twain_huckleberry_finn"],
        ["feline_greenies_dental_treats", "kong_duck_dog_toy"],
        ["first_years_take_and_toss_straw_cup", "kong_sitting_frog_dog_toy"],
        [
            "paper_mate_12_count_mirado_black_warrior",
            "champion_copper_plus_spark_plug"
        ], ["mead_index_cards", "sharpie_accent_tank_style_highlighters"],
        ["mommys_helper_outlet_plugs", "laugh_out_loud_joke_book"],
        [
            "kyjen_squeakin_eggs_plush_puppies",
            "highland_6539_self_stick_notes"
        ],
        [
            "elmers_washable_no_run_school_glue",
            "champion_copper_plus_spark_plug"
        ], ["crayola_64_ct", "stanley_66_052"],
        ["genuine_joe_plastic_stir_sticks", "expo_dry_erase_board_eraser"],
        ["safety_works_safety_glasses"],
        [
            "kong_air_dog_squeakair_tennis_ball",
            "munchkin_white_hot_duck_bath_toy"
        ]
    ]

    #for i, obj_id in enumerate(obj_list):
    for i in range(5, 12):
        pose = detectOneObject(obj_list[i], bin_contents_all[i], i)
        pubFrame(br, pose, 'obj_final', 'map')
        print 'Pose', pose
        pause()
Ejemplo n.º 5
0
def percept_hand(obj_pose = None, binNum = 0, obj_id = None, bin_contents = None, isExecute = True, withPause = True):
    capsen.capsen.init()
    home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4]
    planner = IKJoint(target_joint_pos=home_joint_pose); 
    #plan = Plan()
    #plan.q_traj = [home_joint_pose]; 
    plan = planner.plan()
    plan.visualize(); 
    plan.setSpeedByName('yolo'); 
    if withPause:
        print '[Percept] Going back to percept_hand home'
        pause()
    plan.execute()
    
    bin_cnstr = get_bin_inner_cnstr()
    
    # 1. init target poses
    target_poses = []
    refind = 7
    for i in range(12):
        target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(math.pi, 1.4, math.pi).tolist() # for bin 7 vertical
        target_pose_7_hori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(2.2, 0, math.pi/2).tolist()
        target_pose_7_invhori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(-math.pi/2, 0, -math.pi/2).tolist()
        
        if i < 3:
            target_pose = copy.deepcopy(target_pose_7_invhori)
            if i == 0:
                target_pose[1] -= 0.06
            target_pose[2] -= 0.08  # z
        elif i<6:
            target_pose = copy.deepcopy(target_pose_7_hori)
            target_pose[0] += 0.03  # x
            target_pose[2] -= 0.04  # z
        elif i<9:
            target_pose = copy.deepcopy(target_pose_7_hori)
        else:
            target_pose = copy.deepcopy(target_pose_7_hori)
            target_pose[2] += 0.1  # z

        target_pose[1] += (bin_cnstr[i][0]+bin_cnstr[i][1])/2 - (bin_cnstr[refind][0]+bin_cnstr[refind][1])/2  # y
        target_pose[2] += (bin_cnstr[i][4]+bin_cnstr[i][5])/2 - (bin_cnstr[refind][4]+bin_cnstr[refind][5])/2  # z
        target_poses.append(target_pose)
    
    y_deltas = [0.08, -0.08]
    caption = ['left', 'right']
    max_score = -100
    max_obj_pose = None
    for i in range(len(y_deltas)):
        # 2. plan to target
        tip_hand_transform = xyzrpy_from_xyzquat([-0.067, 0.027, -0.012, -0.007, 0.704, 0.710, -0.002])  # need to be updated
        
        target_pose = copy.deepcopy(target_poses[binNum])
        target_pose[1] += y_deltas[i]
        plan = None
        planner = IK(target_tip_pos = target_pose[0:3], target_tip_ori = target_pose[3:7], 
                     tip_hand_transform=tip_hand_transform, 
                     target_link='link_5', target_joint_bb=[[6, 1.2, 1.2]])
        for j in range(15):
            planner.ori_tol = 0.5*j/10.0
            planner.pos_tol = 0.01*j/10.0
            
            newplan = planner.plan()
            if newplan.success():
                plan = newplan
                print '[Percept] hand planning success at the trial %d' % j
                break
                
        if plan:
            plan.setSpeedByName('fastest')
            plan.visualize()

            if withPause:
                print '[Percept] Going to percept_hand %s ' % caption[i]
                pause()
            plan.execute()
        else:
            print '[Percept] hand planning failed'
            continue
        
        # 3. percept
        rospy.sleep(0.8)  # wait for new pointcloud
        obj_pose, score = capsen.capsen.detectOneObject(obj_id, bin_contents, binNum, mode = 1, withScore = True)
        if score > max_score:
            max_obj_pose = obj_pose
            max_score = score

    # 4. move back to percept home
    planner = IKJoint(target_joint_pos=home_joint_pose)
    plan = planner.plan()
    plan.visualize(); 
    plan.setSpeedByName('yolo'); 
    if withPause:
        print '[Percept] Going back to percept_hand home'
        pause()
    plan.execute()
    
    goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause)
    
    if max_obj_pose is None:
        return None, None, None
    
    pose_type = find_object_pose_type(obj_id, max_obj_pose)
    
    return max_obj_pose, pose_type, max_score
Ejemplo n.º 6
0
def percept_hand(obj_pose=None,
                 binNum=0,
                 obj_id=None,
                 bin_contents=None,
                 isExecute=True,
                 withPause=True):
    capsen.capsen.init()
    home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4]
    planner = IKJoint(target_joint_pos=home_joint_pose)
    #plan = Plan()
    #plan.q_traj = [home_joint_pose];
    plan = planner.plan()
    plan.visualize()
    plan.setSpeedByName('yolo')
    if withPause:
        print '[Percept] Going back to percept_hand home'
        pause()
    plan.execute()

    bin_cnstr = get_bin_inner_cnstr()

    # 1. init target poses
    target_poses = []
    refind = 7
    for i in range(12):
        target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(
            math.pi, 1.4, math.pi).tolist()  # for bin 7 vertical
        target_pose_7_hori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(
            2.2, 0, math.pi / 2).tolist()
        target_pose_7_invhori = [1.1756, 0, 0.79966
                                 ] + tfm.quaternion_from_euler(
                                     -math.pi / 2, 0, -math.pi / 2).tolist()

        if i < 3:
            target_pose = copy.deepcopy(target_pose_7_invhori)
            if i == 0:
                target_pose[1] -= 0.06
            target_pose[2] -= 0.08  # z
        elif i < 6:
            target_pose = copy.deepcopy(target_pose_7_hori)
            target_pose[0] += 0.03  # x
            target_pose[2] -= 0.04  # z
        elif i < 9:
            target_pose = copy.deepcopy(target_pose_7_hori)
        else:
            target_pose = copy.deepcopy(target_pose_7_hori)
            target_pose[2] += 0.1  # z

        target_pose[1] += (bin_cnstr[i][0] + bin_cnstr[i][1]) / 2 - (
            bin_cnstr[refind][0] + bin_cnstr[refind][1]) / 2  # y
        target_pose[2] += (bin_cnstr[i][4] + bin_cnstr[i][5]) / 2 - (
            bin_cnstr[refind][4] + bin_cnstr[refind][5]) / 2  # z
        target_poses.append(target_pose)

    y_deltas = [0.08, -0.08]
    caption = ['left', 'right']
    max_score = -100
    max_obj_pose = None
    for i in range(len(y_deltas)):
        # 2. plan to target
        tip_hand_transform = xyzrpy_from_xyzquat(
            [-0.067, 0.027, -0.012, -0.007, 0.704, 0.710,
             -0.002])  # need to be updated

        target_pose = copy.deepcopy(target_poses[binNum])
        target_pose[1] += y_deltas[i]
        plan = None
        planner = IK(target_tip_pos=target_pose[0:3],
                     target_tip_ori=target_pose[3:7],
                     tip_hand_transform=tip_hand_transform,
                     target_link='link_5',
                     target_joint_bb=[[6, 1.2, 1.2]])
        for j in range(15):
            planner.ori_tol = 0.5 * j / 10.0
            planner.pos_tol = 0.01 * j / 10.0

            newplan = planner.plan()
            if newplan.success():
                plan = newplan
                print '[Percept] hand planning success at the trial %d' % j
                break

        if plan:
            plan.setSpeedByName('fastest')
            plan.visualize()

            if withPause:
                print '[Percept] Going to percept_hand %s ' % caption[i]
                pause()
            plan.execute()
        else:
            print '[Percept] hand planning failed'
            continue

        # 3. percept
        rospy.sleep(0.8)  # wait for new pointcloud
        obj_pose, score = capsen.capsen.detectOneObject(obj_id,
                                                        bin_contents,
                                                        binNum,
                                                        mode=1,
                                                        withScore=True)
        if score > max_score:
            max_obj_pose = obj_pose
            max_score = score

    # 4. move back to percept home
    planner = IKJoint(target_joint_pos=home_joint_pose)
    plan = planner.plan()
    plan.visualize()
    plan.setSpeedByName('yolo')
    if withPause:
        print '[Percept] Going back to percept_hand home'
        pause()
    plan.execute()

    goToHome.goToHome(robotConfig=None,
                      homePos=[1, 0, 1.2],
                      isExecute=isExecute,
                      withPause=withPause)

    if max_obj_pose is None:
        return None, None, None

    pose_type = find_object_pose_type(obj_id, max_obj_pose)

    return max_obj_pose, pose_type, max_score