Beispiel #1
0
def main(execute='apply'):
    #parser = argparse.ArgumentParser()  # Automatically includes help
    #parser.add_argument('-display', action='store_true', help='enable viewer.')
    #args = parser.parse_args()
    #display = args.display
    display = True
    #display = False

    #filename = 'transporter.json' # simple | simple2 | cook | invert | kitchen | nonmonotonic_4_1
    #problem_fn = lambda: load_json_problem(filename)
    problem_fn = cooking_problem
    # holding_problem | stacking_problem | cleaning_problem | cooking_problem
    # cleaning_button_problem | cooking_button_problem

    with HideOutput():
        sim_world = connect(use_gui=False)
    set_client(sim_world)
    add_data_path()
    problem = problem_fn()
    print(problem)
    #state_id = save_state()

    if display:
        with HideOutput():
            real_world = connect(use_gui=True)
        add_data_path()
        with ClientSaver(real_world):
            problem_fn()  # TODO: way of doing this without reloading?
            update_state()
            wait_for_duration(0.1)

    #wait_for_interrupt()
    commands = plan_commands(problem)
    if (commands is None) or not display:
        with HideOutput():
            disconnect()
        return

    time_step = 0.01
    set_client(real_world)
    if execute == 'control':
        enable_gravity()
        control_commands(commands)
    elif execute == 'execute':
        step_commands(commands, time_step=time_step)
    elif execute == 'step':
        step_commands(commands)
    elif execute == 'apply':
        state = State()
        apply_commands(state, commands, time_step=time_step)
    else:
        raise ValueError(execute)

    wait_for_interrupt()
    with HideOutput():
        disconnect()
Beispiel #2
0
def add_scales(task, ros_world):
    scale_stackings = {}
    holding = {grasp.obj_name for grasp in task.init_holding.values()}
    with ClientSaver(ros_world.client):
        perception = ros_world.perception
        items = sorted(set(perception.get_items()) - holding,
                       key=lambda n: get_point(ros_world.get_body(n))[1],
                       reverse=False)  # Right to left
        for i, item in enumerate(items):
            if not POUR and (get_type(item) != SCOOP_CUP):
                continue
            item_body = ros_world.get_body(item)
            scale = create_name(SCALE_TYPE, i + 1)
            with HideOutput():
                scale_body = load_pybullet(get_body_urdf(get_type(scale)),
                                           fixed_base=True)
            ros_world.perception.sim_bodies[scale] = scale_body
            ros_world.perception.sim_items[scale] = None
            item_z = stable_z(item_body, scale_body)
            scale_pose_item = Pose(
                point=Point(z=-item_z))  # TODO: relies on origin in base
            set_pose(scale_body, multiply(get_pose(item_body),
                                          scale_pose_item))
            roll, pitch, _ = get_euler(scale_body)
            set_euler(scale_body, [roll, pitch, 0])
            scale_stackings[scale] = item
        #wait_for_user()
    return scale_stackings
def main(display='execute'):  # control | execute | step
    connect(use_gui=True)
    disable_real_time()
    draw_global_system()
    with HideOutput():
        robot = load_model(DRAKE_IIWA_URDF)  # KUKA_IIWA_URDF | DRAKE_IIWA_URDF
        floor = load_model('models/short_floor.urdf')
    block = load_model(BLOCK_URDF, fixed_base=False)
    set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
    set_default_camera(distance=2)
    dump_world()

    saved_world = WorldSaver()
    command = plan(robot, block, fixed=[floor], teleport=False)
    if (command is None) or (display is None):
        print('Unable to find a plan!')
        return

    saved_world.restore()
    update_state()
    wait_if_gui('{}?'.format(display))
    if display == 'control':
        enable_gravity()
        command.control(real_time=False, dt=0)
    elif display == 'execute':
        command.refine(num_steps=10).execute(time_step=0.005)
    elif display == 'step':
        command.step()
    else:
        raise ValueError(display)

    print('Quit?')
    wait_if_gui()
    disconnect()
Beispiel #4
0
def create_world(items=[], **kwargs):
    state = {name: unit_pose() for name in items}
    with HideOutput():
        world = ROSWorld(sim_only=True, state=state, **kwargs) # state=[]
        table_body = add_table(world)
        create_floor()
    return world, table_body
