def solve_inverse_kinematics(self,
world_from_tool,
nearby_tolerance=INF,
**kwargs):
if self.ik_solver is not None:
return self.solve_trac_ik(world_from_tool, **kwargs)
#if nearby_tolerance != INF:
# return self.solve_pybullet_ik(world_from_tool, nearby_tolerance=nearby_tolerance)
current_conf = get_joint_positions(self.robot, self.arm_joints)
start_time = time.time()
if nearby_tolerance == INF:
generator = ikfast_inverse_kinematics(self.robot,
PANDA_INFO,
self.tool_link,
world_from_tool,
max_attempts=10,
use_halton=True)
else:
generator = closest_inverse_kinematics(
self.robot,
PANDA_INFO,
self.tool_link,
world_from_tool,
max_time=0.01,
max_distance=nearby_tolerance,
use_halton=True)
conf = next(generator, None)
#conf = sample_tool_ik(self.robot, world_from_tool, max_attempts=100)
if conf is None:
return conf
max_distance = get_distance(current_conf, conf, norm=INF)
#print('Time: {:.3f} | distance: {:.5f} | max: {:.5f}'.format(
# elapsed_time(start_time), max_distance, nearby_tolerance))
set_joint_positions(self.robot, self.arm_joints, conf)
return get_configuration(self.robot)
def __init__(self, robot, positions=None, node=None, element=None):
self.robot = robot
self.joints = get_movable_joints(self.robot)
if positions is None:
positions = get_configuration(self.robot)
self.positions = positions
self.node = node
self.element = element
def solve_extrusion(robot, obstacles, element_from_id, node_points, element_bodies, extrusion_path, ground_nodes, args,
partial_orders=[], precompute=False, **kwargs):
# TODO: could treat ground as partial orders
backtrack_limit = INF # 0 | INF
seed = hash((time.time(), args.seed))
set_numpy_seed(seed)
set_random_seed(seed)
# initial_position = point_from_pose(get_link_pose(robot, link_from_name(robot, TOOL_LINK)))
# elements = sorted(element_bodies.keys())
# sequence = plan_stiffness(extrusion_path, element_from_id, node_points, ground_nodes, elements,
# initial_position=initial_position, max_time=INF, max_backtrack=INF)
# if has_gui():
# draw_ordered(sequence, node_points)
# wait_for_user()
# return
initial_conf = get_configuration(robot)
if args.algorithm == 'stripstream':
sampled_trajectories = []
if precompute:
sampled_trajectories = sample_trajectories(robot, obstacles, node_points, element_bodies, ground_nodes)
plan, data = stripstream(robot, obstacles, node_points, element_bodies, ground_nodes,
trajectories=sampled_trajectories, collisions=not args.cfree,
max_time=args.max_time, disable=args.disable, **kwargs)
elif args.algorithm == 'progression':
plan, data = progression(robot, obstacles, element_bodies, extrusion_path, partial_orders=partial_orders,
heuristic=args.bias, max_time=args.max_time,
backtrack_limit=backtrack_limit, collisions=not args.cfree,
disable=args.disable, stiffness=args.stiffness, motions=args.motions, **kwargs)
elif args.algorithm == 'regression':
plan, data = regression(robot, obstacles, element_bodies, extrusion_path, partial_orders=partial_orders,
heuristic=args.bias, max_time=args.max_time,
backtrack_limit=backtrack_limit, collisions=not args.cfree,
disable=args.disable, stiffness=args.stiffness, motions=args.motions, **kwargs)
elif args.algorithm == 'lookahead':
plan, data = lookahead(robot, obstacles, element_bodies, extrusion_path,
partial_orders=partial_orders, heuristic=args.bias,
max_time=args.max_time, backtrack_limit=backtrack_limit,
ee_only=args.ee_only, collisions=not args.cfree,
disable=args.disable, stiffness=args.stiffness, motions=args.motions, **kwargs)
else:
raise ValueError(args.algorithm)
if args.motions:
plan = compute_motions(robot, obstacles, element_bodies, initial_conf, plan, collisions=not args.cfree)
return plan, data
def _load_robot(self, real_world):
with ClientSaver(self.client):
# TODO: set the x,y,theta joints using the base pose
pose = get_pose(self.robot) # base_link is origin
base_pose = get_link_pose(self.robot,
link_from_name(self.robot, BASE_FRAME))
set_pose(self.robot, multiply(invert(base_pose), pose))
# base_pose = real_world.controller.return_cartesian_pose(arm='l')
# Because the robot might have an xyz
movable_names = real_world.controller.get_joint_names()
movable_joints = [
joint_from_name(self.robot, name) for name in movable_names
]
movable_positions = real_world.controller.get_joint_positions(
movable_names)
set_joint_positions(self.robot, movable_joints, movable_positions)
self.initial_config = get_configuration(self.robot)
self.body_mapping[self.robot] = real_world.robot
def solve_trac_ik(self, world_from_tool, nearby_tolerance=INF):
assert self.ik_solver is not None
init_lower, init_upper = self.ik_solver.get_joint_limits()
base_link = link_from_name(self.robot, self.ik_solver.base_link)
world_from_base = get_link_pose(self.robot, base_link)
tip_link = link_from_name(self.robot, self.ik_solver.tip_link)
tool_from_tip = multiply(
invert(get_link_pose(self.robot, self.tool_link)),
get_link_pose(self.robot, tip_link))
world_from_tip = multiply(world_from_tool, tool_from_tip)
base_from_tip = multiply(invert(world_from_base), world_from_tip)
joints = joints_from_names(
self.robot,
self.ik_solver.joint_names) # self.ik_solver.link_names
seed_state = get_joint_positions(self.robot, joints)
# seed_state = [0.0] * self.ik_solver.number_of_joints
lower, upper = init_lower, init_upper
if nearby_tolerance < INF:
tolerance = nearby_tolerance * np.ones(len(joints))
lower = np.maximum(lower, seed_state - tolerance)
upper = np.minimum(upper, seed_state + tolerance)
self.ik_solver.set_joint_limits(lower, upper)
(x, y, z), (rx, ry, rz, rw) = base_from_tip
# TODO: can also adjust tolerances
conf = self.ik_solver.get_ik(seed_state, x, y, z, rx, ry, rz, rw)
self.ik_solver.set_joint_limits(init_lower, init_upper)
if conf is None:
return conf
# if nearby_tolerance < INF:
# print(lower.round(3))
# print(upper.round(3))
# print(conf)
# print(get_difference(seed_state, conf).round(3))
set_joint_positions(self.robot, joints, conf)
return get_configuration(self.robot)
def get_pddlstream(robot,
brick_from_index,
obstacle_from_name,
collisions=True,
teleport=False):
domain_pddl = read(get_file_path(__file__, 'retired.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
conf = np.array(get_configuration(robot))
init = [
('CanMove', ),
('Conf', conf),
('AtConf', conf),
('HandEmpty', ),
]
goal_literals = [
('AtConf', conf),
#('HandEmpty',),
]
#indices = brick_from_index.keys()
#indices = range(2, 5)
indices = [4]
for index in list(indices):
indices.append(index + 6)
for index in indices:
brick = brick_from_index[index]
init += [
('Graspable', index),
('Pose', index, brick.initial_pose),
('AtPose', index, brick.initial_pose),
('Pose', index, brick.goal_pose),
]
goal_literals += [
#('Holding', index),
('AtPose', index, brick.goal_pose),
]
for index2 in brick.goal_supports:
brick2 = brick_from_index[index2]
init += [
('Supported', index, brick.goal_pose, index2,
brick2.goal_pose),
]
goal = And(*goal_literals)
if not collisions:
obstacle_from_name = {}
stream_map = {
'sample-grasp':
from_gen_fn(get_grasp_gen_fn(brick_from_index)),
'inverse-kinematics':
from_gen_fn(get_ik_gen_fn(robot, brick_from_index,
obstacle_from_name)),
'plan-motion':
from_fn(
get_motion_fn(robot,
brick_from_index,
obstacle_from_name,
teleport=teleport)),
'TrajCollision':
get_collision_test(robot, brick_from_index, collisions=collisions),
}
#stream_map = 'debug'
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def regression(robot,
obstacles,
element_bodies,
extrusion_path,
partial_orders=[],
heuristic='z',
max_time=INF,
max_memory=INF,
backtrack_limit=INF,
revisit=False,
stiffness_attempts=1,
collisions=True,
stiffness=True,
motions=True,
lazy=LAZY,
checker=None,
**kwargs):
# Focused has the benefit of reusing prior work
# Greedy has the benefit of conditioning on previous choices
# TODO: max branching factor
# TODO: be more careful when near the end
# TODO: max time spent evaluating successors (less expensive when few left)
# TODO: tree rollouts
# TODO: best-first search with a minimizing path distance cost
# TODO: immediately select if becomes more stable
# TODO: focus branching factor on most stable regions
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)
id_from_element = get_id_from_element(element_from_id)
all_elements = frozenset(element_bodies)
ground_elements = get_ground_elements(all_elements, ground_nodes)
if stiffness and (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,
max_directions=MAX_DIRECTIONS,
max_attempts=MAX_ATTEMPTS,
collisions=collisions,
**kwargs)
heuristic_fn = get_heuristic_fn(robot,
extrusion_path,
heuristic,
checker=checker,
forward=False)
queue = []
outgoing_from_element = outgoing_from_edges(partial_orders)
def add_successors(printed, position, conf):
only_ground = printed <= ground_elements
num_remaining = len(printed) - 1
#assert 0 <= num_remaining
for element in randomize(printed):
if not (outgoing_from_element[element] & printed) and implies(
is_ground(element, ground_nodes), only_ground):
for directed in get_directions(element):
visits = 0
bias = heuristic_fn(printed, directed, position, conf)
priority = (num_remaining, bias, random.random())
heapq.heappush(queue,
(visits, priority, printed, directed, conf))
final_conf = initial_conf # TODO: allow choice of final config
final_position = initial_position
final_printed = all_elements
visited = {final_printed: Node(None, None)}
if check_connected(ground_nodes, final_printed) and \
(not stiffness or test_stiffness(extrusion_path, element_from_id, final_printed, checker=checker)):
add_successors(final_printed, final_position, final_conf)
# if has_gui():
# sequence = sorted(final_printed, key=lambda e: heuristic_fn(final_printed, e, conf=None), reverse=True)
# remove_all_debug()
# draw_ordered(sequence, node_points)
# wait_for_user()
plan = None
min_remaining = len(all_elements)
num_evaluated = max_backtrack = extrusion_failures = transit_failures = stiffness_failures = 0
while queue and (elapsed_time(start_time) <
max_time) and check_memory(): #max_memory):
visits, priority, printed, directed, current_conf = heapq.heappop(
queue)
element = get_undirected(all_elements, directed)
num_remaining = len(printed)
backtrack = num_remaining - min_remaining
max_backtrack = max(max_backtrack, backtrack)
if backtrack_limit < backtrack:
break # continue
num_evaluated += 1
print(
'Iteration: {} | Best: {} | Printed: {} | Element: {} | Index: {} | Time: {:.3f}'
.format(num_evaluated, min_remaining, len(printed), element,
id_from_element[element], elapsed_time(start_time)))
next_printed = printed - {element}
next_nodes = compute_printed_nodes(ground_nodes, next_printed)
#draw_action(node_points, next_printed, element)
#if 3 < backtrack + 1:
# remove_all_debug()
# set_renderer(enable=True)
# draw_model(next_printed, node_points, ground_nodes)
# wait_for_user()
node1, node2 = directed
if (next_printed
in visited) or (node1
not in next_nodes) or not check_connected(
ground_nodes, next_printed):
continue
# TODO: stiffness plan lazily here possibly with reuse
if stiffness and not test_stiffness(
extrusion_path, element_from_id, next_printed,
checker=checker):
stiffness_failures += 1
continue
# TODO: stronger condition for this procedure
# if plan_stiffness(extrusion_path, element_from_id, node_points, ground_nodes, next_printed,
# checker=checker, max_backtrack=0) is None:
# # TODO: cache and reuse prior stiffness plans
# print('Failed stiffness plan') # TODO: require just a short horizon
# continue
if revisit:
heapq.heappush(
queue, (visits + 1, priority, printed, directed, current_conf))
command, = next(print_gen_fn(node1, element, extruded=next_printed),
(None, ))
if command is None:
extrusion_failures += 1
continue
if motions and not lazy:
motion_traj = compute_motion(
robot,
obstacles,
element_bodies,
printed,
command.end_conf,
current_conf,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time)) # TODO: smooth=...)
if motion_traj is None:
transit_failures += 1
continue
command.trajectories.append(motion_traj)
if num_remaining < min_remaining:
min_remaining = num_remaining
#print('New best: {}'.format(num_remaining))
#if has_gui():
# # TODO: change link transparency
# remove_all_debug()
# draw_model(next_printed, node_points, ground_nodes)
# wait_for_duration(0.5)
visited[next_printed] = Node(
command, printed) # TODO: be careful when multiple trajs
if not next_printed:
min_remaining = 0
commands = retrace_commands(visited, next_printed, reverse=True)
if OPTIMIZE:
commands = optimize_commands(robot,
obstacles,
element_bodies,
extrusion_path,
initial_conf,
commands,
motions=motions,
collisions=collisions)
plan = flatten_commands(commands)
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.insert(0, 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(next_printed, node_points[node1], command.start_conf)
#del checker
data = {
#'memory': get_memory_in_kb(), # May need to update instead
'num_evaluated': num_evaluated,
'min_remaining': min_remaining,
'max_backtrack': max_backtrack,
'stiffness_failures': stiffness_failures,
'extrusion_failures': extrusion_failures,
'transit_failures': transit_failures,
}
return plan, data
def progression(robot,
obstacles,
element_bodies,
extrusion_path,
partial_orders=[],
heuristic='z',
max_time=INF,
backtrack_limit=INF,
revisit=False,
stiffness=True,
motions=True,
collisions=True,
lazy=LAZY,
checker=None,
**kwargs):
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,
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)
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):
add_successors(queue,
all_elements,
node_points,
ground_nodes,
heuristic_fn,
initial_printed,
initial_position,
initial_conf,
partial_orders=partial_orders)
plan = None
min_remaining = len(all_elements)
num_evaluated = max_backtrack = 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)
num_remaining = len(all_elements) - len(printed)
backtrack = num_remaining - min_remaining
max_backtrack = max(max_backtrack, backtrack)
if backtrack_limit < backtrack:
break # continue
num_evaluated += 1
if num_remaining < min_remaining:
min_remaining = num_remaining
print(
'Iteration: {} | Best: {} | Printed: {} | Element: {} | Index: {} | Time: {:.3f}'
.format(num_evaluated, min_remaining, len(printed), element,
id_from_element[element], elapsed_time(start_time)))
next_printed = printed | {element}
assert check_connected(ground_nodes, next_printed)
if (next_printed in visited) or (stiffness and not test_stiffness(
extrusion_path, element_from_id, next_printed, checker=checker)
):
stiffness_failures += 1
continue
if revisit: # could also prevent revisiting if command is not None
heapq.heappush(
queue, (visits + 1, priority, printed, directed, current_conf))
node1, node2 = directed
command, = next(print_gen_fn(node1, element, extruded=printed),
(None, ))
if command is None:
extrusion_failures += 1
continue
if motions and not lazy:
# TODO: test reachability from initial_conf
motion_traj = compute_motion(robot,
obstacles,
element_bodies,
printed,
current_conf,
command.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:
min_remaining = 0
commands = retrace_commands(visited, next_printed)
if OPTIMIZE:
commands = optimize_commands(robot,
obstacles,
element_bodies,
extrusion_path,
initial_conf,
commands,
motions=motions,
collisions=collisions)
plan = flatten_commands(commands)
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,
heuristic_fn,
next_printed,
node_points[node2],
command.end_conf,
partial_orders=partial_orders)
data = {
'num_evaluated': num_evaluated,
'min_remaining': min_remaining,
'max_backtrack': max_backtrack,
'stiffness_failures': stiffness_failures,
'extrusion_failures': extrusion_failures,
'transit_failures': transit_failures,
}
return plan, data
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 ss_from_problem(robot,
movable=[],
bound='shared',
teleport=False,
movable_collisions=False,
grasp_name='top'):
#assert (not are_colliding(tree, kin_cache))
rigid = [body for body in get_bodies() if body != robot]
fixed = [body for body in rigid if body not in movable]
print('Robot:', robot)
print('Movable:', movable)
print('Fixed:', fixed)
conf = BodyConf(robot, get_configuration(robot))
initial_atoms = [
HandEmpty(),
CanMove(),
IsConf(conf),
AtConf(conf),
initialize(TotalCost(), 0),
]
for body in movable:
pose = BodyPose(body, get_pose(body))
initial_atoms += [
IsMovable(body),
IsPose(body, pose),
AtPose(body, pose)
]
for surface in fixed:
initial_atoms += [Stackable(body, surface)]
if is_placement(body, surface):
initial_atoms += [IsSupported(pose, surface)]
for body in fixed:
name = get_body_name(body)
if 'sink' in name:
initial_atoms.append(Sink(body))
if 'stove' in name:
initial_atoms.append(Stove(body))
body = movable[0]
goal_literals = [
AtConf(conf),
#Holding(body),
#On(body, fixed[0]),
#On(body, fixed[2]),
#Cleaned(body),
Cooked(body),
]
PlacedCollision = Predicate([T, O, P],
domain=[IsTraj(T), IsPose(O, P)],
fn=get_movable_collision_test(),
bound=False)
actions = [
Action(name='pick',
param=[O, P, G, Q, T],
pre=[
IsKin(O, P, G, Q, T),
HandEmpty(),
AtPose(O, P),
AtConf(Q), ~Unsafe(T)
],
eff=[HasGrasp(O, G),
CanMove(), ~HandEmpty(), ~AtPose(O, P)]),
Action(
name='place',
param=[O, P, G, Q, T],
pre=[IsKin(O, P, G, Q, T),
HasGrasp(O, G),
AtConf(Q), ~Unsafe(T)],
eff=[HandEmpty(),
CanMove(),
AtPose(O, P), ~HasGrasp(O, G)]),
# Can just do move if we check holding collisions
Action(name='move_free',
param=[Q, Q2, T],
pre=[
IsFreeMotion(Q, Q2, T),
HandEmpty(),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(name='move_holding',
param=[Q, Q2, O, G, T],
pre=[
IsHoldingMotion(Q, Q2, O, G, T),
HasGrasp(O, G),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(
name='clean',
param=[O, O2], # Wirelessly communicates to clean
pre=[Stackable(O, O2),
Sink(O2), ~Cooked(O),
On(O, O2)],
eff=[Cleaned(O)]),
Action(
name='cook',
param=[O, O2], # Wirelessly communicates to cook
pre=[Stackable(O, O2),
Stove(O2), Cleaned(O),
On(O, O2)],
eff=[Cooked(O), ~Cleaned(O)]),
]
axioms = [
Axiom(param=[O, G],
pre=[IsGrasp(O, G), HasGrasp(O, G)],
eff=Holding(O)),
Axiom(param=[O, P, O2],
pre=[IsPose(O, P),
IsSupported(P, O2),
AtPose(O, P)],
eff=On(O, O2)),
]
if movable_collisions:
axioms += [
Axiom(param=[T, O, P],
pre=[IsPose(O, P),
PlacedCollision(T, O, P),
AtPose(O, P)],
eff=Unsafe(T)),
]
streams = [
GenStream(name='grasp',
inp=[O],
domain=[IsMovable(O)],
fn=get_grasp_gen(robot, grasp_name),
out=[G],
graph=[IsGrasp(O, G)],
bound=bound),
# TODO: test_support
GenStream(name='support',
inp=[O, O2],
domain=[Stackable(O, O2)],
fn=get_stable_gen(fixed=fixed),
out=[P],
graph=[IsPose(O, P), IsSupported(P, O2)],
bound=bound),
FnStream(name='inverse_kin',
inp=[O, P, G],
domain=[IsPose(O, P), IsGrasp(O, G)],
fn=get_ik_fn(robot, fixed=fixed, teleport=teleport),
out=[Q, T],
graph=[IsKin(O, P, G, Q, T),
IsConf(Q), IsTraj(T)],
bound=bound),
FnStream(name='free_motion',
inp=[Q, Q2],
domain=[IsConf(Q), IsConf(Q2)],
fn=get_free_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsFreeMotion(Q, Q2, T),
IsTraj(T)],
bound=bound),
FnStream(name='holding_motion',
inp=[Q, Q2, O, G],
domain=[IsConf(Q), IsConf(Q2),
IsGrasp(O, G)],
fn=get_holding_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsHoldingMotion(Q, Q2, O, G, T),
IsTraj(T)],
bound=bound),
]
return Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
def get_print_gen_fn(robot,
fixed_obstacles,
node_points,
element_bodies,
ground_nodes,
precompute_collisions=False,
partial_orders=set(),
removed=set(),
collisions=True,
disable=False,
ee_only=False,
allow_failures=False,
p_nearby=0.,
max_directions=MAX_DIRECTIONS,
max_attempts=MAX_ATTEMPTS,
max_time=INF,
**kwargs):
# TODO: print on full sphere and just check for collisions with the printed element
# TODO: can slide a component of the element down
if not collisions:
precompute_collisions = False
#element_neighbors = get_element_neighbors(element_bodies)
node_neighbors = get_node_neighbors(element_bodies)
incoming_supporters, _ = neighbors_from_orders(partial_orders)
initial_conf = get_configuration(robot)
end_effector = EndEffector(robot,
ee_link=link_from_name(robot, EE_LINK),
tool_link=link_from_name(robot, TOOL_LINK))
extrusions = {
reverse_element(element) if reverse else element:
Extrusion(end_effector, element_bodies, node_points, ground_nodes,
element, reverse)
for element in element_bodies for reverse in [False, True]
}
def gen_fn(node1, element, extruded=[], trajectories=[]): # fluents=[]):
start_time = time.time()
assert not is_reversed(element_bodies, element)
idle_time = 0
reverse = is_end(node1, element)
if disable or (len(extruded) < SKIP_PERCENTAGE *
len(element_bodies)): # For quick visualization
path, tool_path = [], []
traj = PrintTrajectory(end_effector, get_movable_joints(robot),
path, tool_path, element, reverse)
command = Command([traj])
yield (command, )
return
directed = reverse_element(element) if reverse else element
extrusion = extrusions[directed]
n1, n2 = reverse_element(element) if reverse else element
neighboring_elements = node_neighbors[n1] & node_neighbors[n2]
supporters = [] # TODO: can also do according to levels
retrace_supporters(element, incoming_supporters, supporters)
element_obstacles = {
element_bodies[e]
for e in supporters + list(extruded)
}
obstacles = set(fixed_obstacles) | element_obstacles
if not collisions:
obstacles = set()
#obstacles = set(fixed_obstacles)
elements_order = [
e for e in element_bodies if (e != element) and (
e not in removed) and (element_bodies[e] not in obstacles)
]
collision_fn = get_element_collision_fn(robot, obstacles)
#print(len(fixed_obstacles), len(element_obstacles), len(elements_order))
trajectories = list(trajectories)
for num in irange(INF):
for attempt, tool_traj in enumerate(
islice(extrusion.generator(), max_directions)):
# TODO: is this slower now for some reason?
#tool_traj.safe_from_body = {} # Disables caching
if not tool_traj.is_safe(obstacles):
continue
for _ in range(max_attempts):
if max_time <= elapsed_time(start_time):
return
nearby_conf = initial_conf if random.random(
) < p_nearby else None
command = compute_direction_path(tool_traj,
collision_fn,
ee_only=ee_only,
initial_conf=nearby_conf,
**kwargs)
if command is None:
continue
command.update_safe(extruded)
if precompute_collisions:
bodies_order = [
element_bodies[e] for e in elements_order
]
colliding = command_collision(end_effector, command,
bodies_order)
for element2, unsafe in zip(elements_order, colliding):
if unsafe:
command.set_unsafe(element2)
else:
command.set_safe(element2)
if not is_ground(element, ground_nodes) and (
neighboring_elements <= command.colliding):
continue # If all neighbors collide
trajectories.append(command)
if precompute_collisions:
prune_dominated(trajectories)
if command not in trajectories:
continue
print(
'{}) {}->{} | EE: {} | Supporters: {} | Attempts: {} | Trajectories: {} | Colliding: {}'
.format(num, n1, n2, ee_only, len(supporters), attempt,
len(trajectories),
sorted(len(t.colliding)
for t in trajectories)))
temp_time = time.time()
yield (command, )
idle_time += elapsed_time(temp_time)
if precompute_collisions:
if len(command.colliding) == 0:
#print('Reevaluated already non-colliding trajectory!')
return
elif len(command.colliding) == 1:
[colliding_element] = command.colliding
obstacles.add(element_bodies[colliding_element])
break
else:
if allow_failures:
yield None
else:
print(
'{}) {}->{} | EE: {} | Supporters: {} | Attempts: {} | Max attempts exceeded!'
.format(num, n1, n2, ee_only, len(supporters),
max_directions))
return
#yield None
return gen_fn