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)),
'TrajCollision': get_movable_collision_test(),
}
if USE_SYNTHESIZERS:
stream_map.update({
'plan-free-motion': empty_gen(),
'plan-holding-motion': empty_gen(),
})
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'))
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 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 = {}
#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',), scale_cost(1)),
Equal(('PlaceCost',), scale_cost(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 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 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():
convbelt = ConveyorBelt(problem_idx=0)
problem_config = convbelt.problem_config
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'))
pick_sampler = PickWithBaseUnif(convbelt)
place_sampler = PlaceUnif(convbelt)
constant_map = {}
stream_map = {'gen-grasp': from_gen_fn(gen_grasp(pick_sampler)),
'gen-placement': from_gen_fn(gen_placement(convbelt, place_sampler)),
#'TrajPoseCollision': fn_from_constant(False)
'TrajPoseCollision': check_traj_collision(convbelt),
}
obj_names = problem_config['obj_poses'].keys()
obj_poses = problem_config['obj_poses'].values()
obj_names = ['obj0', 'obj1', 'obj2', 'obj3', 'obj4']
init = [('Pickable', obj_name) for obj_name in obj_names]
init += [('Robot', 'pr2')]
init += [('BaseConf', [0, 1.05, 0])]
init += [('EmptyArm',)]
init += [('ObjectZero', 'obj0')]
init += [('ObjectOne', 'obj1')]
init += [('ObjectTwo', 'obj2')]
init += [('ObjectThree', 'obj3')]
init += [('ObjectFour', 'obj4')]
#goal = ('Picked', 'obj0')
#goal = ('AtPose', 'obj0', obj_pose)
goal = ['and', ('InLoadingRegion', 'obj0'),
('InLoadingRegion', 'obj1'),
('InLoadingRegion', 'obj2'),
('InLoadingRegion', 'obj3'),
('InLoadingRegion', 'obj4')]
#goal = ['and', ('InLoadingRegion', 'obj0'), ('InLoadingRegion', 'obj1')]
#goal = ('InLoadingRegion', 'obj0')
convbelt.env.SetViewer('qtcoin')
return (domain_pddl, constant_map, stream_pddl, stream_map, init, goal), convbelt
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 get_problem():
namo = StripStreamNAMO()
problem_config = namo.problem_config
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'))
pick_sampler = PickWithBaseUnif(namo)
place_sampler = PlaceUnif(namo)
constant_map = {}
stream_map = {'gen-grasp': from_gen_fn(gen_grasp(pick_sampler)),
'TrajPoseCollision': check_traj_collision(namo),
'TrajPoseCollisionWithObject': check_traj_collision_with_object(namo),
'gen-base-traj': from_gen_fn(gen_base_traj(namo)),
'gen-placement': from_gen_fn(gen_placement(namo, place_sampler)),
#'gen-base-traj-with-obj': from_fn(gen_base_traj_with_object(namo)),
}
obj_names = problem_config['obj_poses'].keys()
obj_poses = problem_config['obj_poses'].values()
init = [('Pickable', obj_name) for obj_name in obj_names]
init += [('Robot', 'pr2')]
init += [('EmptyArm',)]
init += [('Region', 'entire_region')]
init += [('Region', 'loading_region')]
#init += [('InRegion', 'obj0', 'entire_region')]
#init += [('InRegion', 'obj1', 'entire_region')]
init += [('AtPose', obj_name, obj_pose) for obj_name, obj_pose in zip(obj_names, obj_poses)]
init += [('Pose', obj_name, obj_pose) for obj_name, obj_pose in zip(obj_names, obj_poses)]
init_config = np.array([-1, 1, 0])
goal_config = np.array([-2, 1, 0])
init += [('BaseConf', init_config)]
init += [('BaseConf', goal_config)]
init += [('AtConf', init_config)]
#goal = ['and', ('InRegion', 'obj0', 'loading_region')]
#goal = ['and', ('InRegion', 'obj0', 'loading_region')]
goal = ['and', ('Holding', 'obj1')]
#goal = ['and', ('AtConf', goal_config), ('Placed', 'obj0')]
#goal = ['and', ('not', ('EmptyArm',))]
return (domain_pddl, constant_map, stream_pddl, stream_map, init, goal), namo
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):
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_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_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 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)
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_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 opt_from_graph(names, orders, infos={}):
param_from_order = {order: PARAM_TEMPLATE.format(*order) for order in orders}
fact_from_order = {order: (PREDICATE, param_from_order[order]) for order in orders}
object_from_param = {param: parse_value(param) for param in param_from_order.values()}
incoming_from_edges, outgoing_from_edges = neighbors_from_orders(orders)
stream_plan = []
for i, n in enumerate(names):
#info = infos.get(n, StreamInfo(p_success=1, overhead=0, verbose=True))
inputs = [param_from_order[n2, n] for n2 in incoming_from_edges[n]]
outputs = [param_from_order[n, n2] for n2 in outgoing_from_edges[n]]
#gen = get_gen(outputs=outputs, p_success=info.p_success)
#gen = get_gen(infos[i], outputs=outputs)
stream = Stream(
name=n,
#gen_fn=DEBUG,
#gen_fn=from_gen(gen),
gen_fn=from_gen_fn(get_gen_fn(outputs=outputs, **infos[i])),
inputs=inputs,
domain=[fact_from_order[n2, n] for n2 in incoming_from_edges[n]],
fluents=[],
outputs=outputs,
certified=[fact_from_order[n, n2] for n2 in outgoing_from_edges[n]],
#info=info,
info=StreamInfo(),
)
# TODO: dump names
print()
print(stream)
input_objects = safe_apply_mapping(stream.inputs, object_from_param)
instance = stream.get_instance(input_objects)
print(instance)
output_objects = safe_apply_mapping(stream.outputs, object_from_param)
result = instance.get_result(output_objects)
print(result)
stream_plan.append(result)
opt_plan = OptPlan(action_plan=[], preimage_facts=[])
opt_solution = OptSolution(stream_plan, opt_plan, cost=1)
return opt_solution
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)
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_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 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, 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
def get_problem(mover, goal_objects, goal_region_name, config):
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 = {
'gen-pap': from_gen_fn(gen_pap(mover, config)),
}
obj_names = [obj.GetName() for obj in mover.objects]
obj_poses = [
get_body_xytheta(mover.env.GetKinBody(obj_name)).squeeze()
for obj_name in obj_names
]
initial_robot_conf = get_body_xytheta(mover.robot).squeeze()
if mover.name == 'two_arm_mover':
goal_region = 'home_region'
nongoal_regions = ['loading_region']
elif mover.name == 'one_arm_mover':
goal_region = mover.target_box_region.name
nongoal_regions = ['center_shelf_region'] #list(mover.shelf_regions)
else:
raise NotImplementedError
print(goal_region, nongoal_regions, mover.regions.keys())
init = [('Pickable', obj_name) for obj_name in obj_names]
init += [('InRegion', obj_name,
mover.get_region_containing(mover.env.GetKinBody(obj_name)).name)
for obj_name in obj_names]
init += [('Region', region) for region in nongoal_regions + [goal_region]]
init += [('GoalObject', obj_name) for obj_name in goal_objects]
init += [('NonGoalRegion', region) for region in nongoal_regions]
init_state = CustomStateSaver(mover.env)
init += [('State', init_state)]
init += [('AtState', init_state)]
# robot initialization
init += [('EmptyArm', )]
init += [('AtConf', initial_robot_conf)]
init += [('BaseConf', initial_robot_conf)]
# object initialization
init += [('Pose', obj_pose)
for obj_name, obj_pose in zip(obj_names, obj_poses)]
init += [('PoseInRegion', obj_pose, 'loading_region')
for obj_name, obj_pose in zip(obj_names, obj_poses)]
init += [('AtPose', obj_name, obj_pose)
for obj_name, obj_pose in zip(obj_names, obj_poses)]
goal = ['and'] + [('InRegion', obj_name, goal_region_name)
for obj_name in goal_objects]
print('Num init:', Counter(fact[0] for fact in init))
print('Goal:', goal)
print('Streams:', sorted(stream_map))
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_streams(world, debug=False, teleport_base=False, **kwargs):
stream_pddl = read(get_file_path(__file__, '../pddl/stream.pddl'))
if debug:
return stream_pddl, DEBUG
stream_map = {
'test-door':
from_test(get_door_test(world)),
'test-gripper':
from_test(get_gripper_open_test(world)),
'sample-pose':
from_gen_fn(get_stable_gen(world, **kwargs)),
'sample-grasp':
from_gen_fn(get_grasp_gen(world)),
'sample-nearby-pose':
from_gen_fn(get_nearby_stable_gen(world, **kwargs)),
'plan-pick':
from_gen_fn(get_pick_gen_fn(world, **kwargs)),
'plan-pull':
from_gen_fn(get_pull_gen_fn(world, **kwargs)),
'plan-press':
from_gen_fn(get_press_gen_fn(world, **kwargs)),
'plan-pour':
from_gen_fn(get_pour_gen_fn(world, **kwargs)),
'plan-base-motion':
from_fn(
get_base_motion_fn(world, teleport_base=teleport_base, **kwargs)),
'plan-arm-motion':
from_fn(get_arm_motion_gen(world, **kwargs)),
'plan-gripper-motion':
from_fn(get_gripper_motion_gen(world, **kwargs)),
'plan-calibrate-motion':
from_fn(get_calibrate_gen(world, **kwargs)),
'test-near-pose':
from_test(get_test_near_pose(world, **kwargs)),
'test-near-joint':
from_test(get_test_near_joint(world, **kwargs)),
'fixed-plan-pick':
from_gen_fn(get_fixed_pick_gen_fn(world, **kwargs)),
'fixed-plan-pull':
from_gen_fn(get_fixed_pull_gen_fn(world, **kwargs)),
'fixed-plan-press':
from_gen_fn(get_fixed_press_gen_fn(world, **kwargs)),
'fixed-plan-pour':
from_gen_fn(get_fixed_pour_gen_fn(world, **kwargs)),
'compute-pose-kin':
from_fn(get_compute_pose_kin(world)),
# 'compute-angle-kin': from_fn(compute_angle_kin),
'compute-detect':
from_fn(get_compute_detect(world, **kwargs)),
'sample-observation':
from_gen_fn(get_sample_belief_gen(world, **kwargs)),
'update-belief':
from_fn(update_belief_fn(world, **kwargs)),
'test-cfree-worldpose':
from_test(get_cfree_worldpose_test(world, **kwargs)),
'test-cfree-worldpose-worldpose':
from_test(get_cfree_worldpose_worldpose_test(world, **kwargs)),
'test-cfree-pose-pose':
from_test(get_cfree_relpose_relpose_test(world, **kwargs)),
'test-cfree-bconf-pose':
from_test(get_cfree_bconf_pose_test(world, **kwargs)),
'test-cfree-approach-pose':
from_test(get_cfree_approach_pose_test(world, **kwargs)),
'test-cfree-angle-angle':
from_test(get_cfree_angle_angle_test(world, **kwargs)),
'test-cfree-traj-pose':
from_test(get_cfree_traj_pose_test(world, **kwargs)),
'test-ofree-ray-pose':
from_test(get_ofree_ray_pose_test(world, **kwargs)),
'test-ofree-ray-grasp':
from_test(get_ofree_ray_grasp_test(world, **kwargs)),
'DetectCost':
detect_cost_fn,
#'MoveCost': move_cost_fn,
# 'Distance': base_cost_fn,
}
return stream_pddl, stream_map
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
LARGE = 5
SMALL = -1
#MAX_INT = 100000
MAX_INT = 5
pairs = set()
def addition(x1, x2):
pairs.add((x1, x2))
x2 = x1 + x2
return (x2, )
STREAM_MAP = {
'positive': from_gen_fn(lambda: ((x, ) for x in range(1, MAX_INT + 1))),
'negative': from_gen_fn(lambda: ((-x, ) for x in range(1, MAX_INT + 1))),
'addition': from_fn(addition),
'test-large': from_test(lambda x: LARGE <= x),
'test-small': from_test(lambda x: x <= SMALL),
'cost': lambda x: 1. / (abs(x) + 1),
}
def problem1():
#constant_map = {} # TODO: constant_map
init = []
terms = [('Integer', '?x1'), ('Large', '?x1'), ('Integer', '?x2'),
('minimize', ('Cost', '?x1')), ('minimize', ('Cost', '?x2'))]
return SatisfactionProblem(STREAM_PDDL, STREAM_MAP, init, terms)
def pddlstream_from_problem(problem,
base_limits=None,
collisions=True,
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 = {
'@sink': 'sink',
'@stove': 'stove',
}
#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=True) # TODO: supported here
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', body, pose), ('Stackable', body, None)]
for surface in problem.surfaces:
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
for body, ty in problem.body_types:
init += [('Type', body, ty)]
bodies_from_type = get_bodies_from_type(problem)
goal_literals = []
if problem.goal_conf is not None:
goal_conf = Conf(robot, get_group_joints(robot, 'base'),
problem.goal_conf)
init += [('BConf', goal_conf)]
goal_literals += [('AtBConf', goal_conf)]
for ty, s in problem.goal_on:
bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(
ty) else [ty]
init += [('Stackable', b, s) for b in bodies]
goal_literals += [('On', ty, s)]
goal_literals += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
goal_formula = []
for literal in goal_literals:
parameters = [a for a in get_args(literal) if is_parameter(a)]
if parameters:
type_literals = [('Type', p, get_parameter_name(p))
for p in parameters]
goal_formula.append(
Exists(parameters, And(literal, *type_literals)))
else:
goal_formula.append(literal)
goal_formula = And(*goal_formula)
custom_limits = {}
if base_limits is not None:
custom_limits.update(get_custom_limits(robot, problem.base_limits))
stream_map = {
'sample-pose':
from_gen_fn(get_stable_gen(problem, collisions=collisions)),
'sample-grasp':
from_list_fn(get_grasp_gen(problem, collisions=collisions)),
#'sample-grasp': from_gen_fn(get_grasp_gen(problem, collisions=collisions)),
'inverse-kinematics':
from_gen_fn(
get_ik_ir_gen(problem,
custom_limits=custom_limits,
collisions=collisions,
teleport=teleport)),
'plan-base-motion':
from_fn(
get_motion_gen(problem,
custom_limits=custom_limits,
collisions=collisions,
teleport=teleport)),
'test-cfree-pose-pose':
from_test(get_cfree_pose_pose_test(collisions=collisions)),
'test-cfree-approach-pose':
from_test(get_cfree_approach_pose_test(problem,
collisions=collisions)),
'test-cfree-traj-pose':
from_test(get_cfree_traj_pose_test(robot, collisions=collisions)),
#'test-cfree-traj-grasp-pose': from_test(get_cfree_traj_grasp_pose_test(problem, collisions=collisions)),
#'MoveCost': move_cost_fn,
'Distance':
distance_fn,
}
#stream_map = DEBUG
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