Beispiel #5
0
def main():
    connect(use_gui=True)
    add_data_path()

    plane = p.loadURDF("plane.urdf")
    with HideOutput():
        with LockRenderer():
            robot = load_model(FRANKA_URDF, fixed_base=True)
    dump_body(robot)
    print('Start?')
    wait_for_user()

    tool_link = link_from_name(robot, 'panda_hand')
    joints = get_movable_joints(robot)
    print('Joints', [get_joint_name(robot, joint) for joint in joints])
    sample_fn = get_sample_fn(robot, joints)
    for i in range(10):
        print('Iteration:', i)
        conf = sample_fn()
        set_joint_positions(robot, joints, conf)
        wait_for_user()
        test_retraction(robot,
                        PANDA_INFO,
                        tool_link,
                        max_distance=0.01,
                        max_time=0.05)
    disconnect()
Beispiel #6
0
 def __init__(self, task, use_robot=True, visualize=False, **kwargs):
     self.task = task
     self.real_world = None
     self.visualize = visualize
     self.client_saver = None
     self.client = connect(use_gui=visualize, **kwargs)
     print('Planner connected to client {}.'.format(self.client))
     self.robot = None
     with ClientSaver(self.client):
         with HideOutput():
             if use_robot:
                 self.robot = load_pybullet(os.path.join(
                     get_models_path(), PR2_URDF),
                                            fixed_base=True)
             #dump_body(self.robot)
     #compute_joint_weights(self.robot)
     self.world_name = 'world'
     self.world_pose = Pose(unit_pose())
     self.bodies = {}
     self.fixed = []
     self.surfaces = []
     self.items = []
     #self.holding_liquid = []
     self.initial_config = None
     self.initial_poses = {}
     self.body_mapping = {}
Beispiel #7
0
def main():
    parser = argparse.ArgumentParser()  # Automatically includes help
    parser.add_argument('-arm', required=True)
    parser.add_argument('-grasp', required=True)
    parser.add_argument('-viewer', action='store_true', help='enable viewer.')
    args = parser.parse_args()

    arm = args.arm
    other_arm = get_other_arm(arm)
    grasp_type = args.grasp

    connect(use_gui=args.viewer)
    add_data_path()

    with HideOutput():
        robot = load_pybullet(DRAKE_PR2_URDF)
    set_group_conf(robot, 'torso', [0.2])
    set_arm_conf(robot, arm, get_carry_conf(arm, grasp_type))
    set_arm_conf(robot, other_arm, arm_conf(other_arm, REST_LEFT_ARM))

    #plane = p.loadURDF("plane.urdf")
    #table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107)
    table = create_table()
    box = create_box(.07, .07, .14)

    #create_inverse_reachability(robot, box, table, arm=arm, grasp_type=grasp_type)
    create_inverse_reachability2(robot,
                                 box,
                                 table,
                                 arm=arm,
                                 grasp_type=grasp_type)
    disconnect()
Beispiel #8
0
def parse_robot(robot_json):
    pose = parse_pose(robot_json)
    if robot_json['name'] == 'pr2':
        with HideOutput(True):
            robot_id = load_model(DRAKE_PR2_URDF, fixed_base=True)
        set_group_conf(robot_id, 'base', base_values_from_pose(pose))
    else:
        # TODO: set the z?
        #set_pose(robot_id, pose)
        raise NotImplementedError(robot_json['name'])

    for joint, values in robot_json['conf'].items():
        [value] = values
        if has_joint(robot_id, joint):
            set_joint_position(robot_id, joint_from_name(robot_id, joint),
                               value)
        else:
            print('Robot {} lacks joint {}'.format(robot_json['name'], joint))

    if robot_json['name'] == 'pr2':
        set_group_conf(robot_id, 'torso', [0.2])
        set_group_conf(robot_id, 'left_arm', REST_LEFT_ARM)
        set_group_conf(robot_id, 'right_arm',
                       rightarm_from_leftarm(REST_LEFT_ARM))

    return robot_id
Beispiel #9
0
def main():
    # TODO: move to pybullet-planning for now
    parser = argparse.ArgumentParser()
    parser.add_argument('-viewer',
                        action='store_true',
                        help='enable the viewer while planning')
    args = parser.parse_args()
    print(args)

    connect(use_gui=True)
    with LockRenderer():
        draw_pose(unit_pose(), length=1)
        floor = create_floor()
        with HideOutput():
            robot = load_pybullet(get_model_path(ROOMBA_URDF),
                                  fixed_base=True,
                                  scale=2.0)
            for link in get_all_links(robot):
                set_color(robot, link=link, color=ORANGE)
            robot_z = stable_z(robot, floor)
            set_point(robot, Point(z=robot_z))
        #set_base_conf(robot, rover_confs[i])

        data_path = add_data_path()
        shelf, = load_model(os.path.join(data_path, KIVA_SHELF_SDF),
                            fixed_base=True)  # TODO: returns a tuple
        dump_body(shelf)
        #draw_aabb(get_aabb(shelf))

    wait_for_user()
    disconnect()
