def read_data(path):
# TODO: move to pybullet-tools
_, extension = os.path.splitext(path)
if extension == '.json':
return read_json(path)
if extension in ['.pp2', '.pp3', '.pkl', '.pk2', '.pk3']:
return read_pickle(path)
raise NotImplementedError(extension)
def affine_extrusion(extrusion_path, tform, local=True):
assert tform.shape == (4, 4)
data = read_json(extrusion_path)
new_data = {}
for key, value in data.items():
# TODO: separate into two methods
if key == 'base_frame_in_rob_base' and not local:
origin = parse_point(value['Origin'], scale=1)
#new_origin = tform_point(tform, origin)
new_origin = apply_affine(tform, origin)
rob_from_base = parse_transform(value, scale=1) # pose_from_tform
#new_pose = tform_from_pose(multiply(tform, rob_from_base))
new_pose = tform.dot(rob_from_base)
x, y, z = new_pose[:3, 3]
new_data[key] = {
# TODO: some of this is redundant
'Origin': json_from_point(new_origin),
"OriginX": x,
"OriginY": y,
"OriginZ": z,
"XAxis": json_from_point(new_pose[0, :3]),
"YAxis": json_from_point(new_pose[1, :3]),
"ZAxis": json_from_point(new_pose[2, :3]),
"IsValid": value['IsValid'],
}
elif key == 'node_list':
# TODO: be careful when transforming origin and node_points in origin
new_data[key] = []
for node_data in value:
new_node_data = {}
for node_key in node_data:
if node_key == 'point':
point = parse_point(node_data[node_key], scale=1)
#if not local:
# new_point = point
#new_point = tform_point(tform, point)
new_point = apply_affine(tform, point)
new_node_data[node_key] = json_from_point(new_point)
else:
new_node_data[node_key] = node_data[node_key]
new_data[key].append(new_node_data)
else:
new_data[key] = value
return new_data
def _initialize_environment(self):
# wall to fridge: 4cm
# fridge to goal: 1.5cm
# hitman to range: 3.5cm
# range to indigo: 3.5cm
self.environment_poses = read_json(POSES_PATH)
root_from_world = get_link_pose(self.kitchen, self.world_link)
for name, world_from_part in self.environment_poses.items():
if name in ['range']:
continue
visual_path = os.path.join(KITCHEN_LEFT_PATH,
'{}.obj'.format(name))
collision_path = os.path.join(KITCHEN_LEFT_PATH,
'{}_collision.obj'.format(name))
mesh_path = None
for path in [collision_path, visual_path]:
if os.path.exists(path):
mesh_path = path
break
if mesh_path is None:
continue
body = load_pybullet(mesh_path, fixed_base=True)
root_from_part = multiply(root_from_world, world_from_part)
if name in ['axe', 'dishwasher', 'echo', 'fox', 'golf']:
(pos, quat) = root_from_part
# TODO: still not totally aligned
pos = np.array(pos) + np.array([0, -0.035, 0]) # , -0.005])
root_from_part = (pos, quat)
self.environment_bodies[name] = body
set_pose(body, root_from_part)
# TODO: release bounding box or convex hull
# TODO: static object nonconvex collisions
if TABLE_NAME in self.environment_bodies:
body = self.environment_bodies[TABLE_NAME]
_, (w, l, _) = approximate_as_prism(body)
_, _, z = get_point(body)
new_pose = Pose(Point(TABLE_X + l / 2, -TABLE_Y, z),
Euler(yaw=np.pi / 2))
set_pose(body, new_pose)
def main():
parser = argparse.ArgumentParser()
parser.add_argument('experiments',
nargs='+',
help='Name of the experiment')
args = parser.parse_args()
outcomes_per_task = {}
for path in args.experiments:
for result in read_json(path):
experiment = result['experiment']
problem = experiment['problem']
outcome = result['outcome']
#policy = frozenset(experiment['policy'].items())
policy = name_from_policy(experiment['policy'])
outcomes_per_task.setdefault(problem['task'],
{}).setdefault(policy,
[]).append(outcome)
#outcomes_per_task['inspect_drawer']['constrain=0_defer=1'].append(ERROR_OUTCOME)
#outcomes_per_task['detect_block']['constrain=1_defer=0'].append(ERROR_OUTCOME)
# TODO: robust poses
# TODO: intelligent IR for pour
table = ''
for task in TASK_NAMES:
if task not in outcomes_per_task:
continue
print('\nTask: {}'.format(task))
items = [task]
for policy in POLICIES:
policy = name_from_policy(policy)
if policy not in outcomes_per_task[task]:
continue
outcomes = list(take(outcomes_per_task[task][policy], MAX_TRIALS))
value_per_attribute = {}
for outcome in outcomes:
if outcome['error']:
outcome.update(ERROR_OUTCOME)
if MAX_TIME < outcome.get('total_time', INF):
outcome['achieved_goal'] = False
if not outcome['achieved_goal']:
outcome['total_time'] = MAX_TIME
outcome['plan_time'] = MAX_TIME
for attribute, value in outcome.items():
if (attribute not in ['policy']) and (attribute in PRINT_ATTRIBUTES) and \
not isinstance(value, str) and implies(attribute in ACHIEVED_GOAL,
outcome['achieved_goal']):
value_per_attribute.setdefault(attribute,
[]).append(float(value))
statistics = {
attribute: np.round(np.average(values), 3) # '{:.2f}'.format(
for attribute, values in value_per_attribute.items()
} # median, min, max of solved?
statistics['trials'] = len(outcomes)
print('{}: {}'.format(policy, str_from_object(statistics)))
items += [
'{:.0f}'.format(100 * statistics['achieved_goal']),
'{:.0f}'.format(statistics['plan_time']),
]
table += '{}\n\\\\ \hline\n'.format(' & '.join(items))
print(SEPARATOR)
print(POLICIES)
print(table)
import os
import glob
from pybullet_tools.utils import read_json, write_json
from pddlstream.utils import mkdir
if __name__ == '__main__':
SKILL = 'scoop'
MATERIAL = {'pour': 'red_men', 'scoop': 'chickpeas'}
all_dirs = glob.glob('data/pr2_{}_*'.format(SKILL))
all_trials = []
for dir in all_dirs:
trial_file = os.path.join(dir, 'trials.json')
data = read_json(trial_file)
for d in data:
if d['policy'] == 'training' and d['material'] == MATERIAL[SKILL] and d['score'] is not None:
all_trials.append(d)
newdir = 'data/pr2_{}/'.format(SKILL)
mkdir(newdir)
write_json(os.path.join(newdir, 'all_trials.json'), all_trials)
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)
if False:
extrusion_path = transform_json(problem)
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.)
json_data = read_json(extrusion_path)
draw_pose(parse_origin_pose(json_data))
draw_model(elements, node_points, ground_nodes)
obstacles, robot = load_world()
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()
wait_if_gui()
#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)
results.append((args, data))
if True:
data.update({
'assembly': read_json(extrusion_path),
'plan': extract_plan_data(plan), # plan | trajectories
})
plan_file = '{}_solution.json'.format(args.problem)
plan_path = os.path.join('solutions', plan_file)
write_json(plan_path, 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