def pddlstream_from_problem(problem, teleport=False, movable_collisions=False): robot = problem.robot domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = { 'world': 'world', } world = 'world' initial_bq = Pose(robot, get_pose(robot)) init = [ ('CanMove',), ('BConf', initial_bq), # TODO: could make pose as well... ('AtBConf', initial_bq), ('AtAConf', world, None), ('AtPose', world, world, None), ] + [('Sink', s) for s in problem.sinks] + \ [('Stove', s) for s in problem.stoves] + \ [('Connected', b, d) for b, d in problem.buttons] + \ [('Button', b) for b, _ in problem.buttons] #for arm in ARM_JOINT_NAMES: for arm in problem.arms: joints = get_arm_joints(robot, arm) conf = Conf(robot, joints, get_joint_positions(robot, joints)) init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm), ('AtAConf', arm, conf), ('AtPose', arm, arm, None)] if arm in problem.arms: init += [('Controllable', arm)] for body in problem.movable: pose = Pose(body, get_pose(body)) init += [('Graspable', body), ('Pose', body, pose), ('AtPose', world, body, pose)] for surface in problem.surfaces: init += [('Stackable', body, surface)] if is_placement(body, surface): init += [('Supported', body, pose, surface)] goal = ['and'] if problem.goal_conf is not None: goal_conf = Pose(robot, problem.goal_conf) init += [('BConf', goal_conf)] goal += [('AtBConf', goal_conf)] goal += [('Holding', a, b) for a, b in problem.goal_holding] + \ [('On', b, s) for b, s in problem.goal_on] + \ [('Cleaned', b) for b in problem.goal_cleaned] + \ [('Cooked', b) for b in problem.goal_cooked] stream_map = { 'sample-pose': get_stable_gen(problem), 'sample-grasp': from_list_fn(get_grasp_gen(problem)), 'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)), 'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)), #'plan-base-motion': empty_gen(), 'BTrajCollision': fn_from_constant(False), } # get_press_gen(problem, teleport=teleport) return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def create_static_stream(stream, evaluations, fluent_predicates, get_future): def static_fn(*input_values): instance = stream.get_instance(tuple(map(Object.from_value, input_values))) if all(evaluation_from_fact(f) in evaluations for f in instance.get_domain()): return None return tuple(FutureValue(stream.name, input_values, o) for o in stream.outputs) #opt_evaluations = None def static_opt_gen_fn(*input_values): instance = stream.get_instance(tuple(map(Object.from_value, input_values))) if all(evaluation_from_fact(f) in evaluations for f in instance.get_domain()): return for output_values in stream.opt_gen_fn(*input_values): yield output_values # TODO: need to replace regular opt_gen_fn to update opt_evaluations # if I want to prevent switch from normal to static in opt # Focused algorithm naturally biases against using future because of axiom layers stream_name = 'future-{}'.format(stream.name) gen_fn = from_fn(static_fn) static_domain = list(filter(lambda a: get_prefix(a) not in fluent_predicates, stream.domain)) new_domain = list(map(get_future, static_domain)) stream_atom = ('{}-result'.format(stream.name),) + tuple(stream.inputs + stream.outputs) new_certified = [stream_atom] + list(map(get_future, stream.certified)) static_stream = Stream(stream_name, gen_fn, stream.inputs, new_domain, stream.outputs, new_certified, stream.info) static_stream.opt_gen_fn = static_opt_gen_fn return static_stream
def pddlstream_from_problem(robot, movable=[], teleport=False, movable_collisions=False, grasp_name='top'): #assert (not are_colliding(tree, kin_cache)) domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} print('Robot:', robot) conf = BodyConf(robot, get_configuration(robot)) init = [('CanMove', ), ('Conf', conf), ('AtConf', conf), ('HandEmpty', )] fixed = get_fixed(robot, movable) print('Movable:', movable) print('Fixed:', fixed) for body in movable: pose = BodyPose(body, get_pose(body)) init += [('Graspable', body), ('Pose', body, pose), ('AtPose', body, pose)] for surface in fixed: init += [('Stackable', body, surface)] if is_placement(body, surface): init += [('Supported', body, pose, surface)] for body in fixed: name = get_body_name(body) if 'sink' in name: init += [('Sink', body)] if 'stove' in name: init += [('Stove', body)] body = movable[0] goal = ( 'and', ('AtConf', conf), #('Holding', body), #('On', body, fixed[1]), #('On', body, fixed[2]), #('Cleaned', body), ('Cooked', body), ) stream_map = { 'sample-pose': from_gen_fn(get_stable_gen(fixed)), 'sample-grasp': from_gen_fn(get_grasp_gen(robot, grasp_name)), 'inverse-kinematics': from_fn(get_ik_fn(robot, fixed, teleport)), #'plan-free-motion': from_fn(get_free_motion_gen(robot, fixed, teleport)), 'plan-free-motion': empty_gen(), # 'plan-holding-motion': from_fn(get_holding_motion_gen(robot, fixed, teleport)), 'plan-holding-motion': empty_gen(), 'TrajCollision': get_movable_collision_test(), } return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
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 to_pddlstream(belief_problem, collisions=True): locations = {l for (_, l, _) in belief_problem.initial + belief_problem.goal} | \ set(belief_problem.locations) observations = [True, False] uniform = UniformDist(locations) initial_bel = { o: MixtureDD(DeltaDist(l), uniform, p) for o, l, p in belief_problem.initial } max_p_collision = 0.25 if collisions else 1.0 # TODO: separate pick and place for move init = [('Obs', obs) for obs in observations] + \ [('Location', l) for l in locations] for o, d in initial_bel.items(): init += [('Dist', o, d), ('BLoc', o, d)] for (o, l, p) in belief_problem.goal: init += [('Location', l), ('GoalProb', l, p)] goal_literals = [('BLocGE', o, l, p) for (o, l, p) in belief_problem.goal] goal = And(*goal_literals) directory = os.path.dirname(os.path.abspath(__file__)) domain_pddl = read(os.path.join(directory, 'domain.pddl')) stream_pddl = read(os.path.join(directory, 'stream.pddl')) constant_map = {} stream_map = { 'BCollision': get_collision_test(max_p_collision), 'GE': from_test(ge_fn), 'prob-after-move': from_fn(get_move_fn(belief_problem.p_move_s)), 'MoveCost': move_cost_fn, 'prob-after-look': from_fn(get_look_fn(belief_problem.p_look_fp, belief_problem.p_look_fn)), 'LookCost': get_look_cost_fn(belief_problem.p_look_fp, belief_problem.p_look_fn), #'PCollision': from_fn(prob_occupied), # Then can use GE } return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_tamp(tamp_problem): initial = tamp_problem.initial assert (initial.holding is None) domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} init = [ ('CanMove',), ('Conf', initial.conf), ('AtConf', initial.conf), ('HandEmpty',), Equal((TOTAL_COST,), 0)] + \ [('Block', b) for b in initial.block_poses.keys()] + \ [('Pose', b, p) for b, p in initial.block_poses.items()] + \ [('Region', r) for r in tamp_problem.regions.keys()] + \ [('AtPose', b, p) for b, p in initial.block_poses.items()] + \ [('Placeable', b, GROUND) for b in initial.block_poses.keys()] + \ [('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()] goal_literals = [('HandEmpty',)] + \ [('In', b, r) for b, r in tamp_problem.goal_regions.items()] if tamp_problem.goal_conf is not None: goal_literals += [('AtConf', tamp_problem.goal_conf)] goal = And(*goal_literals) stream_map = { 'plan-motion': from_fn(plan_motion), 'sample-pose': from_gen_fn(get_pose_gen(tamp_problem.regions)), 'test-region': from_test(get_region_test(tamp_problem.regions)), 'inverse-kinematics': from_fn(inverse_kin_fn), 'collision-free': from_test(lambda *args: not collision_test(*args)), 'cfree': lambda *args: not collision_test(*args), 'posecollision': collision_test, 'trajcollision': lambda *args: False, 'distance': distance_fn, #'Valid': valid_state_fn, } #stream_map = 'debug' return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_tamp(tamp_problem): initial = tamp_problem.initial assert (initial.holding is None) known_poses = list(initial.block_poses.values()) + \ list(tamp_problem.goal_poses.values()) directory = os.path.dirname(os.path.abspath(__file__)) domain_pddl = read(os.path.join(directory, 'domain.pddl')) stream_pddl = read(os.path.join(directory, 'stream.pddl')) q100 = np.array([100, 100]) constant_map = { 'q100': q100, } init = [ #Type(q100, 'conf'), ('CanMove',), ('Conf', q100), ('Conf', initial.conf), ('AtConf', initial.conf), ('HandEmpty',), Equal((TOTAL_COST,), 0)] + \ [('Block', b) for b in initial.block_poses.keys()] + \ [('Pose', p) for p in known_poses] + \ [('AtPose', b, p) for b, p in initial.block_poses.items()] # [('Pose', p) for p in known_poses + tamp_problem.poses] + \ goal = And(*[('AtPose', b, p) for b, p in tamp_problem.goal_poses.items()]) def collision_test(p1, p2): return np.linalg.norm(p2 - p1) <= 1e-1 def distance_fn(q1, q2): ord = 1 # 1 | 2 return int(math.ceil(np.linalg.norm(q2 - q1, ord=ord))) # TODO: convert to lower case stream_map = { #'sample-pose': from_gen_fn(lambda: ((np.array([x, 0]),) for x in range(len(poses), n_poses))), 'sample-pose': from_gen_fn(lambda: ((p, ) for p in tamp_problem.poses)), 'inverse-kinematics': from_fn(lambda p: (p + GRASP, )), #'inverse-kinematics': IKGenerator, #'inverse-kinematics': IKFactGenerator, 'collision-free': from_test(lambda *args: not collision_test(*args)), 'collision': collision_test, #'constraint-solver': None, 'distance': distance_fn, } return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_belief(): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) constant_map = {} stream_pddl = read(get_file_path(__file__, 'stream.pddl')) stream_map = { 'inv-kin': from_fn(lambda p: (p + np.array([0, 1]), )), 'motion': from_fn(lambda q1, q2: ([t * (q2 - q1) + q1 for t in [0, 1]], )), # linspace, arange } # Options # - observation produces one of several poses # - always at the pose, observation just makes it observable # - object has a unobserved fluent block = 'block1' pose = None # Unknown #pose = np.array([1, 0]) conf = np.array([0, 1]) booth = np.array([0, 2]) init = [ ('Initial', ), # Forces move first ('Conf', conf), ('Conf', booth), ('Booth', booth), ('AtConf', conf), ('HandEmpty', ), ('Block', block), ('Pose', pose), ('AtPose', block, pose), #('Observable', pose), #('Latent', pose), ] goal = ('Holding', block) return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
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(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 problem = problem_fn() state_id = p.saveState() #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): p.disconnect() return if viewer: p.restoreState(state_id) else: disconnect() connect(use_gui=True) problem = problem_fn() # TODO: way of doing this without reloading? 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 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): p.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() 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 pddlstream_from_state(state, teleport=False): task = state.task robot = task.robot # TODO: infer open world from task domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = { 'base': 'base', 'left': 'left', 'right': 'right', 'head': 'head', } #base_conf = state.poses[robot] base_conf = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base')) scan_cost = 1 init = [ ('BConf', base_conf), ('AtBConf', base_conf), Equal(('MoveCost', ), scale_cost(1)), Equal(('PickCost', ), scale_cost(1)), Equal(('PlaceCost', ), scale_cost(1)), Equal(('ScanCost', ), scale_cost(scan_cost)), Equal(('RegisterCost', ), scale_cost(1)), ] holding_arms = set() holding_bodies = set() for attach in state.attachments.values(): holding_arms.add(attach.arm) holding_bodies.add(attach.body) init += [('Grasp', attach.body, attach.grasp), ('AtGrasp', attach.arm, attach.body, attach.grasp)] for arm in ARM_NAMES: joints = get_arm_joints(robot, arm) conf = Conf(robot, joints, get_joint_positions(robot, joints)) init += [('Arm', arm), ('AConf', arm, conf), ('AtAConf', arm, conf)] if arm in task.arms: init += [('Controllable', arm)] if arm not in holding_arms: init += [('HandEmpty', arm)] for body in task.get_bodies(): if body in holding_bodies: continue # TODO: no notion whether observable actually corresponds to the correct thing pose = state.poses[body] init += [ ('Pose', body, pose), ('AtPose', body, pose), ('Observable', pose), ] for body in task.movable: init += [('Graspable', body)] for body in task.get_bodies(): supports = task.get_supports(body) if supports is None: continue for surface in supports: p_obs = state.b_on[body].prob(surface) cost = revisit_mdp_cost(0, scan_cost, p_obs) # TODO: imperfect observation model init += [('Stackable', body, surface), Equal(('LocalizeCost', surface, body), scale_cost(cost))] #if is_placement(body, surface): if is_center_stable(body, surface): if body in holding_bodies: continue pose = state.poses[body] init += [('Supported', body, pose, surface)] for body in task.get_bodies(): if state.is_localized(body): init.append(('Localized', body)) else: init.append(('Uncertain', body)) if body in state.registered: init.append(('Registered', body)) goal = And(*[('Holding', a, b) for a, b in task.goal_holding] + \ [('On', b, s) for b, s in task.goal_on] + \ [('Localized', b) for b in task.goal_localized] + \ [('Registered', b) for b in task.goal_registered]) stream_map = { 'sample-pose': get_stable_gen(task), 'sample-grasp': from_list_fn(get_grasp_gen(task)), 'inverse-kinematics': from_gen_fn(get_ik_ir_gen(task, teleport=teleport)), 'plan-base-motion': from_fn(get_motion_gen(task, teleport=teleport)), 'inverse-visibility': from_gen_fn(get_vis_gen(task)), 'plan-scan': from_gen_fn(get_scan_gen(state)), } return Problem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)