Beispiel #10
0
def main():
    parser = argparse.ArgumentParser()  # Automatically includes help
    parser.add_argument('-viewer', action='store_true', help='enable viewer.')
    args = parser.parse_args()

    connect(use_gui=True)

    with LockRenderer():
        draw_pose(unit_pose(), length=1, width=1)
        floor = create_floor()
        set_point(floor, Point(z=-EPSILON))

        table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
                              top_color=TAN, cylinder=False)
        set_point(table1, Point(y=+0.5))

        table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
                              top_color=TAN, cylinder=False)
        set_point(table2, Point(y=-0.5))

        tables = [table1, table2]

        #set_euler(table1, Euler(yaw=np.pi/2))
        with HideOutput():
            # data_path = add_data_path()
            # robot_path = os.path.join(data_path, WSG_GRIPPER)
            robot_path = get_model_path(WSG_50_URDF)  # WSG_50_URDF | PANDA_HAND_URDF
            robot = load_pybullet(robot_path, fixed_base=True)
            #dump_body(robot)

        block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED)
        block_z = stable_z(block1, table1)
        set_point(block1, Point(y=-0.5, z=block_z))

        block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN)
        set_point(block2, Point(x=-0.25, y=-0.5, z=block_z))

        block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE)
        set_point(block3, Point(x=-0.15, y=+0.5, z=block_z))

        blocks = [block1, block2, block3]

        set_camera_pose(camera_point=Point(x=-1, z=block_z+1), target_point=Point(z=block_z))

    block_pose = get_pose(block1)
    open_gripper(robot)
    tool_link = link_from_name(robot, 'tool_link')
    base_from_tool = get_relative_pose(robot, tool_link)
    #draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
    grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi/2)),
                             top_offset=0.02, grasp_length=0.03, under=False)[1:2]
    for grasp in grasps:
        gripper_pose = multiply(block_pose, invert(grasp))
        set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
        wait_for_user()

    wait_for_user('Finish?')
    disconnect()
Beispiel #11
0
def main():
    # The URDF loader seems robust to package:// and slightly wrong relative paths?
    connect(use_gui=True)
    add_data_path()
    plane = p.loadURDF("plane.urdf")
    with LockRenderer():
        with HideOutput():
            robot = load_model(MOVO_URDF, fixed_base=True)
        for link in get_links(robot):
            set_color(robot,
                      color=apply_alpha(0.25 * np.ones(3), 1),
                      link=link)
        base_joints = joints_from_names(robot, BASE_JOINTS)
        draw_base_limits((get_min_limits(
            robot, base_joints), get_max_limits(robot, base_joints)),
                         z=1e-2)
    dump_body(robot)
    wait_for_user('Start?')

    #for arm in ARMS:
    #    test_close_gripper(robot, arm)

    arm = 'right'
    tool_link = link_from_name(robot, TOOL_LINK.format(arm))
    #joint_names = HEAD_JOINTS
    #joints = joints_from_names(robot, joint_names)
    joints = base_joints + get_arm_joints(robot, arm)
    #joints = get_movable_joints(robot)
    print('Joints:', get_joint_names(robot, joints))

    ik_joints = get_ik_joints(robot, MOVO_INFOS[arm], tool_link)
    #fixed_joints = []
    fixed_joints = ik_joints[:1]
    #fixed_joints = ik_joints

    sample_fn = get_sample_fn(robot, joints)
    handles = []
    for i in range(10):
        print('Iteration:', i)
        conf = sample_fn()
        print(conf)
        set_joint_positions(robot, joints, conf)
        tool_pose = get_link_pose(robot, tool_link)
        remove_handles(handles)
        handles = draw_pose(tool_pose)
        wait_for_user()

        #conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
        #                                      fixed_joints=fixed_joints, max_time=0.1), None)
        #if conf is not None:
        #    set_joint_positions(robot, ik_joints, conf)
        #wait_for_user()
        test_retraction(robot,
                        MOVO_INFOS[arm],
                        tool_link,
                        fixed_joints=fixed_joints,
                        max_time=0.1)
    disconnect()
Beispiel #12
0
def load_robot():
    root_directory = os.path.dirname(os.path.abspath(__file__))
    kuka_path = os.path.join(root_directory, KUKA_DIR, KUKA_PATH)
    with HideOutput():
        robot = load_pybullet(kuka_path, fixed_base=True)
        #print([get_max_velocity(robot, joint) for joint in get_movable_joints(robot)])
        set_static(robot)
        set_configuration(robot, INITIAL_CONF)
    return robot
