def display_plan(problem, state, plan, time_step=0.01, sec_per_step=0.005): 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)) colors = {robot: COLOR_FROM_NAME[robot] for robot in problem.robots} for i, t in enumerate(inclusive_range(0, duration, time_step)): print('Step={} | t={}'.format(i, t)) for action in plan: name, args, start, duration = action if not (action.start <= t <= get_end(action)): continue if name == 'move': robot, conf1, traj, conf2 = args traj = [conf1.values, conf2.values] fraction = (t - action.start) / action.duration conf = get_value_at_time(traj, fraction) body = problem.get_body(robot) color = COLOR_FROM_NAME[robot] if colors[robot] != color: set_color(body, color, link_from_name(body, TOP_LINK)) colors[robot] = color set_base_conf(body, conf) elif name == 'recharge': robot, conf = args body = problem.get_body(robot) color = YELLOW if colors[robot] != color: set_color(body, color, link_from_name(body, TOP_LINK)) colors[robot] = color else: raise ValueError(name) if sec_per_step is None: wait_if_gui('Continue?') else: wait_for_duration(sec_per_step)
def simulate_parallel(robots, plan, time_step=0.1, speed_up=10., record=None): # None | video.mp4 # TODO: ensure the step size is appropriate makespan = compute_duration(plan) print('\nMakespan: {:.3f}'.format(makespan)) trajectories = extract_parallel_trajectories(plan) if trajectories is None: return wait_if_gui('Begin?') num_motion = sum(action.name == 'move' for action in plan) with VideoSaver(record): for t in inclusive_range(0, makespan, time_step): # if action.start <= t <= get_end(action): executing = Counter(traj.robot for traj in trajectories if traj.at(t) is not None) print('t={:.3f}/{:.3f} | executing={}'.format( t, makespan, executing)) for robot in robots: num = executing.get(robot, 0) if 2 <= num: raise RuntimeError( 'Robot {} simultaneously executing {} trajectories'. format(robot, num)) if (num_motion == 0) and (num == 0): set_configuration(robot, DUAL_CONF) #step_simulation() wait_for_duration(time_step / speed_up) wait_if_gui('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 simulate_parallel(robots, plan, time_step=0.1, speed_up=10., record=None): # None | video.mp4 # TODO: ensure the step size is appropriate makespan = compute_duration(plan) print('\nMakespan: {:.3f}'.format(makespan)) if plan is None: return trajectories = [] for action in plan: command = action.args[-1] if (action.name == 'move') and (command.start_conf is action.args[-2].positions): command = command.reverse() command.retime(start_time=action.start) #print(action) #print(action.start, get_end(action), action.duration) #print(command.start_time, command.end_time, command.duration) #for traj in command.trajectories: # print(traj, traj.start_time, traj.end_time, traj.duration) trajectories.extend(command.trajectories) #print(sum(traj.duration for traj in trajectories)) num_motion = sum(action.name == 'move' for action in plan) wait_if_gui('Begin?') with VideoSaver(record): for t in inclusive_range(0, makespan, time_step): # if action.start <= t <= get_end(action): executing = Counter(traj.robot for traj in trajectories if traj.at(t) is not None) print('t={:.3f}/{:.3f} | executing={}'.format( t, makespan, executing)) for robot in robots: num = executing.get(robot, 0) if 2 <= num: raise RuntimeError( 'Robot {} simultaneously executing {} trajectories'. format(robot, num)) if (num_motion == 0) and (num == 0): set_configuration(robot, DUAL_CONF) #step_simulation() wait_for_duration(time_step / speed_up) wait_if_gui('Finish?')
def stripstream(robot1, obstacles, node_points, element_bodies, ground_nodes, dual=True, serialize=False, hierarchy=False, **kwargs): robots = mirror_robot(robot1, node_points) if dual else [robot1] elements = set(element_bodies) initial_confs = { ROBOT_TEMPLATE.format(i): Conf(robot) for i, robot in enumerate(robots) } saver = WorldSaver() layer_from_n = compute_layer_from_vertex(elements, node_points, ground_nodes) #layer_from_n = cluster_vertices(elements, node_points, ground_nodes) # TODO: increase resolution for small structures # TODO: compute directions from first, layer from second max_layer = max(layer_from_n.values()) print('Max layer: {}'.format(max_layer)) data = {} if serialize: plan, certificate = solve_serialized(robots, obstacles, node_points, element_bodies, ground_nodes, layer_from_n, initial_confs=initial_confs, **kwargs) else: plan, certificate = solve_joint(robots, obstacles, node_points, element_bodies, ground_nodes, layer_from_n, initial_confs=initial_confs, **kwargs) if plan is None: return None, data if hierarchy: print(SEPARATOR) static_facts = extract_static_facts(plan, certificate, initial_confs) partial_orders = compute_total_orders(plan) plan, certificate = solve_joint(robots, obstacles, node_points, element_bodies, ground_nodes, layer_from_n, initial_confs=initial_confs, can_print=False, can_transit=True, additional_init=static_facts, additional_orders=partial_orders, **kwargs) if plan is None: return None, data if plan and not isinstance(plan[0], DurativeAction): time_from_start = 0. retimed_plan = [] for name, args in plan: command = args[-1] command.retime(start_time=time_from_start) retimed_plan.append( DurativeAction(name, args, time_from_start, command.duration)) time_from_start += command.duration plan = retimed_plan plan = reverse_plan(plan) print('\nLength: {} | Makespan: {:.3f}'.format(len(plan), compute_duration(plan))) # TODO: retime using the TFD duration # TODO: attempt to resolve once without any optimistic facts to see if a solution exists # TODO: choose a better initial config # TODO: decompose into layers hierarchically #planned_elements = [args[2] for name, args, _, _ in sorted(plan, key=lambda a: get_end(a))] # TODO: remove approach #if not check_plan(extrusion_path, planned_elements): # return None, data if has_gui(): saver.restore() #label_nodes(node_points) # commands = [action.args[-1] for action in reversed(plan) if action.name == 'print'] # trajectories = flatten_commands(commands) # elements = recover_sequence(trajectories) # draw_ordered(elements, node_points) # wait_if_gui('Continue?') #simulate_printing(node_points, trajectories) #display_trajectories(node_points, ground_nodes, trajectories) simulate_parallel(robots, plan) return None, data
def solve_serialized(robots, obstacles, node_points, element_bodies, ground_nodes, layer_from_n, trajectories=[], post_process=False, collisions=True, disable=False, max_time=INF, **kwargs): start_time = time.time() saver = WorldSaver() elements = set(element_bodies) elements_from_layers = compute_elements_from_layer(elements, layer_from_n) layers = sorted(elements_from_layers.keys()) print('Layers:', layers) full_plan = [] makespan = 0. removed = set() for layer in reversed(layers): print(SEPARATOR) print('Layer: {}'.format(layer)) saver.restore() remaining = elements_from_layers[layer] printed = elements - remaining - removed draw_model(remaining, node_points, ground_nodes, color=GREEN) draw_model(printed, node_points, ground_nodes, color=RED) problem = get_pddlstream(robots, obstacles, node_points, element_bodies, ground_nodes, layer_from_n, printed=printed, removed=removed, return_home=False, trajectories=trajectories, collisions=collisions, disable=disable, **kwargs) layer_plan, certificate = solve_pddlstream(problem, node_points, element_bodies, max_time=max_time - elapsed_time(start_time)) remove_all_debug() if layer_plan is None: return None if post_process: print(SEPARATOR) # Allows the planner to continue to check collisions problem.init[:] = certificate.all_facts #static_facts = extract_static_facts(layer_plan, ...) #problem.init.extend(('Order',) + pair for pair in compute_total_orders(layer_plan)) for fact in [('print', ), ('move', )]: if fact in problem.init: problem.init.remove(fact) new_layer_plan, _ = solve_pddlstream(problem, node_points, element_bodies, planner=POSTPROCESS_PLANNER, max_time=max_time - elapsed_time(start_time)) if (new_layer_plan is not None) and (compute_duration(new_layer_plan) < compute_duration(layer_plan)): layer_plan = new_layer_plan user_input('{:.3f}->{:.3f}'.format( compute_duration(layer_plan), compute_duration(new_layer_plan))) # TODO: replan in a cost sensitive way layer_plan = apply_start(layer_plan, makespan) duration = compute_duration(layer_plan) makespan += duration print( '\nLength: {} | Start: {:.3f} | End: {:.3f} | Duration: {:.3f} | Makespan: {:.3f}' .format(len(layer_plan), compute_start(layer_plan), compute_end(layer_plan), duration, makespan)) full_plan.extend(layer_plan) removed.update(remaining) print(SEPARATOR) print_plan(full_plan) return full_plan, None