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?')