def main():
    connect(use_gui=True)
    with HideOutput():
        pr2 = load_model(
            "models/drake/pr2_description/urdf/pr2_simplified.urdf")
    set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']),
                        REST_LEFT_ARM)
    set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']),
                        rightarm_from_leftarm(REST_LEFT_ARM))
    set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']),
                        [0.2])
    dump_body(pr2)

    block = load_model(BLOCK_URDF, fixed_base=False)
    set_point(block, [2, 0.5, 1])
    target_point = point_from_pose(get_pose(block))

    # head_link = link_from_name(pr2, HEAD_LINK)
    head_joints = joints_from_names(pr2, PR2_GROUPS['head'])
    head_link = head_joints[-1]

    #max_detect_distance = 2.5
    max_register_distance = 1.0
    distance_range = (max_register_distance / 2, max_register_distance)
    base_generator = visible_base_generator(pr2, target_point, distance_range)

    base_joints = joints_from_names(pr2, PR2_GROUPS['base'])
    for i in range(5):
        base_conf = next(base_generator)
        set_joint_positions(pr2, base_joints, base_conf)

        p.addUserDebugLine(point_from_pose(get_link_pose(pr2, head_link)),
                           target_point,
                           lineColorRGB=(1, 0, 0))  # addUserDebugText
        p.addUserDebugLine(point_from_pose(
            get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))),
                           target_point,
                           lineColorRGB=(0, 0, 1))  # addUserDebugText

        # head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, )
        head_conf = inverse_visibility(pr2, target_point)
        set_joint_positions(pr2, head_joints, head_conf)
        print(get_detections(pr2))
        # TODO: does this detect the robot sometimes?

        detect_mesh, z = get_detection_cone(pr2, block)
        detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5))
        set_pose(detect_cone,
                 get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
        view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25))
        set_pose(view_cone,
                 get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
        wait_for_user()
        remove_body(detect_cone)
        remove_body(view_cone)

    disconnect()
Beispiel #14
0
def main():
    connect(use_gui=True)

    with HideOutput():
        pr2 = load_model("models/pr2_description/pr2.urdf")
    test_clone_robot(pr2)
    test_clone_arm(pr2)

    user_input('Finish?')
    disconnect()
def main(use_pr2_drake=True):
    connect(use_gui=True)
    add_data_path()

    plane = p.loadURDF("plane.urdf")
    table_path = "models/table_collision/table.urdf"
    table = load_pybullet(table_path, fixed_base=True)
    set_quat(table, quat_from_euler(Euler(yaw=PI / 2)))
    # table/table.urdf, table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf
    obstacles = [plane, table]

    pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF
    with HideOutput():
        pr2 = load_model(pr2_urdf, fixed_base=True)  # TODO: suppress warnings?
    dump_body(pr2)

    z = base_aligned_z(pr2)
    print(z)
    #z = stable_z_on_aabb(pr2, AABB(np.zeros(3), np.zeros(3)))
    print(z)

    set_point(pr2, Point(z=z))
    print(get_aabb(pr2))
    wait_if_gui()

    base_start = (-2, -2, 0)
    base_goal = (2, 2, 0)
    arm_start = SIDE_HOLDING_LEFT_ARM
    #arm_start = TOP_HOLDING_LEFT_ARM
    #arm_start = REST_LEFT_ARM
    arm_goal = TOP_HOLDING_LEFT_ARM
    #arm_goal = SIDE_HOLDING_LEFT_ARM

    left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm'])
    right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm'])
    torso_joints = joints_from_names(pr2, PR2_GROUPS['torso'])
    set_joint_positions(pr2, left_joints, arm_start)
    set_joint_positions(pr2, right_joints,
                        rightarm_from_leftarm(REST_LEFT_ARM))
    set_joint_positions(pr2, torso_joints, [0.2])
    open_arm(pr2, 'left')
    # test_ikfast(pr2)

    add_line(base_start, base_goal, color=RED)
    print(base_start, base_goal)
    if use_pr2_drake:
        test_drake_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
    else:
        test_base_motion(pr2, base_start, base_goal, obstacles=obstacles)

    test_arm_motion(pr2, left_joints, arm_goal)
    # test_arm_control(pr2, left_joints, arm_start)

    wait_if_gui('Finish?')
    disconnect()
Beispiel #16
0
    def _create_robot(self):
        with ClientSaver(self.client):
            with HideOutput():
                pr2_path = os.path.join(get_models_path(), PR2_URDF)
                self.pr2 = load_pybullet(pr2_path, fixed_base=True)

            # Base link is the origin
            base_pose = get_link_pose(self.robot,
                                      link_from_name(self.robot, BASE_FRAME))
            set_pose(self.robot, invert(base_pose))
        return self.pr2
Beispiel #17
0
def main(display='execute'): # control | execute | step
    # One of the arm's gantry carriage is fixed when the other is moving.
    connect(use_gui=True)
    set_camera(yaw=-90, pitch=-40, distance=10, target_position=(0, 7.5, 0))
    draw_pose(unit_pose(), length=1.0)
    disable_real_time()

    with HideOutput():
        root_directory = os.path.dirname(os.path.abspath(__file__))
        robot = load_pybullet(os.path.join(root_directory, ETH_RFL_URDF), fixed_base=True)
    # floor = load_model('models/short_floor.urdf')
    block = load_model(BLOCK_URDF, fixed_base=False)
    #link = link_from_name(robot, 'gantry_base_link')
    #print(get_aabb(robot, link))

    block_x = -0.2
    #block_y = 1 if ARM == 'right' else 13.5
    #block_x = 10
    block_y = 5.

    # set_pose(floor, Pose(Point(x=floor_x, y=1, z=1.3)))
    # set_pose(block, Pose(Point(x=floor_x, y=0.6, z=stable_z(block, floor))))
    set_pose(block, Pose(Point(x=block_x, y=block_y, z=3.5)))
    # set_default_camera()
    dump_world()

    #print(get_camera())
    saved_world = WorldSaver()
    with LockRenderer():
        command = plan(robot, block, fixed=[], teleport=False) # fixed=[floor],
    if (command is None) or (display is None):
        print('Unable to find a plan! Quit?')
        wait_for_interrupt()
        disconnect()
        return

    saved_world.restore()
    update_state()
    print('{}?'.format(display))
    wait_for_interrupt()
    if display == 'control':
        enable_gravity()
        command.control(real_time=False, dt=0)
    elif display == 'execute':
        command.refine(num_steps=10).execute(time_step=0.002)
    elif display == 'step':
        command.step()
    else:
        raise ValueError(display)

    print('Quit?')
    wait_for_interrupt()
    disconnect()
Beispiel #18
0
 def load_body(self, name, pose=None, fixed_base=False):
     assert name not in self.sim_bodies
     ty = get_type(name)
     with ClientSaver(self.client):
         with HideOutput():
             body = load_cup_bowl(ty)
             if body is None:
                 body = load_pybullet(get_body_urdf(name),
                                      fixed_base=fixed_base)
         if pose is not None:
             set_pose(body, pose)
         self.sim_bodies[name] = body
         return body
Beispiel #19
0
def main(use_pr2_drake=False):
    connect(use_gui=True)
    add_data_path()

    plane = p.loadURDF("plane.urdf")
    #table_path = "table/table.urdf"
    # table_path = "models/table_collision/table.urdf"
    # table = p.loadURDF(table_path, 0, 0, 0, 0, 0, 0.707107, 0.707107)
    # table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf

    pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF
    with HideOutput():
        pr2 = load_model(pr2_urdf, fixed_base=True)  # TODO: suppress warnings?
    dump_body(pr2)

    base_start = (-2, -2, 0)
    base_goal = (2, 2, 0)
    arm_start = SIDE_HOLDING_LEFT_ARM
    #arm_start = TOP_HOLDING_LEFT_ARM
    #arm_start = REST_LEFT_ARM
    arm_goal = TOP_HOLDING_LEFT_ARM
    #arm_goal = SIDE_HOLDING_LEFT_ARM

    left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm'])
    right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm'])
    torso_joints = joints_from_names(pr2, PR2_GROUPS['torso'])
    set_joint_positions(pr2, left_joints, arm_start)
    set_joint_positions(pr2, right_joints,
                        rightarm_from_leftarm(REST_LEFT_ARM))
    set_joint_positions(pr2, torso_joints, [0.2])
    open_arm(pr2, 'left')
    # test_ikfast(pr2)

    p.addUserDebugLine(base_start, base_goal,
                       lineColorRGB=(1, 0, 0))  # addUserDebugText
    print(base_start, base_goal)
    if use_pr2_drake:
        test_drake_base_motion(pr2, base_start, base_goal)
    else:
        test_base_motion(pr2, base_start, base_goal)

    test_arm_motion(pr2, left_joints, arm_goal)
    # test_arm_control(pr2, left_joints, arm_start)

    user_input('Finish?')
    disconnect()
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5):
    floor_extent = 5.0
    base_limits = (-floor_extent/2.*np.ones(2),
                   +floor_extent/2.*np.ones(2))

    floor_height = 0.001
    floor = create_box(floor_extent, floor_extent, floor_height, color=TAN)
    set_point(floor, Point(z=-floor_height/2.))

    wall1 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY)
    set_point(wall1, Point(y=floor_extent/2., z=wall_side/2.))
    wall2 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY)
    set_point(wall2, Point(y=-floor_extent/2., z=wall_side/2.))
    wall3 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY)
    set_point(wall3, Point(x=floor_extent/2., z=wall_side/2.))
    wall4 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY)
    set_point(wall4, Point(x=-floor_extent/2., z=wall_side/2.))
    walls = [wall1, wall2, wall3, wall4]

    initial_surfaces = OrderedDict()
    for _ in range(n_obstacles):
        body = create_box(obst_width, obst_width, obst_height, color=GREY)
        initial_surfaces[body] = floor
    obstacles = walls + list(initial_surfaces)

    initial_conf = np.array([+floor_extent/3, -floor_extent/3, 3*PI/4])
    goal_conf = -initial_conf

    with HideOutput():
        robot = load_model(TURTLEBOT_URDF)
        base_joints = joints_from_names(robot, BASE_JOINTS)
        # base_link = child_link_from_joint(base_joints[-1])
        base_link = link_from_name(robot, BASE_LINK_NAME)
        set_all_color(robot, BLUE)
    dump_body(robot)
    set_point(robot, Point(z=stable_z(robot, floor)))
    draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5)
    set_joint_positions(robot, base_joints, initial_conf)

    sample_placements(initial_surfaces, obstacles=[robot] + walls,
                      savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)],
                      min_distances=10e-2)

    return robot, base_limits, goal_conf, obstacles
