def pddlstream_from_problem(robot, movable=[], teleport=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-holding-motion': from_fn(get_holding_motion_gen(robot, fixed, teleport)), 'test-cfree-pose-pose': from_test(get_cfree_pose_pose_test()), 'test-cfree-approach-pose': from_test(get_cfree_obj_approach_pose_test()), 'test-cfree-traj-pose': from_test(negate_test(get_movable_collision_test())), #get_cfree_traj_pose_test()), 'TrajCollision': get_movable_collision_test(), } return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def pddlstream_from_problem(problem): # TODO: push and attach to movable objects domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} # TODO: action to generically connect to the roadmap # TODO: could check individual vertices first # TODO: dynamically generate the roadmap in interesting parts of the space # TODO: visibility graphs for sparse roadmaps # TODO: approximate robot with isotropic geometry # TODO: make the effort finite if applied to the roadmap vertex samples = [] init = [] for robot, conf in problem.initial_confs.items(): samples.append(conf) init += [('Robot', robot), ('Conf', robot, conf), ('AtConf', robot, conf), ('Free', robot)] for body, pose in problem.initial_poses.items(): init += [('Body', body), ('Pose', body, pose), ('AtPose', body, pose)] goal_literals = [] goal_literals += [('Holding', robot, body) for robot, body in problem.goal_holding.items()] for robot, base_values in problem.goal_confs.items(): q_goal = Conf(robot, get_base_joints(robot), base_values) samples.append(q_goal) init += [('Conf', robot, q_goal)] goal_literals += [('AtConf', robot, q_goal)] goal_formula = And(*goal_literals) # TODO: assuming holonomic for now [body] = problem.robots with LockRenderer(): init += create_vertices(problem, body, samples) #init += create_edges(problem, body, samples) stream_map = { 'test-cfree-conf-pose': from_test(get_test_cfree_conf_pose(problem)), 'test-cfree-traj-pose': from_test(get_test_cfree_traj_pose(problem)), # TODO: sample pushes rather than picks/places 'sample-grasp': from_gen_fn(get_grasp_generator(problem)), 'compute-ik': from_fn(get_ik_fn(problem)), 'compute-motion': from_fn(get_motion_fn(problem)), 'test-reachable': from_test(lambda *args: False), 'Cost': get_cost_fn(problem), } #stream_map = 'debug' return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
def pddlstream_from_tamp(tamp_problem, use_stream=True, use_optimizer=False, collisions=True): initial = tamp_problem.initial assert(initial.holding is None) domain_pddl = read(get_file_path(__file__, 'domain.pddl')) external_paths = [] if use_stream: external_paths.append(get_file_path(__file__, 'stream.pddl')) if use_optimizer: external_paths.append(get_file_path(__file__, 'optimizer.pddl')) external_pddl = [read(path) for path in external_paths] 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()] + \ [('AtPose', b, p) for b, p in initial.block_poses.items()] + \ [('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \ [('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()] + \ [('Region', r) for r in tamp_problem.goal_regions.values() + [GROUND_NAME]] goal_literals = [('In', b, r) for b, r in tamp_problem.goal_regions.items()] #+ [('HandEmpty',)] if tamp_problem.goal_conf is not None: goal_literals += [('AtConf', tamp_problem.goal_conf)] goal = And(*goal_literals) stream_map = { 's-motion': from_fn(plan_motion), 's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)), 't-region': from_test(get_region_test(tamp_problem.regions)), 's-ik': from_fn(inverse_kin_fn), #'s-ik': from_gen_fn(unreliable_ik_fn), 'distance': distance_fn, 't-cfree': from_test(lambda *args: implies(collisions, not collision_test(*args))), #'posecollision': collision_test, # Redundant 'trajcollision': lambda *args: False, } if use_optimizer: stream_map.update({ 'gurobi': from_fn(get_optimize_fn(tamp_problem.regions)), 'rrt': from_fn(cfree_motion_fn), }) #stream_map = 'debug' return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem, use_stream=True, use_optimizer=False, collisions=True): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) external_paths = [] if use_stream: external_paths.append(get_file_path(__file__, 'stream.pddl')) if use_optimizer: external_paths.append( get_file_path( __file__, 'optimizer/optimizer.pddl')) # optimizer | optimizer_hard external_pddl = [read(path) for path in external_paths] constant_map = {} stream_map = { 's-grasp': from_fn(lambda b: (GRASP, )), 's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)), 's-ik': from_fn(inverse_kin_fn), #'s-ik': from_gen_fn(unreliable_ik_fn), 's-motion': from_fn(plan_motion), 't-region': from_test(get_region_test(tamp_problem.regions)), 't-cfree': from_test( lambda *args: implies(collisions, not collision_test(*args))), 'dist': distance_fn, 'duration': duration_fn, } if use_optimizer: # To avoid loading gurobi stream_map.update({ 'gurobi': from_list_fn( get_optimize_fn(tamp_problem.regions, collisions=collisions)), 'rrt': from_fn(cfree_motion_fn), }) #stream_map = 'debug' init, goal = create_problem(tamp_problem) return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
def get_pddlstream(): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) constant_map = {} stream_pddl = read(get_file_path(__file__, 'stream.pddl')) stream_map = { 'test-pose': from_test(empty_test), # universe_test | empty_test 'sample-pose': from_constant((np.array([2, 0]),)), 'inv-kin': from_fn(ik_fn), 'motion': from_fn(motion_fn), } block = 'block1' region = 'region1' pose = np.array([1, 0]) conf = np.array([0, 0]) init = [ ('Conf', conf), ('AtConf', conf), ('HandEmpty',), ('Block', block), ('Pose', pose), ('AtPose', block, pose), ('Region', region), ] goal = ('In', block, region) return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} initial = tamp_problem.initial mode = {b: Mode(p, None) for b, p in initial.block_poses.items()} conf = conf_from_state(initial) init = [ ('CanMove',), ('Mode', mode), ('AtMode', mode), ('Conf', mode, conf), ('AtConf', conf), ] goal = Exists(['?m', '?q'], And(('GoalState', '?m', '?q'), ('AtMode', '?m'), ('AtConf', '?q'))) stream_map = { 's-forward': from_gen_fn(sample_forward(tamp_problem)), 's-intersection': from_gen_fn(sample_intersection(tamp_problem)), 's-connection': from_gen_fn(sample_connection(tamp_problem)), 't-goal': from_test(test_goal_state(tamp_problem)), } return PDDLProblem(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 get_pddlstream_problem(): # TODO: bug where a trajectory sample could be used in a different state than anticipated (don't return the sample) # TODO: enforce positive axiom preconditions requiring the state to be exactly some given value # then, the can outputs can be used in other streams only present at that state # TODO: explicitly don't let the outputs of one fluent stream be the input to another on a different state domain_pddl = read(get_file_path(__file__, 'domain.pddl')) constant_map = {} stream_pddl = read(get_file_path(__file__, 'stream.pddl')) stream_map = { 'sample-pickable': from_fn(feasibility_fn), 'test-cleanable': from_test(universe_test), #'test-cleanable': from_fn(lambda o, fluents=set(): None if fluents else (TRAJ,)), } # Currently tests whether one can clean twice init = [ ('Block', 'b1'), ('Block', 'b2'), ('OnTable', 'b1'), ('OnTable', 'b2'), ] #goal = ('Holding', 'b1') goal = And( ('Clean', 'b1'), ('Cooked', 'b1'), ) return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def main(deterministic=False, observable=False, collisions=True, factor=True): parser = create_parser() args = parser.parse_args() print('Arguments:', args) # TODO: AssertionError: degenerate distribution DDist{l0: 0.00000} # TODO: global search over the state belief_problem = get_belief_problem(deterministic, observable) pddlstream_problem = to_pddlstream(belief_problem, collisions) print('Cost scale:', get_cost_scale()) 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)), } planner = 'ff-wastar1' success_cost = 0 # 0 | MAX_COST with Profiler(field='tottime', num=10): solution = solve(pddlstream_problem, algorithm=args.algorithm, unit_costs=args.unit, stream_info=stream_info, planner=planner, debug=False, success_cost=success_cost, max_time=30) print_solution(solution)
def get_pddlstream(trajectories, element_bodies, ground_nodes): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) constant_map = {} stream_pddl = read(get_file_path(__file__, 'stream.pddl')) stream_map = { 'test-cfree': from_test(get_test_cfree(element_bodies)), } init = [] for n in ground_nodes: init.append(('Connected', n)) for t in trajectories: init.extend([ ('Node', t.n1), ('Node', t.n2), ('Element', t.element), ('Traj', t), ('Connection', t.n1, t.element, t, t.n2), ]) goal_literals = [('Printed', e) for e in element_bodies] goal = And(*goal_literals) # TODO: weight or order these in some way # TODO: instantiation slowness is due to condition effects. Change! return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} stream_map = { #'s-motion': from_fn(plan_motion), 't-reachable': from_test(test_reachable), 's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)), 't-region': from_test(get_region_test(tamp_problem.regions)), 's-ik': from_fn(lambda b, p, g: (inverse_kin(p, g),)), 'dist': distance_fn, } init, goal = create_problem(tamp_problem) return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def get_pddlstream_test(node_points, elements, ground_nodes): # stripstream/lis_scripts/run_print.py # stripstream/lis_scripts/print_data.txt domain_pddl = read(get_file_path(__file__, 'pddl/domain.pddl')) constant_map = {} stream_pddl = read(get_file_path(__file__, 'pddl/stream.pddl')) #stream_pddl = None stream_map = { 'test-cfree': from_test(get_test_cfree({})), } nodes = list(range(len(node_points))) # TODO: sort nodes by height? init = [] for n in nodes: init.append(('Node', n)) for n in ground_nodes: init.append(('Connected', n)) for e in elements: init.append(('Element', e)) n1, n2 = e t = None init.extend([ ('Connection', n1, e, t, n2), ('Connection', n2, e, t, n1), ]) #init.append(('Edge', n1, n2)) goal_literals = [('Printed', e) for e in elements] goal = And(*goal_literals) return PDDLProblem(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_push.pddl')) stream_pddl = read(os.path.join(directory, 'stream_push.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', p) for p in known_poses] + \ [('AtPose', b, p) for b, p in initial.block_poses.items()] + \ [('GoalPose', p) for p in tamp_problem.goal_poses.values()] goal = And(('AtConf', initial.conf), *[('AtPose', b, p) for b, p in tamp_problem.goal_poses.items()]) # TODO: convert to lower case stream_map = { 'push-target': from_list_fn(push_target_fn), 'push-direction': push_direction_gen_fn, 'test-cfree': from_test(lambda *args: not collision_test(*args)), 'distance': distance_fn, } 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')) external_pddl = [ read(get_file_path(__file__, 'stream.pddl')), #read(get_file_path(__file__, 'optimizer.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()] + \ [('AtPose', b, p) for b, p in initial.block_poses.items()] + \ [('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \ [('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()] goal_literals = [('In', b, r) for b, r in tamp_problem.goal_regions.items()] #+ [('HandEmpty',)] if tamp_problem.goal_conf is not None: goal_literals += [('AtConf', tamp_problem.goal_conf)] goal = And(*goal_literals) stream_map = { 's-motion': from_fn(plan_motion), 's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)), 't-region': from_test(get_region_test(tamp_problem.regions)), 's-ik': from_fn(inverse_kin_fn), 'distance': distance_fn, 't-cfree': from_test(lambda *args: not collision_test(*args)), 'posecollision': collision_test, # Redundant 'trajcollision': lambda *args: False, 'gurobi': from_fn(get_optimize_fn(tamp_problem.regions)), 'rrt': from_fn(cfree_motion_fn), #'reachable': from_test(reachable_test), #'Valid': valid_state_fn, } #stream_map = 'debug' return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
def get_pddlstream(world, debug=False, collisions=True, teleport=False, parameter_fns={}): domain_pddl = read(get_file_path(__file__, 'pddl/domain.pddl')) stream_pddl = read(get_file_path(__file__, 'pddl/stream.pddl')) # TODO: increase number of attempts when collecting data constant_map, initial_atoms, goal_formula = get_initial_and_goal(world) stream_map = { 'sample-motion': from_fn(get_motion_fn(world, collisions=collisions, teleport=teleport)), 'sample-pick': from_gen_fn(get_pick_gen_fn(world, collisions=collisions)), 'sample-place': from_fn(get_place_fn(world, collisions=collisions)), 'sample-pose': from_gen_fn(get_stable_pose_gen_fn(world, collisions=collisions)), #'sample-grasp': from_gen_fn(get_grasp_gen_fn(world)), 'sample-pour': from_gen_fn( get_pour_gen_fn(world, collisions=collisions, parameter_fns=parameter_fns)), 'sample-push': from_gen_fn( get_push_gen_fn(world, collisions=collisions, parameter_fns=parameter_fns)), 'sample-stir': from_gen_fn( get_stir_gen_fn(world, collisions=collisions, parameter_fns=parameter_fns)), 'sample-scoop': from_gen_fn( get_scoop_gen_fn(world, collisions=collisions, parameter_fns=parameter_fns)), 'sample-press': from_gen_fn(get_press_gen_fn(world, collisions=collisions)), 'test-reachable': from_test(get_reachable_test(world)), 'ControlPoseCollision': get_control_pose_collision_test(world, collisions=collisions), 'ControlConfCollision': get_control_conf_collision_test(world, collisions=collisions), 'PosePoseCollision': get_pose_pose_collision_test(world, collisions=collisions), 'ConfConfCollision': get_conf_conf_collision_test(world, collisions=collisions), } if debug: # Uses an automatically constructed debug generator for each stream stream_map = DEBUG return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, initial_atoms, goal_formula)
def get_problem(): domain_pddl = read_file('domain.pddl') constant_map = dict() table_aabb, robot, region, objects, poses, init_extend = load_world() stream_pddl = read_file('stream.pddl') stream_map = { 's-pose-kin': from_fn(get_pose_kin), 's-push-conf': from_fn(get_push_conf_fn(robot.conf.pos[2])), # 's-touch-region': from_gen_fn(pose_gen), 't-cfree-push': from_test(test_cfree_push), 't-feasible': from_test(test_feasible), 't-touches': from_test(test_touches), } init = [ ('Robot', robot), ('Conf', robot.conf), ('AtConf', robot, robot.conf), ('Region', region), ] for i in range(len(objects)): obj = objects[i] init.append(('Obj', obj)) init.append(('WorldPose', obj, obj.pose)) init.append(('AtWorldPose', obj, obj.pose)) for pose in poses[i]: init.append(('WorldPose', obj, pose)) init.extend(init_extend) both_in_region = ('and', ('Overlaps', objects[0], region), ('Overlaps', objects[1], region)) in_region = ('and', ('Overlaps', objects[0], region)) goal = in_region goal_no_side_effects = goal + tuple( ('not', ('UnknownPose', obj)) for obj in objects) print('init:') for pred in init: print(pred) print('goal:', goal) return domain_pddl, constant_map, stream_pddl, stream_map, init, goal_no_side_effects
def create_problem(goal, obstacles=(), regions={}, max_distance=.5): 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 = {} q0 = array([0, 0]) init = [ ('Conf', q0), ('AtConf', q0), ] + [('Region', r) for r in regions] if isinstance(goal, str): goal = ('In', goal) else: init += [('Conf', goal)] goal = ('AtConf', goal) np.set_printoptions(precision=3) samples = [] def region_gen(region): lower, upper = regions[region] area = np.product(upper - lower) # TODO: sample proportional to area while True: q = array(sample_box(regions[region])) samples.append(q) yield (q, ) # http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf #d = 2 #vol_free = (1 - 0) * (1 - 0) #vol_ball = math.pi * (1 ** 2) #gamma = 2 * ((1 + 1. / d) * (vol_free / vol_ball)) ** (1. / d) roadmap = [] def connected_test(q1, q2): #n = len(samples) #threshold = gamma * (math.log(n) / n) ** (1. / d) threshold = max_distance are_connected = (get_distance(q1, q2) <= threshold) and \ is_collision_free((q1, q2), obstacles) if are_connected: roadmap.append((q1, q2)) return are_connected stream_map = { 'sample-region': from_gen_fn(region_gen), 'connect': from_test(connected_test), 'distance': get_distance, } problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal) return problem, samples, roadmap
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 = {} # TODO: can always make the state the set of fluents #state = tamp_problem.initial state = { R: tamp_problem.initial.conf, H: tamp_problem.initial.holding, } for b, p in tamp_problem.initial.block_poses.items(): state[b] = p init = [ ('State', state), ('AtState', state), ('Conf', initial.conf)] + \ [('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 = ('AtGoal',) 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), #'posecollision': collision_test, #'trajcollision': lambda *args: False, 'forward-move': from_fn(move_fn), 'forward-pick': from_fn(pick_fn), 'forward-place': from_fn(place_fn), 'test-goal': from_test(get_goal_test(tamp_problem)), } #stream_map = 'debug' return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def create_equality_stream(): return Stream(name='equality', gen_fn=from_test(universe_test), inputs=['?o'], domain=[('Object', '?o')], outputs=[], certified=[('=', '?o', '?o')], info=StreamInfo(eager=True), fluents=[])
def pddlstream_from_tamp(tamp_problem): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) # TODO: algorithm that prediscretized once constant_map = {} stream_map = { 's-motion': from_fn(plan_motion), 't-reachable': from_test(test_reachable), 's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)), 't-region': from_test(get_region_test(tamp_problem.regions)), 's-ik': from_fn(lambda b, p, g: (inverse_kin(p, g),)), 'dist': distance_fn, } init, goal = create_problem(tamp_problem) init.extend(('Grasp', b, GRASP) for b in tamp_problem.initial.block_poses) return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def get_pddlstream2(robot, obstacles, node_points, element_bodies, ground_nodes, trajectories=[]): domain_pddl = read(get_file_path( __file__, 'regression.pddl')) # progression | regression constant_map = {} stream_pddl = read(get_file_path(__file__, 'stream.pddl')) stream_map = { 'test-cfree': from_test(get_test_cfree(element_bodies)), #'sample-print': from_gen_fn(get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes)), 'sample-print': get_wild_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes), } # TODO: assert that all elements have some support init = [] for n in ground_nodes: init.append(('Grounded', n)) for e in element_bodies: for n in e: if element_supports(e, n, node_points): init.append(('Supports', e, n)) if is_start_node(n, e, node_points): init.append(('StartNode', n, e)) for e in element_bodies: n1, n2 = e init.extend([ ('Node', n1), ('Node', n2), ('Element', e), ('Printed', e), ('Edge', n1, e, n2), ('Edge', n2, e, n1), #('StartNode', n1, e), #('StartNode', n2, e), ]) #if is_ground(e, ground_nodes): # init.append(('Grounded', e)) #for e1, neighbors in get_element_neighbors(element_bodies).items(): # for e2 in neighbors: # init.append(('Supports', e1, e2)) for t in trajectories: init.extend([ ('Traj', t), ('PrintAction', t.n1, t.element, t), ]) goal = And(*[('Removed', e) for e in element_bodies]) return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def create_inequality_stream(): #from pddlstream.algorithms.downward import IDENTICAL return Stream(name='inequality', gen_fn=from_test(lambda o1, o2: o1 != o2), inputs=['?o1', '?o2'], domain=[('Object', '?o1'), ('Object', '?o2')], outputs=[], certified=[('=', '?o1', '?o2')], info=StreamInfo(eager=True), fluents=[])
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 parse_rule(lisp_list, stream_map, stream_info): value_from_attribute = parse_lisp_list(lisp_list[1:]) assert set(value_from_attribute) <= {':inputs', ':domain', ':certified'} # TODO: if len(certified) == 1, augment existing streams RULES.append(Stream(name='rule{}'.format(len(RULES)), gen_fn=from_test(lambda *args: True), inputs=value_from_attribute.get(':inputs', []), domain=list_from_conjunction(value_from_attribute.get(':domain', [])), fluents=[], outputs=[], certified=list_from_conjunction(value_from_attribute.get(':certified', [])), info=StreamInfo(eager=True, p_success=1, overhead=0))) return RULES[-1]
def pddlstream_from_tamp(tamp_problem): initial = tamp_problem.initial assert(not initial.holding) # is None domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} # TODO: can always make the state a set of fluents init = [ ('State', initial), ('AtState', initial)] + \ [('Robot', r) for r in initial.robot_confs.keys()] + \ [('Conf', q) for q in initial.robot_confs.values()] + \ [('Block', b) for b in initial.block_poses.keys()] + \ [('Pose', b, p) for b, p in initial.block_poses.items()] + \ [('Grasp', b, GRASP) for b in initial.block_poses.keys()] + \ [('Region', r) for r in tamp_problem.regions.keys()] + \ [('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \ [('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()] # TODO: could populate AtConf, AtPose, HandEmpty, Holding goal = ('AtGoal',) 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), #'posecollision': collision_test, #'trajcollision': lambda *args: False, 'forward-move': from_fn(move_fn), 'forward-pick': from_fn(pick_fn), 'forward-place': from_fn(place_fn), 'test-goal': from_test(get_goal_test(tamp_problem)), } #stream_map = 'debug' return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def get_problem1(n=5): # TODO: consider even/odd and version with infinite generator # TODO: replicate the effect where a stream fails constant_map = {} stream_map = { 'increment': from_fn(lambda x: (x + 1, )), # Increment by different amounts 'decrement': from_fn(lambda x: (x - 1, )), 'test-large': from_test(lambda x: n <= x), 'test-small': from_test(lambda x: x <= -n), } init = [ ('Integer', 0), ] goal = Or( ('Goal', ) #Exists(['?x'], ('Large', '?x')), #Exists(['?x'], ('Small', '?x')), ) # TODO: sort actions when renaming to make deterministic return PDDLProblem(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, # As an example } 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() ]) # 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,)), 'test-cfree': from_test(lambda *args: not collision_test(*args)), 'collision': collision_test, 'distance': distance_fn, } return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
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', ), 1), Equal(('PickCost', ), 1), Equal(('PlaceCost', ), 1), Equal(('ScanCost', ), scan_cost), Equal(('RegisterCost', ), 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), ] init += [('Scannable', body) for body in task.rooms + task.surfaces] init += [('Registerable', body) for body in task.movable] init += [('Graspable', body) for body in task.movable] 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), clip_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)), 'test-vis-base': from_test(get_in_range_test(task, VIS_RANGE)), 'test-reg-base': from_test(get_in_range_test(task, REG_RANGE)), 'sample-vis-base': accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, VIS_RANGE)), max_attempts=25), 'sample-reg-base': accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, REG_RANGE)), max_attempts=25), 'inverse-visibility': from_fn(get_inverse_visibility_fn(task)), } return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)