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 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 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 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 get_kitchen_task(arm='left', grasp_type='top'): with HideOutput(): pr2 = create_pr2(use_drake=USE_DRAKE_PR2) set_arm_conf(pr2, arm, get_carry_conf(arm, grasp_type)) open_arm(pr2, arm) other_arm = get_other_arm(arm) set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM)) close_arm(pr2, other_arm) table, cabbage, sink, stove = create_kitchen() floor = get_bodies()[1] class_from_body = { table: 'table', cabbage: 'cabbage', sink: 'sink', stove: 'stove', } # TODO: use for debug movable = [cabbage] surfaces = [table, sink, stove] rooms = [floor] return BeliefTask(robot=pr2, arms=[arm], grasp_types=[grasp_type], class_from_body=class_from_body, movable=movable, surfaces=surfaces, rooms=rooms, #goal_localized=[cabbage], #goal_registered=[cabbage], #goal_holding=[(arm, cabbage)], #goal_on=[(cabbage, table)], goal_on=[(cabbage, sink)], )
def plan_commands(state, viewer=False, teleport=False, profile=False, verbose=True): # TODO: could make indices into set of bodies to ensure the same... # TODO: populate the bodies here from state and not the real world sim_world = connect(use_gui=viewer) #clone_world(client=sim_world) task = state.task robot_conf = get_configuration(task.robot) robot_pose = get_pose(task.robot) with ClientSaver(sim_world): with HideOutput(): robot = create_pr2(use_drake=USE_DRAKE_PR2) set_pose(robot, robot_pose) set_configuration(robot, robot_conf) mapping = clone_world(client=sim_world, exclude=[task.robot]) assert all(i1 == i2 for i1, i2 in mapping.items()) set_client(sim_world) saved_world = WorldSaver() # StateSaver() pddlstream_problem = pddlstream_from_state(state, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) if verbose: print('Goal:', goal) print('Streams:', stream_map.keys()) stream_info = { 'test-vis-base': StreamInfo(eager=True, p_success=0), 'test-reg-base': StreamInfo(eager=True, p_success=0), } hierarchy = [ ABSTRIPSLayer(pos_pre=['AtBConf']), ] pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, stream_info=stream_info, hierarchy=hierarchy, debug=False, success_cost=MAX_COST, verbose=verbose) plan, cost, evaluations = solution if MAX_COST <= cost: plan = None print_solution(solution) print('Finite cost:', cost < MAX_COST) commands = post_process(state, plan) pr.disable() if profile: pstats.Stats(pr).sort_stats('cumtime').print_stats(10) saved_world.restore() disconnect() return commands
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) #ycb_path = os.path.join(DRAKE_PATH, DRAKE_YCB) #ycb_path = os.path.join(YCB_PATH, YCB_TEMPLATE.format('003_cracker_box')) #print(ycb_path) #load_pybullet(ycb_path) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False) #set_euler(table, Euler(yaw=np.pi/2)) with HideOutput(False): # 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_path = get_file_path(__file__, 'mit_arch_suction_gripper/mit_arch_suction_gripper.urdf') robot = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) #robot = create_cylinder(radius=0.5*BLOCK_SIDE, height=4*BLOCK_SIDE) # vacuum gripper block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table) set_point(block1, Point(z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=+0.25, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, z=block_z)) blocks = [block1, block2, block3] add_line(Point(x=-TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON), Point(x=+TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON), color=RED) set_camera_pose(camera_point=Point(y=-1, z=block_z+1), target_point=Point(z=block_z)) wait_for_user() 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) y_grasp, x_grasp = get_top_grasps(block1, tool_pose=unit_pose(), grasp_length=0.03, under=False) grasp = y_grasp # fingers won't collide gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user('Finish?') disconnect()
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
def plan_commands(state, teleport=False, profile=False, verbose=True): # TODO: could make indices into set of bodies to ensure the same... # TODO: populate the bodies here from state task = state.task robot_conf = get_configuration(task.robot) robot_pose = get_pose(task.robot) sim_world = connect(use_gui=False) with ClientSaver(sim_world): with HideOutput(): robot = create_pr2(use_drake=USE_DRAKE_PR2) #robot = load_model(DRAKE_PR2_URDF, fixed_base=True) set_pose(robot, robot_pose) set_configuration(robot, robot_conf) clone_world(client=sim_world, exclude=[task.robot]) set_client(sim_world) saved_world = WorldSaver() # StateSaver() pddlstream_problem = pddlstream_from_state(state, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) if verbose: print('Goal:', goal) print('Streams:', stream_map.keys()) stream_info = { 'test-vis-base': StreamInfo(eager=True, p_success=0), 'test-reg-base': StreamInfo(eager=True, p_success=0), } pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, stream_info=stream_info, max_cost=MAX_COST, verbose=verbose) pr.disable() plan, cost, evaluations = solution if MAX_COST <= cost: plan = None print_solution(solution) print('Finite cost:', cost < MAX_COST) print('Real cost:', float(cost) / SCALE_COST) if profile: pstats.Stats(pr).sort_stats('tottime').print_stats(10) saved_world.restore() commands = post_process(state, plan) disconnect() return commands
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 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) #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 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 main(display=True, teleport=False): parser = argparse.ArgumentParser() parser.add_argument('-problem', default='rovers1', help='The name of the problem to solve') parser.add_argument('-algorithm', default='focused', help='Specifies the algorithm') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-unit', action='store_true', help='Uses unit costs') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_fn_from_name: raise ValueError(args.problem) problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn() saver = WorldSaver() draw_base_limits(problem.limits, color=(1, 0, 0)) pddlstream_problem = pddlstream_from_problem(problem, collisions=not args.cfree, teleport=teleport) stream_info = { 'test-cfree-ray-conf': StreamInfo(negate=True), 'test-reachable': StreamInfo(p_success=1e-1), 'obj-inv-visible': StreamInfo(), 'com-inv-visible': StreamInfo(), 'sample-above': StreamInfo(), 'sample-motion': StreamInfo(overhead=10), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) #print('Streams:', stream_map.keys()) success_cost = 0 if args.optimal else INF planner = 'ff-wastar3' search_sample_ratio = 2 max_planner_time = 10 # TODO: need to accelerate samples here because of the failed test reachable pr = cProfile.Profile() pr.enable() with LockRenderer(False): if args.algorithm == 'focused': # TODO: option to only consider costs during local optimization solution = solve_focused( pddlstream_problem, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, debug=False, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, unit_efforts=True, effort_weight=1, #bind=True, max_skeletons=None, search_sample_ratio=search_sample_ratio) elif args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True) else: raise ValueError(args.algorithm) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime if plan is None: return if (not display) or (plan is None): disconnect() return # Maybe openrave didn't actually sample any joints... # http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/ with LockRenderer(): commands = post_process(problem, plan, teleport=teleport) saver.restore() # Assumes bodies are ordered the same way if not args.viewer: disconnect() connect(use_gui=True) with LockRenderer(): with HideOutput(): problem_fn() # TODO: way of doing this without reloading? saver.restore() # Assumes bodies are ordered the same way if args.simulate: control_commands(commands) else: time_step = None if teleport else 0.01 apply_commands(BeliefState(problem), commands, time_step) wait_for_user() disconnect()
def main(): parser = create_parser() parser.add_argument('-problem', default='rovers1', help='The name of the problem to solve') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_fn_from_name: raise ValueError(args.problem) problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): rovers_problem = problem_fn() saver = WorldSaver() draw_base_limits(rovers_problem.limits, color=RED) pddlstream_problem = pddlstream_from_problem(rovers_problem, collisions=not args.cfree, teleport=args.teleport, holonomic=False, reversible=True, use_aabb=True) stream_info = { 'test-cfree-ray-conf': StreamInfo(), 'test-reachable': StreamInfo(p_success=1e-1), 'obj-inv-visible': StreamInfo(), 'com-inv-visible': StreamInfo(), 'sample-above': StreamInfo(), 'sample-motion': StreamInfo(overhead=10), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) #print('Streams:', stream_map.keys()) success_cost = 0 if args.optimal else INF planner = 'ff-wastar3' search_sample_ratio = 2 max_planner_time = 10 # TODO: need to accelerate samples here because of the failed test reachable with Profiler(field='tottime', num=25): with LockRenderer(lock=not args.enable): # TODO: option to only consider costs during local optimization solution = solve(pddlstream_problem, algorithm=args.algorithm, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, debug=False, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, unit_efforts=True, effort_weight=1, search_sample_ratio=search_sample_ratio) for body in get_bodies(): if body not in saver.bodies: remove_body(body) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return # Maybe OpenRAVE didn't actually sample any joints... # http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/ with LockRenderer(): commands = post_process(rovers_problem, plan) saver.restore() wait_for_user('Begin?') if args.simulate: control_commands(commands) else: time_step = None if args.teleport else 0.01 apply_commands(BeliefState(rovers_problem), commands, time_step) wait_for_user('Finish?') disconnect()
def main(teleport=False): #parser = create_parser() parser = argparse.ArgumentParser() parser.add_argument('-algorithm', default='incremental', help='Specifies the algorithm') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=5*60, type=int, help='The max time') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) connect(use_gui=args.viewer) with HideOutput(): namo_problem = problem_fn(collisions=not args.cfree) saver = WorldSaver() draw_base_limits(namo_problem.limits, color=RED) pddlstream_problem, edges = pddlstream_from_problem(namo_problem, teleport=teleport) _, constant_map, _, stream_map, init, goal = pddlstream_problem print('Constants:', constant_map) print('Init:', init) print('Goal:', goal) stream_info = { 'compute-motion': StreamInfo(eager=True, p_success=0), 'ConfConfCollision': PredicateInfo(p_success=1, overhead=0.1), 'TrajConfCollision': PredicateInfo(p_success=1, overhead=1), 'TrajTrajCollision': PredicateInfo(p_success=1, overhead=10), 'TrajDistance': FunctionInfo(eager=True), # Need to eagerly evaluate otherwise 0 duration (failure) } success_cost = 0 if args.optimal else INF max_planner_time = 10 with Profiler(field='tottime', num=25): # cumtime | tottime with LockRenderer(lock=not args.enable): # TODO: solution = solve_incremental(pddlstream_problem if args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, max_planner_time=max_planner_time, success_cost=success_cost, max_time=args.max_time, start_complexity=INF, verbose=True, debug=True) elif args.algorithm == 'focused': solution = solve_focused(pddlstream_problem, stream_info=stream_info, max_planner_time=max_planner_time, success_cost=success_cost, max_time=args.max_time, max_skeletons=None, bind=True, max_failures=INF, verbose=True, debug=True) else: raise ValueError(args.algorithm) print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return saver.restore() draw_edges(edges) state = BeliefState(namo_problem) wait_for_user('Begin?') #time_step = None if teleport else 0.01 #with VideoSaver('video.mp4'): display_plan(namo_problem, state, plan) wait_for_user('Finish?') disconnect()
def load_pick_and_place(extrusion_name, scale=MILLIMETER, max_bricks=6): assert extrusion_name == 'choreo_brick_demo' root_directory = os.path.dirname(os.path.abspath(__file__)) bricks_directory = os.path.join(root_directory, PICKNPLACE_DIRECTORY, 'bricks') print('Name: {}'.format(extrusion_name)) with open( os.path.join(bricks_directory, PICKNPLACE_FILENAMES[extrusion_name]), 'r') as f: json_data = json.loads(f.read()) kuka_urdf = '../models/framefab_kr6_r900_support/urdf/kr6_r900_mit_suction_gripper.urdf' obj_directory = os.path.join(bricks_directory, 'meshes', 'collision') with HideOutput(): #world = load_pybullet(os.path.join(bricks_directory, 'urdf', 'brick_demo.urdf')) robot = load_pybullet(os.path.join(root_directory, kuka_urdf), fixed_base=True) #set_point(robot, (0.14, 0, 0)) #dump_body(robot) pick_base_point = parse_point(json_data['pick_base_center_point']) draw_pose((pick_base_point, unit_quat())) place_base_point = parse_point(json_data['place_base_center_point']) draw_pose((place_base_point, unit_quat())) # workspace_geo_place_support_object_11 = pick_left_over_bricks_11 obstacle_from_name = {} for filename in json_data['pick_support_surface_file_names']: obstacle_from_name[strip_extension(filename)] = \ create_obj(os.path.join(obj_directory, filename), scale=scale, color=(0.5, 0.5, 0.5, 1)) for filename in json_data['place_support_surface_file_names']: obstacle_from_name[strip_extension(filename)] = \ create_obj(os.path.join(obj_directory, filename), scale=scale, color=(1., 0., 0., 1)) brick_from_index = {} for json_element in json_data['sequenced_elements']: index = json_element['order_id'] pick_body = create_obj(os.path.join( obj_directory, json_element['pick_element_geometry_file_name']), scale=scale, color=(0, 0, 1, 1)) add_body_name(pick_body, index) #place_body = create_obj(os.path.join(obj_directory, json_element['place_element_geometry_file_name']), # scale=scale, color=(0, 1, 0, 1)) #add_body_name(place_body, name) world_from_obj_pick = get_pose(pick_body) # [u'pick_grasp_plane', u'pick_grasp_retreat_plane', u'place_grasp_retreat_plane', # u'pick_grasp_approach_plane', u'place_grasp_approach_plane', u'place_grasp_plane'] ee_grasp_poses = [{ name: pose_from_tform(parse_transform(approach)) for name, approach in json_grasp.items() } for json_grasp in json_element['grasps']] # pick_grasp_plane is at the top of the object with z facing downwards grasp_index = 0 world_from_ee_pick = ee_grasp_poses[grasp_index]['pick_grasp_plane'] world_from_ee_place = ee_grasp_poses[grasp_index]['place_grasp_plane'] #draw_pose(world_from_ee_pick, length=0.04) #draw_pose(world_from_ee_place, length=0.04) ee_from_obj = multiply(invert(world_from_ee_pick), world_from_obj_pick) # Using pick frame world_from_obj_place = multiply(world_from_ee_place, ee_from_obj) #set_pose(pick_body, world_from_obj_place) grasps = [ Grasp( index, num, *[ multiply(invert(world_from_ee_pick[name]), world_from_obj_pick) for name in GRASP_NAMES ]) for num, world_from_ee_pick in enumerate(ee_grasp_poses) ] brick_from_index[index] = Brick( index=index, body=pick_body, initial_pose=WorldPose(index, world_from_obj_pick), goal_pose=WorldPose(index, world_from_obj_place), grasps=grasps, goal_supports=json_element.get('place_contact_ngh_ids', [])) # pick_contact_ngh_ids are movable element contact partial orders # pick_support_surface_file_names are fixed element contact partial orders return robot, brick_from_index, obstacle_from_name
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()
def main(partial=False, defer=False, verbose=True): parser = create_parser() parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) connect(use_gui=args.viewer) problem_fn = cooking_problem # holding_problem | stacking_problem | cleaning_problem | cooking_problem # cleaning_button_problem | cooking_button_problem with HideOutput(): problem = problem_fn() #state_id = save_state() saver = WorldSaver() #dump_world() pddlstream_problem = pddlstream_from_problem(problem, teleport=args.teleport) stream_info = { # 'test-cfree-pose-pose': StreamInfo(p_success=1e-3, verbose=verbose), # 'test-cfree-approach-pose': StreamInfo(p_success=1e-2, verbose=verbose), # 'test-cfree-traj-pose': StreamInfo(p_success=1e-1, verbose=verbose), 'MoveCost': FunctionInfo(opt_move_cost_fn), } stream_info.update({ 'sample-pose': StreamInfo(PartialInputs('?r')), 'inverse-kinematics': StreamInfo(PartialInputs('?p')), 'plan-base-motion': StreamInfo(PartialInputs('?q1 ?q2'), defer_fn=defer_shared if defer else never_defer), } if partial else { 'sample-pose': StreamInfo(from_fn(opt_pose_fn)), 'inverse-kinematics': StreamInfo(from_fn(opt_ik_fn)), 'plan-base-motion': StreamInfo(from_fn(opt_motion_fn)), }) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', str_from_object(set(stream_map))) print(SEPARATOR) with Profiler(): with LockRenderer(lock=not args.enable): solution = solve(pddlstream_problem, algorithm=args.algorithm, unit_costs=args.unit, stream_info=stream_info, success_cost=INF, verbose=True, debug=False) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return print(SEPARATOR) with LockRenderer(lock=not args.enable): commands = post_process(problem, plan) problem.remove_gripper() saver.restore() #restore_state(state_id) saver.restore() wait_if_gui('Execute?') if args.simulate: control_commands(commands) else: apply_commands(State(), commands, time_step=0.01) wait_if_gui('Finish?') disconnect()
def main(): parser = create_parser() parser.add_argument('-problem', default='problem1', help='The name of the problem to solve') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_fn_from_name: raise ValueError(args.problem) problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn() saver = WorldSaver() draw_base_limits(problem.limits, color=RED) pddlstream_problem = pddlstream_from_problem(problem, collisions=not args.cfree, teleport=args.teleport) stream_info = { 'inverse-kinematics': StreamInfo(), 'plan-base-motion': StreamInfo(overhead=1e1), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) #print('Streams:', stream_map.keys()) success_cost = 0 if args.optimal else INF planner = 'ff-astar' search_sample_ratio = 1 max_planner_time = 10 with Profiler(field='cumtime', num=25): # cumtime | tottime with LockRenderer(lock=not args.enable): solution = solve(pddlstream_problem, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, debug=False, unit_efforts=True, effort_weight=1, search_sample_ratio=search_sample_ratio) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return # Maybe openrave didn't actually sample any joints... # http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/ with LockRenderer(lock=not args.enable): commands = post_process(problem, plan, teleport=args.teleport) saver.restore() if args.simulate: control_commands(commands) else: time_step = None if args.teleport else 0.01 apply_commands(BeliefState(problem), commands, time_step) wait_for_user() disconnect()
def main(display=True, teleport=False, partial=False, defer=False): parser = argparse.ArgumentParser() parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') #parser.add_argument('-display', action='store_true', help='displays the solution') args = parser.parse_args() connect(use_gui=args.viewer) problem_fn = cooking_problem # holding_problem | stacking_problem | cleaning_problem | cooking_problem # cleaning_button_problem | cooking_button_problem with HideOutput(): problem = problem_fn() state_id = save_state() #saved_world = WorldSaver() #dump_world() pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport) stream_info = { 'sample-pose': StreamInfo(PartialInputs('?r')), 'inverse-kinematics': StreamInfo(PartialInputs('?p')), 'plan-base-motion': StreamInfo(PartialInputs('?q1 ?q2'), defer_fn=defer_shared if defer else never_defer), 'MoveCost': FunctionInfo(opt_move_cost_fn), } if partial else { 'sample-pose': StreamInfo(from_fn(opt_pose_fn)), 'inverse-kinematics': StreamInfo(from_fn(opt_ik_fn)), 'plan-base-motion': StreamInfo(from_fn(opt_motion_fn)), 'MoveCost': FunctionInfo(opt_move_cost_fn), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) pr = cProfile.Profile() pr.enable() with LockRenderer(): #solution = solve_incremental(pddlstream_problem, debug=True) solution = solve_focused(pddlstream_problem, stream_info=stream_info, success_cost=INF, debug=False) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is None: return if (not display) or (plan is None): disconnect() return with LockRenderer(): commands = post_process(problem, plan) if args.viewer: restore_state(state_id) else: disconnect() connect(use_gui=True) with HideOutput(): problem_fn() # TODO: way of doing this without reloading? if args.simulate: control_commands(commands) else: apply_commands(State(), commands, time_step=0.01) user_input('Finish?') disconnect()
def main(display=True, teleport=False): parser = argparse.ArgumentParser() #parser.add_argument('-problem', default='rovers1', help='The name of the problem to solve') parser.add_argument('-algorithm', default='focused', help='Specifies the algorithm') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-unit', action='store_true', help='Uses unit costs') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) #problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} #if args.problem not in problem_fn_from_name: # raise ValueError(args.problem) #problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn(collisions=not args.cfree) saver = WorldSaver() draw_base_limits(problem.limits, color=RED) pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport) stream_info = { 'test-cfree-conf-pose': StreamInfo(negate=True, p_success=1e-2), 'test-cfree-traj-pose': StreamInfo(negate=True, p_success=1e-1), 'compute-motion': StreamInfo(eager=True, p_success=0), 'test-reachable': StreamInfo(eager=True), 'Distance': FunctionInfo(eager=True), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) success_cost = 0 if args.optimal else INF planner = 'ff-wastar1' search_sample_ratio = 0 max_planner_time = 10 pr = cProfile.Profile() pr.enable() with LockRenderer(True): if args.algorithm == 'focused': solution = solve_focused(pddlstream_problem, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, debug=False, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, unit_efforts=True, effort_weight=1, bind=True, max_skeletons=None, search_sample_ratio=search_sample_ratio) elif args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True) else: raise ValueError(args.algorithm) saver.restore() print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime if plan is None: wait_for_user() return if (not display) or (plan is None): disconnect() return with LockRenderer(): commands = post_process(problem, plan, teleport=teleport) saver.restore() # Assumes bodies are ordered the same way if not args.viewer: disconnect() connect(use_gui=True) with LockRenderer(): with HideOutput(): problem_fn() # TODO: way of doing this without reloading? saver.restore() # Assumes bodies are ordered the same way wait_for_user() if args.simulate: control_commands(commands) else: time_step = None if teleport else 0.01 apply_commands(BeliefState(problem), commands, time_step=time_step) wait_for_user() disconnect()
def main(): parser = create_parser(default_algorithm='binding') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn(collisions=not args.cfree) saver = WorldSaver() draw_base_limits(problem.limits, color=RED) pddlstream_problem = pddlstream_from_problem(problem) stream_info = { 'test-cfree-conf-pose': StreamInfo(p_success=1e-2), 'test-cfree-traj-pose': StreamInfo(p_success=1e-1), 'compute-motion': StreamInfo(eager=True, p_success=0), 'test-reachable': StreamInfo(eager=True), 'Distance': FunctionInfo(eager=True), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) success_cost = 0 if args.optimal else INF planner = 'ff-wastar1' search_sample_ratio = 0 max_planner_time = 10 with Profiler(field='tottime', num=25): # cumtime | tottime with LockRenderer(lock=not args.enable): solution = solve(pddlstream_problem, algorithm=args.algorithm, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, debug=False, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, unit_efforts=True, effort_weight=1, search_sample_ratio=search_sample_ratio) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return with LockRenderer(): commands = post_process(problem, plan) saver.restore() # Assumes bodies are ordered the same way wait_for_user() if args.simulate: control_commands(commands) else: apply_commands(BeliefState(problem), commands, time_step=1e-2) # 1e-2 | None wait_for_user() disconnect()
def main(verbose=True): # TODO: could work just on postprocessing # TODO: try the other reachability database # TODO: option to only consider costs during local optimization parser = create_parser() parser.add_argument('-problem', default='packed', help='The name of the problem to solve') parser.add_argument('-n', '--number', default=5, type=int, help='The number of objects') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_fn_from_name: raise ValueError(args.problem) problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn(num=args.number) draw_base_limits(problem.base_limits, color=(1, 0, 0)) saver = WorldSaver() #handles = [] #for link in get_group_joints(problem.robot, 'left_arm'): # handles.append(draw_link_name(problem.robot, link)) #wait_for_user() pddlstream_problem = pddlstream_from_problem(problem, collisions=not args.cfree, teleport=args.teleport) stream_info = { 'inverse-kinematics': StreamInfo(), 'plan-base-motion': StreamInfo(overhead=1e1), 'test-cfree-pose-pose': StreamInfo(p_success=1e-3, verbose=verbose), 'test-cfree-approach-pose': StreamInfo(p_success=1e-2, verbose=verbose), 'test-cfree-traj-pose': StreamInfo(p_success=1e-1, verbose=verbose), # TODO: apply to arm and base trajs #'test-cfree-traj-grasp-pose': StreamInfo(verbose=verbose), 'Distance': FunctionInfo(p_success=0.99, opt_fn=lambda q1, q2: BASE_CONSTANT), #'MoveCost': FunctionInfo(lambda t: BASE_CONSTANT), } #stream_info = {} _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', str_from_object(set(stream_map))) success_cost = 0 if args.optimal else INF planner = 'ff-astar' if args.optimal else 'ff-wastar3' search_sample_ratio = 2 max_planner_time = 10 # effort_weight = 0 if args.optimal else 1 effort_weight = 1e-3 if args.optimal else 1 with Profiler(field='tottime', num=25): # cumtime | tottime with LockRenderer(lock=not args.enable): solution = solve(pddlstream_problem, algorithm=args.algorithm, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, debug=False, unit_efforts=True, effort_weight=effort_weight, search_sample_ratio=search_sample_ratio) saver.restore() cost_over_time = [(s.cost, s.time) for s in SOLUTIONS] for i, (cost, runtime) in enumerate(cost_over_time): print('Plan: {} | Cost: {:.3f} | Time: {:.3f}'.format( i, cost, runtime)) #print(SOLUTIONS) print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return with LockRenderer(lock=not args.enable): commands = post_process(problem, plan, teleport=args.teleport) saver.restore() draw_base_limits(problem.base_limits, color=(1, 0, 0)) wait_for_user() if args.simulate: control_commands(commands) else: time_step = None if args.teleport else 0.01 apply_commands(State(), commands, time_step) wait_for_user() disconnect()
def main(display=True, simulate=False, teleport=False): parser = argparse.ArgumentParser() parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') #parser.add_argument('-display', action='store_true', help='displays the solution') args = parser.parse_args() connect(use_gui=args.viewer) problem_fn = cooking_problem # holding_problem | stacking_problem | cleaning_problem | cooking_problem # cleaning_button_problem | cooking_button_problem with HideOutput(): problem = problem_fn() state_id = save_state() #saved_world = WorldSaver() #dump_world() pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport) stream_info = { 'sample-pose': StreamInfo(PartialInputs('?r')), 'inverse-kinematics': StreamInfo(PartialInputs('?p')), 'plan-base-motion': StreamInfo(PartialInputs('?q1 ?q2')), 'MoveCost': FunctionInfo(opt_move_cost_fn), } synthesizers = [ StreamSynthesizer( 'safe-base-motion', { 'plan-base-motion': 1, 'TrajPoseCollision': 0, 'TrajGraspCollision': 0, 'TrajArmCollision': 0, }, from_fn(get_base_motion_synth(problem, teleport))), ] if USE_SYNTHESIZERS else [] _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) print('Synthesizers:', synthesizers) pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, stream_info=stream_info, synthesizers=synthesizers, success_cost=INF) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is None: return if (not display) or (plan is None): disconnect() return commands = post_process(problem, plan) if args.viewer: restore_state(state_id) else: disconnect() connect(use_gui=True) with HideOutput(): problem_fn() # TODO: way of doing this without reloading? if simulate: enable_gravity() control_commands(commands) else: step_commands(commands, time_step=0.01) user_input('Finish?') disconnect()
def main(display=True, teleport=False): parser = argparse.ArgumentParser() parser.add_argument('-algorithm', default='incremental', help='Specifies the algorithm') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=5*60, type=int, help='The max time') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) #problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} #if args.problem not in problem_fn_from_name: # raise ValueError(args.problem) #problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn(collisions=not args.cfree) saver = WorldSaver() draw_base_limits(problem.limits, color=RED) pddlstream, edges = pddlstream_from_problem(problem, teleport=teleport) _, constant_map, _, stream_map, init, goal = pddlstream print('Constants:', constant_map) print('Init:', init) print('Goal:', goal) success_cost = 0 if args.optimal else INF max_planner_time = 10 stream_info = { 'compute-motion': StreamInfo(eager=True, p_success=0), 'ConfConfCollision': FunctionInfo(p_success=1, overhead=0.1), 'TrajConfCollision': FunctionInfo(p_success=1, overhead=1), 'TrajTrajCollision': FunctionInfo(p_success=1, overhead=10), 'TrajDistance': FunctionInfo(eager=True), # Need to eagerly evaluate otherwise 0 duration (failure) } pr = cProfile.Profile() pr.enable() with LockRenderer(False): if args.algorithm == 'incremental': solution = solve_incremental(pddlstream, max_planner_time=max_planner_time, success_cost=success_cost, max_time=args.max_time, start_complexity=INF, verbose=True, debug=True) elif args.algorithm == 'focused': solution = solve_focused(pddlstream, stream_info=stream_info, max_planner_time=max_planner_time, success_cost=success_cost, max_time=args.max_time, max_skeletons=None, bind=True, max_failures=INF, verbose=True, debug=True) else: raise ValueError(args.algorithm) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime if plan is None: wait_for_user() return if (not display) or (plan is None): disconnect() return if not args.viewer: disconnect() connect(use_gui=True) with LockRenderer(): with HideOutput(): problem_fn() # TODO: way of doing this without reloading? saver.restore() # Assumes bodies are ordered the same way draw_edges(edges) state = BeliefState(problem) wait_for_user() #time_step = None if teleport else 0.01 #with VideoSaver('video.mp4'): display_plan(problem, state, plan) wait_for_user() disconnect()