Beispiel #21
0
def load_world(use_floor=True):
    obstacles = []
    #side, height = 10, 0.01
    robot = load_robot()
    with HideOutput():
        lower, _ = get_aabb(robot)
        if use_floor:
            #floor = load_model('models/short_floor.urdf', fixed_base=True)
            #add_data_path()
            #floor = load_pybullet('plane.urdf', fixed_base=True)
            #set_color(floor, TAN)
            #floor = create_box(w=side, l=side, h=height, color=apply_alpha(GROUND_COLOR))
            floor = create_plane(color=apply_alpha(GROUND_COLOR))
            obstacles.append(floor)
            #set_point(floor, Point(z=lower[2]))
            set_point(floor, Point(x=1.2, z=0.023 - 0.025))  # -0.02
        else:
            floor = None  # TODO: make this an empty list of obstacles
        #set_all_static()
    return obstacles, robot
Beispiel #22
0
def main(display='execute'):  # control | execute | step
    connect(use_gui=True)
    disable_real_time()

    with HideOutput():
        root_directory = os.path.dirname(os.path.abspath(__file__))
        robot = load_pybullet(os.path.join(root_directory, IRB6600_TRACK_URDF), fixed_base=True)
    floor = load_model('models/short_floor.urdf')
    block = load_model(BLOCK_URDF, fixed_base=False)
    floor_x = 2
    set_pose(floor, Pose(Point(x=floor_x, z=0.5)))
    set_pose(block, Pose(Point(x=floor_x, y=0, z=stable_z(block, floor))))
    # set_default_camera()
    dump_world()

    saved_world = WorldSaver()
    with LockRenderer():
        command = plan(robot, block, fixed=[floor], teleport=False)
    if (command is None) or (display is None):
        print('Unable to find a plan!')
        print('Quit?')
        wait_for_interrupt()
        disconnect()
        return

    saved_world.restore()
    update_state()
    user_input('{}?'.format(display))
    if display == 'control':
        enable_gravity()
        command.control(real_time=False, dt=0)
    elif display == 'execute':
        command.refine(num_steps=10).execute(time_step=0.002)
    elif display == 'step':
        command.step()
    else:
        raise ValueError(display)

    print('Quit?')
    wait_for_interrupt()
    disconnect()
