def write_results(filename, results):
if not results or (filename is None):
return False
# path = '{}.pk{}'.format(filename, get_python_version())
# write_pickle(path, results)
path = '{}.json'.format(filename)
try:
write_json(path, results)
except KeyboardInterrupt:
write_json(path, results)
finally:
print('Saved {} trials to {}'.format(len(results), path))
return True
def transform_json(problem):
original_path = get_extrusion_path(problem)
#new_path = '{}_transformed.json'.format(problem)
new_path = 'transformed.json'.format(problem) # TODO: create folder
#pose = Pose(euler=Euler(roll=np.pi / 2.))
yaw = np.pi / 4.
pose = Pose(euler=Euler(yaw=yaw))
tform = tform_from_pose(pose)
tform = rotation_matrix(angle=yaw, direction=[0, 0, 1])
scale = SCALE_ASSEMBLY.get(problem, DEFAULT_SCALE)
#tform = scale*np.identity(4)
tform = scale_matrix(scale, origin=None, direction=None)
json_data = affine_extrusion(original_path, tform)
write_json(new_path, json_data)
# TODO: rotate the whole robot as well
# TODO: could also use the z heuristic when upside down
return new_path
def collect_place(world, object_name, surface_name, grasp_type, args):
date = get_date()
#set_seed(args.seed)
#dump_body(world.robot)
surface_pose = get_surface_reference_pose(world.kitchen, surface_name) # TODO: assumes the drawer is open
stable_gen_fn = get_stable_gen(world, z_offset=Z_EPSILON, visibility=False,
learned=False, collisions=not args.cfree)
grasp_gen_fn = get_grasp_gen(world)
ik_ir_gen = get_pick_gen_fn(world, learned=False, collisions=not args.cfree, teleport=args.teleport)
stable_gen = stable_gen_fn(object_name, surface_name)
grasps = list(grasp_gen_fn(object_name, grasp_type))
robot_name = get_body_name(world.robot)
path = get_place_path(robot_name, surface_name, grasp_type)
print(SEPARATOR)
print('Robot name: {} | Object name: {} | Surface name: {} | Grasp type: {} | Filename: {}'.format(
robot_name, object_name, surface_name, grasp_type, path))
entries = []
start_time = time.time()
failures = 0
while (len(entries) < args.num_samples) and \
(elapsed_time(start_time) < args.max_time): #and (failures <= max_failures):
(rel_pose,) = next(stable_gen)
if rel_pose is None:
break
(grasp,) = random.choice(grasps)
with LockRenderer(lock=True):
result = next(ik_ir_gen(object_name, rel_pose, grasp), None)
if result is None:
print('Failure! | {} / {} [{:.3f}]'.format(
len(entries), args.num_samples, elapsed_time(start_time)))
failures += 1
continue
# TODO: ensure an arm motion exists
bq, aq, at = result
rel_pose.assign()
bq.assign()
aq.assign()
base_pose = get_link_pose(world.robot, world.base_link)
object_pose = rel_pose.get_world_from_body()
tool_pose = multiply(object_pose, invert(grasp.grasp_pose))
entries.append({
'tool_from_base': multiply(invert(tool_pose), base_pose),
'surface_from_object': multiply(invert(surface_pose), object_pose),
'base_from_object': multiply(invert(base_pose), object_pose),
})
print('Success! | {} / {} [{:.3f}]'.format(
len(entries), args.num_samples, elapsed_time(start_time)))
if has_gui():
wait_for_user()
#visualize_database(tool_from_base_list)
if not entries:
safe_remove(path)
return None
# Assuming the kitchen is fixed but the objects might be open world
data = {
'date': date,
'robot_name': robot_name, # get_name | get_body_name | get_base_name | world.robot_name
'base_link': get_link_name(world.robot, world.base_link),
'tool_link': get_link_name(world.robot, world.tool_link),
'kitchen_name': get_body_name(world.kitchen),
'surface_name': surface_name,
'object_name': object_name,
'grasp_type': grasp_type,
'entries': entries,
'failures': failures,
'successes': len(entries),
}
write_json(path, data)
print('Saved', path)
return data
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 main():
parser = create_parser()
args = parser.parse_args()
print(args)
# https://stackoverflow.com/questions/15314189/python-multiprocessing-pool-hangs-at-join
# https://stackoverflow.com/questions/39884898/large-amount-of-multiprocessing-process-causing-deadlock
# TODO: alternatively don't destroy the world
num_cores = max(1, cpu_count() - SPARE_CORES)
json_path = os.path.abspath(
os.path.join(EXPERIMENTS_DIRECTORY, '{}.json'.format(get_date())))
#memory_per_core = float(MAX_RAM) / num_cores # gigabytes
#set_soft_limit(resource.RLIMIT_AS, int(BYTES_PER_GIGABYTE * memory_per_core)) # bytes
#set_soft_limit(resource.RLIMIT_CPU, 2*MAX_TIME) # seconds
# RLIMIT_MEMLOCK, RLIMIT_STACK, RLIMIT_DATA
print('Results:', json_path)
print('Num Cores:', num_cores)
#print('Memory per Core: {:.2f}'.format(memory_per_core))
print('Tasks: {} | {}'.format(len(TASK_NAMES), TASK_NAMES))
print('Policies: {} | {}'.format(len(POLICIES), POLICIES))
print('Num Trials:', N_TRIALS)
num_experiments = len(TASK_NAMES) * len(POLICIES) * N_TRIALS
print('Num Experiments:', num_experiments)
max_parallel = math.ceil(float(num_experiments) / num_cores)
print('Estimated duration: {:.2f} hours'.format(
MEAN_TIME_PER_TRIAL * max_parallel / HOURS_TO_SECS))
user_input('Begin?')
print(SEPARATOR)
print('Creating problems')
start_time = time.time()
problems = create_problems(args)
experiments = [
{
'problem': copy.deepcopy(problem),
'policy': policy
} #, 'args': args}
for problem in problems for policy in POLICIES
]
print('Created {} problems and {} experiments in {:.3f} seconds'.format(
len(problems), len(experiments), elapsed_time(start_time)))
print(SEPARATOR)
ensure_dir(EXPERIMENTS_DIRECTORY)
safe_rm_dir(TEMP_DIRECTORY)
ensure_dir(TEMP_DIRECTORY)
start_time = time.time()
results = []
try:
for result in map_parallel(run_experiment,
experiments,
num_cores=num_cores):
results.append(result)
print('{}\nExperiments: {} / {} | Time: {:.3f}'.format(
SEPARATOR, len(results), len(experiments),
elapsed_time(start_time)))
print('Experiment:', str_from_object(result['experiment']))
print('Outcome:', str_from_object(result['outcome']))
write_json(json_path, results)
#except BaseException as e:
# traceback.print_exc() # e
finally:
if results:
write_json(json_path, results)
print(SEPARATOR)
print('Saved:', json_path)
print('Results:', len(results))
print('Duration / experiment: {:.3f}'.format(
num_cores * elapsed_time(start_time) / len(experiments)))
print('Duration: {:.2f} hours'.format(
elapsed_time(start_time) / HOURS_TO_SECS))
safe_rm_dir(TEMP_DIRECTORY)
# TODO: dump results automatically?
return results
def collect_pull(world, joint_name, args):
date = get_date()
#set_seed(args.seed)
robot_name = get_body_name(world.robot)
if is_press(joint_name):
press_gen = get_press_gen_fn(world,
collisions=not args.cfree,
teleport=args.teleport,
learned=False)
else:
joint = joint_from_name(world.kitchen, joint_name)
open_conf = Conf(world.kitchen, [joint], [world.open_conf(joint)])
closed_conf = Conf(world.kitchen, [joint], [world.closed_conf(joint)])
pull_gen = get_pull_gen_fn(world,
collisions=not args.cfree,
teleport=args.teleport,
learned=False)
#handle_link, handle_grasp, _ = get_handle_grasp(world, joint)
path = get_pull_path(robot_name, joint_name)
print(SEPARATOR)
print('Robot name {} | Joint name: {} | Filename: {}'.format(
robot_name, joint_name, path))
entries = []
failures = 0
start_time = time.time()
while (len(entries) < args.num_samples) and \
(elapsed_time(start_time) < args.max_time):
if is_press(joint_name):
result = next(press_gen(joint_name), None)
else:
result = next(pull_gen(joint_name, open_conf, closed_conf),
None) # Open to closed
if result is None:
print('Failure! | {} / {} [{:.3f}]'.format(
len(entries), args.num_samples, elapsed_time(start_time)))
failures += 1
continue
if not is_press(joint_name):
open_conf.assign()
joint_pose = get_joint_reference_pose(world.kitchen, joint_name)
bq, aq1 = result[:2]
bq.assign()
aq1.assign()
#next(at.commands[2].iterate(None, None))
base_pose = get_link_pose(world.robot, world.base_link)
#handle_pose = get_link_pose(world.robot, base_link)
entries.append({
'joint_from_base':
multiply(invert(joint_pose), base_pose),
})
print('Success! | {} / {} [{:.3f}]'.format(len(entries),
args.num_samples,
elapsed_time(start_time)))
if has_gui():
wait_for_user()
if not entries:
safe_remove(path)
return None
#visualize_database(joint_from_base_list)
# Assuming the kitchen is fixed but the objects might be open world
# TODO: could store per data point
data = {
'date': date,
'robot_name':
robot_name, # get_name | get_body_name | get_base_name | world.robot_name
'base_link': get_link_name(world.robot, world.base_link),
'tool_link': get_link_name(world.robot, world.tool_link),
'kitchen_name': get_body_name(world.kitchen),
'joint_name': joint_name,
'entries': entries,
'failures': failures,
'successes': len(entries),
}
if not is_press(joint_name):
data.update({
'open_conf': open_conf.values,
'closed_conf': closed_conf.values,
})
write_json(path, data)
print('Saved', path)
return data
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