def load_json_problem(problem_filename):
reset_simulation()
json_path = os.path.join(get_json_directory(), problem_filename)
with open(json_path, 'r') as f:
problem_json = json.loads(f.read())
set_camera_pose(point_from_pose(parse_pose(problem_json['camera'])))
task_json = problem_json['task']
important_bodies = []
for field in BODY_FIELDS:
important_bodies.extend(task_json[field])
robots = {
robot['name']: parse_robot(robot)
for robot in problem_json['robots']
}
bodies = {
body['name']: parse_body(body, (body['name'] in important_bodies))
for body in problem_json['bodies']
}
regions = {
region['name']: parse_region(region)
for region in task_json['regions']
}
#print(get_image())
return parse_task(task_json, robots, bodies, regions)
def display_failure(node_points, extruded_elements, element):
client = connect(use_gui=True)
with ClientSaver(client):
obstacles, robot = load_world()
handles = []
for e in extruded_elements:
handles.append(draw_element(node_points, e, color=(0, 1, 0)))
handles.append(draw_element(node_points, element, color=(1, 0, 0)))
print('Failure!')
wait_for_user()
reset_simulation()
disconnect()
def reset(self, keep_robot=False):
# Avoid using remove_body(body)
# https://github.com/bulletphysics/bullet3/issues/2086
self.controller.reset()
self.initial_beads = {}
with ClientSaver(self.client):
for name in list(self.perception.sim_bodies):
self.perception.remove(name)
for constraint in get_constraints():
remove_constraint(constraint)
remove_all_debug()
reset_simulation()
if keep_robot:
self._create_robot()
def destroy(self):
reset_simulation()
disconnect()
def plan_extrusion(args_list, viewer=False, verify=False, verbose=False, watch=False):
results = []
if not args_list:
return results
# TODO: setCollisionFilterGroupMask
if not verbose:
sys.stdout = open(os.devnull, 'w')
problems = {args.problem for args in args_list}
assert len(problems) == 1
[problem] = problems
# TODO: change dir for pddlstream
extrusion_path = get_extrusion_path(problem)
#extrusion_path = rotate_problem(extrusion_path)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path, verbose=True)
elements = sorted(element_from_id.values())
#node_points = transform_model(problem, elements, node_points, ground_nodes)
connect(use_gui=viewer, shadows=SHADOWS, color=BACKGROUND_COLOR)
with LockRenderer(lock=True):
#draw_pose(unit_pose(), length=1.)
obstacles, robot = load_world()
#draw_model(elements, node_points, ground_nodes)
#wait_for_user()
color = apply_alpha(BLACK, alpha=0) # 0, 1
#color = None
element_bodies = dict(zip(elements, create_elements_bodies(node_points, elements, color=color)))
set_extrusion_camera(node_points)
#if viewer:
# label_nodes(node_points)
saver = WorldSaver()
#visualize_stiffness(extrusion_path)
#debug_elements(robot, node_points, node_order, elements)
initial_position = point_from_pose(get_link_pose(robot, link_from_name(robot, TOOL_LINK)))
checker = None
plan = None
for args in args_list:
if args.stiffness and (checker is None):
checker = create_stiffness_checker(extrusion_path, verbose=False)
saver.restore()
#initial_conf = get_joint_positions(robot, get_movable_joints(robot))
with LockRenderer(lock=not viewer):
start_time = time.time()
plan, data = None, {}
with timeout(args.max_time):
with Profiler(num=10, cumulative=False):
plan, data = solve_extrusion(robot, obstacles, element_from_id, node_points, element_bodies,
extrusion_path, ground_nodes, args, checker=checker)
runtime = elapsed_time(start_time)
sequence = recover_directed_sequence(plan)
if verify:
max_translation, max_rotation = compute_plan_deformation(extrusion_path, recover_sequence(plan))
valid = check_plan(extrusion_path, sequence)
print('Valid:', valid)
safe = validate_trajectories(element_bodies, obstacles, plan)
print('Safe:', safe)
data.update({
'safe': safe,
'valid': valid,
'max_translation': max_translation,
'max_rotation': max_rotation,
})
data.update({
'runtime': runtime,
'num_elements': len(elements),
'ee_distance': compute_sequence_distance(node_points, sequence, start=initial_position, end=initial_position),
'print_distance': get_print_distance(plan, teleport=True),
'distance': get_print_distance(plan, teleport=False),
'sequence': sequence,
'parameters': get_global_parameters(),
'problem': args.problem,
'algorithm': args.algorithm,
'heuristic': args.bias,
'plan_extrusions': not args.disable,
'use_collisions': not args.cfree,
'use_stiffness': args.stiffness,
})
print(data)
#plan_path = '{}_solution.json'.format(args.problem)
#with open(plan_path, 'w') as f:
# json.dump(plan_data, f, indent=2, sort_keys=True)
results.append((args, data))
reset_simulation()
disconnect()
if watch and (plan is not None):
args = args_list[-1]
animate = not (args.disable or args.ee_only)
connect(use_gui=True, shadows=SHADOWS, color=BACKGROUND_COLOR) # TODO: avoid reconnecting
obstacles, robot = load_world()
display_trajectories(node_points, ground_nodes, plan, #time_step=None, video=True,
animate=animate)
reset_simulation()
disconnect()
if not verbose:
sys.stdout.close()
return results
def stop(self):
with ClientSaver(self.client):
reset_simulation()
disconnect()