Beispiel #23
0
    def __init__(self,
                 sim_only=False,
                 visualize=False,
                 use_robot=True,
                 **kwargs):
        self.simulation = sim_only
        if not self.simulation:
            import rospy
            rospy.init_node(self.__class__.__name__, anonymous=False)
        self.client_saver = None
        with HideOutput():
            self.client = connect(
                visualize
            )  # TODO: causes assert (len(visual_data) == 1) error?
            #self.client = p.connect(p.GUI if visualize else p.DIRECT)

        p.setPhysicsEngineParameter(enableFileCaching=0,
                                    physicsClientId=self.client)
        p.setRealTimeSimulation(0, physicsClientId=self.client)
        p.configureDebugVisualizer(p.COV_ENABLE_GUI,
                                   False,
                                   physicsClientId=self.client)
        p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS,
                                   True,
                                   physicsClientId=self.client)
        self.pr2 = None
        if use_robot:
            self._create_robot()
        self.initial_beads = {}

        if sim_only:
            self.perception = PBPerception(self, **kwargs)
            self.controller = PBController(self)
            self.alternate_controller = None
        else:
            from perception_tools.retired.ros_perception import ROSPerception
            from control_tools.retired.ros_controller import ROSController
            self.perception = ROSPerception(self)
            self.controller = ROSController(self)
            self.alternate_controller = PBController(self)
            self.sync_controllers()
