def gen(robot, body): link = link_from_name(robot, BASE_LINK) with BodySaver(robot): set_base_conf(robot, np.zeros(3)) robot_pose = get_link_pose(robot, link) robot_aabb = get_turtle_aabb(robot) # _, upper = robot_aabb # radius = upper[0] extent = get_aabb_extent(robot_aabb) radius = max(extent[:2]) / 2. #draw_aabb(robot_aabb) center, (diameter, height) = approximate_as_cylinder(body) distance = radius + diameter / 2. + epsilon _, _, z = get_point(body) # Assuming already placed stably for [scale] in unit_generator(d=1, use_halton=use_halton): #theta = PI # 0 | PI theta = random.uniform(*theta_interval) position = np.append(distance * unit_from_theta(theta=theta), [z]) # TODO: halton yaw = scale * 2 * PI - PI quat = quat_from_euler(Euler(yaw=yaw)) body_pose = (position, quat) robot_from_body = multiply(invert(robot_pose), body_pose) grasp = Pose(body, robot_from_body) # TODO: grasp instead of pose if draw: world_pose = multiply(get_link_pose(robot, link), grasp.value) set_pose(body, world_pose) handles = draw_pose(get_pose(body), length=1) wait_for_user() remove_handles(handles) #continue yield (grasp, )
def main(): parser = argparse.ArgumentParser() parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) connect(use_gui=True) with LockRenderer(): draw_pose(unit_pose(), length=1) floor = create_floor() with HideOutput(): robot = load_pybullet(get_model_path(ROOMBA_URDF), fixed_base=True, scale=2.0) for link in get_all_links(robot): set_color(robot, link=link, color=ORANGE) robot_z = stable_z(robot, floor) set_point(robot, Point(z=robot_z)) #set_base_conf(robot, rover_confs[i]) data_path = add_data_path() shelf, = load_model(os.path.join(data_path, KIVA_SHELF_SDF), fixed_base=True) # TODO: returns a tuple dump_body(shelf) #draw_aabb(get_aabb(shelf)) wait_for_user() disconnect()
def main(time_step=0.01): parser = create_parser() parser.add_argument('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') # TODO: argument for selecting prior args = parser.parse_args() print('Arguments:', args) # TODO: nonuniform distribution to bias towards other actions # TODO: closed world and open world real_world = connect(use_gui=not args.viewer) add_data_path() task, state = get_problem1(localized='rooms', p_other=0.25) # surfaces | rooms for body in task.get_bodies(): add_body_name(body) robot = task.robot #dump_body(robot) assert (USE_DRAKE_PR2 == is_drake_pr2(robot)) attach_viewcone(robot) # Doesn't work for the normal pr2? draw_base_limits(get_base_limits(robot), color=(0, 1, 0)) #wait_for_user() # TODO: partially observable values # TODO: base movements preventing pick without look # TODO: do everything in local coordinate frame # TODO: automatically determine an action/command cannot be applied # TODO: convert numpy arrays into what they are close to # TODO: compute whether a plan will still achieve a goal and do that # TODO: update the initial state directly and then just redraw it to ensure uniqueness step = 0 while True: step += 1 print('\n' + 50 * '-') print(step, state) wait_for_user() #print({b: p.value for b, p in state.poses.items()}) with ClientSaver(): commands = plan_commands(state, args) print() if commands is None: print('Failure!') break if not commands: print('Success!') break apply_commands(state, commands, time_step=time_step) print(state) wait_for_user() disconnect()
def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() connect(use_gui=True) #ycb_path = os.path.join(DRAKE_PATH, DRAKE_YCB) #ycb_path = os.path.join(YCB_PATH, YCB_TEMPLATE.format('003_cracker_box')) #print(ycb_path) #load_pybullet(ycb_path) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False) #set_euler(table, Euler(yaw=np.pi/2)) with HideOutput(False): # data_path = add_data_path() # robot_path = os.path.join(data_path, WSG_GRIPPER) robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF #robot_path = get_file_path(__file__, 'mit_arch_suction_gripper/mit_arch_suction_gripper.urdf') robot = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) #robot = create_cylinder(radius=0.5*BLOCK_SIDE, height=4*BLOCK_SIDE) # vacuum gripper block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table) set_point(block1, Point(z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=+0.25, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, z=block_z)) blocks = [block1, block2, block3] add_line(Point(x=-TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON), Point(x=+TABLE_WIDTH/2, z=block_z - BLOCK_SIDE/2 + EPSILON), color=RED) set_camera_pose(camera_point=Point(y=-1, z=block_z+1), target_point=Point(z=block_z)) wait_for_user() block_pose = get_pose(block1) open_gripper(robot) tool_link = link_from_name(robot, 'tool_link') base_from_tool = get_relative_pose(robot, tool_link) #draw_pose(unit_pose(), parent=robot, parent_link=tool_link) y_grasp, x_grasp = get_top_grasps(block1, tool_pose=unit_pose(), grasp_length=0.03, under=False) grasp = y_grasp # fingers won't collide gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user('Finish?') disconnect()
def apply(self, state, **kwargs): mesh, _ = get_detection_cone(self.robot, self.body, camera_link=self.camera_frame, depth=self.max_depth) if mesh is None: wait_for_user() assert (mesh is not None) with LockRenderer(): cone = create_mesh(mesh, color=(0, 1, 0, 0.5)) set_pose(cone, get_link_pose(self.robot, self.link)) wait_for_duration(1e-2) wait_for_duration(self._duration) remove_body(cone) wait_for_duration(1e-2) state.registered.add(self.body) yield
def test(r, t, b2, p2): if not problem.collisions: return True p2.assign() state = State() for _ in t.apply(state): state.assign() #for b1 in state.attachments: # if pairwise_collision(b1, b2): # return False if pairwise_collision(r, b2): set_renderer(True) color = get_visual_data(b2)[0].rgbaColor handles = add_line(point_from_conf(t.path[0].values, z=0.02), point_from_conf(t.path[-1].values, z=0.02), color=color) wait_for_user() set_renderer(False) return False return True
def apply(self, state, **kwargs): # TODO: check if actually can register mesh, _ = get_detection_cone(self.robot, self.body, camera_link=self.camera_frame, depth=self.max_depth) if mesh is None: wait_for_user() assert ( mesh is not None ) # TODO: is_visible_aabb(body_aabb, **kwargs)=False sometimes without collisions with LockRenderer(): cone = create_mesh(mesh, color=apply_alpha(GREEN, 0.5)) set_pose(cone, get_link_pose(self.robot, self.link)) wait_for_duration(1e-2) wait_for_duration(self._duration) remove_body(cone) wait_for_duration(1e-2) state.registered.add(self.body) yield
def main(display=True, teleport=False): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') parser.add_argument('-simulate', action='store_true', help='enable viewer.') args = parser.parse_args() connect(use_gui=args.viewer) robot, names, movable = load_world() saved_world = WorldSaver() #dump_world() pddlstream_problem = pddlstream_from_problem(robot, movable=movable, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) print('Synthesizers:', stream_map.keys()) print(names) pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, success_cost=INF) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is None: return if (not display) or (plan is None): disconnect() return if not args.viewer: # TODO: how to reenable the viewer disconnect() connect(use_gui=True) load_world() else: saved_world.restore() command = postprocess_plan(plan) if args.simulate: wait_for_user('Simulate?') command.control() else: wait_for_user('Execute?') #command.step() command.refine(num_steps=10).execute(time_step=0.001) wait_for_user('Finish?') disconnect()
def main(): parser = create_parser() parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) connect(use_gui=args.viewer) robot, names, movable = load_world() print('Objects:', names) saver = WorldSaver() problem = pddlstream_from_problem(robot, movable=movable, teleport=args.teleport) _, _, _, stream_map, init, goal = problem print('Init:', init) print('Goal:', goal) print('Streams:', str_from_object(set(stream_map))) with Profiler(): with LockRenderer(lock=not args.enable): solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit, success_cost=INF) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return command = postprocess_plan(plan) if args.simulate: wait_for_user('Simulate?') command.control() else: wait_for_user('Execute?') #command.step() command.refine(num_steps=10).execute(time_step=0.001) wait_for_user('Finish?') disconnect()
def main(): parser = create_parser() parser.add_argument('-problem', default='problem1', help='The name of the problem to solve') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_fn_from_name: raise ValueError(args.problem) problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn() saver = WorldSaver() draw_base_limits(problem.limits, color=RED) pddlstream_problem = pddlstream_from_problem(problem, collisions=not args.cfree, teleport=args.teleport) stream_info = { 'inverse-kinematics': StreamInfo(), 'plan-base-motion': StreamInfo(overhead=1e1), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) #print('Streams:', stream_map.keys()) success_cost = 0 if args.optimal else INF planner = 'ff-astar' search_sample_ratio = 1 max_planner_time = 10 with Profiler(field='cumtime', num=25): # cumtime | tottime with LockRenderer(lock=not args.enable): solution = solve(pddlstream_problem, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, debug=False, unit_efforts=True, effort_weight=1, search_sample_ratio=search_sample_ratio) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return # Maybe openrave didn't actually sample any joints... # http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/ with LockRenderer(lock=not args.enable): commands = post_process(problem, plan, teleport=args.teleport) saver.restore() if args.simulate: control_commands(commands) else: time_step = None if args.teleport else 0.01 apply_commands(BeliefState(problem), commands, time_step) wait_for_user() disconnect()
def main(): parser = create_parser(default_algorithm='binding') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn(collisions=not args.cfree) saver = WorldSaver() draw_base_limits(problem.limits, color=RED) pddlstream_problem = pddlstream_from_problem(problem) stream_info = { 'test-cfree-conf-pose': StreamInfo(p_success=1e-2), 'test-cfree-traj-pose': StreamInfo(p_success=1e-1), 'compute-motion': StreamInfo(eager=True, p_success=0), 'test-reachable': StreamInfo(eager=True), 'Distance': FunctionInfo(eager=True), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) success_cost = 0 if args.optimal else INF planner = 'ff-wastar1' search_sample_ratio = 0 max_planner_time = 10 with Profiler(field='tottime', num=25): # cumtime | tottime with LockRenderer(lock=not args.enable): solution = solve(pddlstream_problem, algorithm=args.algorithm, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, debug=False, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, unit_efforts=True, effort_weight=1, search_sample_ratio=search_sample_ratio) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return with LockRenderer(): commands = post_process(problem, plan) saver.restore() # Assumes bodies are ordered the same way wait_for_user() if args.simulate: control_commands(commands) else: apply_commands(BeliefState(problem), commands, time_step=1e-2) # 1e-2 | None wait_for_user() disconnect()
def main(teleport=False): #parser = create_parser() parser = argparse.ArgumentParser() parser.add_argument('-algorithm', default='incremental', help='Specifies the algorithm') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=5*60, type=int, help='The max time') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) connect(use_gui=args.viewer) with HideOutput(): namo_problem = problem_fn(collisions=not args.cfree) saver = WorldSaver() draw_base_limits(namo_problem.limits, color=RED) pddlstream_problem, edges = pddlstream_from_problem(namo_problem, teleport=teleport) _, constant_map, _, stream_map, init, goal = pddlstream_problem print('Constants:', constant_map) print('Init:', init) print('Goal:', goal) stream_info = { 'compute-motion': StreamInfo(eager=True, p_success=0), 'ConfConfCollision': PredicateInfo(p_success=1, overhead=0.1), 'TrajConfCollision': PredicateInfo(p_success=1, overhead=1), 'TrajTrajCollision': PredicateInfo(p_success=1, overhead=10), 'TrajDistance': FunctionInfo(eager=True), # Need to eagerly evaluate otherwise 0 duration (failure) } success_cost = 0 if args.optimal else INF max_planner_time = 10 with Profiler(field='tottime', num=25): # cumtime | tottime with LockRenderer(lock=not args.enable): # TODO: solution = solve_incremental(pddlstream_problem if args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, max_planner_time=max_planner_time, success_cost=success_cost, max_time=args.max_time, start_complexity=INF, verbose=True, debug=True) elif args.algorithm == 'focused': solution = solve_focused(pddlstream_problem, stream_info=stream_info, max_planner_time=max_planner_time, success_cost=success_cost, max_time=args.max_time, max_skeletons=None, bind=True, max_failures=INF, verbose=True, debug=True) else: raise ValueError(args.algorithm) print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return saver.restore() draw_edges(edges) state = BeliefState(namo_problem) wait_for_user('Begin?') #time_step = None if teleport else 0.01 #with VideoSaver('video.mp4'): display_plan(namo_problem, state, plan) wait_for_user('Finish?') disconnect()
def main(): parser = create_parser() parser.add_argument('-problem', default='rovers1', help='The name of the problem to solve') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_fn_from_name: raise ValueError(args.problem) problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): rovers_problem = problem_fn() saver = WorldSaver() draw_base_limits(rovers_problem.limits, color=RED) pddlstream_problem = pddlstream_from_problem(rovers_problem, collisions=not args.cfree, teleport=args.teleport, holonomic=False, reversible=True, use_aabb=True) stream_info = { 'test-cfree-ray-conf': StreamInfo(), 'test-reachable': StreamInfo(p_success=1e-1), 'obj-inv-visible': StreamInfo(), 'com-inv-visible': StreamInfo(), 'sample-above': StreamInfo(), 'sample-motion': StreamInfo(overhead=10), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) #print('Streams:', stream_map.keys()) success_cost = 0 if args.optimal else INF planner = 'ff-wastar3' search_sample_ratio = 2 max_planner_time = 10 # TODO: need to accelerate samples here because of the failed test reachable with Profiler(field='tottime', num=25): with LockRenderer(lock=not args.enable): # TODO: option to only consider costs during local optimization solution = solve(pddlstream_problem, algorithm=args.algorithm, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, debug=False, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, unit_efforts=True, effort_weight=1, search_sample_ratio=search_sample_ratio) for body in get_bodies(): if body not in saver.bodies: remove_body(body) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return # Maybe OpenRAVE didn't actually sample any joints... # http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/ with LockRenderer(): commands = post_process(rovers_problem, plan) saver.restore() wait_for_user('Begin?') if args.simulate: control_commands(commands) else: time_step = None if args.teleport else 0.01 apply_commands(BeliefState(rovers_problem), commands, time_step) wait_for_user('Finish?') disconnect()
def main(display=True, teleport=False): parser = argparse.ArgumentParser() parser.add_argument('-algorithm', default='incremental', help='Specifies the algorithm') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=5*60, type=int, help='The max time') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) #problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} #if args.problem not in problem_fn_from_name: # raise ValueError(args.problem) #problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn(collisions=not args.cfree) saver = WorldSaver() draw_base_limits(problem.limits, color=RED) pddlstream, edges = pddlstream_from_problem(problem, teleport=teleport) _, constant_map, _, stream_map, init, goal = pddlstream print('Constants:', constant_map) print('Init:', init) print('Goal:', goal) success_cost = 0 if args.optimal else INF max_planner_time = 10 stream_info = { 'compute-motion': StreamInfo(eager=True, p_success=0), 'ConfConfCollision': FunctionInfo(p_success=1, overhead=0.1), 'TrajConfCollision': FunctionInfo(p_success=1, overhead=1), 'TrajTrajCollision': FunctionInfo(p_success=1, overhead=10), 'TrajDistance': FunctionInfo(eager=True), # Need to eagerly evaluate otherwise 0 duration (failure) } pr = cProfile.Profile() pr.enable() with LockRenderer(False): if args.algorithm == 'incremental': solution = solve_incremental(pddlstream, max_planner_time=max_planner_time, success_cost=success_cost, max_time=args.max_time, start_complexity=INF, verbose=True, debug=True) elif args.algorithm == 'focused': solution = solve_focused(pddlstream, stream_info=stream_info, max_planner_time=max_planner_time, success_cost=success_cost, max_time=args.max_time, max_skeletons=None, bind=True, max_failures=INF, verbose=True, debug=True) else: raise ValueError(args.algorithm) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime if plan is None: wait_for_user() return if (not display) or (plan is None): disconnect() return if not args.viewer: disconnect() connect(use_gui=True) with LockRenderer(): with HideOutput(): problem_fn() # TODO: way of doing this without reloading? saver.restore() # Assumes bodies are ordered the same way draw_edges(edges) state = BeliefState(problem) wait_for_user() #time_step = None if teleport else 0.01 #with VideoSaver('video.mp4'): display_plan(problem, state, plan) wait_for_user() disconnect()
def main(verbose=True): # TODO: could work just on postprocessing # TODO: try the other reachability database # TODO: option to only consider costs during local optimization parser = create_parser() parser.add_argument('-problem', default='packed', help='The name of the problem to solve') parser.add_argument('-n', '--number', default=5, type=int, help='The number of objects') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_fn_from_name: raise ValueError(args.problem) problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn(num=args.number) draw_base_limits(problem.base_limits, color=(1, 0, 0)) saver = WorldSaver() #handles = [] #for link in get_group_joints(problem.robot, 'left_arm'): # handles.append(draw_link_name(problem.robot, link)) #wait_for_user() pddlstream_problem = pddlstream_from_problem(problem, collisions=not args.cfree, teleport=args.teleport) stream_info = { 'inverse-kinematics': StreamInfo(), 'plan-base-motion': StreamInfo(overhead=1e1), 'test-cfree-pose-pose': StreamInfo(p_success=1e-3, verbose=verbose), 'test-cfree-approach-pose': StreamInfo(p_success=1e-2, verbose=verbose), 'test-cfree-traj-pose': StreamInfo(p_success=1e-1, verbose=verbose), # TODO: apply to arm and base trajs #'test-cfree-traj-grasp-pose': StreamInfo(verbose=verbose), 'Distance': FunctionInfo(p_success=0.99, opt_fn=lambda q1, q2: BASE_CONSTANT), #'MoveCost': FunctionInfo(lambda t: BASE_CONSTANT), } #stream_info = {} _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', str_from_object(set(stream_map))) success_cost = 0 if args.optimal else INF planner = 'ff-astar' if args.optimal else 'ff-wastar3' search_sample_ratio = 2 max_planner_time = 10 # effort_weight = 0 if args.optimal else 1 effort_weight = 1e-3 if args.optimal else 1 with Profiler(field='tottime', num=25): # cumtime | tottime with LockRenderer(lock=not args.enable): solution = solve(pddlstream_problem, algorithm=args.algorithm, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, debug=False, unit_efforts=True, effort_weight=effort_weight, search_sample_ratio=search_sample_ratio) saver.restore() cost_over_time = [(s.cost, s.time) for s in SOLUTIONS] for i, (cost, runtime) in enumerate(cost_over_time): print('Plan: {} | Cost: {:.3f} | Time: {:.3f}'.format( i, cost, runtime)) #print(SOLUTIONS) print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return with LockRenderer(lock=not args.enable): commands = post_process(problem, plan, teleport=args.teleport) saver.restore() draw_base_limits(problem.base_limits, color=(1, 0, 0)) wait_for_user() if args.simulate: control_commands(commands) else: time_step = None if args.teleport else 0.01 apply_commands(State(), commands, time_step) wait_for_user() disconnect()
def main(display=True, teleport=False): parser = argparse.ArgumentParser() parser.add_argument('-problem', default='rovers1', help='The name of the problem to solve') parser.add_argument('-algorithm', default='focused', help='Specifies the algorithm') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-unit', action='store_true', help='Uses unit costs') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_fn_from_name: raise ValueError(args.problem) problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn() saver = WorldSaver() draw_base_limits(problem.limits, color=(1, 0, 0)) pddlstream_problem = pddlstream_from_problem(problem, collisions=not args.cfree, teleport=teleport) stream_info = { 'test-cfree-ray-conf': StreamInfo(negate=True), 'test-reachable': StreamInfo(p_success=1e-1), 'obj-inv-visible': StreamInfo(), 'com-inv-visible': StreamInfo(), 'sample-above': StreamInfo(), 'sample-motion': StreamInfo(overhead=10), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) #print('Streams:', stream_map.keys()) success_cost = 0 if args.optimal else INF planner = 'ff-wastar3' search_sample_ratio = 2 max_planner_time = 10 # TODO: need to accelerate samples here because of the failed test reachable pr = cProfile.Profile() pr.enable() with LockRenderer(False): if args.algorithm == 'focused': # TODO: option to only consider costs during local optimization solution = solve_focused( pddlstream_problem, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, debug=False, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, unit_efforts=True, effort_weight=1, #bind=True, max_skeletons=None, search_sample_ratio=search_sample_ratio) elif args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True) else: raise ValueError(args.algorithm) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime if plan is None: return if (not display) or (plan is None): disconnect() return # Maybe openrave didn't actually sample any joints... # http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/ with LockRenderer(): commands = post_process(problem, plan, teleport=teleport) saver.restore() # Assumes bodies are ordered the same way if not args.viewer: disconnect() connect(use_gui=True) with LockRenderer(): with HideOutput(): problem_fn() # TODO: way of doing this without reloading? saver.restore() # Assumes bodies are ordered the same way if args.simulate: control_commands(commands) else: time_step = None if teleport else 0.01 apply_commands(BeliefState(problem), commands, time_step) wait_for_user() disconnect()
def main(display=True, teleport=False): parser = argparse.ArgumentParser() #parser.add_argument('-problem', default='rovers1', help='The name of the problem to solve') parser.add_argument('-algorithm', default='focused', help='Specifies the algorithm') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-unit', action='store_true', help='Uses unit costs') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) #problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} #if args.problem not in problem_fn_from_name: # raise ValueError(args.problem) #problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn(collisions=not args.cfree) saver = WorldSaver() draw_base_limits(problem.limits, color=RED) pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport) stream_info = { 'test-cfree-conf-pose': StreamInfo(negate=True, p_success=1e-2), 'test-cfree-traj-pose': StreamInfo(negate=True, p_success=1e-1), 'compute-motion': StreamInfo(eager=True, p_success=0), 'test-reachable': StreamInfo(eager=True), 'Distance': FunctionInfo(eager=True), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) success_cost = 0 if args.optimal else INF planner = 'ff-wastar1' search_sample_ratio = 0 max_planner_time = 10 pr = cProfile.Profile() pr.enable() with LockRenderer(True): if args.algorithm == 'focused': solution = solve_focused(pddlstream_problem, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, debug=False, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, unit_efforts=True, effort_weight=1, bind=True, max_skeletons=None, search_sample_ratio=search_sample_ratio) elif args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True) else: raise ValueError(args.algorithm) saver.restore() print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime if plan is None: wait_for_user() return if (not display) or (plan is None): disconnect() return with LockRenderer(): commands = post_process(problem, plan, teleport=teleport) saver.restore() # Assumes bodies are ordered the same way if not args.viewer: disconnect() connect(use_gui=True) with LockRenderer(): with HideOutput(): problem_fn() # TODO: way of doing this without reloading? saver.restore() # Assumes bodies are ordered the same way wait_for_user() if args.simulate: control_commands(commands) else: time_step = None if teleport else 0.01 apply_commands(BeliefState(problem), commands, time_step=time_step) wait_for_user() disconnect()
def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() connect(use_gui=True) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH / 2, height=TABLE_WIDTH / 2, top_color=TAN, cylinder=False) set_point(table1, Point(y=+0.5)) table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH / 2, height=TABLE_WIDTH / 2, top_color=TAN, cylinder=False) set_point(table2, Point(y=-0.5)) tables = [table1, table2] #set_euler(table1, Euler(yaw=np.pi/2)) with HideOutput(): # data_path = add_data_path() # robot_path = os.path.join(data_path, WSG_GRIPPER) robot_path = get_model_path( WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF robot = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table1) set_point(block1, Point(y=-0.5, z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=-0.25, y=-0.5, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, y=+0.5, z=block_z)) blocks = [block1, block2, block3] set_camera_pose(camera_point=Point(x=-1, z=block_z + 1), target_point=Point(z=block_z)) block_pose = get_pose(block1) open_gripper(robot) tool_link = link_from_name(robot, 'tool_link') base_from_tool = get_relative_pose(robot, tool_link) #draw_pose(unit_pose(), parent=robot, parent_link=tool_link) grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi / 2)), top_offset=0.02, grasp_length=0.03, under=False)[1:2] for grasp in grasps: gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user() wait_for_user('Finish?') disconnect()