def load_world(): root_directory = os.path.dirname(os.path.abspath(__file__)) with HideOutput(): floor = load_model('models/short_floor.urdf') robot = load_pybullet(os.path.join(root_directory, KUKA_PATH), fixed_base=True) set_point(floor, Point(z=-0.01)) return floor, robot
def main(): 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()
def load_world(): # TODO: store internal world info here to be reloaded robot = load_model(DRAKE_IIWA_URDF) # robot = load_model(KUKA_IIWA_URDF) floor = load_model('models/short_floor.urdf') sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5))) stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5))) block = load_model(BLOCK_URDF, fixed_base=False) #cup = load_model('models/dinnerware/cup/cup_small.urdf', Pose(Point(x=+0.5, y=+0.5, z=0.5)), fixed_base=False) body_names = { sink: 'sink', stove: 'stove', block: 'celery', } movable_bodies = [block] set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor)))) set_default_camera() return robot, body_names, movable_bodies
def load_world(): # TODO: store internal world info here to be reloaded set_default_camera() draw_global_system() with HideOutput(): #add_data_path() robot = load_model(DRAKE_IIWA_URDF, fixed_base=True) # DRAKE_IIWA_URDF | KUKA_IIWA_URDF floor = load_model('models/short_floor.urdf') sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5))) stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5))) celery = load_model(BLOCK_URDF, fixed_base=False) radish = load_model(SMALL_BLOCK_URDF, fixed_base=False) #cup = load_model('models/dinnerware/cup/cup_small.urdf', # Pose(Point(x=+0.5, y=+0.5, z=0.5)), fixed_base=False) draw_pose(Pose(), parent=robot, parent_link=get_tool_link(robot)) # TODO: not working # dump_body(robot) # wait_for_user() body_names = { sink: 'sink', stove: 'stove', celery: 'celery', radish: 'radish', } movable_bodies = [celery, radish] set_pose(celery, Pose(Point(y=0.5, z=stable_z(celery, floor)))) set_pose(radish, Pose(Point(y=-0.5, z=stable_z(radish, floor)))) return robot, body_names, movable_bodies
def load_world(): # TODO: store internal world info here to be reloaded with HideOutput(): robot = load_model(DRAKE_IIWA_URDF) #add_data_path() #robot = load_pybullet(KUKA_IIWA_URDF) floor = load_model('models/short_floor.urdf') sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5))) stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5))) celery = load_model(BLOCK_URDF, fixed_base=False) radish = load_model(SMALL_BLOCK_URDF, fixed_base=False) #cup = load_model('models/dinnerware/cup/cup_small.urdf', # Pose(Point(x=+0.5, y=+0.5, z=0.5)), fixed_base=False) body_names = { sink: 'sink', stove: 'stove', celery: 'celery', radish: 'radish', } movable_bodies = [celery, radish] set_pose(celery, Pose(Point(y=0.5, z=stable_z(celery, floor)))) set_pose(radish, Pose(Point(y=-0.5, z=stable_z(radish, floor)))) set_default_camera() return robot, body_names, movable_bodies
def problem_fn(n_rovers=1, collisions=True): base_extent = 2.5 base_limits = (-base_extent / 2. * np.ones(2), base_extent / 2. * np.ones(2)) mound_height = 0.1 floor = create_box(base_extent, base_extent, 0.001, color=TAN) # TODO: two rooms set_point(floor, Point(z=-0.001 / 2.)) wall1 = create_box(base_extent + mound_height, mound_height, mound_height, color=GREY) set_point(wall1, Point(y=base_extent / 2., z=mound_height / 2.)) wall2 = create_box(base_extent + mound_height, mound_height, mound_height, color=GREY) set_point(wall2, Point(y=-base_extent / 2., z=mound_height / 2.)) wall3 = create_box(mound_height, base_extent + mound_height, mound_height, color=GREY) set_point(wall3, Point(x=base_extent / 2., z=mound_height / 2.)) wall4 = create_box(mound_height, base_extent + mound_height, mound_height, color=GREY) set_point(wall4, Point(x=-base_extent / 2., z=mound_height / 2.)) wall5 = create_box(mound_height, (base_extent + mound_height)/ 2., mound_height, color=GREY) set_point(wall5, Point(y=base_extent / 4., z=mound_height / 2.)) rover_confs = [(+1, 0, np.pi), (-1, 0, 0)] assert n_rovers <= len(rover_confs) robots = [] for i in range(n_rovers): with HideOutput(): rover = load_model(TURTLEBOT_URDF) robot_z = stable_z(rover, floor) set_point(rover, Point(z=robot_z)) set_base_conf(rover, rover_confs[i]) robots.append(rover) goal_confs = {robots[0]: rover_confs[-1]} #goal_confs = {} # TODO: make the objects smaller cylinder_radius = 0.25 body1 = create_cylinder(cylinder_radius, mound_height, color=RED) set_point(body1, Point(y=-base_extent / 4., z=mound_height / 2.)) body2 = create_cylinder(cylinder_radius, mound_height, color=BLUE) set_point(body2, Point(x=base_extent / 4., y=3*base_extent / 8., z=mound_height / 2.)) movable = [body1, body2] #goal_holding = {robots[0]: body1} goal_holding = {} return NAMOProblem(robots, base_limits, movable, collisions=collisions, goal_holding=goal_holding, goal_confs=goal_confs)
def problem_fn(n_robots=2, collisions=True): base_extent = 2.0 base_limits = (-base_extent / 2. * np.ones(2), base_extent / 2. * np.ones(2)) mound_height = 0.1 floor, walls = create_environment(base_extent, mound_height) width = base_extent / 2. - 4 * mound_height wall5 = create_box(mound_height, width, mound_height, color=GREY) set_point(wall5, Point(y=-(base_extent / 2 - width / 2.), z=mound_height / 2.)) wall6 = create_box(mound_height, width, mound_height, color=GREY) set_point(wall6, Point(y=+(base_extent / 2 - width / 2.), z=mound_height / 2.)) distance = 0.5 #initial_confs = [(-distance, -distance, 0), # (-distance, +distance, 0)] initial_confs = [(-distance, -distance, 0), (+distance, +distance, 0)] assert n_robots <= len(initial_confs) body_from_name = {} #robots = ['green'] robots = ['green', 'blue'] with LockRenderer(): for i, name in enumerate(robots): with HideOutput(): body = load_model( TURTLEBOT_URDF) # TURTLEBOT_URDF | ROOMBA_URDF body_from_name[name] = body robot_z = stable_z(body, floor) set_point(body, Point(z=robot_z)) set_base_conf(body, initial_confs[i]) set_body_color(body, COLOR_FROM_NAME[name]) goals = [(+distance, -distance, 0), (+distance, +distance, 0)] #goals = goals[::-1] goals = initial_confs[::-1] goal_confs = dict(zip(robots, goals)) return NAMOProblem(body_from_name, robots, base_limits, collisions=collisions, goal_confs=goal_confs)
def rovers1(n_rovers=2, n_objectives=4, n_rocks=3, n_soil=3, n_stores=1, n_obstacles=8): base_extent = 5.0 base_limits = (-base_extent / 2. * np.ones(2), base_extent / 2. * np.ones(2)) mount_width = 0.5 mound_height = 0.1 floor = create_box(base_extent, base_extent, 0.001, color=TAN) # TODO: two rooms set_point(floor, Point(z=-0.001 / 2.)) wall1 = create_box(base_extent + mound_height, mound_height, mound_height, color=GREY) set_point(wall1, Point(y=base_extent / 2., z=mound_height / 2.)) wall2 = create_box(base_extent + mound_height, mound_height, mound_height, color=GREY) set_point(wall2, Point(y=-base_extent / 2., z=mound_height / 2.)) wall3 = create_box(mound_height, base_extent + mound_height, mound_height, color=GREY) set_point(wall3, Point(x=base_extent / 2., z=mound_height / 2.)) wall4 = create_box(mound_height, base_extent + mound_height, mound_height, color=GREY) set_point(wall4, Point(x=-base_extent / 2., z=mound_height / 2.)) # TODO: can add obstacles along the wall wall = create_box(mound_height, base_extent, mound_height, color=GREY) # TODO: two rooms set_point(wall, Point(z=mound_height / 2.)) add_data_path() with HideOutput(): lander = load_pybullet(HUSKY_URDF, scale=1) lander_z = stable_z(lander, floor) set_point(lander, Point(-1.9, -2, lander_z)) mound1 = create_box(mount_width, mount_width, mound_height, color=GREY) set_point(mound1, [+2, 2, mound_height / 2.]) mound2 = create_box(mount_width, mount_width, mound_height, color=GREY) set_point(mound2, [-2, 2, mound_height / 2.]) mound3 = create_box(mount_width, mount_width, mound_height, color=GREY) set_point(mound3, [+0.5, 2, mound_height / 2.]) mound4 = create_box(mount_width, mount_width, mound_height, color=GREY) set_point(mound4, [-0.5, 2, mound_height / 2.]) mounds = [mound1, mound2, mound3, mound4] random.shuffle(mounds) body_types = [] initial_surfaces = OrderedDict() min_distances = {} for _ in range(n_obstacles): body = create_box(mound_height, mound_height, 4 * mound_height, color=GREY) initial_surfaces[body] = floor rover_confs = [(+1, -1.75, np.pi), (-1, -1.75, 0)] assert n_rovers <= len(rover_confs) landers = [lander] stores = ['store{}'.format(i) for i in range(n_stores)] rovers = [] for i in range(n_rovers): # camera_rgb_optical_frame with HideOutput(): rover = load_model(TURTLEBOT_URDF) robot_z = stable_z(rover, floor) set_point(rover, Point(z=robot_z)) #handles = draw_aabb(get_aabb(rover)) # Includes the origin #print(get_center_extent(rover)) #wait_for_user() set_base_conf(rover, rover_confs[i]) rovers.append(rover) #dump_body(rover) #draw_pose(get_link_pose(rover, link_from_name(rover, KINECT_FRAME))) obj_width = 0.07 obj_height = 0.2 objectives = [] for i in range(n_objectives): body = create_box(obj_width, obj_width, obj_height, color=BLUE) objectives.append(body) #initial_surfaces[body] = random.choice(mounds) initial_surfaces[body] = mounds[i] min_distances.update({r: 0.05 for r in objectives}) # TODO: it is moving to intermediate locations attempting to reach the rocks rocks = [] for _ in range(n_rocks): body = create_box(0.075, 0.075, 0.01, color=BLACK) rocks.append(body) body_types.append((body, 'stone')) for _ in range(n_soil): body = create_box(0.1, 0.1, 0.005, color=BROWN) rocks.append(body) body_types.append((body, 'soil')) soils = [] # Treating soil as rocks for simplicity initial_surfaces.update({r: floor for r in rocks}) min_distances.update({r: 0.2 for r in rocks}) sample_placements(initial_surfaces, min_distances=min_distances) return RoversProblem(rovers, landers, objectives, rocks, soils, stores, base_limits, body_types)
def load_world(): # TODO: store internal world info here to be reloaded with HideOutput(): robot = load_model(DRAKE_IIWA_URDF) # robot = load_model(KUKA_IIWA_URDF) floor = load_model('models/short_floor.urdf') sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5))) stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5))) block = load_model(BLOCK_URDF, fixed_base=False) block1 = load_model(BLOCK_URDF, fixed_base=False) block2 = load_model(BLOCK_URDF, fixed_base=False) block3 = load_model(BLOCK_URDF, fixed_base=False) block4 = load_model(BLOCK_URDF, fixed_base=False) block5 = load_model(BLOCK_URDF, fixed_base=False) block6 = load_model(BLOCK_URDF, fixed_base=False) block7 = load_model(BLOCK_URDF, fixed_base=False) block8 = load_model(BLOCK_URDF, fixed_base=False) cup = load_model( 'models/cup.urdf', #'models/dinnerware/cup/cup_small.urdf' fixed_base=False) body_names = { sink: 'sink', stove: 'stove', block: 'celery', cup: 'cup', } movable_bodies = [ block, cup, block1, block2, block3, block4, block5, block6, block7, block8 ] set_pose(block, Pose(Point(x=0.1, y=0.5, z=stable_z(block, floor)))) set_pose(block1, Pose(Point(x=-0.1, y=0.5, z=stable_z(block1, floor)))) set_pose(block2, Pose(Point(y=0.35, z=stable_z(block2, floor)))) set_pose(block3, Pose(Point(y=0.65, z=stable_z(block3, floor)))) set_pose(block4, Pose(Point(x=0.1, y=0.65, z=stable_z(block4, floor)))) set_pose(block5, Pose(Point(x=-0.1, y=0.65, z=stable_z(block5, floor)))) set_pose(block6, Pose(Point(x=0.1, y=0.35, z=stable_z(block6, floor)))) set_pose(block7, Pose(Point(x=-0.1, y=0.35, z=stable_z(block7, floor)))) set_pose(block8, Pose(Point(x=0, y=0.5, z=0.45))) set_pose(cup, Pose(Point(y=0.5, z=stable_z(cup, floor)))) set_default_camera() return robot, body_names, movable_bodies