Beispiel #24
0
def create_stiffness_checker(extrusion_path, verbose=False):
    if not USE_CONMECH:
        return None
    from pyconmech import StiffnessChecker
    # TODO: the stiffness checker likely has a memory leak
    # https://github.com/yijiangh/conmech/blob/master/src/bindings/pyconmech/pyconmech.cpp
    if not os.path.exists(extrusion_path):
        raise FileNotFoundError(extrusion_path)
    with HideOutput():
        #checker = StiffnessChecker(json_file_path=extrusion_path, verbose=verbose)
        checker = StiffnessChecker.from_json(json_file_path=extrusion_path,
                                             verbose=verbose)
    #checker.set_output_json(True)
    #checker.set_output_json_path(file_path=os.getcwd(), file_name="stiffness-results.json")
    checker.set_self_weight_load(True)
    #checker.set_nodal_displacement_tol(transl_tol=0.005, rot_tol=10 * np.pi / 180)
    #checker.set_nodal_displacement_tol(transl_tol=0.003, rot_tol=5 * np.pi / 180)
    # checker.set_nodal_displacement_tol(transl_tol=1e-3, rot_tol=3 * (np.pi / 360))
    checker.set_nodal_displacement_tol(trans_tol=TRANS_TOL, rot_tol=ROT_TOL)
    #checker.set_loads(point_loads=None, include_self_weight=False, uniform_distributed_load={})
    return checker
def main():
    connect(use_gui=True)
    add_data_path()
    draw_pose(Pose(), length=1.)
    set_camera_pose(camera_point=[1, -1, 1])

    plane = p.loadURDF("plane.urdf")
    with LockRenderer():
        with HideOutput(True):
            robot = load_pybullet(FRANKA_URDF, fixed_base=True)
            assign_link_colors(robot, max_colors=3, s=0.5, v=1.)
            #set_all_color(robot, GREEN)
    obstacles = [plane]  # TODO: collisions with the ground

    dump_body(robot)
    print('Start?')
    wait_for_user()

    info = PANDA_INFO
    tool_link = link_from_name(robot, 'panda_hand')
    draw_pose(Pose(), parent=robot, parent_link=tool_link)
    joints = get_movable_joints(robot)
    print('Joints', [get_joint_name(robot, joint) for joint in joints])
    check_ik_solver(info)

    sample_fn = get_sample_fn(robot, joints)
    for i in range(10):
        print('Iteration:', i)
        conf = sample_fn()
        set_joint_positions(robot, joints, conf)
        wait_for_user()
        #test_ik(robot, info, tool_link, get_link_pose(robot, tool_link))
        test_retraction(robot,
                        info,
                        tool_link,
                        use_pybullet=False,
                        max_distance=0.1,
                        max_time=0.05,
                        max_candidates=100)
    disconnect()
Beispiel #26
0
def main():
    # https://github.com/ros-teleop/teleop_twist_keyboard
    # http://openrave.org/docs/latest_stable/_modules/openravepy/misc/#SetViewerUserThread

    connect(use_gui=True)
    add_data_path()
    load_pybullet("plane.urdf")
    #load_pybullet("models/table_collision/table.urdf")
    with HideOutput():
        pr2 = load_model(DRAKE_PR2_URDF, fixed_base=True)
    enable_gravity()
    enable_real_time(
    )  # TODO: won't work as well on OS X due to simulation thread

    #run_simulate(pr2)
    run_thread(pr2)
    # TODO: keep working on this
    #userthread = threading.Thread(target=run_thread, args=[pr2])
    #userthread.start()
    #userthread.join()

    disconnect()
