def get_pddlstream_problem():
# TODO: bug where a trajectory sample could be used in a different state than anticipated (don't return the sample)
# TODO: enforce positive axiom preconditions requiring the state to be exactly some given value
# then, the can outputs can be used in other streams only present at that state
# TODO: explicitly don't let the outputs of one fluent stream be the input to another on a different state
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
'sample-pickable': from_fn(feasibility_fn),
'test-cleanable': from_fn(lambda o, fluents=set(): (TRAJ, )),
#'test-cleanable': from_fn(lambda o, fluents=set(): None if fluents else (TRAJ,)),
}
init = [
('Block', 'b1'),
('Block', 'b2'),
('OnTable', 'b1'),
('OnTable', 'b2'),
]
#goal = ('Holding', 'b1')
goal = And(('Clean', 'b1'), ('Cooked', 'b1'))
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_problem(problem, teleport=False, movable_collisions=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'world': 'world',
}
world = 'world'
initial_bq = Pose(robot, get_pose(robot))
init = [
('CanMove',),
('BConf', initial_bq), # TODO: could make pose as well...
('AtBConf', initial_bq),
('AtAConf', world, None),
('AtPose', world, world, None),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
#for arm in ARM_JOINT_NAMES:
for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf),
('HandEmpty', arm), ('AtAConf', arm, conf), ('AtPose', arm, arm, None)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', world, body, pose)]
for surface in problem.surfaces:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
goal = ['and']
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
init += [('BConf', goal_conf)]
goal += [('AtBConf', goal_conf)]
goal += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('On', b, s) for b, s in problem.goal_on] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
stream_map = {
'sample-pose': get_stable_gen(problem),
'sample-grasp': from_list_fn(get_grasp_gen(problem)),
'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)),
'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)),
#'plan-base-motion': empty_gen(),
'BTrajCollision': fn_from_constant(False),
}
# get_press_gen(problem, teleport=teleport)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_pddlstream():
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
'test-pose': from_test(empty_test), # universe_test | empty_test
'sample-pose': from_constant((np.array([2, 0]),)),
'inv-kin': from_fn(ik_fn),
'motion': from_fn(motion_fn),
}
block = 'block1'
region = 'region1'
pose = np.array([1, 0])
conf = np.array([0, 0])
init = [
('Conf', conf),
('AtConf', conf),
('HandEmpty',),
('Block', block),
('Pose', pose),
('AtPose', block, pose),
('Region', region),
]
goal = ('In', block, region)
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
initial = tamp_problem.initial
mode = {b: Mode(p, None) for b, p in initial.block_poses.items()}
conf = conf_from_state(initial)
init = [
('CanMove',),
('Mode', mode),
('AtMode', mode),
('Conf', mode, conf),
('AtConf', conf),
]
goal = Exists(['?m', '?q'], And(('GoalState', '?m', '?q'),
('AtMode', '?m'), ('AtConf', '?q')))
stream_map = {
's-forward': from_gen_fn(sample_forward(tamp_problem)),
's-intersection': from_gen_fn(sample_intersection(tamp_problem)),
's-connection': from_gen_fn(sample_connection(tamp_problem)),
't-goal': from_test(test_goal_state(tamp_problem)),
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def 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 get_pddlstream(trajectories, element_bodies, ground_nodes):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
'test-cfree': from_test(get_test_cfree(element_bodies)),
}
init = []
for n in ground_nodes:
init.append(('Connected', n))
for t in trajectories:
init.extend([
('Node', t.n1),
('Node', t.n2),
('Element', t.element),
('Traj', t),
('Connection', t.n1, t.element, t, t.n2),
])
goal_literals = [('Printed', e) for e in element_bodies]
goal = And(*goal_literals)
# TODO: weight or order these in some way
# TODO: instantiation slowness is due to condition effects. Change!
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def pddlstream_from_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 get_pddlstream_test(node_points, elements, ground_nodes):
# stripstream/lis_scripts/run_print.py
# stripstream/lis_scripts/print_data.txt
domain_pddl = read(get_file_path(__file__, 'pddl/domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'pddl/stream.pddl'))
#stream_pddl = None
stream_map = {
'test-cfree': from_test(get_test_cfree({})),
}
nodes = list(range(len(node_points))) # TODO: sort nodes by height?
init = []
for n in nodes:
init.append(('Node', n))
for n in ground_nodes:
init.append(('Connected', n))
for e in elements:
init.append(('Element', e))
n1, n2 = e
t = None
init.extend([
('Connection', n1, e, t, n2),
('Connection', n2, e, t, n1),
])
#init.append(('Edge', n1, n2))
goal_literals = [('Printed', e) for e in elements]
goal = And(*goal_literals)
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def to_pddlstream(belief_problem, collisions=True):
locations = {l for (_, l, _) in belief_problem.initial + belief_problem.goal} | \
set(belief_problem.locations)
observations = [True, False]
uniform = UniformDist(locations)
initial_bel = {o: MixtureDD(DeltaDist(l), uniform, p) for o, l, p in belief_problem.initial}
max_p_collision = 0.25 if collisions else 1.0
# TODO: separate pick and place for move
init = [('Obs', obs) for obs in observations] + \
[('Location', l) for l in locations]
for o, d in initial_bel.items():
init += [('Dist', o, d), ('BLoc', o, d)]
for (o, l, p) in belief_problem.goal:
init += [('Location', l), ('GoalProb', l, p)]
goal_literals = [('BLocGE', o, l, p) for (o, l, p) in belief_problem.goal]
goal = And(*goal_literals)
directory = os.path.dirname(os.path.abspath(__file__))
domain_pddl = read(os.path.join(directory, 'domain.pddl'))
stream_pddl = read(os.path.join(directory, 'stream.pddl'))
constant_map = {}
stream_map = {
'BCollision': get_collision_test(max_p_collision),
'GE': from_test(ge_fn),
'prob-after-move': from_fn(get_move_fn(belief_problem.p_move_s)),
'MoveCost': move_cost_fn,
'prob-after-look': from_fn(get_look_fn(belief_problem.p_look_fp, belief_problem.p_look_fn)),
'LookCost': get_look_cost_fn(belief_problem.p_look_fp, belief_problem.p_look_fn),
#'PCollision': from_fn(prob_occupied), # Then can use GE
}
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert (initial.holding is None)
known_poses = list(initial.block_poses.values()) + \
list(tamp_problem.goal_poses.values())
directory = os.path.dirname(os.path.abspath(__file__))
domain_pddl = read(os.path.join(directory, 'domain_push.pddl'))
stream_pddl = read(os.path.join(directory, 'stream_push.pddl'))
constant_map = {}
init = [
('CanMove',),
('Conf', initial.conf),
('AtConf', initial.conf),
('HandEmpty',),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', p) for p in known_poses] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('GoalPose', p) for p in tamp_problem.goal_poses.values()]
goal = And(('AtConf', initial.conf),
*[('AtPose', b, p) for b, p in tamp_problem.goal_poses.items()])
# TODO: convert to lower case
stream_map = {
'push-target': from_list_fn(push_target_fn),
'push-direction': push_direction_gen_fn,
'test-cfree': from_test(lambda *args: not collision_test(*args)),
'distance': distance_fn,
}
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def 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 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_pddlstream2(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=[]):
domain_pddl = read(get_file_path(
__file__, 'regression.pddl')) # progression | regression
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
'test-cfree':
from_test(get_test_cfree(element_bodies)),
#'sample-print': from_gen_fn(get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes)),
'sample-print':
get_wild_print_gen_fn(robot, obstacles, node_points, element_bodies,
ground_nodes),
}
# TODO: assert that all elements have some support
init = []
for n in ground_nodes:
init.append(('Grounded', n))
for e in element_bodies:
for n in e:
if element_supports(e, n, node_points):
init.append(('Supports', e, n))
if is_start_node(n, e, node_points):
init.append(('StartNode', n, e))
for e in element_bodies:
n1, n2 = e
init.extend([
('Node', n1),
('Node', n2),
('Element', e),
('Printed', e),
('Edge', n1, e, n2),
('Edge', n2, e, n1),
#('StartNode', n1, e),
#('StartNode', n2, e),
])
#if is_ground(e, ground_nodes):
# init.append(('Grounded', e))
#for e1, neighbors in get_element_neighbors(element_bodies).items():
# for e2 in neighbors:
# init.append(('Supports', e1, e2))
for t in trajectories:
init.extend([
('Traj', t),
('PrintAction', t.n1, t.element, t),
])
goal = And(*[('Removed', e) for e in element_bodies])
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def pddlstream_from_problem(problem):
# TODO: push and attach to movable objects
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
# TODO: action to generically connect to the roadmap
# TODO: could check individual vertices first
# TODO: dynamically generate the roadmap in interesting parts of the space
# TODO: visibility graphs for sparse roadmaps
# TODO: approximate robot with isotropic geometry
# TODO: make the effort finite if applied to the roadmap vertex
samples = []
init = []
for robot, conf in problem.initial_confs.items():
samples.append(conf)
init += [('Robot', robot), ('Conf', robot, conf),
('AtConf', robot, conf), ('Free', robot)]
for body, pose in problem.initial_poses.items():
init += [('Body', body), ('Pose', body, pose), ('AtPose', body, pose)]
goal_literals = []
goal_literals += [('Holding', robot, body)
for robot, body in problem.goal_holding.items()]
for robot, base_values in problem.goal_confs.items():
q_goal = Conf(robot, get_base_joints(robot), base_values)
samples.append(q_goal)
init += [('Conf', robot, q_goal)]
goal_literals += [('AtConf', robot, q_goal)]
goal_formula = And(*goal_literals)
# TODO: assuming holonomic for now
[body] = problem.robots
with LockRenderer():
init += create_vertices(problem, body, samples)
#init += create_edges(problem, body, samples)
stream_map = {
'test-cfree-conf-pose': from_test(get_test_cfree_conf_pose(problem)),
'test-cfree-traj-pose': from_test(get_test_cfree_traj_pose(problem)),
# TODO: sample pushes rather than picks/places
'sample-grasp': from_gen_fn(get_grasp_generator(problem)),
'compute-ik': from_fn(get_ik_fn(problem)),
'compute-motion': from_fn(get_motion_fn(problem)),
'test-reachable': from_test(lambda *args: False),
'Cost': get_cost_fn(problem),
}
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
def pddlstream_from_tamp(tamp_problem, use_stream=True, use_optimizer=False, collisions=True):
initial = tamp_problem.initial
assert(initial.holding is None)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
external_paths = []
if use_stream:
external_paths.append(get_file_path(__file__, 'stream.pddl'))
if use_optimizer:
external_paths.append(get_file_path(__file__, 'optimizer.pddl'))
external_pddl = [read(path) for path in external_paths]
constant_map = {}
init = [
('CanMove',),
('Conf', initial.conf),
('AtConf', initial.conf),
('HandEmpty',),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', b, p) for b, p in initial.block_poses.items()] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \
[('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()] + \
[('Region', r) for r in tamp_problem.goal_regions.values() + [GROUND_NAME]]
goal_literals = [('In', b, r) for b, r in tamp_problem.goal_regions.items()] #+ [('HandEmpty',)]
if tamp_problem.goal_conf is not None:
goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal = And(*goal_literals)
stream_map = {
's-motion': from_fn(plan_motion),
's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)),
't-region': from_test(get_region_test(tamp_problem.regions)),
's-ik': from_fn(inverse_kin_fn),
#'s-ik': from_gen_fn(unreliable_ik_fn),
'distance': distance_fn,
't-cfree': from_test(lambda *args: implies(collisions, not collision_test(*args))),
#'posecollision': collision_test, # Redundant
'trajcollision': lambda *args: False,
}
if use_optimizer:
stream_map.update({
'gurobi': from_fn(get_optimize_fn(tamp_problem.regions)),
'rrt': from_fn(cfree_motion_fn),
})
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem):
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 create_problem(initial_poses):
block_goal = (-25, 0, 0)
initial_atoms = [
('IsPose', COASTER, block_goal),
('Empty', ROBOT),
('CanMove', ROBOT),
('HasSugar', 'sugar_cup'),
('HasCream', 'cream_cup'),
('IsPourable', 'cream_cup'),
('Stackable', CUP, COASTER),
('Clear', COASTER),
]
goal_literals = [
('AtPose', COASTER, block_goal),
('On', CUP, COASTER),
('HasCoffee', CUP),
('HasCream', CUP),
('HasSugar', CUP),
('Mixed', CUP),
('Empty', ROBOT),
]
for name, pose in initial_poses.items():
if 'gripper' in name:
initial_atoms += [('IsGripper', name)]
if 'cup' in name:
initial_atoms += [('IsCup', name)]
if 'spoon' in name:
initial_atoms += [('IsSpoon', name), ('IsStirrer', name)]
if 'stirrer' in name:
initial_atoms += [('IsStirrer', name)]
if 'block' in name:
initial_atoms += [('IsBlock', name)]
initial_atoms += [
('IsPose', name, pose),
('AtPose', name, pose),
('TableSupport', pose),
]
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
stream_map = DEBUG
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
initial_atoms, And(*goal_literals))
def read_minizinc_data(path):
# https://github.com/yijiangh/Choreo/blob/9481202566a4cff4d49591bec5294b1e02abcc57/framefab_task_sequence_planning/framefab_task_sequence_planner/minizinc/as_minizinc_data_layer_1.dzn
data = read(path)
n = int(re.findall(r'n = (\d+);', data)[0])
m = int(re.findall(r'm = (\d+);', data)[0])
# print re.search(r'n = (\d+);', data).group(0)
g_data = np.array(re.findall(r'G_data = \[([01,]*)\];',
data)[0].split(',')[:-1],
dtype=int)
a_data = np.array(re.findall(r'A_data = \[([01,]*)\];',
data)[0].split(',')[:-1],
dtype=int).reshape([n, n])
t_data = np.array(re.findall(r'T_data = \[([01,]*)\];',
data)[0].split(',')[:-1],
dtype=int).reshape([n, n, m])
print(n, m)
print(g_data.shape, g_data.size,
np.sum(g_data)) # 1 if edge(e) is grounded
print(a_data.shape, a_data.size,
np.sum(a_data)) # 1 if edge(e) and edge(j) share a node
print(
t_data.shape, t_data.size, np.sum(t_data)
) # 1 if printing edge e with orientation a does not collide with edge j
elements = list(range(n))
orientations = list(range(m))
#orientations = random.sample(orientations, 10)
return g_data, a_data, t_data
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 run_search(temp_dir,
planner='max-astar',
max_time=INF,
max_cost=INF,
debug=False):
if max_time == INF:
max_time = 'infinity'
else:
max_time = int(max_time)
if max_cost == INF:
max_cost = 'infinity'
else:
max_cost = int(max_cost)
t0 = time()
search = os.path.join(FD_BIN, SEARCH_COMMAND)
planner_config = SEARCH_OPTIONS[planner] % (max_time, max_cost)
command = search % (temp_dir + SEARCH_OUTPUT, planner_config,
temp_dir + TRANSLATE_OUTPUT)
if debug:
print('\nSearch command:', command)
p = os.popen(command) # NOTE - cannot pipe input easily with subprocess
output = p.read()
if debug:
print(output[:-1])
print('Search runtime:', time() - t0)
if not os.path.exists(temp_dir + SEARCH_OUTPUT):
return None
return read(temp_dir + SEARCH_OUTPUT)
def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert(initial.holding is None)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
external_pddl = [
read(get_file_path(__file__, 'stream.pddl')),
#read(get_file_path(__file__, 'optimizer.pddl')),
]
constant_map = {}
init = [
('CanMove',),
('Conf', initial.conf),
('AtConf', initial.conf),
('HandEmpty',),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', b, p) for b, p in initial.block_poses.items()] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \
[('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]
goal_literals = [('In', b, r) for b, r in tamp_problem.goal_regions.items()] #+ [('HandEmpty',)]
if tamp_problem.goal_conf is not None:
goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal = And(*goal_literals)
stream_map = {
's-motion': from_fn(plan_motion),
's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)),
't-region': from_test(get_region_test(tamp_problem.regions)),
's-ik': from_fn(inverse_kin_fn),
'distance': distance_fn,
't-cfree': from_test(lambda *args: not collision_test(*args)),
'posecollision': collision_test, # Redundant
'trajcollision': lambda *args: False,
'gurobi': from_fn(get_optimize_fn(tamp_problem.regions)),
'rrt': from_fn(cfree_motion_fn),
#'reachable': from_test(reachable_test),
#'Valid': valid_state_fn,
}
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
def get_pddlstream_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 parse_plans(temp_path, plan_files):
best_plan, best_makespan = None, INF
for plan_file in plan_files:
solution = read(os.path.join(temp_path, plan_file))
plan, makespan = parse_temporal_solution(solution)
if makespan < best_makespan:
best_plan, best_makespan = plan, makespan
return best_plan, best_makespan
def parse_solutions(temp_path, plan_files):
best_plan, best_cost = None, INF
for plan_file in plan_files:
solution = read(os.path.join(temp_path, plan_file))
plan, cost = parse_solution(solution)
if cost < best_cost:
best_plan, best_cost = plan, cost
return best_plan, best_cost
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_belief(initial_belief):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = None
stream_map = {}
init = [
#('CanMove',),
Equal(('RegisterCost', ), scale_cost(1)),
Equal(('PickCost', ),
scale_cost(1)), # TODO: imperfect transition model
Equal(('PlaceCost', ), scale_cost(1)),
]
for item, dist in initial_belief.items():
support = dist.support()
if len(support) == 1:
init += [('On', item, support[0]), ('Localized', item)]
else:
init += [('Unknown', item)]
for i2 in support:
p_obs = dist.prob(i2)
cost = revisit_mdp_cost(
1, 1, p_obs) # TODO: imperfect observation model
if cost == INF:
continue
if i2 in initial_belief:
init += [('FiniteScanCost', i2, item),
Equal(('ScanCost', i2, item), scale_cost(cost))]
graspable_classes = [SOUP, GREEN]
for item in initial_belief:
for cl in filter(lambda c: is_class(item, c), CLASSES):
init += [('Class', item, cl)]
if cl in graspable_classes:
init += [('Graspable', item)] # TODO: include hand?
stackable_classes = [(TABLE, ROOM), (SOUP, TABLE), (GREEN, TABLE)]
for cl1, cl2 in stackable_classes:
for i1 in filter(lambda i: is_class(i, cl1), initial_belief):
for i2 in filter(lambda i: is_class(i, cl2), initial_belief):
init += [('Stackable', i1, i2)]
arms = ['left', 'right']
for arm in arms:
init += [('Arm', arm), ('HandEmpty', arm)]
goal_literals = [('On', 'soup0', 'table1'), ('Registered', 'soup0'),
('HoldingClass', 'green')]
#goal_literals = [('Holding', 'left', 'soup0')]
#goal_literals = [('HoldingClass', soup)]
#goal_literals = [Nearby('table1')]
#goal_literals = [('HoldingClass', 'green'), ('HoldingClass', 'soup')]
goal = And(*goal_literals)
return 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
