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 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', ), 1),
Equal(('PickCost', ), 1), # TODO: imperfect transition model
Equal(('PlaceCost', ), 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), 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 create_problem(tamp_problem, hand_empty=False, manipulate_cost=1.):
initial = tamp_problem.initial
assert (not initial.holding)
init = [
#('Region', GROUND_NAME),
Equal(('Cost',), manipulate_cost),
Equal((TOTAL_COST,), 0)] + \
[('Stove', r) for r in STOVE_NAMES if r in tamp_problem.regions] + \
[('Placeable', b, r) for b in initial.block_poses.keys()
for r in tamp_problem.regions if (r in ENVIRONMENT_NAMES) or
((b in tamp_problem.goal_cooked) and (r in STOVE_NAMES))]
for b, p in initial.block_poses.items():
init += [
('Block', b),
('Pose', b, p),
('AtPose', b, p),
#('Grasp', b, GRASP),
]
goal_literals = [] + \
[('Cooked', b) for b in tamp_problem.goal_cooked] # Placeable for the stove
for b, r in tamp_problem.goal_regions.items():
if isinstance(r, np.ndarray):
init += [('Pose', b, r)]
goal_literals += [('AtPose', b, r)]
else:
blocks = [b] if isinstance(b, str) else b
regions = [r] if isinstance(r, str) else r
conditions = []
for body, region in product(blocks, regions):
init += [('Region', region), ('Placeable', body, region)]
conditions += [('In', body, region)]
goal_literals.append(Or(*conditions))
for r, q in initial.robot_confs.items():
init += [
('Robot', r),
('CanMove', r),
('Conf', q),
('AtConf', r, q),
('HandEmpty', r),
]
if hand_empty:
goal_literals += [('HandEmpty', r)]
if tamp_problem.goal_conf is not None:
# goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal_literals += [('AtConf', r, q)]
# goal_literals += [Not(('Unsafe',))] # ('HandEmpty',)
goal = And(*goal_literals)
return init, goal
def create_problem(tamp_problem):
initial = tamp_problem.initial
assert (not initial.holding)
init = [
#('Region', GROUND_NAME),
Equal(('Cost',), 0),
Equal((TOTAL_COST,), 0)] + \
[('Stove', r) for r in STOVE_NAMES if r in tamp_problem.regions] + \
[('Placeable', b, r) for b in initial.block_poses.keys()
for r in tamp_problem.regions if (r in ENVIRONMENT_NAMES) or
((b in tamp_problem.goal_cooked) and (r in STOVE_NAMES))]
for b, p in initial.block_poses.items():
init += [
('Block', b),
('Pose', b, p),
('AtPose', b, p),
('Grasp', b, GRASP),
]
goal_literals = [] + \
[('Cooked', b) for b in tamp_problem.goal_cooked] # Placeable for the stove
for b, r in tamp_problem.goal_regions.items():
if isinstance(r, str):
init += [('Region', r), ('Placeable', b, r)]
goal_literals += [('In', b, r)]
elif isinstance(r, list):
for region in r:
init += [('Region', region), ('Placeable', b, region)]
goal_literals.append(Or(*[('In', b, region) for region in r]))
else:
init += [('Pose', b, r)]
goal_literals += [('AtPose', b, r)]
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)]
# goal_literals += [Not(('Unsafe',))] # ('HandEmpty',)
goal = And(*goal_literals)
return init, goal
def fact_from_evaluation(evaluation):
fact = Fact(evaluation.head.function, evaluation.head.args)
if is_atom(evaluation):
return fact
elif is_negated_atom(evaluation):
return Not(fact)
return Equal(fact, evaluation.value)
def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert (initial.holding is None)
known_poses = list(initial.block_poses.values()) + \
list(tamp_problem.goal_poses.values())
directory = os.path.dirname(os.path.abspath(__file__))
domain_pddl = read(os.path.join(directory, 'domain_push.pddl'))
stream_pddl = read(os.path.join(directory, 'stream_push.pddl'))
constant_map = {}
init = [
('CanMove',),
('Conf', initial.conf),
('AtConf', initial.conf),
('HandEmpty',),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', p) for p in known_poses] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('GoalPose', p) for p in tamp_problem.goal_poses.values()]
goal = And(('AtConf', initial.conf),
*[('AtPose', b, p) for b, p in tamp_problem.goal_poses.items()])
# TODO: convert to lower case
stream_map = {
'push-target': from_list_fn(push_target_fn),
'push-direction': push_direction_gen_fn,
'test-cfree': from_test(lambda *args: not collision_test(*args)),
'distance': distance_fn,
}
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_tamp(tamp_problem, 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)
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)
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)),
#'test-cfree': from_gen_fn(noisy_collision_gen_fn),
'collision': collision_test,
#'constraint-solver': None,
'distance': distance_fn,
}
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def create_problem(tamp_problem):
initial = tamp_problem.initial
assert (not initial.holding)
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()] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()]
#[('Grasp', b, GRASP) for b in initial.block_poses]
goal_literals = []
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)]
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)]
# goal_literals += [Not(('Unsafe',))] # ('HandEmpty',)
goal = And(*goal_literals)
return init, goal
def certified(self):
if self._certified is None:
self._certified = [Equal(self.instance.head, self.value)]
return self._certified
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(robots,
static_obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
initial_confs={},
printed=set(),
removed=set(),
additional_init=[],
additional_orders=set(),
trajectories=[],
temporal=True,
sequential=False,
local=False,
can_print=True,
can_transit=False,
checker=None,
**kwargs):
try:
get_tfd_path()
except RuntimeError:
temporal = False
print(
'Temporal Fast Downward is not installed. Disabling temporal planning.'
)
# TODO: TFD submodule
assert not removed & printed
remaining = set(element_bodies) - removed - printed
element_obstacles = {element_bodies[e] for e in printed}
obstacles = set(static_obstacles) | element_obstacles
max_layer = max(layer_from_n.values())
directions = compute_directions(remaining, layer_from_n)
if local:
# makespan seems more effective than CEA
partial_orders = compute_local_orders(
remaining, layer_from_n) # makes the makespan heuristic slow
else:
partial_orders = compute_global_orders(remaining, layer_from_n)
partial_orders.update(additional_orders)
# draw_model(supporters, node_points, ground_nodes, color=RED)
# wait_if_gui()
domain_pddl = read(
get_file_path(
__file__,
os.path.join('pddl',
'temporal.pddl' if temporal else 'domain.pddl')))
stream_pddl = read(get_file_path(__file__, 'pddl/stream.pddl'))
constant_map = {}
# TODO: don't evaluate TrajTrajCollision until the plan is retimed
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-move':
get_wild_move_gen_fn(robots,
obstacles,
element_bodies,
partial_orders=partial_orders,
**kwargs),
'sample-print':
get_wild_print_gen_fn(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
initial_confs=initial_confs,
partial_orders=partial_orders,
removed=removed,
**kwargs),
#'test-stiffness': from_test(test_stiffness),
#'test-cfree-traj-conf': from_test(lambda *args: True),
#'test-cfree-traj-traj': from_test(get_cfree_test(**kwargs)),
'TrajTrajCollision':
get_collision_test(robots, **kwargs),
'TrajConfCollision':
lambda *args, **kwargs:
False, # TODO: could treat conf as length 1 traj
'Length':
lambda e: get_element_length(e, node_points),
'Distance':
lambda r, t: t.get_link_distance(),
'Duration':
lambda r, t: t.get_link_distance() / TOOL_VELOCITY,
'Euclidean':
lambda n1, n2: get_element_length((n1, n2), node_points),
}
assignments = compute_assignments(robots, remaining, node_points,
initial_confs)
transits = compute_transits(layer_from_n, directions)
init = [
('Stationary', ),
Equal(('Speed', ), TOOL_VELOCITY),
]
init.extend(additional_init)
if can_print:
init.append(('Print', ))
if can_transit:
init.append(('Move', ))
if sequential:
init.append(('Sequential', ))
for name, conf in initial_confs.items():
robot = index_from_name(robots, name)
#init_node = -robot
init_node = '{}-q0'.format(robot)
init.extend([
#('Node', init_node),
('BackoffConf', name, conf),
('Robot', name),
('Conf', name, conf),
('AtConf', name, conf),
('Idle', name),
#('CanMove', name),
#('Start', name, init_node, None, conf),
#('End', name, None, init_node, conf),
])
for (n1, e, n2) in directions:
if layer_from_n[n1] == 0:
transits.append((None, init_node, n1, e))
if layer_from_n[n2] == max_layer:
transits.append((e, n2, init_node, None))
#init.extend(('Grounded', n) for n in ground_nodes)
init.extend(('Direction', ) + tup for tup in directions)
init.extend(('Order', ) + tup for tup in partial_orders)
# TODO: can relax assigned if I go by layers
init.extend(
('Assigned', r, e) for r in assignments for e in assignments[r])
#init.extend(('Transit',) + tup for tup in transits)
# TODO: only move actions between adjacent layers
for e in remaining:
n1, n2 = e
#n1, n2 = ['n{}'.format(i) for i in e]
init.extend([
('Node', n1),
('Node', n2),
('Element', e),
('Printed', e),
])
assert not trajectories
# for t in trajectories:
# init.extend([
# ('Traj', t),
# ('PrintAction', t.n1, t.element, t),
# ])
goal_literals = []
if can_transit:
goal_literals.extend(
('AtConf', r, q) for r, q in initial_confs.items())
goal_literals.extend(('Removed', e) for e in remaining)
goal = And(*goal_literals)
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def get_certified(self):
return [Equal(self.instance.get_head(), self.value)]
def pddlstream_from_problem(problem, teleport=False):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {'{}'.format(name).lower(): name
for name in problem.initial_confs.keys()}
edges = set()
init = [
]
for name, conf in problem.initial_confs.items():
init += [
('Safe',),
('Robot', name),
('Conf', conf),
('AtConf', name, conf),
Equal(('Speed', name), DIST_PER_TIME),
Equal(('BatteryCapacity', name), MAX_ENERGY),
Equal(('RechargeRate', name), CHARGE_PER_TIME),
Equal(('ConsumptionRate', name), BURN_PER_TIME),
Equal(('Energy', name), INITIAL_ENERGY),
]
goal_literals = [
#('Safe',),
#('Unachievable',),
]
for name, base_values in problem.goal_confs.items():
body = problem.get_body(name)
joints = get_base_joints(body)
#extend_fn = get_extend_fn(body, joints, resolutions=5*BASE_RESOLUTIONS)
#q_init = problem.initial_confs[name]
#path = [q_init] + [Conf(body, joints, q) for q in extend_fn(q_init.values, base_values)]
#edges.update(zip(path, path[1:]))
#q_goal = path[-1]
q_goal = Conf(body, joints, base_values)
init += [('Conf', q_goal)]
goal_literals += [('AtConf', name, q_goal)]
goal_formula = And(*goal_literals)
robot = list(map(problem.get_body, problem.initial_confs))[0]
with LockRenderer():
init += [('Conf', q) for _, n, q in create_vertices(problem, robot, [], samples_per_ft2=6)]
#vertices = {v for edge in edges for v in edge}
#handles = []
#for vertex in vertices:
# handles.extend(draw_point(point_from_conf(vertex.values), size=0.05))
#for conf1, conf2 in edges:
# traj = linear_trajectory(conf1, conf2)
# init += [
# ('Conf', conf1),
# ('Traj', traj),
# ('Conf', conf2),
# ('Motion', conf1, traj, conf2),
# ]
#draw_edges(edges)
cfree_traj_traj_test = get_test_cfree_traj_traj(problem)
cfree_traj_traj_list_gen_fn = from_test(cfree_traj_traj_test)
traj_traj_collision_test = negate_test(cfree_traj_traj_test)
stream_map = {
'test-cfree-conf-conf': cfree_traj_traj_list_gen_fn,
'test-cfree-traj-conf': cfree_traj_traj_list_gen_fn,
'test-cfree-traj-traj': cfree_traj_traj_list_gen_fn,
'ConfConfCollision': traj_traj_collision_test,
'TrajConfCollision': traj_traj_collision_test,
'TrajTrajCollision': traj_traj_collision_test,
'compute-motion': from_fn(get_motion_fn2(problem)),
'TrajDistance': get_distance_fn(),
}
#stream_map = 'debug'
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
return problem, edges
def pdddlstream_from_problem(belief,
additional_init=[],
fixed_base=True,
**kwargs):
world = belief.world # One world per state
task = world.task # One task per world
print(task)
domain_pddl = read(get_file_path(__file__, '../pddl/domain.pddl'))
# TODO: repackage stream outputs to avoid recomputation
# Despite the base not moving, it could be re-estimated
init_bq = belief.base_conf
init_aq = belief.arm_conf
init_gq = belief.gripper_conf
carry_aq = world.carry_conf
init_aq = carry_aq if are_confs_close(init_aq, carry_aq) else init_aq
# TODO: the following doesn't work. Maybe because carry_conf is used elsewhere
#carry_aq = init_aq if are_confs_close(init_aq, world.carry_conf) else world.carry_conf
#calibrate_aq = init_aq if are_confs_close(init_aq, world.calibrate_conf) else world.calibrate_conf
# Don't need this now that returning to old confs
#open_gq = init_gq if are_confs_close(init_gq, world.open_gq) else world.open_gq
#closed_gq = init_gq if are_confs_close(init_gq, world.closed_gq) else world.closed_gq
open_gq = world.open_gq
closed_gq = world.closed_gq
constant_map = {
'@world': 'world',
'@gripper': 'gripper',
'@stove': 'stove',
'@none': None,
'@rest_aq': carry_aq,
#'@calibrate_aq': calibrate_aq,
'@open_gq': open_gq,
'@closed_gq': closed_gq,
'@open': OPEN,
'@closed': CLOSED,
'@top': TOP_GRASP,
'@side': SIDE_GRASP,
'@bq0': init_bq,
}
top_joint = JOINT_TEMPLATE.format(TOP_DRAWER)
bottom_joint = JOINT_TEMPLATE.format(BOTTOM_DRAWER)
init = [
('BConf', init_bq),
('AtBConf', init_bq),
('AConf', init_bq, carry_aq),
#('RestAConf', carry_aq),
#('AConf', init_bq, calibrate_aq),
(
'Stationary', ),
('AConf', init_bq, init_aq),
('AtAConf', init_aq),
('GConf', open_gq),
('GConf', closed_gq),
('Grasp', None, None),
('AtGrasp', None, None),
('Above', top_joint, bottom_joint),
('Adjacent', top_joint, bottom_joint),
('Adjacent', bottom_joint, top_joint),
('Calibrated', ),
('CanMoveBase', ),
('CanMoveArm', ),
('CanMoveGripper', ),
] + list(task.init) + list(additional_init)
for action_name, cost in ACTION_COSTS.items():
function_name = '{}Cost'.format(title_from_snake(action_name))
function = (function_name, )
init.append(Equal(function, cost)) # TODO: stove state
init += [('Stackable', name, surface) for name, surface in task.goal_on.items()] + \
[('Stackable', name, stove) for name, stove in product(task.goal_cooked, STOVES)] + \
[('Pressed', name) for name in belief.pressed] + \
[('Cookable', name) for name in task.goal_cooked] + \
[('Cooked', name) for name in belief.cooked] + \
[('Status', status) for status in DOOR_STATUSES] + \
[('Knob', knob) for knob in KNOBS] + \
[('Joint', knob) for knob in KNOBS] + \
[('Liquid', liquid) for _, liquid in task.init_liquid] + \
[('HasLiquid', cup, liquid) for cup, liquid in belief.liquid] + \
[('StoveKnob', STOVE_TEMPLATE.format(loc), KNOB_TEMPLATE.format(loc)) for loc in STOVE_LOCATIONS] + \
[('GraspType', ty) for ty in task.grasp_types] # TODO: grasp_type per object
#[('Type', obj_name, 'stove') for obj_name in STOVES] + \
#[('Camera', name) for name in world.cameras]
if task.movable_base:
init.append(('MovableBase', ))
if fixed_base:
init.append(('InitBConf', init_bq))
if task.noisy_base:
init.append(('NoisyBase', ))
compute_pose_kin = get_compute_pose_kin(world)
compute_angle_kin = get_compute_angle_kin(world)
initial_poses = {}
for joint_name, init_conf in belief.door_confs.items():
if joint_name in DRAWER_JOINTS:
init.append(('Drawer', joint_name))
if joint_name in CABINET_JOINTS:
init.append(('Cabinet', joint_name))
joint = joint_from_name(world.kitchen, joint_name)
surface_name = surface_from_joint(joint_name)
init.append(('SurfaceJoint', surface_name, joint_name))
# Relies on the fact that drawers have identical surface and link names
link_name = get_link_name(world.kitchen, child_link_from_joint(joint))
#link_name = str(link_name.decode('UTF-8'))
#link_name = str(link_name.encode('ascii','ignore'))
for conf in {
init_conf, world.open_kitchen_confs[joint],
world.closed_kitchen_confs[joint]
}:
# TODO: return to initial poses?
world_pose, = compute_angle_kin(link_name, joint_name, conf)
init.extend([
('Joint', joint_name),
('Angle', joint_name, conf),
('Obstacle', link_name),
('AngleKin', link_name, world_pose, joint_name, conf),
('WorldPose', link_name, world_pose),
])
if joint in world.kitchen_joints:
init.extend([
('Sample', world_pose),
#('Value', world_pose), # comment out?
])
if conf == init_conf:
initial_poses[link_name] = world_pose
init.extend([
('AtAngle', joint_name, conf),
('AtWorldPose', link_name, world_pose),
])
for surface_name in ALL_SURFACES:
if surface_name in OPEN_SURFACES:
init.append(('Counter', surface_name)) # Fixed surface
if surface_name in DRAWERS:
init.append(('Drawer', surface_name))
surface = surface_from_name(surface_name)
surface_link = link_from_name(world.kitchen, surface.link)
parent_joint = parent_joint_from_link(surface_link)
if parent_joint not in world.kitchen_joints:
# TODO: attach to world frame?
world_pose = RelPose(world.kitchen, surface_link, init=True)
initial_poses[surface_name] = world_pose
init += [
#('RelPose', surface_name, world_pose, 'world'),
('WorldPose', surface_name, world_pose),
#('AtRelPose', surface_name, world_pose, 'world'),
('AtWorldPose', surface_name, world_pose),
('Sample', world_pose),
#('Value', world_pose),
]
init.extend([
('CheckNearby', surface_name),
#('InitPose', world_pose),
('Localized', surface_name),
])
for grasp_type in task.grasp_types:
if (surface_name in OPEN_SURFACES) or has_place_database(
world.robot_name, surface_name, grasp_type):
init.append(('AdmitsGraspType', surface_name, grasp_type))
if belief.grasped is None:
init.extend([
('HandEmpty', ),
('GConf', init_gq),
('AtGConf', init_gq),
])
else:
obj_name = belief.grasped.body_name
assert obj_name not in belief.pose_dists
grasp = belief.grasped
init += [
# Static
#('Graspable', obj_name),
('Grasp', obj_name, grasp),
('IsGraspType', obj_name, grasp, grasp.grasp_type),
# Fluent
('AtGrasp', obj_name, grasp),
('Holding', obj_name),
('Localized', obj_name),
]
init.extend(('ValidGraspType', obj_name, grasp_type)
for grasp_type in task.grasp_types
if implies(world.is_real(),
is_valid_grasp_type(obj_name, grasp_type)))
for obj_name in world.movable:
obj_type = type_from_name(obj_name)
if obj_type in BOWLS:
init.append(('Bowl', obj_name))
else:
init.append(
('Obstacle', obj_name)) # TODO: hack to place within bowls
if obj_type in COOKABLE:
init.append(('Cookable', obj_name))
if obj_type in POURABLE:
init.append(('Pourable', obj_name))
init += [
('Entity', obj_name),
('CheckNearby', obj_name),
] + [('Stackable', obj_name, counter)
for counter in set(ALL_SURFACES) & set(COUNTERS)]
# TODO: track poses over time to produce estimates
for obj_name, pose_dist in belief.pose_dists.items():
dist_support = pose_dist.dist.support()
localized = pose_dist.is_localized()
graspable = True
if localized:
init.append(('Localized', obj_name))
[rel_pose] = dist_support
roll, pitch, yaw = euler_from_quat(
quat_from_pose(rel_pose.get_reference_from_body()))
if (MAX_ERROR < abs(roll)) or (MAX_ERROR < abs(pitch)):
graspable = False
print(
'{} has an invalid orientation: roll={:.3f}, pitch={:.3f}'.
format(obj_name, roll, pitch))
if graspable:
#init.append(('Graspable', obj_name))
init.extend(('ValidGraspType', obj_name, grasp_type)
for grasp_type in task.grasp_types
if implies(world.is_real(),
is_valid_grasp_type(obj_name, grasp_type)))
# Could also fully decompose into points (but many samples)
# Could immediately add likely points for collision checking
for rel_pose in (dist_support if localized else pose_dist.decompose()):
surface_name = rel_pose.support
if surface_name is None:
# Treats as obstacle
# TODO: could temporarily add to fixed
world_pose = rel_pose
init += [
('WorldPose', obj_name, world_pose),
('AtWorldPose', obj_name, world_pose),
]
poses = [world_pose]
#raise RuntimeError(obj_name, supporting)
else:
surface_pose = initial_poses[surface_name]
world_pose, = compute_pose_kin(obj_name, rel_pose,
surface_name, surface_pose)
init += [
# Static
('RelPose', obj_name, rel_pose, surface_name),
('WorldPose', obj_name, world_pose),
('PoseKin', obj_name, world_pose, rel_pose, surface_name,
surface_pose),
# Fluent
('AtRelPose', obj_name, rel_pose, surface_name),
('AtWorldPose', obj_name, world_pose),
]
if localized:
init.append(('On', obj_name, surface_name))
poses = [rel_pose, world_pose]
for pose in poses:
if isinstance(pose, PoseDist):
init.append(('Dist', pose))
else:
init.extend([('Sample', pose)]) #, ('Value', pose)])
#for body, ty in problem.body_types:
# init += [('Type', body, ty)]
#bodies_from_type = get_bodies_from_type(problem)
#bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(ty) else [ty]
goal_formula = get_goal(belief, init)
stream_pddl, stream_map = get_streams(world,
teleport_base=task.teleport_base,
**kwargs)
print('Constants:', constant_map)
print('Init:', sorted(init, key=lambda f: f[0]))
print('Goal:', goal_formula)
#print('Streams:', stream_map.keys()) # 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 (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_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)