def save_inverse_reachability(robot, arm, grasp_type, tool_link,
gripper_from_base_list):
# TODO: store value of torso and roll joint for the IK database. Sample the roll joint.
# TODO: hash the pr2 urdf as well
filename = IR_FILENAME.format(grasp_type, arm)
path = get_database_file(filename)
data = {
'filename': filename,
'robot': get_body_name(robot),
'grasp_type': grasp_type,
'arm': arm,
'torso': get_group_conf(robot, 'torso'),
'carry_conf': get_carry_conf(arm, grasp_type),
'tool_link': tool_link,
'ikfast': is_ik_compiled(),
'gripper_from_base': gripper_from_base_list,
}
write_pickle(path, data)
if has_gui():
handles = []
for gripper_from_base in gripper_from_base_list:
handles.extend(
draw_point(point_from_pose(gripper_from_base),
color=(1, 0, 0)))
wait_for_user()
return path
def are_visible(world):
ray_names = []
rays = []
for name in world.movable:
for camera, info in world.cameras.items():
camera_pose = get_pose(info.body)
camera_point = point_from_pose(camera_pose)
point = point_from_pose(get_pose(world.get_body(name)))
if is_visible_point(CAMERA_MATRIX,
KINECT_DEPTH,
point,
camera_pose=camera_pose):
ray_names.append(name)
rays.append(Ray(camera_point, point))
ray_results = batch_ray_collision(rays)
visible_indices = [
idx for idx, (name, result) in enumerate(zip(ray_names, ray_results))
if result.objectUniqueId == world.get_body(name)
]
visible_names = {ray_names[idx] for idx in visible_indices}
print('Detected:', sorted(visible_names))
if has_gui():
handles = [
add_line(rays[idx].start, rays[idx].end, color=BLUE)
for idx in visible_indices
]
wait_for_duration(1.0)
remove_handles(handles)
# TODO: the object drop seems to happen around here
return visible_names
def draw_action(node_points, printed, element):
if not has_gui():
return []
with LockRenderer():
remove_all_debug()
handles = [draw_element(node_points, element, color=(1, 0, 0))]
handles.extend(
draw_element(node_points, e, color=(0, 1, 0)) for e in printed)
wait_for_user()
return handles
def simulate_trial(sim_world,
task,
parameter_fns,
teleport=True,
collisions=True,
max_time=20,
verbose=False,
**kwargs):
with ClientSaver(sim_world.client):
viewer = has_gui()
planning_world = PlanningWorld(task, visualize=False)
planning_world.load(sim_world)
print(planning_world)
start_time = time.time()
plan = plan_actions(planning_world,
max_time=max_time,
verbose=verbose,
collisions=collisions,
teleport=teleport,
parameter_fns=parameter_fns) # **kwargs
# plan = None
plan_time = time.time() - start_time
planning_world.stop()
if plan is None:
print('Failed to find a plan')
# TODO: allow option of scoring these as well?
return None, plan_time
if teleport:
# TODO: skipping for scooping sometimes places the spoon in the bowl
plan = skip_to_end(sim_world, planning_world, plan)
if viewer:
# wait_for_user('Execute?')
wait_for_user()
sim_world.controller.set_gravity()
videos_directory = os.path.abspath(
os.path.join(__file__, os.pardir, os.pardir, VIDEOS_DIRECTORY))
#skill_videos = [filename.startswith(task.skill) for filename in os.listdir(videos_directory)]
#suffix = len(skill_videos)
suffix = datetime.datetime.now().strftime(DATE_FORMAT)
video_path = os.path.join(videos_directory,
'{}_{}.mp4'.format(task.skill, suffix))
video_path = None
with VideoSaver(video_path): # requires ffmpeg
# TODO: teleport=False
execute_plan(sim_world, plan, default_sleep=0.)
if viewer:
wait_for_user('Finished!')
return plan, plan_time
def main():
parser = argparse.ArgumentParser()
# choreo_brick_demo | choreo_eth-trees_demo
parser.add_argument('-p',
'--problem',
default='choreo_brick_demo',
help='The name of the problem to solve')
parser.add_argument('-c',
'--cfree',
action='store_true',
help='Disables collisions with obstacles')
parser.add_argument('-t',
'--teleport',
action='store_true',
help='Teleports instead of computing motions')
parser.add_argument('-v',
'--viewer',
action='store_true',
help='Enables the viewer during planning (slow!)')
args = parser.parse_args()
print('Arguments:', args)
connect(use_gui=args.viewer)
robot, brick_from_index, obstacle_from_name = load_pick_and_place(
args.problem)
np.set_printoptions(precision=2)
pr = cProfile.Profile()
pr.enable()
with WorldSaver():
pddlstream_problem = get_pddlstream(robot,
brick_from_index,
obstacle_from_name,
collisions=not args.cfree,
teleport=args.teleport)
with LockRenderer():
solution = solve_focused(pddlstream_problem,
planner='ff-wastar1',
max_time=30)
pr.disable()
pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
print_solution(solution)
plan, _, _ = solution
if plan is None:
return
if not has_gui():
connect(use_gui=True)
_ = load_pick_and_place(args.problem)
step_plan(plan, time_step=(np.inf if args.teleport else 0.01))
def create_inverse_reachability(robot,
body,
table,
arm,
grasp_type,
max_attempts=500,
num_samples=500):
tool_link = get_gripper_link(robot, arm)
robot_saver = BodySaver(robot)
gripper_from_base_list = []
grasps = GET_GRASPS[grasp_type](body)
start_time = time.time()
while len(gripper_from_base_list) < num_samples:
box_pose = sample_placement(body, table)
set_pose(body, box_pose)
grasp_pose = random.choice(grasps)
gripper_pose = multiply(box_pose, invert(grasp_pose))
for attempt in range(max_attempts):
robot_saver.restore()
base_conf = next(uniform_pose_generator(
robot, gripper_pose)) #, reachable_range=(0., 1.)))
#set_base_values(robot, base_conf)
set_group_conf(robot, 'base', base_conf)
if pairwise_collision(robot, table):
continue
grasp_conf = pr2_inverse_kinematics(
robot, arm, gripper_pose) #, nearby_conf=USE_CURRENT)
#conf = inverse_kinematics(robot, link, gripper_pose)
if (grasp_conf is None) or pairwise_collision(robot, table):
continue
gripper_from_base = multiply(
invert(get_link_pose(robot, tool_link)), get_base_pose(robot))
#wait_for_user()
gripper_from_base_list.append(gripper_from_base)
print('{} / {} | {} attempts | [{:.3f}]'.format(
len(gripper_from_base_list), num_samples, attempt,
elapsed_time(start_time)))
if has_gui():
wait_for_user()
break
else:
print(
'Failed to find a kinematic solution after {} attempts'.format(
max_attempts))
return save_inverse_reachability(robot, arm, grasp_type, tool_link,
gripper_from_base_list)
def create_inverse_reachability2(robot,
body,
table,
arm,
grasp_type,
max_attempts=500,
num_samples=500):
tool_link = get_gripper_link(robot, arm)
problem = MockProblem(robot, fixed=[table], grasp_types=[grasp_type])
placement_gen_fn = get_stable_gen(problem)
grasp_gen_fn = get_grasp_gen(problem, collisions=True)
ik_ir_fn = get_ik_ir_gen(problem,
max_attempts=max_attempts,
learned=False,
teleport=True)
placement_gen = placement_gen_fn(body, table)
grasps = list(grasp_gen_fn(body))
print('Grasps:', len(grasps))
# TODO: sample the torso height
# TODO: consider IK with respect to the torso frame
start_time = time.time()
gripper_from_base_list = []
while len(gripper_from_base_list) < num_samples:
[(p, )] = next(placement_gen)
(g, ) = random.choice(grasps)
output = next(ik_ir_fn(arm, body, p, g), None)
if output is None:
print('Failed to find a solution after {} attempts'.format(
max_attempts))
else:
(_, ac) = output
[
at,
] = ac.commands
at.path[-1].assign()
gripper_from_base = multiply(
invert(get_link_pose(robot, tool_link)), get_base_pose(robot))
gripper_from_base_list.append(gripper_from_base)
print('{} / {} [{:.3f}]'.format(len(gripper_from_base_list),
num_samples,
elapsed_time(start_time)))
if has_gui():
wait_for_user()
return save_inverse_reachability(robot, arm, grasp_type, tool_link,
gripper_from_base_list)
def validate_trajectories(element_bodies, fixed_obstacles, trajectories):
if trajectories is None:
return False
# TODO: combine all validation procedures
remove_all_debug()
for body in element_bodies.values():
set_color(body, np.zeros(4))
print('Trajectories:', len(trajectories))
obstacles = list(fixed_obstacles)
for i, trajectory in enumerate(trajectories):
for _ in trajectory.iterate():
#wait_for_user()
if any(pairwise_collision(trajectory.robot, body) for body in obstacles):
if has_gui():
print('Collision on trajectory {}'.format(i))
wait_for_user()
return False
if isinstance(trajectory, PrintTrajectory):
body = element_bodies[trajectory.element]
set_color(body, apply_alpha(RED))
obstacles.append(body)
return True
def check_plan(extrusion_path, planned_elements, verbose=False):
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
#checker = create_stiffness_checker(extrusion_name)
connected_nodes = set(ground_nodes)
handles = []
all_connected = True
all_stiff = True
extruded_elements = set()
for element in planned_elements:
extruded_elements.add(element)
n1, n2 = element
#is_connected &= any(n in connected_nodes for n in element)
is_connected = (n1 in connected_nodes)
connected_nodes.update(element)
#is_connected = check_connected(ground_nodes, extruded_elements)
#all_connected &= is_connected
is_stiff = test_stiffness(extrusion_path,
element_from_id,
extruded_elements,
verbose=verbose)
all_stiff &= is_stiff
if verbose:
structures = get_connected_structures(extruded_elements)
print('Elements: {} | Structures: {} | Connected: {} | Stiff: {}'.
format(len(extruded_elements), len(structures), is_connected,
is_stiff))
if has_gui():
is_stable = is_connected and is_stiff
color = BLACK if is_stable else RED
handles.append(draw_element(node_points, element, color))
wait_for_duration(0.1)
if not is_stable:
wait_for_user()
# Make these counts instead
print('Connected: {} | Stiff: {}'.format(all_connected, all_stiff))
return all_connected and all_stiff
def main():
# TODO: link_from_name is slow (see python -m plan_tools.run_simulation -p test_cook)
problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS}
parser = argparse.ArgumentParser()
parser.add_argument('-c', '--cfree', action='store_true',
help='When enabled, disables collision checking (for debugging).')
parser.add_argument('-e', '--execute', action='store_true',
help='When enabled, executes the plan using physics simulation.')
#parser.add_argument('-l', '--learning', action='store_true',
# help='When enabled, uses learned generators when applicable.')
parser.add_argument('-p', '--problem', default=test_pour.__name__,
choices=sorted(problem_fn_from_name),
help='The name of the problem to solve.')
parser.add_argument('-s', '--seed', default=None,
help='The random seed to use.')
parser.add_argument('-v', '--visualize_planning', action='store_true',
help='When enabled, visualizes planning rather than the world (for debugging).')
parser.add_argument('-d', '--disable_drawing', action='store_true',
help='When enabled, disables drawing names and forward reachability.')
args = parser.parse_args()
if args.seed is not None:
set_seed(args.seed)
print('Problem:', args.problem)
problem_fn = problem_fn_from_name[args.problem]
sim_world, task = problem_fn(visualize=not args.visualize_planning)
#sim_world, task = problem_fn(visualize=False)
if not args.disable_drawing:
draw_names(sim_world)
draw_forward_reachability(sim_world, task.arms)
planning_world = PlanningWorld(task, visualize=args.visualize_planning)
planning_world.load(sim_world)
print(planning_world)
plan = plan_actions(planning_world, collisions=not args.cfree)
planning_world.stop()
if (plan is None) or not has_gui(sim_world.client):
#print('Failed to find a plan')
wait_for_user('Finished!')
sim_world.stop()
return
# TODO: see-through cup underneath the table
wait_for_user('Execute?')
video_path = os.path.join(VIDEOS_DIRECTORY, '{}_{}.mp4'.format(
args.problem, datetime.datetime.now().strftime(DATE_FORMAT)))
video_path = None
with VideoSaver(video_path): # requires ffmpeg
if args.execute:
# Only used by pb_controller, for ros_controller it's assumed always True
sim_world.controller.execute_motor_control = True
sim_world.controller.set_gravity()
execute_plan(sim_world, plan, default_sleep=0.)
else:
step_plan(sim_world, plan, task.get_attachments(sim_world),
#time_step=None)
time_step=0.04)
#step_plan(world, plan, end_only=True, time_step=None)
if args.execute:
wait_for_user('Finished!')
sim_world.stop()
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
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 visualize_stiffness(extrusion_path):
if not has_gui():
return
#label_elements(element_bodies)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
elements = list(element_from_id.values())
#draw_model(elements, node_points, ground_nodes)
# Freeform Assembly Planning
# TODO: https://arxiv.org/pdf/1801.00527.pdf
# Though assembly sequencing is often done by finding a disassembly sequence and reversing it, we will use a forward search.
# Thus a low-cost state will usually be correctly identified by considering only the deflection of the cantilevered beam path
# and approximating the rest of the beams as being infinitely stiff
reaction_from_node = compute_node_reactions(extrusion_path, elements)
#reaction_from_node = deformation.displacements # For visualizing displacements
#test_node_forces(node_points, reaction_from_node)
force_from_node = {
node: sum(
np.linalg.norm(force_from_reaction(reaction))
for reaction in reactions)
for node, reactions in reaction_from_node.items()
}
sorted_nodes = sorted(reaction_from_node,
key=lambda n: force_from_node[n],
reverse=True)
for i, node in enumerate(sorted_nodes):
print('{}) node={}, point={}, magnitude={:.3E}'.format(
i, node, node_points[node], force_from_node[node]))
#max_force = max(force_from_node.values())
max_force = max(
np.linalg.norm(reaction[:3])
for reactions in reaction_from_node.values() for reaction in reactions)
print('Max force:', max_force)
neighbors_from_node = get_node_neighbors(elements)
colors = sample_colors(len(sorted_nodes))
handles = []
for node, color in zip(sorted_nodes, colors):
#color = (0, 0, 0)
reactions = reaction_from_node[node]
#print(np.array(reactions))
start = node_points[node]
handles.extend(draw_point(start, color=color))
for reaction in reactions[:1]:
handles.append(
draw_reaction(start, reaction, max_force=max_force, color=RED))
for reaction in reactions[1:]:
handles.append(
draw_reaction(start,
reaction,
max_force=max_force,
color=GREEN))
print(
'Node: {} | Ground: {} | Neighbors: {} | Reactions: {} | Magnitude: {:.3E}'
.format(node,
(node in ground_nodes), len(neighbors_from_node[node]),
len(reactions), force_from_node[node]))
print('Total:', np.sum(reactions, axis=0))
wait_for_user()
#for handle in handles:
# remove_debug(handle)
#handles = []
#remove_all_debug()
# TODO: could compute the least balanced node with respect to original forces
# TODO: sum the norms of all the forces in the structure
#draw_sequence(sequence, node_points)
wait_for_user()
def lookahead(robot,
obstacles,
element_bodies,
extrusion_path,
partial_orders=[],
num_ee=0,
num_arm=1,
plan_all=False,
use_conflicts=False,
use_replan=False,
heuristic='z',
max_time=INF,
backtrack_limit=INF,
revisit=False,
ee_only=False,
collisions=True,
stiffness=True,
motions=True,
lazy=LAZY,
checker=None,
**kwargs):
if not use_conflicts:
num_ee, num_arm = min(num_ee, 1), min(num_arm, 1)
if ee_only:
num_ee, num_arm = max(num_arm, num_ee), 0
print('#EE: {} | #Arm: {}'.format(num_ee, num_arm))
# TODO: only check nearby remaining_elements
# TODO: only check collisions conditioned on current decisions
start_time = time.time()
initial_conf = get_configuration(robot)
initial_position = get_tool_position(robot)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
if checker is None:
checker = create_stiffness_checker(extrusion_path, verbose=False)
#print_gen_fn = get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes,
# precompute_collisions=False, supports=False, ee_only=ee_only,
# max_directions=MAX_DIRECTIONS, max_attempts=MAX_ATTEMPTS, collisions=collisions, **kwargs)
full_print_gen_fn = get_print_gen_fn(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
precompute_collisions=False,
ee_only=ee_only,
allow_failures=True,
collisions=collisions,
**kwargs)
# TODO: could just check environment collisions & kinematics instead of element collisions
ee_print_gen_fn = get_print_gen_fn(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
precompute_collisions=False,
ee_only=True,
allow_failures=True,
collisions=collisions,
**kwargs)
id_from_element = get_id_from_element(element_from_id)
all_elements = frozenset(element_bodies)
heuristic_fn = get_heuristic_fn(robot,
extrusion_path,
heuristic,
checker=checker,
forward=True)
#distance_fn = get_distance_fn(robot, joints, weights=JOINT_WEIGHTS)
# TODO: 2-step lookahead based on neighbors or spatial proximity
full_sample_traj, full_trajs_from_element = get_sample_traj(
ground_nodes, element_bodies, full_print_gen_fn, collisions=collisions)
ee_sample_traj, ee_trajs_from_element = get_sample_traj(
ground_nodes, element_bodies, ee_print_gen_fn, collisions=collisions)
if ee_only:
full_sample_traj = ee_sample_traj
#ee_sample_traj, ee_trajs_from_element = full_sample_traj, full_trajs_from_element
#heuristic_trajs_from_element = full_trajs_from_element if (num_ee == 0) else ee_trajs_from_element
heuristic_trajs_from_element = full_trajs_from_element if (
num_arm != 0) else ee_trajs_from_element
#########################
def sample_remaining(printed, next_printed, sample_fn, num=1, **kwargs):
if num == 0:
return True
remaining_elements = (all_elements - next_printed) if plan_all else \
compute_printable_elements(all_elements, ground_nodes, next_printed)
# TODO: could just consider nodes in printed (connected=True)
return all(
sample_fn(printed,
next_printed,
element,
connected=False,
num=num,
**kwargs) for element in randomize(remaining_elements))
def conflict_fn(printed, element, conf):
# Dead-end detection without stability performs reasonably well
# TODO: could add element if desired
order = retrace_elements(visited, printed)
printed = frozenset(
order[:-1]) # Remove last element (to ensure at least one traj)
if use_replan:
remaining = list(all_elements - printed)
requires_replan = [
all(element in traj.colliding
for traj in ee_trajs_from_element[e2]
if not (traj.colliding & printed)) for e2 in remaining
if e2 != element
]
return len(requires_replan)
else:
safe_trajectories = [
traj for traj in heuristic_trajs_from_element[element]
if not (traj.colliding & printed)
]
assert safe_trajectories
best_traj = max(safe_trajectories,
key=lambda traj: len(traj.colliding))
num_colliding = len(best_traj.colliding)
return -num_colliding
#distance = distance_fn(conf, best_traj.start_conf)
# TODO: ee distance vs conf distance
# TODO: l0 distance based on whether we remain at the same node
# TODO: minimize instability while printing (dynamic programming)
#return (-num_colliding, distance)
if use_conflicts:
priority_fn = lambda *args: (conflict_fn(*args), heuristic_fn(*args))
else:
priority_fn = heuristic_fn
#########################
initial_printed = frozenset()
queue = []
visited = {initial_printed: Node(None, None)}
if check_connected(ground_nodes, all_elements) and \
test_stiffness(extrusion_path, element_from_id, all_elements) and \
sample_remaining(initial_printed, initial_printed, ee_sample_traj, num=num_ee) and \
sample_remaining(initial_printed, initial_printed, full_sample_traj, num=num_arm):
add_successors(queue,
all_elements,
node_points,
ground_nodes,
priority_fn,
initial_printed,
initial_position,
initial_conf,
partial_orders=partial_orders)
plan = None
min_remaining = INF
num_evaluated = worst_backtrack = num_deadends = stiffness_failures = extrusion_failures = transit_failures = 0
while queue and (elapsed_time(start_time) < max_time):
num_evaluated += 1
visits, priority, printed, directed, current_conf = heapq.heappop(
queue)
element = get_undirected(all_elements,
directed) # TODO: use the correct direction
num_remaining = len(all_elements) - len(printed)
backtrack = num_remaining - min_remaining
worst_backtrack = max(worst_backtrack, backtrack)
if backtrack_limit < backtrack:
break # continue
num_evaluated += 1
if num_remaining < min_remaining:
min_remaining = num_remaining
print(
'Iteration: {} | Best: {} | Backtrack: {} | Deadends: {} | Printed: {} | Element: {} | Index: {} | Time: {:.3f}'
.format(num_evaluated, min_remaining, worst_backtrack,
num_deadends, len(printed), element,
id_from_element[element], elapsed_time(start_time)))
if has_gui():
color_structure(element_bodies, printed, element)
next_printed = printed | {element}
if next_printed in visited:
continue
assert check_connected(ground_nodes, next_printed)
if stiffness and not test_stiffness(extrusion_path,
element_from_id,
next_printed,
checker=checker,
verbose=False):
# Hard dead-end
#num_deadends += 1
stiffness_failures += 1
print('Partial structure is not stiff!')
continue
if revisit:
heapq.heappush(
queue, (visits + 1, priority, printed, element, current_conf))
#condition = frozenset()
#condition = set(retrace_elements(visited, printed, horizon=2))
#condition = printed # horizon=1
condition = next_printed
if not sample_remaining(
condition, next_printed, ee_sample_traj, num=num_ee):
num_deadends += 1
print('An end-effector successor could not be sampled!')
continue
print('Sampling transition')
#command = sample_extrusion(print_gen_fn, ground_nodes, printed, element)
command = next(
iter(full_sample_traj(printed, printed, element, connected=True)),
None)
if command is None:
# Soft dead-end
print('The transition could not be sampled!')
extrusion_failures += 1
continue
print('Sampling successors')
if not sample_remaining(
condition, next_printed, full_sample_traj, num=num_arm):
num_deadends += 1
print('A successor could not be sampled!')
continue
start_conf = end_conf = None
if not ee_only:
start_conf, end_conf = command.start_conf, command.end_conf
if (start_conf is not None) and motions and not lazy:
motion_traj = compute_motion(robot,
obstacles,
element_bodies,
printed,
current_conf,
start_conf,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time))
if motion_traj is None:
transit_failures += 1
continue
command.trajectories.insert(0, motion_traj)
visited[next_printed] = Node(command, printed)
if all_elements <= next_printed:
# TODO: anytime mode
min_remaining = 0
plan = retrace_trajectories(visited, next_printed)
if motions and not lazy:
motion_traj = compute_motion(robot,
obstacles,
element_bodies,
frozenset(),
initial_conf,
plan[0].start_conf,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time))
if motion_traj is None:
plan = None
transit_failures += 1
else:
plan.append(motion_traj)
if motions and lazy:
plan = compute_motions(robot,
obstacles,
element_bodies,
initial_conf,
plan,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time))
break
# if plan is not None:
# break
add_successors(queue,
all_elements,
node_points,
ground_nodes,
priority_fn,
next_printed,
node_points[directed[1]],
end_conf,
partial_orders=partial_orders)
data = {
'num_evaluated': num_evaluated,
'min_remaining': min_remaining,
'max_backtrack': worst_backtrack,
'stiffness_failures': stiffness_failures,
'extrusion_failures': extrusion_failures,
'transit_failures': transit_failures,
'num_deadends': num_deadends,
}
return plan, data
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
