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