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 get_pddlstream_info(robot, fixed, movable, add_slanted_grasps, approach_frame, use_vision, home_poses=None): domain_pddl = read('tamp/domain_stacking.pddl') stream_pddl = read('tamp/stream_stacking.pddl') constant_map = {} fixed = [f for f in fixed if f is not None] stream_map = { 'sample-pose-table': from_list_fn(primitives.get_stable_gen_table(fixed)), 'sample-pose-home': from_list_fn(primitives.get_stable_gen_home(home_poses, fixed)), 'sample-pose-block': from_fn(primitives.get_stable_gen_block(fixed)), 'sample-grasp': from_list_fn( primitives.get_grasp_gen(robot, add_slanted_grasps=True, add_orthogonal_grasps=False)), # 'sample-grasp': from_gen_fn(primitives.get_grasp_gen(robot, add_slanted_grasps=True, add_orthogonal_grasps=False)), 'pick-inverse-kinematics': from_fn( primitives.get_ik_fn(robot, fixed, approach_frame='gripper', backoff_frame='global', use_wrist_camera=use_vision)), 'place-inverse-kinematics': from_fn( primitives.get_ik_fn(robot, fixed, approach_frame='global', backoff_frame='gripper', use_wrist_camera=False)), 'plan-free-motion': from_fn(primitives.get_free_motion_gen(robot, fixed)), 'plan-holding-motion': from_fn(primitives.get_holding_motion_gen(robot, fixed)), } return domain_pddl, constant_map, stream_pddl, stream_map
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 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) set_cost_scale(SCALE_COST) print('Cost scale:', get_cost_scale()) 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, success_cost=0, unit_costs=False, max_time=30) else: solution = solve_incremental(pddlstream_problem, planner=planner, debug=True, success_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:', cost)
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 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 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) 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(ik_fn), 'motion': from_fn(motion_fn), } # 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 = Uncertain() #pose = np.array([1, 0]) conf = np.array([0, 1]) booth = np.array([0, 2]) init = [ ('Initial',), # Forces move first ('Conf', conf), #('Booth', 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 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 pddlstream_from_problem(robot, movable): domain_pddl = read('tamp/domain_stacking.pddl') stream_pddl = read('tamp/stream_stacking.pddl') constant_map = {} fixed = misc.get_fixed(robot, movable) stream_map = { 'sample-pose-table': from_gen_fn(primitives.get_stable_gen_table(fixed)), 'sample-pose-block': from_fn(primitives.get_stable_gen_block(fixed)), 'sample-grasp': from_gen_fn(primitives.get_grasp_gen(robot)), 'inverse-kinematics': from_fn(primitives.get_ik_fn(robot, fixed)), 'plan-free-motion': from_fn(primitives.get_free_motion_gen(robot, fixed)), 'plan-holding-motion': from_fn(primitives.get_holding_motion_gen(robot, fixed)), } return domain_pddl, constant_map, stream_pddl, stream_map
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): 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 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 get_pddlstream_problem(): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) constant_map = {} stream_pddl = read(get_file_path(__file__, 'stream.pddl')) stream_map = { #'test-feasible': from_test(test_feasible), 'test-feasible': from_fn(feasibility_fn), } init = [ ('Block', 'b1'), ('Block', 'b2'), ('OnTable', 'b1'), ('OnTable', 'b2'), ] goal = ('Holding', 'b1') return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def __init__(self, name, gen_fn, inputs, domain, outputs, certified, info): if info is None: info = StreamInfo(p_success=None, overhead=None) for p, c in Counter(outputs).items(): if c != 1: raise ValueError( 'Output [{}] for stream [{}] is not unique'.format( p, name)) for p in set(inputs) & set(outputs): raise ValueError( 'Parameter [{}] for stream [{}] is both an input and output'. format(p, name)) parameters = { a for i in certified for a in get_args(i) if is_parameter(a) } for p in (parameters - set(inputs + outputs)): raise ValueError( 'Parameter [{}] for stream [{}] is not included within outputs' .format(p, name)) super(Stream, self).__init__(name, info, inputs, domain) # Each stream could certify a stream-specific fact as well if gen_fn == DEBUG: #gen_fn = from_fn(lambda *args: tuple(object() for _ in self.outputs)) gen_fn = from_fn(lambda *args: tuple( DebugValue(name, args, o) for o in self.outputs)) self.gen_fn = gen_fn self.outputs = tuple(outputs) self.certified = tuple(certified) self.constants.update(a for i in certified for a in get_args(i) if not is_parameter(a)) # TODO: generalize to a hierarchical sequence always_unique = False if always_unique: self.num_opt_fns = 0 #self.opt_list_fn = get_unique_fn(self) self.opt_gen_fn = get_constant_gen_fn(self, None) else: self.num_opt_fns = 1 self.opt_gen_fn = get_shared_gen_fn(self) if ( self.info.opt_gen_fn is None) else self.info.opt_gen_fn
def create_static_stream(stream, evaluations, fluent_predicates, future_fn): 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 fluent_domain = list( filter(lambda a: get_prefix(a) in fluent_predicates, stream.domain)) static_domain = list( filter(lambda a: a not in fluent_domain, stream.domain)) new_domain = list(map(future_fn, static_domain)) stream_atom = ('{}-result'.format( stream.name), ) + tuple(stream.inputs + stream.outputs) new_certified = [stream_atom] + list(map(future_fn, stream.certified)) static_stream = FutureStream(stream, new_domain, fluent_domain, new_certified) if REPLACE_STREAM: static_stream.gen_fn = from_fn(static_fn) 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 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)
def get_debug_gen_fn(stream): return from_fn(lambda *args: tuple( DebugValue(stream.name, args, o) for o in stream.outputs))
def get_pddlstream(robot, brick_from_index, obstacle_from_name, collisions=True, teleport=False): domain_pddl = read(get_file_path(__file__, 'retired.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} conf = np.array(get_configuration(robot)) init = [ ('CanMove', ), ('Conf', conf), ('AtConf', conf), ('HandEmpty', ), ] goal_literals = [ ('AtConf', conf), #('HandEmpty',), ] #indices = brick_from_index.keys() #indices = range(2, 5) indices = [4] for index in list(indices): indices.append(index + 6) for index in indices: brick = brick_from_index[index] init += [ ('Graspable', index), ('Pose', index, brick.initial_pose), ('AtPose', index, brick.initial_pose), ('Pose', index, brick.goal_pose), ] goal_literals += [ #('Holding', index), ('AtPose', index, brick.goal_pose), ] for index2 in brick.goal_supports: brick2 = brick_from_index[index2] init += [ ('Supported', index, brick.goal_pose, index2, brick2.goal_pose), ] goal = And(*goal_literals) if not collisions: obstacle_from_name = {} stream_map = { 'sample-grasp': from_gen_fn(get_grasp_gen_fn(brick_from_index)), 'inverse-kinematics': from_gen_fn(get_ik_gen_fn(robot, brick_from_index, obstacle_from_name)), 'plan-motion': from_fn( get_motion_fn(robot, brick_from_index, obstacle_from_name, teleport=teleport)), 'TrajCollision': get_collision_test(robot, brick_from_index, collisions=collisions), } #stream_map = 'debug' return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_pddlstream_problem(task, context, collisions=True): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} plant = task.mbp robot = task.robot robot_name = get_model_name(plant, robot) world = plant.world_frame() # mbp.world_body() robot_joints = get_movable_joints(plant, robot) robot_conf = Conf(robot_joints, get_configuration(plant, context, robot)) init = [ ('Robot', robot_name), ('CanMove', robot_name), ('Conf', robot_name, robot_conf), ('AtConf', robot_name, robot_conf), ('HandEmpty', robot_name), ] goal_literals = [] if task.reset_robot: goal_literals.append(('AtConf', robot_name, robot_conf), ) for obj in task.movable: obj_name = get_model_name(plant, obj) #obj_frame = get_base_body(mbp, obj).body_frame() obj_pose = Pose(plant, world, obj, get_world_pose(plant, context, obj)) # get_relative_transform init += [ ('Graspable', obj_name), ('Pose', obj_name, obj_pose), #('InitPose', obj_name, obj_pose), ('AtPose', obj_name, obj_pose) ] for surface in task.surfaces: init += [('Stackable', obj_name, surface)] # TODO: detect already stacked for surface in task.surfaces: surface_name = get_model_name(plant, surface.model_index) if 'sink' in surface_name: init += [('Sink', surface)] if 'stove' in surface_name: init += [('Stove', surface)] for door in task.doors: door_body = plant.tree().get_body(door) door_name = door_body.name() door_joints = get_parent_joints(plant, door_body) door_conf = Conf(door_joints, get_joint_positions(door_joints, context)) init += [ ('Door', door_name), ('Conf', door_name, door_conf), ('AtConf', door_name, door_conf), ] for positions in [get_door_positions(door_body, DOOR_OPEN)]: conf = Conf(door_joints, positions) init += [('Conf', door_name, conf)] if task.reset_doors: goal_literals += [('AtConf', door_name, door_conf)] for obj, transform in task.goal_poses.items(): obj_name = get_model_name(plant, obj) obj_pose = Pose(plant, world, obj, transform) init += [('Pose', obj_name, obj_pose)] goal_literals.append(('AtPose', obj_name, obj_pose)) for obj in task.goal_holding: goal_literals.append( ('Holding', robot_name, get_model_name(plant, obj))) for obj, surface in task.goal_on: goal_literals.append(('On', get_model_name(plant, obj), surface)) for obj in task.goal_cooked: goal_literals.append(('Cooked', get_model_name(plant, obj))) goal = And(*goal_literals) print('Initial:', init) print('Goal:', goal) stream_map = { #'sample-pose': from_gen_fn(get_stable_gen(task, context, collisions=collisions)), 'sample-reachable-pose': from_gen_fn( get_reachable_pose_gen_fn(task, context, collisions=collisions)), 'sample-grasp': from_gen_fn(get_grasp_gen_fn(task)), 'plan-ik': from_gen_fn(get_ik_gen_fn(task, context, collisions=collisions)), 'plan-motion': from_fn(get_motion_fn(task, context, collisions=collisions)), 'plan-pull': from_fn(get_pull_fn(task, context, collisions=collisions)), 'TrajPoseCollision': get_collision_test(task, context, collisions=collisions), 'TrajConfCollision': get_collision_test(task, context, collisions=collisions), } #stream_map = 'debug' # Runs PDDLStream with "placeholder streams" for debugging return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_debug_gen_fn(stream, shared=True): if shared: return from_fn(lambda *args, **kwargs: tuple( SharedDebugValue(stream.name, o) for o in stream.outputs)) return from_fn(lambda *args, **kwargs: tuple( DebugValue(stream.name, args, o) for o in stream.outputs))
def pddlstream_from_problem(problem, collisions=True, teleport=False): #rovers = problem.rovers domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} camera = 'RGBD' mode = 'color' # highres | lowres | color | depth init = [] goal_literals = [ #('Calibrated', camera, problem.rovers[0]) #('Analyzed', problem.rovers[0], problem.rocks[0]) #('ReceivedAnalysis', problem.rocks[0]) ] init += [('Lander', l) for l in problem.landers] init += [('Camera', camera), ('Supports', camera, mode), ('Mode', mode)] for rover in problem.rovers: base_joints = get_group_joints(rover, 'base') bq0 = Conf(rover, base_joints) head_joints = get_group_joints(rover, 'head') hq0 = Conf(rover, head_joints) init += [('Rover', rover), ('OnBoard', camera, rover), ('BConf', bq0), ('AtBConf', rover, bq0), ('HConf', hq0), ('AtHConf', rover, hq0)] for name in problem.stores: init += [('Store', name)] init += [('Free', rover, name) for rover in problem.rovers] for name in problem.objectives: init += [('Objective', name)] #initial_atoms += [IsClass(name, cl) for cl in CLASSES if cl in name] goal_literals += [('ReceivedImage', name, mode)] for name in problem.rocks + problem.soils: # pose = Pose(name, get_pose(env.GetKinBody(name))) init += [('Rock', name)] # , IsPose(name, pose), AtPose(name, pose)] #init += [('IsClass', name, cl) for cl in CLASSES if cl in name] #if problem.rocks: # goal_literals.append(Exists(['?rock'], And(('Rock', '?rock'), # ('ReceivedAnalysis', '?rock')))) #if problem.soils: # goal_literals.append(Exists(['?soil'], And(('Soil', '?soil'), # ('ReceivedAnalysis', '?soil')))) # TODO: soil class goal_formula = And(*goal_literals) custom_limits = {} if problem.limits is not None: for rover in problem.rovers: custom_limits.update(get_base_custom_limits(rover, problem.limits)) stream_map = { 'test-reachable': from_test(get_reachable_test(problem)), 'obj-inv-visible': from_gen_fn(get_inv_vis_gen(problem, custom_limits=custom_limits)), 'com-inv-visible': from_gen_fn(get_inv_com_gen(problem, custom_limits=custom_limits)), 'sample-above': from_gen_fn(get_above_gen(problem, custom_limits=custom_limits)), 'sample-base-motion': from_fn(get_base_motion_fn(problem, custom_limits=custom_limits, teleport=teleport)), 'sample-head-motion': from_fn(get_head_motion_fn(problem, teleport=teleport)), } #stream_map = 'debug' return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
def pddlstream_from_problem(problem, teleport=False): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {'{}'.format(name).lower(): name for name in problem.initial_confs.keys()} edges = set() init = [ ] for name, conf in problem.initial_confs.items(): init += [ ('Safe',), ('Robot', name), ('Conf', conf), ('AtConf', name, conf), Equal(('Speed', name), DIST_PER_TIME), Equal(('BatteryCapacity', name), MAX_ENERGY), Equal(('RechargeRate', name), CHARGE_PER_TIME), Equal(('ConsumptionRate', name), BURN_PER_TIME), Equal(('Energy', name), INITIAL_ENERGY), ] goal_literals = [ #('Safe',), #('Unachievable',), ] for name, base_values in problem.goal_confs.items(): body = problem.get_body(name) joints = get_base_joints(body) #extend_fn = get_extend_fn(body, joints, resolutions=5*BASE_RESOLUTIONS) #q_init = problem.initial_confs[name] #path = [q_init] + [Conf(body, joints, q) for q in extend_fn(q_init.values, base_values)] #edges.update(zip(path, path[1:])) #q_goal = path[-1] q_goal = Conf(body, joints, base_values) init += [('Conf', q_goal)] goal_literals += [('AtConf', name, q_goal)] goal_formula = And(*goal_literals) robot = list(map(problem.get_body, problem.initial_confs))[0] with LockRenderer(): init += [('Conf', q) for _, n, q in create_vertices(problem, robot, [], samples_per_ft2=6)] #vertices = {v for edge in edges for v in edge} #handles = [] #for vertex in vertices: # handles.extend(draw_point(point_from_conf(vertex.values), size=0.05)) #for conf1, conf2 in edges: # traj = linear_trajectory(conf1, conf2) # init += [ # ('Conf', conf1), # ('Traj', traj), # ('Conf', conf2), # ('Motion', conf1, traj, conf2), # ] #draw_edges(edges) cfree_traj_traj_test = get_test_cfree_traj_traj(problem) cfree_traj_traj_list_gen_fn = from_test(cfree_traj_traj_test) traj_traj_collision_test = negate_test(cfree_traj_traj_test) stream_map = { 'test-cfree-conf-conf': cfree_traj_traj_list_gen_fn, 'test-cfree-traj-conf': cfree_traj_traj_list_gen_fn, 'test-cfree-traj-traj': cfree_traj_traj_list_gen_fn, 'ConfConfCollision': traj_traj_collision_test, 'TrajConfCollision': traj_traj_collision_test, 'TrajTrajCollision': traj_traj_collision_test, 'compute-motion': from_fn(get_motion_fn2(problem)), 'TrajDistance': get_distance_fn(), } #stream_map = 'debug' problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula) return problem, edges
def main(display=True, simulate=False, teleport=False): parser = argparse.ArgumentParser() parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') #parser.add_argument('-display', action='store_true', help='displays the solution') args = parser.parse_args() connect(use_gui=args.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(PartialInputs('?r')), 'inverse-kinematics': StreamInfo(PartialInputs('?p')), 'plan-base-motion': StreamInfo(PartialInputs('?q1 ?q2')), '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, 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 commands = post_process(problem, plan) if args.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(display=True, teleport=False, partial=False, defer=False): parser = argparse.ArgumentParser() parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') #parser.add_argument('-display', action='store_true', help='displays the solution') args = parser.parse_args() connect(use_gui=args.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(PartialInputs('?r')), 'inverse-kinematics': StreamInfo(PartialInputs('?p')), 'plan-base-motion': StreamInfo(PartialInputs('?q1 ?q2'), defer_fn=defer_shared if defer else never_defer), 'MoveCost': FunctionInfo(opt_move_cost_fn), } if partial else { '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), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) pr = cProfile.Profile() pr.enable() with LockRenderer(): #solution = solve_incremental(pddlstream_problem, debug=True) solution = solve_focused(pddlstream_problem, stream_info=stream_info, success_cost=INF, debug=False) 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 with LockRenderer(): commands = post_process(problem, plan) if args.viewer: restore_state(state_id) else: disconnect() connect(use_gui=True) with HideOutput(): problem_fn() # TODO: way of doing this without reloading? if args.simulate: control_commands(commands) else: apply_commands(State(), commands, time_step=0.01) user_input('Finish?') disconnect()
def get_problem(optimize=True): initial_atm = 30 initial_person = 2 min_take = 1 #max_take = 10 max_take = initial_atm target = 50 # 1 | 3 | 50 num_atms = 2 domain_pddl = read_pddl(__file__, 'domain0.pddl') constant_map = { #'@min': min_take, #'@max': max_take, '@amount': target, } if optimize: stream_pddl = read_pddl(__file__, 'optimizer.pddl') else: stream_pddl = read_pddl(__file__, 'stream.pddl') # TODO: store history of withdraws to ensure not taking too much #stream_pddl = None stream_map = { #'s-cash': from_gen((c,) for c in randomize(irange(min_take, max_take + 1))), #'s-cash': from_gen((c,) for c in reversed(list(irange(min_take, max_take+1)))), 's-cash': from_sampler(lambda: (round(uniform(min_take, max_take), 2), )), 't-ge': from_test(lambda c1, c2: c1 >= c2), 'add': from_fn(lambda c1, c2: (c1 + c2, )), 'subtract': from_fn(lambda c3, c2: (c3 - c2, ) if c3 - c2 >= 0 else None), 'withdraw': from_fn(lambda wc, pc1, mc1: (pc1 + wc, mc1 - wc) if mc1 - wc >= 0 else None), 'withdrawcost': lambda c: c, # TODO: withdraw fee 'gurobi': create_optimizer(min_take, max_take), } person = 'Emre' #initial_people = {'Emre': initial_person} #initial_atms = {'Emre': initial_person} #amounts = [min_take, max_take, target, initial_atm, initial_person] init = [ ('person', person), ('pcash', initial_person), ('pcash', target), ('tcash', target), ('inpocket', person, initial_person), ] # + [('cash', amount) for amount in amounts] for num in range(num_atms): name = 'atm{}'.format(num + 1) init.extend([ ('machine', name), ('mcash', initial_atm), ('maxwithdraw', name, initial_atm), ]) #goal = Exists(['?c1'], And(('person', person), ('ge', '?c1', target), # ('inpocket', person, '?c1'))) goal = ('finished', ) # TODO: focused bug when goal initially satisfied return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def pddlstream_from_problem(problem, teleport=False): robot = problem.robot domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} #initial_bq = Pose(robot, get_pose(robot)) initial_bq = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base')) init = [ ('CanMove',), ('BConf', initial_bq), ('AtBConf', initial_bq), Equal(('PickCost',), 1), Equal(('PlaceCost',), 1), ] + [('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_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)] 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', 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)), 'MoveCost': move_cost_fn, 'TrajPoseCollision': fn_from_constant(False), 'TrajArmCollision': fn_from_constant(False), 'TrajGraspCollision': fn_from_constant(False), } if USE_SYNTHESIZERS: stream_map['plan-base-motion'] = empty_gen(), # get_press_gen(problem, teleport=teleport) return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
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)) initial_bq = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base')) 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