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 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) 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 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 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 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(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_tamp(tamp_problem, use_stream=True, use_optimizer=False, collisions=True): initial = tamp_problem.initial assert (not initial.holding) 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')) # optimizer | optimizer_hard external_pddl = [read(path) for path in external_paths] constant_map = {} init = [ ('Region', GROUND_NAME), Equal((TOTAL_COST,), 0)] + \ [('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] + \ [('AtPose', b, p) for b, p in initial.block_poses.items()] + \ [('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] goal_literals = [] for r, q in initial.robot_confs.items(): init += [ ('Robot', r), ('CanMove', r), ('Conf', q), ('AtConf', r, q), ('HandEmpty', r), ] if tamp_problem.goal_conf is not None: #goal_literals += [('AtConf', tamp_problem.goal_conf)] goal_literals += [('AtConf', r, q)] for b, r in tamp_problem.goal_regions.items(): if isinstance(r, str): init += [('Region', r), ('Placeable', b, r)] goal_literals += [('In', b, r)] else: init += [('Pose', b, r)] goal_literals += [('AtPose', b, r)] #goal_literals += [Not(('Unsafe',))] # ('HandEmpty',) 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), 'dist': distance_fn, 'duration': duration_fn, 't-cfree': from_test( lambda *args: implies(collisions, not collision_test(*args))), } 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' return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)