Example #1
0
def pick_problem(arm='left', grasp_type='side'):
    other_arm = get_other_arm(arm)
    initial_conf = get_carry_conf(arm, grasp_type)

    pr2 = create_pr2()
    set_base_values(pr2, (0, -1.2, np.pi / 2))
    set_arm_conf(pr2, arm, initial_conf)
    open_arm(pr2, arm)
    set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
    close_arm(pr2, other_arm)

    plane = create_floor()
    table = load_pybullet(TABLE_URDF)
    # table = create_table(height=0.8)
    # table = load_pybullet("table_square/table_square.urdf")
    box = create_box(.07, .05, .25)
    set_point(box, (-0.25, -0.3, TABLE_MAX_Z + .25 / 2))
    # set_point(box, (0.2, -0.2, 0.8 + .25/2 + 0.1))

    return Problem(robot=pr2,
                   movable=[box],
                   arms=[arm],
                   grasp_types=[grasp_type],
                   surfaces=[table],
                   goal_conf=get_pose(pr2),
                   goal_holding=[(arm, box)])
Example #2
0
def test_clone_arm(pr2):
    first_joint_name = PR2_GROUPS['left_arm'][0]
    first_joint = joint_from_name(pr2, first_joint_name)
    parent_joint = get_link_parent(pr2, first_joint)
    print(get_link_name(pr2, parent_joint), parent_joint, first_joint_name,
          first_joint)
    print(get_link_descendants(pr2,
                               first_joint))  # TODO: least common ancestor
    links = [first_joint] + get_link_descendants(pr2, first_joint)
    new_arm = clone_body(pr2, links=links, collision=False)
    dump_world()
    set_base_values(pr2, (-2, 0, 0))
    wait_for_interrupt()
Example #3
0
def test_base_motion(pr2, base_start, base_goal):
    #disabled_collisions = get_disabled_collisions(pr2)
    set_base_values(pr2, base_start)
    user_input('Plan Base?')
    base_limits = ((-2.5, -2.5), (2.5, 2.5))
    base_path = plan_base_motion(pr2, base_goal, base_limits)
    if base_path is None:
        print('Unable to find a base path')
        return
    print(len(base_path))
    for bq in base_path:
        set_base_values(pr2, bq)
        user_input('Continue?')
Example #4
0
def test_base_motion(pr2, base_start, base_goal, obstacles=[]):
    #disabled_collisions = get_disabled_collisions(pr2)
    set_base_values(pr2, base_start)
    wait_for_user('Plan Base?')
    base_limits = ((-2.5, -2.5), (2.5, 2.5))
    base_path = plan_base_motion(
        pr2, base_goal, base_limits, obstacles=obstacles)
    if base_path is None:
        print('Unable to find a base path')
        return
    print(len(base_path))
    for bq in base_path:
        set_base_values(pr2, bq)
        if SLEEP is None:
            wait_for_user('Continue?')
        else:
            time.sleep(SLEEP)
def test_base_motion(pr2, base_start, base_goal, obstacles=[]):
    #disabled_collisions = get_disabled_collisions(pr2)
    set_base_values(pr2, base_start)
    wait_if_gui('Plan Base-pr2?')
    base_limits = ((-2.5, -2.5), (2.5, 2.5))
    with LockRenderer(lock=False):
        base_path = plan_base_motion(pr2, base_goal, base_limits, obstacles=obstacles)
    if base_path is None:
        print('Unable to find a base path')
        return
    print(len(base_path))
    for bq in base_path:
        set_base_values(pr2, bq)
        if SLEEP is None:
            wait_if_gui('Continue?')
        else:
            wait_for_duration(SLEEP)