Beispiel #27
0
def parse_robot(robot):
    name = robot.find('name').text
    urdf = robot.find('urdf').text
    fixed_base = not parse_boolean(robot.find('movebase'))
    print(name, urdf, fixed_base)
    pose = parse_pose(robot.find('basepose'))
    torso = parse_array(robot.find('torso'))
    left_arm = parse_array(robot.find('left_arm'))
    right_arm = parse_array(robot.find('right_arm'))
    assert (name == 'pr2')

    with HideOutput():
        robot_id = load_model(DRAKE_PR2_URDF, fixed_base=True)
    set_group_conf(robot_id, 'base', base_values_from_pose(pose))
    set_group_conf(robot_id, 'torso', torso)
    set_group_conf(robot_id, 'left_arm', left_arm)
    set_group_conf(robot_id, 'right_arm', right_arm)
    #set_point(robot_id, Point(z=point_from_pose(pose)[2]))
    # TODO: base pose isn't right
    # print(robot.tag)
    # print(robot.attrib)
    # print(list(robot.iter('basepose')))
    return robot_id
Beispiel #28
0
def create_table_bodies(world, item_ranges, randomize=True):
    perception = world.perception
    with HideOutput():
        add_data_path()
        floor_body = load_pybullet("plane.urdf")
        set_pose(floor_body, get_pose(world.robot))
        add_table(world)
        for name, limits in sorted(item_ranges.items()):
            perception.sim_items[name] = None
            if randomize:
                body = randomize_body(name,
                                      width_range=limits.width_range,
                                      height_range=limits.height_range,
                                      mass_range=limits.mass_range)
            else:
                body = load_body(name)
            perception.sim_bodies[name] = body
            # perception.add_item(name, unit_pose())
            x, y, yaw = np.random.uniform(*limits.pose2d_range)
            surface_body = perception.get_body(limits.surface)
            z = stable_z(body, surface_body) + Z_EPSILON
            pose = Pose((x, y, z), Euler(yaw=yaw))
            perception.set_pose(name, *pose)
Beispiel #29
0
def sample_task(skill, **kwargs):
    # TODO: extract and just control robot gripper
    with HideOutput():
        sim_world = ROSWorld(sim_only=True, visualize=kwargs['visualize'])
    # Only used by pb_controller, for ros_controller it's assumed always True
    sim_world.controller.execute_motor_control = True

    collector = SKILL_COLLECTORS[skill]
    while True:  # TODO: timeout if too many failures in a row
        sim_world.controller.set_gravity()
        task, feature, evaluation_fn = collector.collect_fn(
            sim_world, **kwargs['collect_kwargs'])
        # evaluation_fn is the score function for the skill
        if task is not None:
            task.skill = skill
            feature['simulation'] = True
            break
        sim_world.reset(keep_robot=True)
    saver = WorldSaver()
    if kwargs['visualize'] and kwargs['visualize'] != 'No_name':
        draw_names(sim_world)
        draw_forward_reachability(sim_world, task.arms)
    return sim_world, collector, task, feature, evaluation_fn, saver
Beispiel #30
0
 def _load_bodies(self, real_world):
     with ClientSaver(self.client):
         assert not self.bodies
         #for real_body in self.bodies.values():
         #    remove_body(real_body)
         self.bodies = {}
         self.initial_poses = {}
         with HideOutput():
             for name, real_body in real_world.perception.sim_bodies.items(
             ):
                 if name in real_world.perception.get_surfaces():
                     self.add_body(name, fixed=True)
                 elif name in real_world.perception.get_items():
                     with real_world:
                         model_info = get_model_info(real_body)
                         # self.bodies[item] = clone_body(real_world.perception.get_body(item), client=self.client)
                     if 'wall' in name:
                         with real_world:
                             self.bodies[name] = clone_body(
                                 real_body, client=self.client)
                     elif 'cylinder' in name:
                         self.bodies[name] = create_cylinder(
                             *model_info.path)
                     elif 'box' in name:
                         self.bodies[name] = create_box(*model_info.path)
                     elif model_info is None:
                         self.add_body(name, fixed=False)
                     else:
                         self.bodies[name] = load_model_info(model_info)
                 else:
                     raise ValueError(name)
                 # TODO: the floor might not be added which causes the indices to misalign
                 self.body_mapping[self.bodies[name]] = real_body
                 if name not in self.get_held():
                     self.initial_poses[name] = Pose(
                         real_world.perception.get_pose(name))
     self.set_initial()