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