Example #6
0
def test_clone_robot(pr2):
    # TODO: j toggles frames, p prints timings, w is wire, a is boxes
    new_pr2 = clone_body(pr2, visual=True, collision=False)
    #new_pr2 = clone_body_editor(pr2, visual=True, collision=True)
    dump_world()
    #print(load_srdf_collisions())
    #print(load_dae_collisions())

    # TODO: some unimportant quats are off for both URDF and other
    # TODO: maybe all the frames are actually correct when I load things this way?
    # TODO: the visual geometries are off but not the collision frames?
    """
    import numpy as np
    for link in get_links(pr2):
        if not get_visual_data(new_pr2, link): # pybullet.error: Error receiving visual shape info?
            continue
        #print(get_link_name(pr2, link))
        data1 = VisualShapeData(*get_visual_data(pr2, link)[0])
        data2 = VisualShapeData(*get_visual_data(new_pr2, link)[0])
        pose1 = (data1.localVisualFrame_position, data1.localVisualFrame_orientation)
        pose2 = (data2.localVisualFrame_position, data2.localVisualFrame_orientation)

        #pose1 = get_link_pose(pr2, link) # Links are fine
        #pose2 = get_link_pose(new_pr2, link)
        #pose1 = get_joint_parent_frame(pr2, link)
        #pose2 = get_joint_parent_frame(new_pr2, link)
        #pose1 = get_joint_inertial_pose(pr2, link) # Inertia is fine
        #pose2 = get_joint_inertial_pose(new_pr2, link)
        if not np.allclose(pose1[0], pose2[0], rtol=0, atol=1e-3):
            print('Point', get_link_name(pr2, link), link, pose1[0], pose2[0])
            #print(data1)
            #print(data2)
            #print(get_joint_parent_frame(pr2, link), get_joint_parent_frame(new_pr2, link))
            print(get_joint_inertial_pose(pr2, link)) #, get_joint_inertial_pose(new_pr2, link))
            print()
        if not np.allclose(euler_from_quat(pose1[1]), euler_from_quat(pose2[1]), rtol=0, atol=1e-3):
            print('Quat', get_link_name(pr2, link), link, euler_from_quat(pose1[1]), euler_from_quat(pose2[1]))
        joint_info1 = get_joint_info(pr2, link)
        joint_info2 = get_joint_info(new_pr2, link)
        # TODO: the axis is off for some of these
        if not np.allclose(joint_info1.jointAxis, joint_info2.jointAxis, rtol=0, atol=1e-3):
            print('Axis', get_link_name(pr2, link), link, joint_info1.jointAxis, joint_info2.jointAxis)
    """
    set_base_values(new_pr2, (2, 0, 0))
    wait_for_interrupt()
def create_inverse_reachability(robot, body, table, arm, grasp_type, num_samples=500):
    link = get_gripper_link(robot, arm)
    movable_joints = get_movable_joints(robot)
    default_conf = get_joint_positions(robot, movable_joints)
    gripper_from_base_list = []
    grasps = GET_GRASPS[grasp_type](body)

    while len(gripper_from_base_list) < num_samples:
        box_pose = sample_placement(body, table)
        set_pose(body, box_pose)
        grasp_pose = random.choice(grasps)
        gripper_pose = multiply(box_pose, invert(grasp_pose))
        set_joint_positions(robot, movable_joints, default_conf)
        base_conf = next(uniform_pose_generator(robot, gripper_pose))
        set_base_values(robot, base_conf)
        if pairwise_collision(robot, table):
            continue
        conf = inverse_kinematics(robot, link, gripper_pose)
        if (conf is None) or pairwise_collision(robot, table):
            continue
        gripper_from_base = multiply(invert(get_link_pose(robot, link)), get_pose(robot))
        gripper_from_base_list.append(gripper_from_base)
        print('{} / {}'.format(len(gripper_from_base_list), num_samples))

    filename = IR_FILENAME.format(grasp_type, arm)
    path = get_database_file(filename)
    data = {
        'filename': filename,
        'robot': get_body_name(robot),
        'grasp_type': grasp_type,
        'arg': arm,
        'carry_conf': get_carry_conf(arm, grasp_type),
        'gripper_link': link,
        'gripper_from_base': gripper_from_base_list,
    }
    write_pickle(path, data)
    return path