def main(focused=False, deterministic=False, unit_costs=True): np.set_printoptions(precision=2) if deterministic: seed = 0 np.random.seed(seed) print('Seed:', get_random_seed()) problem_fn = get_tight_problem # get_tight_problem | get_blocked_problem tamp_problem = problem_fn() print(tamp_problem) stream_info = { #'test-region': StreamInfo(eager=True, p_success=0), # bound_fn is None #'plan-motion': StreamInfo(p_success=1), # bound_fn is None #'trajcollision': StreamInfo(p_success=1), # bound_fn is None #'cfree': StreamInfo(eager=True), } pddlstream_problem = pddlstream_from_tamp(tamp_problem) pr = cProfile.Profile() pr.enable() if focused: solution = solve_focused(pddlstream_problem, stream_info=stream_info, max_time=10, max_cost=INF, debug=False, effort_weight=None, unit_costs=unit_costs, postprocess=False, visualize=False) else: solution = solve_incremental(pddlstream_problem, layers=1, unit_costs=unit_costs, verbose=False) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is None: return colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS)) viewer = ContinuousTMPViewer(tamp_problem.regions, title='Continuous TAMP') state = tamp_problem.initial print() print(state) draw_state(viewer, state, colors) for i, (action, args) in enumerate(plan): user_input('Continue?') print(i, action, args) s2 = args[-1] state = TAMPState( s2[R], s2[H], {b: s2[b] for b in state.block_poses if s2[b] is not None}) print(state) draw_state(viewer, state, colors) user_input('Finish?')
def main(deterministic=False, observable=False, collisions=True, focused=True, factor=True): # TODO: global search over the state belief_problem = get_belief_problem(deterministic, observable) pddlstream_problem = to_pddlstream(belief_problem, collisions) pr = cProfile.Profile() pr.enable() planner = 'ff-wastar1' if focused: stream_info = { 'GE': StreamInfo(from_test(ge_fn), eager=False), 'prob-after-move': StreamInfo(from_fn(get_opt_move_fn(factor=factor))), 'MoveCost': FunctionInfo(move_cost_fn), 'prob-after-look': StreamInfo(from_fn(get_opt_obs_fn(factor=factor))), 'LookCost': FunctionInfo(get_look_cost_fn(p_look_fp=0, p_look_fn=0)), } solution = solve_focused(pddlstream_problem, stream_info=stream_info, planner=planner, debug=False, max_cost=0, unit_costs=False, max_time=30) else: solution = solve_incremental(pddlstream_problem, planner=planner, debug=True, max_cost=MAX_COST, unit_costs=False, max_time=30) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) print_solution(solution) plan, cost, init = solution print('Real cost:', float(cost)/SCALE_COST)
def main(): uniform_rooms = UniformDist(['room0', OTHER]) #uniform_tables = UniformDist(['table0', 'table1']) #uniform_tables = UniformDist(['table0', OTHER]) uniform_tables = UniformDist(['table0', 'table1', OTHER]) #initial_belief = get_room_belief(uniform_rooms, uniform_tables, 1.0) initial_belief = get_room_belief(uniform_rooms, uniform_tables, 0.2) #initial_belief = get_table_belief(uniform_tables, 1.0) #initial_belief = get_table_belief(uniform_tables, 0.2) #initial_belief = get_item_belief() pddlstream_problem = pddlstream_from_belief(initial_belief) _, _, _, _, init, goal = pddlstream_problem print(sorted(init)) print(goal) pr = cProfile.Profile() pr.enable() planner = 'max-astar' #solution = solve_incremental(pddlstream_problem, planner=planner, unit_costs=False) solution = solve_focused(pddlstream_problem, planner=planner, unit_costs=False) print_solution(solution) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10)
def plan_trajectories(task, context, collisions=True, use_impedance=False, max_time=180): stream_info = { 'TrajPoseCollision': FunctionInfo(p_success=1e-3), 'TrajConfCollision': FunctionInfo(p_success=1e-3), } pr = cProfile.Profile() pr.enable() problem = get_pddlstream_problem(task, context, collisions=collisions, use_impedance=use_impedance) solution = solve_focused(problem, stream_info=stream_info, planner='ff-wastar2', max_time=max_time, search_sampling_ratio=0) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(5) print_solution(solution) plan, cost, evaluations = solution if plan is None: print('Unable to find a solution in under {} seconds'.format(max_time)) return None return postprocess_plan(task.mbp, task.gripper, plan)
def main(focused=True, unit_costs=False): problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem tamp_problem = problem_fn() print(tamp_problem) pddlstream_problem = pddlstream_from_tamp(tamp_problem) if focused: solution = solve_focused(pddlstream_problem, unit_costs=unit_costs) else: #solution = solve_exhaustive(pddlstream_problem, unit_costs=unit_costs) solution = solve_incremental(pddlstream_problem, unit_costs=unit_costs) print_solution(solution) plan, cost, evaluations = solution if plan is None: return print(evaluations) colors = dict(zip(tamp_problem.initial.block_poses, COLORS)) viewer = DiscreteTAMPViewer(1, len(tamp_problem.poses), title='Initial') state = tamp_problem.initial print(state) draw_state(viewer, state, colors) for action in plan: user_input('Continue?') state = apply_action(state, action) print(state) draw_state(viewer, state, colors) user_input('Finish?')
def main(viewer=False, display=True, simulate=False, teleport=False): # TODO: fix argparse & FastDownward #parser = argparse.ArgumentParser() # Automatically includes help #parser.add_argument('-viewer', action='store_true', help='enable viewer.') #parser.add_argument('-display', action='store_true', help='enable viewer.') #args = parser.parse_args() connect(use_gui=viewer) problem_fn = holding_problem # holding_problem | stacking_problem | cleaning_problem | cooking_problem # cleaning_button_problem | cooking_button_problem problem = problem_fn() state_id = save_state() #saved_world = WorldSaver() dump_world() pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem synthesizers = [ #StreamSynthesizer('safe-base-motion', {'plan-base-motion': 1, # 'TrajPoseCollision': 0, 'TrajGraspCollision': 0, 'TrajArmCollision': 0}, # from_fn(get_base_motion_synth(problem, teleport))), ] print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) print('Synthesizers:', synthesizers) pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, synthesizers=synthesizers, max_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 viewer: restore_state(state_id) else: disconnect() connect(use_gui=True) problem = problem_fn() # TODO: way of doing this without reloading? user_input('Execute?') commands = post_process(problem, plan) if simulate: enable_gravity() control_commands(commands) else: step_commands(commands, time_step=0.01) user_input('Finish?') disconnect()
def solve_pddlstream(focused=True): pddlstream_problem = get_problem() if focused: solution = solve_focused(pddlstream_problem, unit_costs=True) else: #solution = solve_exhaustive(pddlstream_problem, unit_costs=True) solution = solve_incremental(pddlstream_problem, unit_costs=True) print_solution(solution)
def plan_sequence(robot, obstacles, node_points, element_bodies, ground_nodes, trajectories=[]): if trajectories is None: return None pddlstream_fn = get_pddlstream2 # get_pddlstream | get_pddlstream2 # TODO: Iterated search # http://www.fast-downward.org/Doc/Heuristic#h.5Em_heuristic # randomize_successors=True pr = cProfile.Profile() pr.enable() pddlstream_problem = pddlstream_fn(robot, obstacles, node_points, element_bodies, ground_nodes, trajectories=trajectories) #solution = solve_exhaustive(pddlstream_problem, planner='goal-lazy', max_time=300, debug=True) #solution = solve_incremental(pddlstream_problem, planner='add-random-lazy', max_time=600, # max_planner_time=300, debug=True) stream_info = { #'sample-print': StreamInfo(PartialInputs(unique=True)), } planner = 'ff-ehc' #planner = 'ff-wastar1000' # Branching factor becomes large. Rely on preferred. Preferred should also be cheaper #planner = 'ff-eager-wastar1000' #planner = 'ff-wastar5' solution = solve_focused( pddlstream_problem, stream_info=stream_info, max_time=600, effort_weight=1, unit_efforts=True, use_skeleton=False, #unit_costs=True, planner=planner, max_planner_time=15, debug=True) # solve_exhaustive | solve_incremental # Reachability heuristics good for detecting dead-ends # Infeasibility from the start means disconnected or collision # Random restart based strategy here print_solution(solution) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) plan, _, _ = solution if plan is None: return None if pddlstream_fn == get_pddlstream: return [t for _, (n1, e, t, n2) in plan] elif pddlstream_fn == get_pddlstream2: return [t for _, (n1, e, t) in reversed(plan)] else: raise ValueError(pddlstream_fn)
def main(): # TODO: maybe load problems as a domain explicitly pddlstream_problem = get_pddlstream_problem() stream_info = { #'test-feasible': StreamInfo(negate=True), } solution = solve_focused(pddlstream_problem, stream_info=stream_info) #solution = solve_incremental(pddlstream_problem) # Should throw an error print_solution(solution)
def solve_pddlstream(focused=True): problem_fn = get_problem1 # get_problem1 | get_problem2 pddlstream_problem = problem_fn() print('Init:', pddlstream_problem.init) print('Goal:', pddlstream_problem.goal) if focused: solution = solve_focused(pddlstream_problem, unit_costs=True) else: solution = solve_incremental(pddlstream_problem, unit_costs=True) print_solution(solution)
def plan_commands(state, teleport=False, profile=False, verbose=True): # TODO: could make indices into set of bodies to ensure the same... # TODO: populate the bodies here from state and not the real world task = state.task robot_conf = get_configuration(task.robot) robot_pose = get_pose(task.robot) sim_world = connect(use_gui=False) with ClientSaver(sim_world): with HideOutput(): robot = create_pr2(use_drake=USE_DRAKE_PR2) #robot = load_model(DRAKE_PR2_URDF, fixed_base=True) set_pose(robot, robot_pose) set_configuration(robot, robot_conf) clone_world(client=sim_world, exclude=[task.robot]) set_client(sim_world) saved_world = WorldSaver() # StateSaver() pddlstream_problem = pddlstream_from_state(state, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) if verbose: print('Goal:', goal) print('Streams:', stream_map.keys()) stream_info = { 'test-vis-base': StreamInfo(eager=True, p_success=0), 'test-reg-base': StreamInfo(eager=True, p_success=0), } hierarchy = [ ABSTRIPSLayer(pos_pre=['AtBConf']), ] pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, stream_info=stream_info, hierarchy=hierarchy, debug=False, max_cost=MAX_COST, verbose=verbose) pr.disable() plan, cost, evaluations = solution if MAX_COST <= cost: plan = None print_solution(solution) print('Finite cost:', cost < MAX_COST) if profile: pstats.Stats(pr).sort_stats('tottime').print_stats(10) saved_world.restore() commands = post_process(state, plan) disconnect() return commands
def main(max_time=20): """ Creates and solves the 2D motion planning problem. """ obstacles = [create_box((.5, .5), (.2, .2))] regions = { 'env': create_box((.5, .5), (1, 1)), 'green': create_box((.8, .8), (.4, .4)), } goal = 'green' if goal not in regions: goal = array([1, 1]) max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0 problem, samples, roadmap = create_problem(goal, obstacles, regions, max_distance=max_distance) print('Initial:', str_from_object(problem.init)) print('Goal:', str_from_object(problem.goal)) pr = cProfile.Profile() pr.enable() solution = solve_incremental(problem, unit_costs=False, max_cost=0, max_time=max_time, verbose=False) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) print_solution(solution) plan, cost, evaluations = solution #viewer = draw_environment(obstacles, regions) #for sample in samples: # viewer.draw_point(sample) #user_input('Continue?') # TODO: use the same viewer here draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime user_input('Continue?') if plan is None: return segments = [args for name, args in plan] draw_solution(segments, obstacles, regions) user_input('Finish?')
def main(focused=True, unit_costs=False): problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem tamp_problem = problem_fn() print(tamp_problem) pddlstream_problem = pddlstream_from_tamp(tamp_problem) if focused: solution = solve_focused(pddlstream_problem, unit_costs=unit_costs) else: solution = solve_incremental(pddlstream_problem, unit_costs=unit_costs) print_solution(solution) plan, cost, evaluations = solution if plan is None: return apply_plan(tamp_problem, plan)
def plan_sequence_test(node_points, elements, ground_nodes): pr = cProfile.Profile() pr.enable() pddlstream_problem = get_pddlstream_test(node_points, elements, ground_nodes) #solution = solve_focused(pddlstream_problem, planner='goal-lazy', max_time=10, debug=False) solution = solve_exhaustive(pddlstream_problem, planner='goal-lazy', max_time=10, debug=False) print_solution(solution) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) plan, _, _ = solution return plan
def main(focused=True): # TODO: maybe load problems as a domain explicitly pddlstream_problem = pddlstream_from_belief() _, _, _, _, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) print('Goal:', goal) pr = cProfile.Profile() pr.enable() if focused: solution = solve_focused(pddlstream_problem, unit_costs=False) else: #solution = solve_exhaustive(pddlstream_problem, unit_costs=False) solution = solve_incremental(pddlstream_problem, unit_costs=False) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(5) print_solution(solution)
def solve_pddlstream(): pddlstream_problem, namo = get_problem() namo.env.SetViewer('qtcoin') stime = time.time() #solution = solve_incremental(pddlstream_problem, unit_costs=True, max_time=500) solution = solve_focused(pddlstream_problem, unit_costs=True, max_time=500) search_time = time.time()-stime plan, cost, evaluations = solution print "Search time", search_time if solution[0] is None: print "No Solution" sys.exit(-1) print_solution(solution) process_plan(plan, namo) namo.problem_config['env'].Destroy() openravepy.RaveDestroy() return plan, search_time
def main(max_time=20): """ Creates and solves the 2D motion planning problem. """ obstacles = [create_box((.5, .5), (.2, .2))] regions = { 'env': create_box((.5, .5), (1, 1)), #'goal': create_box((.8, .8), (.4, .4)), } goal = np.array([1, 1]) #goal = 'goal' max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0 problem, roadmap = create_problem(goal, obstacles, regions, max_distance=max_distance) pr = cProfile.Profile() pr.enable() solution = solve_incremental(problem, unit_costs=False, max_cost=0, max_time=max_time, verbose=False) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) print_solution(solution) plan, cost, evaluations = solution print('Plan:', plan) if plan is None: return # TODO: use the same viewer here draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime user_input('Continue?') segments = [args for name, args in plan] draw_solution(segments, obstacles, regions) user_input('Finish?')
def main(): initial_poses = { ROBOT: (0., 15., 0.), CUP: (7.5, 0., 0.), 'sugar_cup': (-10., 0., 0.), 'cream_cup': (15., 0, 0), 'spoon': (0.5, 0.5, 0), 'stirrer': (20, 0.5, 0), COASTER: (-20., 0, 0), } problem = create_problem(initial_poses) pr = cProfile.Profile() pr.enable() solution = solve_focused(problem, unit_costs=True, planner='ff-eager', debug=True) # max_planner_time=5, pr.disable() print_solution(solution) pstats.Stats(pr).sort_stats('tottime').print_stats(10)
def main(viewer=True): connect(use_gui=viewer) robot, brick_from_index, obstacle_from_name = load_pick_and_place( 'choreo_brick_demo') # choreo_brick_demo | choreo_eth-trees_demo np.set_printoptions(precision=2) pr = cProfile.Profile() pr.enable() with WorldSaver(): pddlstream_problem = get_pddlstream(robot, brick_from_index, obstacle_from_name) 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 step_plan(plan)
def main(focused=True, unit_costs=False): problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem tamp_problem = problem_fn() print(tamp_problem) stream_info = { 'test-cfree': StreamInfo(negate=True), } pddlstream_problem = pddlstream_from_tamp(tamp_problem) if focused: #solution = solve_execution(pddlstream_problem, unit_costs=unit_costs, stream_info=stream_info) solution = solve_focused(pddlstream_problem, unit_costs=unit_costs, stream_info=stream_info, debug=False) else: solution = solve_exhaustive(pddlstream_problem, unit_costs=unit_costs) print_solution(solution) plan, cost, evaluations = solution if plan is None: return apply_plan(tamp_problem, plan)
def main(): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) # starting objects start_conf = np.array([0, 5]) p0 = np.array([0, 0]) goal_region = np.array([5, 10]) b0 = 'block0' ground = np.array([-5, 15]) initial_atoms = [('CanMove', ), ('Conf', start_conf), ('AtConf', start_conf), ('HandEmpty', ), ('Region', goal_region), ('Block', b0), ('Pose', b0, p0), ('AtPose', b0, p0), ('Region', ground), ('Placeable', b0, ground), ('Placeable', b0, goal_region)] goal_literals = [('HandEmpty', ), ('In', b0, goal_region), ('AtConf', start_conf)] goal_formula = And(*goal_literals) ## environments # The pose of block is the center of bottom line # The pose of the gripper is the center of mass BLOCK_WIDTH = 2 BLOCK_HEIGHT = BLOCK_WIDTH GRIPPER_WIDTH = 2 GRIPPER_HEIGHT = GRIPPER_WIDTH GRASP = -np.array([0, BLOCK_HEIGHT + GRIPPER_HEIGHT / 2]) def distance(q1, q2): return int(np.linalg.norm(q1 - q2)) def posecollision(blk1, pose1, blk2, pose2): i1 = pose1[0] * np.ones(2) + np.array([-BLOCK_WIDTH, +BLOCK_WIDTH ]) / 2. i2 = pose2[0] * np.ones(2) + np.array([-BLOCK_WIDTH, +BLOCK_WIDTH ]) / 2. return (i2[0] <= i1[1]) and (i1[0] <= i2[1]) def sample_pose(blk, region): return ((np.random.uniform(region[0] + BLOCK_WIDTH / 2, region[1] - BLOCK_WIDTH / 2), 0), ) def inverse_kinematics(blk, pose): return (pose - GRASP, ) def plan_motion(conf1, conf2): traj = [conf1, conf2] return (traj, ) constant_map = {} # Upper case does not matter at all stream_map = { 'distance': distance, 'posecollision': posecollision, 'sample-pose': from_fn(sample_pose), 'inverse-kinematics': from_fn(inverse_kinematics), 'plan-motion': from_fn(plan_motion) } # A pddlstream problem problem = domain_pddl, constant_map, stream_pddl, stream_map, initial_atoms, goal_formula # solution = solve_incremental(problem, unit_costs=False) solution = solve_incremental(problem, unit_costs=False) # plan, cost, evaluations = solution print_solution(solution)
def main(viewer=False, display=True, simulate=False, teleport=False): # TODO: fix argparse & FastDownward #parser = argparse.ArgumentParser() # Automatically includes help #parser.add_argument('-viewer', action='store_true', help='enable viewer.') #parser.add_argument('-display', action='store_true', help='enable viewer.') #args = parser.parse_args() connect(use_gui=viewer) robot, names, movable = load_world() saved_world = WorldSaver() dump_world() pddlstream_problem = pddlstream_from_problem(robot, movable=movable, teleport=teleport, movable_collisions=True) _, _, _, stream_map, init, goal = pddlstream_problem synthesizers = [ #StreamSynthesizer('safe-free-motion', {'plan-free-motion': 1, 'trajcollision': 0}, # from_fn(get_free_motion_synth(robot, movable, teleport))), #StreamSynthesizer('safe-holding-motion', {'plan-holding-motion': 1, 'trajcollision': 0}, # from_fn(get_holding_motion_synth(robot, movable, teleport))), ] 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, synthesizers=synthesizers, max_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 paths = [] for name, args in plan: if name == 'place': paths += args[-1].reverse().body_paths elif name in ['move', 'move_free', 'move_holding', 'pick']: paths += args[-1].body_paths print(paths) command = Command(paths) if not viewer: # TODO: how to reenable the viewer disconnect() connect(use_gui=True) load_world() else: saved_world.restore() user_input('Execute?') if simulate: command.control() else: #command.step() command.refine(num_steps=10).execute(time_step=0.001) #wait_for_interrupt() user_input('Finish?') disconnect()
def main(viewer=False, display=True, simulate=False, teleport=False): # TODO: fix argparse & FastDownward #parser = argparse.ArgumentParser() # Automatically includes help #parser.add_argument('-viewer', action='store_true', help='enable viewer.') #parser.add_argument('-display', action='store_true', help='enable viewer.') #args = parser.parse_args() connect(use_gui=viewer) problem_fn = cooking_problem # holding_problem | stacking_problem | cleaning_problem | cooking_problem # cleaning_button_problem | cooking_button_problem with HideOutput(): problem = problem_fn() state_id = save_state() #saved_world = WorldSaver() #dump_world() pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport) stream_info = { 'sample-pose': StreamInfo(from_fn(opt_pose_fn)), 'inverse-kinematics': StreamInfo(from_fn(opt_ik_fn)), 'plan-base-motion': StreamInfo(from_fn(opt_motion_fn)), 'MoveCost': FunctionInfo(opt_move_cost_fn), } synthesizers = [ StreamSynthesizer('safe-base-motion', {'plan-base-motion': 1, 'TrajPoseCollision': 0, 'TrajGraspCollision': 0, 'TrajArmCollision': 0}, from_fn(get_base_motion_synth(problem, teleport))), ] if USE_SYNTHESIZERS else [] _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) print('Synthesizers:', synthesizers) pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, stream_info=stream_info, synthesizers=synthesizers, max_cost=INF) print_solution(solution) plan, cost, evaluations = solution print('Real cost:', float(cost)/SCALE_COST) 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 commands = post_process(problem, plan) if viewer: restore_state(state_id) else: disconnect() connect(use_gui=True) with HideOutput(): problem_fn() # TODO: way of doing this without reloading? if simulate: enable_gravity() control_commands(commands) else: step_commands(commands, time_step=0.01) user_input('Finish?') disconnect()
def main(focused=True, deterministic=False, unit_costs=False): np.set_printoptions(precision=2) if deterministic: seed = 0 np.random.seed(seed) print('Seed:', get_random_seed()) problem_fn = get_blocked_problem # get_tight_problem | get_blocked_problem tamp_problem = problem_fn() print(tamp_problem) action_info = { #'move': ActionInfo(terminal=True), #'pick': ActionInfo(terminal=True), #'place': ActionInfo(terminal=True), } stream_info = { #'test-region': StreamInfo(eager=True, p_success=0), # bound_fn is None #'plan-motion': StreamInfo(p_success=1), # bound_fn is None #'trajcollision': StreamInfo(p_success=1), # bound_fn is None #'cfree': StreamInfo(eager=True), } dynamic = [ StreamSynthesizer('cfree-motion', { 'plan-motion': 1, 'trajcollision': 0 }, gen_fn=from_fn(cfree_motion_fn)), #StreamSynthesizer('optimize', {'sample-pose': 1, 'inverse-kinematics': 1, # 'posecollision': 0, 'distance': 0}, # gen_fn=from_fn(get_optimize_fn(tamp_problem.regions))), ] pddlstream_problem = pddlstream_from_tamp(tamp_problem) pr = cProfile.Profile() pr.enable() if focused: solution = solve_focused(pddlstream_problem, action_info=action_info, stream_info=stream_info, synthesizers=dynamic, max_time=10, max_cost=INF, debug=False, effort_weight=None, unit_costs=unit_costs, postprocess=False, visualize=False) else: solution = solve_incremental(pddlstream_problem, layers=1, unit_costs=unit_costs) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is None: return colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS)) viewer = ContinuousTMPViewer(tamp_problem.regions, title='Continuous TAMP') state = tamp_problem.initial print() print(state) draw_state(viewer, state, colors) for i, action in enumerate(plan): user_input('Continue?') print(i, *action) state = apply_action(state, action) print(state) draw_state(viewer, state, colors) user_input('Finish?')
def main(deterministic=True): # TODO: GeometryInstance, InternalGeometry, & GeometryContext to get the shape of objects # TODO: cost-sensitive planning to avoid large kuka moves # get_contact_results_output_port # TODO: gripper closing via collision information time_step = 0.0002 # TODO: context.get_continuous_state_vector() fails if deterministic: random.seed(0) np.random.seed(0) parser = argparse.ArgumentParser() parser.add_argument('-p', '--problem', default='load_manipulation', help='The name of the problem to solve.') parser.add_argument('-c', '--cfree', action='store_true', help='Disables collisions') parser.add_argument('-v', '--visualizer', action='store_true', help='Use the drake visualizer') parser.add_argument('-s', '--simulate', action='store_true', help='Simulate') args = parser.parse_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) print('Problem:', args.problem) problem_fn = problem_fn_from_name[args.problem] meshcat_vis = None if not args.visualizer: import meshcat # Important that variable is saved meshcat_vis = meshcat.Visualizer() # vis.set_object # http://127.0.0.1:7000/static/ mbp, scene_graph, task = problem_fn(time_step=time_step) #station, mbp, scene_graph = load_station(time_step=time_step) #builder.AddSystem(station) #dump_plant(mbp) #dump_models(mbp) print(task) #print(sorted(body.name() for body in task.movable_bodies())) #print(sorted(body.name() for body in task.fixed_bodies())) ################################################## builder, _ = build_diagram(mbp, scene_graph, not args.visualizer) if args.simulate: state_machine = connect_controllers(builder, mbp, task.robot, task.gripper) else: state_machine = None diagram = builder.Build() RenderSystemWithGraphviz(diagram) # Useful for getting port names diagram_context = diagram.CreateDefaultContext() context = diagram.GetMutableSubsystemContext(mbp, diagram_context) task.diagram = diagram task.diagram_context = diagram_context #context = mbp.CreateDefaultContext() # scene_graph.CreateDefaultContext() for joint, position in task.initial_positions.items(): set_joint_position(joint, context, position) for model, pose in task.initial_poses.items(): set_world_pose(mbp, context, model, pose) open_wsg50_gripper(mbp, context, task.gripper) #close_wsg50_gripper(mbp, context, task.gripper) #set_configuration(mbp, context, task.gripper, [-0.05, 0.05]) # from underactuated.meshcat_visualizer import MeshcatVisualizer # #add_meshcat_visualizer(scene_graph) # viz = MeshcatVisualizer(scene_graph, draw_timestep=0.033333) # viz.load() # viz.draw(context) diagram.Publish(diagram_context) initial_state = get_state(mbp, context) ################################################## problem = get_pddlstream_problem(mbp, context, scene_graph, task, collisions=not args.cfree) pr = cProfile.Profile() pr.enable() solution = solve_focused(problem, planner='ff-wastar2', max_cost=INF, max_time=120, debug=False) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) print_solution(solution) plan, cost, evaluations = solution if plan is None: return trajectories = postprocess_plan(mbp, task.gripper, plan) ################################################## set_state(mbp, context, initial_state) if args.simulate: splines, gripper_setpoints = convert_splines(mbp, task.robot, task.gripper, context, trajectories) sim_duration = compute_duration(splines) print('Splines: {}\nDuration: {:.3f} seconds'.format(len(splines), sim_duration)) set_state(mbp, context, initial_state) if True: state_machine.Load(splines, gripper_setpoints) simulate_splines(diagram, diagram_context, sim_duration) else: # NOTE: there is a plan that moves home initially for 15 seconds from .lab_1.robot_plans import JointSpacePlan plan_list = [JointSpacePlan(spline) for spline in splines] #meshcat_vis.delete() user_input('Simulate?') test_manipulation(plan_list, gripper_setpoints) else: #time_step = None #time_step = 0.001 time_step = 0.02 step_trajectories(diagram, diagram_context, context, trajectories, time_step=time_step) #, teleport=True)
def main(focused=True, deterministic=True, unit_costs=False, use_synthesizers=False): np.set_printoptions(precision=2) if deterministic: seed = 0 np.random.seed(seed) print('Seed:', get_random_seed()) if use_synthesizers and not has_gurobi(): use_synthesizers = False print( 'Warning! use_synthesizers=True requires gurobipy. Setting use_synthesizers=False.' ) print('Focused: {} | Costs: {} | Synthesizers: {}'.format( focused, not unit_costs, use_synthesizers)) problem_fn = get_blocked_problem # get_tight_problem | get_blocked_problem tamp_problem = problem_fn() print(tamp_problem) action_info = { #'move': ActionInfo(terminal=True), #'pick': ActionInfo(terminal=True), #'place': ActionInfo(terminal=True), } stream_info = { 't-region': StreamInfo(eager=True, p_success=0), # bound_fn is None 't-cfree': StreamInfo(eager=False, negate=True), #'distance': FunctionInfo(opt_fn=lambda *args: 1), #'gurobi': OptimizerInfo(p_success=0), #'rrt': OptimizerInfo(p_success=0), } hierarchy = [ #ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1), ] synthesizers = [ #StreamSynthesizer('cfree-motion', {'s-motion': 1, 'trajcollision': 0}, # gen_fn=from_fn(cfree_motion_fn)), StreamSynthesizer('optimize', { 's-region': 1, 's-ik': 1, 'posecollision': 0, 't-cfree': 0, 'distance': 0 }, gen_fn=from_fn(get_optimize_fn( tamp_problem.regions))), ] if use_synthesizers else [] pddlstream_problem = pddlstream_from_tamp(tamp_problem) print('Initial:', str_from_object(pddlstream_problem.init)) print('Goal:', str_from_object(pddlstream_problem.goal)) pr = cProfile.Profile() pr.enable() if focused: solution = solve_focused( pddlstream_problem, action_info=action_info, stream_info=stream_info, planner='ff-wastar1', max_planner_time=10, synthesizers=synthesizers, verbose=True, max_time=300, max_cost=INF, debug=False, hierarchy=hierarchy, effort_weight=1, search_sampling_ratio=0, # TODO: run without to see difference unit_costs=unit_costs, postprocess=False, visualize=False) else: solution = solve_incremental(pddlstream_problem, layers=1, hierarchy=hierarchy, unit_costs=unit_costs, verbose=False) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is not None: display_plan(tamp_problem, plan)
def main(viewer=False, display=True, simulate=False, teleport=False): # TODO: fix argparse & FastDownward # parser = argparse.ArgumentParser() # Automatically includes help # parser.add_argument('-viewer', action='store_true', help='enable viewer.') # parser.add_argument('-display', action='store_true', help='enable viewer.') # args = parser.parse_args() # TODO: getopt connect(use_gui=viewer) robot, names, movable = load_world() saved_world = WorldSaver() # dump_world() pddlstream_problem = pddlstream_from_problem(robot, movable=movable, teleport=teleport, movable_collisions=True) _, _, _, stream_map, init, goal = pddlstream_problem synthesizers = [ StreamSynthesizer( 'safe-free-motion', { 'plan-free-motion': 1, 'trajcollision': 0 }, from_fn(get_free_motion_synth(robot, movable, teleport))), StreamSynthesizer( 'safe-holding-motion', { 'plan-holding-motion': 1, 'trajcollision': 0 }, from_fn(get_holding_motion_synth(robot, movable, teleport))), ] if USE_SYNTHESIZERS else [] print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) print('Synthesizers:', stream_map.keys()) print('Names:', names) # initialize and measure performance pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, synthesizers=synthesizers, max_cost=INF) print_solution(solution) plan, cost, evaluations = solution pr.disable() # print stats 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 viewer: # TODO: how to reenable the viewer disconnect() connect(use_gui=True) load_world() else: saved_world.restore() command = postprocess_plan(plan) #user_input('Execute?') if simulate: command.control() else: # command.step() command.refine(num_steps=10).execute(time_step=0.001) # wait_for_interrupt() #user_input('Finish?') disconnect()