def main(focused=True, unit_costs=False): problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem tamp_problem = problem_fn() print(tamp_problem) pddlstream_problem = pddlstream_from_tamp(tamp_problem) if focused: solution = solve_focused(pddlstream_problem, unit_costs=unit_costs) else: #solution = solve_exhaustive(pddlstream_problem, unit_costs=unit_costs) solution = solve_incremental(pddlstream_problem, unit_costs=unit_costs) print_solution(solution) plan, cost, evaluations = solution if plan is None: return print(evaluations) colors = dict(zip(tamp_problem.initial.block_poses, COLORS)) viewer = DiscreteTAMPViewer(1, len(tamp_problem.poses), title='Initial') state = tamp_problem.initial print(state) draw_state(viewer, state, colors) for action in plan: user_input('Continue?') state = apply_action(state, action) print(state) draw_state(viewer, state, colors) user_input('Finish?')
def main(focused=False, deterministic=False, unit_costs=True): np.set_printoptions(precision=2) if deterministic: seed = 0 np.random.seed(seed) print('Seed:', get_random_seed()) problem_fn = get_tight_problem # get_tight_problem | get_blocked_problem tamp_problem = problem_fn() print(tamp_problem) stream_info = { #'test-region': StreamInfo(eager=True, p_success=0), # bound_fn is None #'plan-motion': StreamInfo(p_success=1), # bound_fn is None #'trajcollision': StreamInfo(p_success=1), # bound_fn is None #'cfree': StreamInfo(eager=True), } pddlstream_problem = pddlstream_from_tamp(tamp_problem) pr = cProfile.Profile() pr.enable() if focused: solution = solve_focused(pddlstream_problem, stream_info=stream_info, max_time=10, max_cost=INF, debug=False, effort_weight=None, unit_costs=unit_costs, postprocess=False, visualize=False) else: solution = solve_incremental(pddlstream_problem, layers=1, unit_costs=unit_costs, verbose=False) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is None: return colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS)) viewer = ContinuousTMPViewer(tamp_problem.regions, title='Continuous TAMP') state = tamp_problem.initial print() print(state) draw_state(viewer, state, colors) for i, (action, args) in enumerate(plan): user_input('Continue?') print(i, action, args) s2 = args[-1] state = TAMPState( s2[R], s2[H], {b: s2[b] for b in state.block_poses if s2[b] is not None}) print(state) draw_state(viewer, state, colors) user_input('Finish?')
def apply_plan(tamp_problem, plan): colors = dict(zip(tamp_problem.initial.block_poses, COLORS)) viewer = DiscreteTAMPViewer(1, len(tamp_problem.poses), title='Initial') state = tamp_problem.initial print(state) draw_state(viewer, state, colors) for action in plan: user_input('Continue?') state = apply_action(state, action) print(state) draw_state(viewer, state, colors) user_input('Finish?')
def main(max_time=20): """ Creates and solves the 2D motion planning problem. """ parser = create_parser() args = parser.parse_args() print('Arguments:', args) obstacles = [create_box((.5, .5), (.2, .2))] regions = { 'env': create_box((.5, .5), (1, 1)), 'green': create_box((.8, .8), (.4, .4)), } goal = 'green' if goal not in regions: goal = ARRAY([1, 1]) max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0 problem, samples, roadmap = create_problem(goal, obstacles, regions, max_distance=max_distance) print('Initial:', str_from_object(problem.init)) print('Goal:', str_from_object(problem.goal)) constraints = PlanConstraints(max_cost=1.25) # max_cost=INF) with Profiler(field='tottime', num=10): solution = solve_incremental(problem, constraints=constraints, unit_costs=args.unit, success_cost=0, max_time=max_time, verbose=False) print_solution(solution) plan, cost, evaluations = solution #viewer = draw_environment(obstacles, regions) #for sample in samples: # viewer.draw_point(sample) #user_input('Continue?') # TODO: use the same viewer here draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime user_input('Continue?') if plan is None: return segments = [args for name, args in plan] draw_solution(segments, obstacles, regions) user_input('Finish?')
def main(max_time=20): """ Creates and solves the 2D motion planning problem. """ obstacles = [create_box((.5, .5), (.2, .2))] regions = { 'env': create_box((.5, .5), (1, 1)), 'green': create_box((.8, .8), (.4, .4)), } goal = 'green' if goal not in regions: goal = array([1, 1]) max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0 problem, samples, roadmap = create_problem(goal, obstacles, regions, max_distance=max_distance) print('Initial:', str_from_object(problem.init)) print('Goal:', str_from_object(problem.goal)) pr = cProfile.Profile() pr.enable() solution = solve_incremental(problem, unit_costs=False, max_cost=0, max_time=max_time, verbose=False) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) print_solution(solution) plan, cost, evaluations = solution #viewer = draw_environment(obstacles, regions) #for sample in samples: # viewer.draw_point(sample) #user_input('Continue?') # TODO: use the same viewer here draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime user_input('Continue?') if plan is None: return segments = [args for name, args in plan] draw_solution(segments, obstacles, regions) user_input('Finish?')
def display_image(filename): import matplotlib.pyplot as plt import matplotlib.image as mpimg img = mpimg.imread(filename) plt.imshow(img) plt.title(filename) plt.axis('off') plt.tight_layout() #plt.show() plt.draw() #plt.waitforbuttonpress(0) # this will wait for indefinite time plt.pause(interval=1e-3) user_input() plt.close(plt.figure())
def main(max_time=20): """ Creates and solves the 2D motion planning problem. """ obstacles = [create_box((.5, .5), (.2, .2))] regions = { 'env': create_box((.5, .5), (1, 1)), #'goal': create_box((.8, .8), (.4, .4)), } goal = np.array([1, 1]) #goal = 'goal' max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0 problem, roadmap = create_problem(goal, obstacles, regions, max_distance=max_distance) pr = cProfile.Profile() pr.enable() solution = solve_incremental(problem, unit_costs=False, max_cost=0, max_time=max_time, verbose=False) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) print_solution(solution) plan, cost, evaluations = solution print('Plan:', plan) if plan is None: return # TODO: use the same viewer here draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime user_input('Continue?') segments = [args for name, args in plan] draw_solution(segments, obstacles, regions) user_input('Finish?')
def display_plan(tamp_problem, plan, display=True): from examples.continuous_tamp.viewer import ContinuousTMPViewer from examples.discrete_tamp.viewer import COLORS example_name = os.path.basename(os.path.dirname(__file__)) directory = os.path.join(VISUALIZATIONS_DIR, example_name + '/') ensure_dir(directory) colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS)) viewer = ContinuousTMPViewer(SUCTION_HEIGHT, tamp_problem.regions, title='Continuous TAMP') state = tamp_problem.initial print() print(state) draw_state(viewer, state, colors) if display: user_input('Continue?') if plan is not None: for i, action in enumerate(plan): print(i, *action) for j, state in enumerate(apply_action(state, action)): print(i, j, state) draw_state(viewer, state, colors) viewer.save(os.path.join(directory, '{}_{}'.format(i, j))) if display: user_input('Continue?') if display: user_input('Finish?')
def display_plan(tamp_problem, plan, display=True, time_step=0.01, sec_per_step=0.002): from examples.continuous_tamp.viewer import ContinuousTMPViewer from examples.discrete_tamp.viewer import COLORS example_name = os.path.basename(os.path.dirname(__file__)) directory = os.path.join(VISUALIZATIONS_DIR, example_name + '/') safe_rm_dir(directory) ensure_dir(directory) colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS)) viewer = ContinuousTMPViewer(SUCTION_HEIGHT, tamp_problem.regions, title='Continuous TAMP') state = tamp_problem.initial print() print(state) duration = compute_duration(plan) real_time = None if sec_per_step is None else (duration * sec_per_step / time_step) print('Duration: {} | Step size: {} | Real time: {}'.format( duration, time_step, real_time)) draw_state(viewer, state, colors) if display: user_input('Start?') if plan is not None: #for action in plan: # i = action.start # print(action) # for j, state in enumerate(apply_action(state, action)): # print(i, j, state) # draw_state(viewer, state, colors) # viewer.save(os.path.join(directory, '{}_{}'.format(i, j))) # if display: # if sec_per_step is None: # user_input('Continue?') # else: # time.sleep(sec_per_step) for t in inclusive_range(0, duration, time_step): for action in plan: if action.start <= t <= get_end(action): update_state(state, action, t - action.start) print('t={} | {}'.format(t, state)) draw_state(viewer, state, colors) viewer.save(os.path.join(directory, 't={}'.format(t))) if display: if sec_per_step is None: user_input('Continue?') else: time.sleep(sec_per_step) if display: user_input('Finish?')
def display_rmmp_plan(tamp_problem, plan): viewer = ContinuousTMPViewer(SUCTION_HEIGHT, tamp_problem.regions, title='Continuous TAMP') conf = conf_from_state(tamp_problem.initial) print() print(conf) draw_conf(viewer, tamp_problem, conf) user_input('Start?') for i, (action, args) in enumerate(plan): print(i, action, args) if action == 'switch': continue traj = args[-1] for conf in traj[1:]: print(conf) draw_conf(viewer, tamp_problem, conf) user_input('Continue?') user_input('Finish?')
def step_plan(tamp_problem, plan): if plan is None: return # TODO: could also use the old version of this colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS)) viewer = ContinuousTMPViewer(SUCTION_HEIGHT, tamp_problem.regions, title='Continuous TAMP') state = tamp_problem.initial print() print(state) draw_state(viewer, state, colors) user_input('Start?') for i, action in enumerate(plan): name, args = action print(i, name, args) for state in apply_action(state, action): print(state) draw_state(viewer, state, colors) user_input('Continue?') user_input('Finish?')
def main(deterministic=False, unit_costs=True): np.set_printoptions(precision=2) if deterministic: seed = 0 np.random.seed(seed) print('Seed:', get_random_seed()) problem_fn = tight # get_tight_problem | get_blocked_problem tamp_problem = problem_fn(n_blocks=2, n_goals=2, n_robots=1) print(tamp_problem) pddlstream_problem = pddlstream_from_tamp(tamp_problem) with Profiler(): solution = solve_incremental(pddlstream_problem, complexity_step=1, max_time=30, planner='dijkstra', unit_costs=unit_costs, verbose=False) print_solution(solution) plan, cost, evaluations = solution if plan is None: return # TODO: might still be a planning bug viewer = ContinuousTMPViewer(SUCTION_HEIGHT, tamp_problem.regions, title='Continuous TAMP') conf = conf_from_state(tamp_problem.initial) print() print(conf) draw_conf(viewer, tamp_problem, conf) user_input('Start?') for i, (action, args) in enumerate(plan): print(i, action, args) if action == 'switch': continue traj = args[-1] for conf in traj[1:]: print(conf) draw_conf(viewer, tamp_problem, conf) user_input('Continue?') user_input('Finish?')
def main(precompute=False): parser = argparse.ArgumentParser() # djmm_test_block | mars_bubble | sig_artopt-bunny | topopt-100 | topopt-205 | topopt-310 | voronoi parser.add_argument('-p', '--problem', default='simple_frame', help='The name of the problem to solve') parser.add_argument('-c', '--cfree', action='store_true', help='Disables collisions with obstacles') parser.add_argument('-m', '--motions', action='store_true', help='Plans motions between each extrusion') parser.add_argument('-v', '--viewer', action='store_true', help='Enables the viewer during planning (slow!)') args = parser.parse_args() print('Arguments:', args) # TODO: setCollisionFilterGroupMask # TODO: fail if wild stream produces unexpected facts # TODO: try search at different cost levels (i.e. w/ and w/o abstract) # TODO: submodule in https://github.mit.edu/yijiangh/assembly-instances elements, node_points, ground_nodes = load_extrusion(args.problem) node_order = list(range(len(node_points))) #np.random.shuffle(node_order) node_order = sorted(node_order, key=lambda n: node_points[n][2]) elements = sorted(elements, key=lambda e: min(node_points[n][2] for n in e)) #node_order = node_order[:100] ground_nodes = [n for n in ground_nodes if n in node_order] elements = [ element for element in elements if all(n in node_order for n in element) ] #plan = plan_sequence_test(node_points, elements, ground_nodes) connect(use_gui=args.viewer) floor, robot = load_world() obstacles = [floor] initial_conf = get_joint_positions(robot, get_movable_joints(robot)) #dump_body(robot) #if has_gui(): # draw_model(elements, node_points, ground_nodes) # wait_for_interrupt('Continue?') #joint_weights = compute_joint_weights(robot, num=1000) #elements = elements[:50] # 10 | 50 | 100 | 150 #debug_elements(robot, node_points, node_order, elements) element_bodies = dict(zip(elements, create_elements(node_points, elements))) if precompute: pr = cProfile.Profile() pr.enable() trajectories = sample_trajectories(robot, obstacles, node_points, element_bodies, ground_nodes) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) user_input('Continue?') else: trajectories = [] plan = plan_sequence(robot, obstacles, node_points, element_bodies, ground_nodes, trajectories=trajectories, collisions=not args.cfree) if args.motions: plan = compute_motions(robot, obstacles, element_bodies, initial_conf, plan) disconnect() display_trajectories(ground_nodes, plan)
def gen_fn(node1, element): # fluents=[]): reverse = (node1 != element[0]) element_body = element_bodies[element] n1, n2 = reversed(element) if reverse else element delta = node_points[n2] - node_points[n1] # if delta[2] < 0: # continue length = np.linalg.norm(delta) # 5cm #supporters = {e for e in node_neighbors[n1] if element_supports(e, n1, node_points)} supporters = [] retrace_supporters(element, incoming_supporters, supporters) elements_order = [ e for e in element_bodies if (e != element) and (e not in supporters) ] bodies_order = [element_bodies[e] for e in elements_order] obstacles = fixed_obstacles + [element_bodies[e] for e in supporters] collision_fn = get_collision_fn( robot, movable_joints, obstacles, attachments=[], self_collisions=SELF_COLLISIONS, disabled_collisions=disabled_collisions, custom_limits={}) # TODO: get_custom_limits trajectories = [] for num in irange(max_trajectories): for attempt in range(max_attempts): path = sample_print_path(robot, length, reverse, element_body, collision_fn) if path is None: continue if check_collisions: collisions = check_trajectory_collision( robot, path, bodies_order) colliding = { e for k, e in enumerate(elements_order) if (element != e) and collisions[k] } else: colliding = set() if (node_neighbors[n1] <= colliding) and not any( n in ground_nodes for n in element): continue print_traj = PrintTrajectory(robot, movable_joints, path, element, reverse, colliding) trajectories.append(print_traj) # TODO: need to prune dominated trajectories if print_traj not in trajectories: continue print('{}) {}->{} ({}) | {} | {} | {}'.format( num, n1, n2, len(supporters), attempt, len(trajectories), sorted(len(t.colliding) for t in trajectories))) yield (print_traj, ) if not colliding: return else: print('{}) {}->{} ({}) | {} | Max attempts exceeded!'.format( num, len(supporters), n1, n2, max_attempts)) user_input('Continue?') return
def main(viewer=True): # TODO: setCollisionFilterGroupMask # TODO: only produce collisions rather than collision-free # TODO: return collisions using wild-stream functionality # TODO: handle movements between selected edges # TODO: fail if wild stream produces unexpected facts # TODO: try search at different cost levels (i.e. w/ and w/o abstract) # https://github.mit.edu/yijiangh/assembly-instances #read_minizinc_data(os.path.join(root_directory, 'print_data.txt')) #return # djmm_test_block | mars_bubble | sig_artopt-bunny | topopt-100 | topopt-205 | topopt-310 | voronoi elements, node_points, ground_nodes = load_extrusion('voronoi') node_order = list(range(len(node_points))) #np.random.shuffle(node_order) node_order = sorted(node_order, key=lambda n: node_points[n][2]) elements = sorted(elements, key=lambda e: min(node_points[n][2] for n in e)) #node_order = node_order[:100] ground_nodes = [n for n in ground_nodes if n in node_order] elements = [ element for element in elements if all(n in node_order for n in element) ] print('Nodes: {} | Ground: {} | Elements: {}'.format( len(node_points), len(ground_nodes), len(elements))) #plan = plan_sequence_test(node_points, elements, ground_nodes) connect(use_gui=viewer) floor, robot = load_world() obstacles = [floor] initial_conf = get_joint_positions(robot, get_movable_joints(robot)) #dump_body(robot) #if has_gui(): # draw_model(elements, node_points, ground_nodes) # wait_for_interrupt('Continue?') #joint_weights = compute_joint_weights(robot, num=1000) #elements = elements[:5] #elements = elements[:10] #elements = elements[:25] #elements = elements[:35] #elements = elements[:50] #elements = elements[:75] #elements = elements[:100] #elements = elements[:150] #elements = elements[150:] # TODO: prune if it collides with any of its supports # TODO: prioritize choices that don't collide with too many edges # TODO: accumulate edges along trajectory #test_grasps(robot, node_points, elements) #test_print(robot, node_points, elements) #return #for element in elements: # color = (0, 0, 1) if doubly_printable(element, node_points) else (1, 0, 0) # draw_element(node_points, element, color=color) #wait_for_interrupt('Continue?') # TODO: topological sort #node = node_order[40] #node_neighbors = get_node_neighbors(elements) #for element in node_neighbors[node]: # color = (0, 1, 0) if element_supports(element, node, node_points) else (1, 0, 0) # draw_element(node_points, element, color) #element = elements[-1] #draw_element(node_points, element, (0, 1, 0)) #incoming_edges, _ = neighbors_from_orders(get_supported_orders(elements, node_points)) #supporters = [] #retrace_supporters(element, incoming_edges, supporters) #for e in supporters: # draw_element(node_points, e, (1, 0, 0)) #wait_for_interrupt('Continue?') #for name, args in plan: # n1, e, n2 = args # draw_element(node_points, e) # user_input('Continue?') #test_ik(robot, node_order, node_points) element_bodies = dict(zip(elements, create_elements(node_points, elements))) precompute = False if precompute: pr = cProfile.Profile() pr.enable() trajectories = sample_trajectories(robot, obstacles, node_points, element_bodies, ground_nodes) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) user_input('Continue?') else: trajectories = [] motions = False plan = plan_sequence(robot, obstacles, node_points, element_bodies, ground_nodes, trajectories=trajectories) if motions: plan = compute_motions(robot, obstacles, element_bodies, initial_conf, plan) disconnect() display_trajectories(ground_nodes, plan)
def main(focused=True, deterministic=False, unit_costs=False): np.set_printoptions(precision=2) if deterministic: seed = 0 np.random.seed(seed) print('Seed:', get_random_seed()) problem_fn = get_blocked_problem # get_tight_problem | get_blocked_problem tamp_problem = problem_fn() print(tamp_problem) action_info = { #'move': ActionInfo(terminal=True), #'pick': ActionInfo(terminal=True), #'place': ActionInfo(terminal=True), } stream_info = { #'test-region': StreamInfo(eager=True, p_success=0), # bound_fn is None #'plan-motion': StreamInfo(p_success=1), # bound_fn is None #'trajcollision': StreamInfo(p_success=1), # bound_fn is None #'cfree': StreamInfo(eager=True), } dynamic = [ StreamSynthesizer('cfree-motion', { 'plan-motion': 1, 'trajcollision': 0 }, gen_fn=from_fn(cfree_motion_fn)), #StreamSynthesizer('optimize', {'sample-pose': 1, 'inverse-kinematics': 1, # 'posecollision': 0, 'distance': 0}, # gen_fn=from_fn(get_optimize_fn(tamp_problem.regions))), ] pddlstream_problem = pddlstream_from_tamp(tamp_problem) pr = cProfile.Profile() pr.enable() if focused: solution = solve_focused(pddlstream_problem, action_info=action_info, stream_info=stream_info, synthesizers=dynamic, max_time=10, max_cost=INF, debug=False, effort_weight=None, unit_costs=unit_costs, postprocess=False, visualize=False) else: solution = solve_incremental(pddlstream_problem, layers=1, unit_costs=unit_costs) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is None: return colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS)) viewer = ContinuousTMPViewer(tamp_problem.regions, title='Continuous TAMP') state = tamp_problem.initial print() print(state) draw_state(viewer, state, colors) for i, action in enumerate(plan): user_input('Continue?') print(i, *action) state = apply_action(state, action) print(state) draw_state(viewer, state, colors) user_input('Finish?')