def get_problem():
domain_pddl = read_file('domain.pddl')
constant_map = dict()
table_aabb, robot, region, objects, poses, init_extend = load_world()
stream_pddl = read_file('stream.pddl')
stream_map = {
's-pose-kin': from_fn(get_pose_kin),
's-push-conf': from_fn(get_push_conf_fn(robot.conf.pos[2])),
# 's-touch-region': from_gen_fn(pose_gen),
't-cfree-push': from_test(test_cfree_push),
't-feasible': from_test(test_feasible),
't-touches': from_test(test_touches),
}
init = [
('Robot', robot),
('Conf', robot.conf),
('AtConf', robot, robot.conf),
('Region', region),
]
for i in range(len(objects)):
obj = objects[i]
init.append(('Obj', obj))
init.append(('WorldPose', obj, obj.pose))
init.append(('AtWorldPose', obj, obj.pose))
for pose in poses[i]:
init.append(('WorldPose', obj, pose))
init.extend(init_extend)
both_in_region = ('and', ('Overlaps', objects[0], region),
('Overlaps', objects[1], region))
in_region = ('and', ('Overlaps', objects[0], region))
goal = in_region
goal_no_side_effects = goal + tuple(
('not', ('UnknownPose', obj)) for obj in objects)
print('init:')
for pred in init:
print(pred)
print('goal:', goal)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal_no_side_effects
def get_pddlstream_info(robot,
fixed,
movable,
add_slanted_grasps,
approach_frame,
use_vision,
home_poses=None):
domain_pddl = read('tamp/domain_stacking.pddl')
stream_pddl = read('tamp/stream_stacking.pddl')
constant_map = {}
fixed = [f for f in fixed if f is not None]
stream_map = {
'sample-pose-table':
from_list_fn(primitives.get_stable_gen_table(fixed)),
'sample-pose-home':
from_list_fn(primitives.get_stable_gen_home(home_poses, fixed)),
'sample-pose-block':
from_fn(primitives.get_stable_gen_block(fixed)),
'sample-grasp':
from_list_fn(
primitives.get_grasp_gen(robot,
add_slanted_grasps=True,
add_orthogonal_grasps=False)),
# 'sample-grasp': from_gen_fn(primitives.get_grasp_gen(robot, add_slanted_grasps=True, add_orthogonal_grasps=False)),
'pick-inverse-kinematics':
from_fn(
primitives.get_ik_fn(robot,
fixed,
approach_frame='gripper',
backoff_frame='global',
use_wrist_camera=use_vision)),
'place-inverse-kinematics':
from_fn(
primitives.get_ik_fn(robot,
fixed,
approach_frame='global',
backoff_frame='gripper',
use_wrist_camera=False)),
'plan-free-motion':
from_fn(primitives.get_free_motion_gen(robot, fixed)),
'plan-holding-motion':
from_fn(primitives.get_holding_motion_gen(robot, fixed)),
}
return domain_pddl, constant_map, stream_pddl, stream_map
def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert(initial.holding is None)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
# TODO: can always make the state the set of fluents
#state = tamp_problem.initial
state = {
R: tamp_problem.initial.conf,
H: tamp_problem.initial.holding,
}
for b, p in tamp_problem.initial.block_poses.items():
state[b] = p
init = [
('State', state),
('AtState', state),
('Conf', initial.conf)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', b, p) for b, p in initial.block_poses.items()] + \
[('Region', r) for r in tamp_problem.regions.keys()] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND) for b in initial.block_poses.keys()] + \
[('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]
goal = ('AtGoal',)
stream_map = {
'plan-motion': from_fn(plan_motion),
'sample-pose': from_gen_fn(get_pose_gen(tamp_problem.regions)),
'test-region': from_test(get_region_test(tamp_problem.regions)),
'inverse-kinematics': from_fn(inverse_kin_fn),
#'posecollision': collision_test,
#'trajcollision': lambda *args: False,
'forward-move': from_fn(move_fn),
'forward-pick': from_fn(pick_fn),
'forward-place': from_fn(place_fn),
'test-goal': from_test(get_goal_test(tamp_problem)),
}
#stream_map = 'debug'
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def main(deterministic=False,
observable=False,
collisions=True,
focused=True,
factor=True):
# TODO: global search over the state
belief_problem = get_belief_problem(deterministic, observable)
pddlstream_problem = to_pddlstream(belief_problem, collisions)
set_cost_scale(SCALE_COST)
print('Cost scale:', get_cost_scale())
pr = cProfile.Profile()
pr.enable()
planner = 'ff-wastar1'
if focused:
stream_info = {
'GE': StreamInfo(from_test(ge_fn), eager=False),
'prob-after-move':
StreamInfo(from_fn(get_opt_move_fn(factor=factor))),
'MoveCost': FunctionInfo(move_cost_fn),
'prob-after-look':
StreamInfo(from_fn(get_opt_obs_fn(factor=factor))),
'LookCost': FunctionInfo(get_look_cost_fn(p_look_fp=0,
p_look_fn=0)),
}
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
planner=planner,
debug=False,
success_cost=0,
unit_costs=False,
max_time=30)
else:
solution = solve_incremental(pddlstream_problem,
planner=planner,
debug=True,
success_cost=MAX_COST,
unit_costs=False,
max_time=30)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
print_solution(solution)
plan, cost, init = solution
print('Real cost:', cost)
def pddlstream_from_tamp(tamp_problem):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
# TODO: algorithm that prediscretized once
constant_map = {}
stream_map = {
's-motion': from_fn(plan_motion),
't-reachable': from_test(test_reachable),
's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)),
't-region': from_test(get_region_test(tamp_problem.regions)),
's-ik': from_fn(lambda b, p, g: (inverse_kin(p, g),)),
'dist': distance_fn,
}
init, goal = create_problem(tamp_problem)
init.extend(('Grasp', b, GRASP) for b in tamp_problem.initial.block_poses)
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def to_pddlstream(belief_problem, collisions=True):
locations = {l for (_, l, _) in belief_problem.initial + belief_problem.goal} | \
set(belief_problem.locations)
observations = [True, False]
uniform = UniformDist(locations)
initial_bel = {
o: MixtureDD(DeltaDist(l), uniform, p)
for o, l, p in belief_problem.initial
}
max_p_collision = 0.25 if collisions else 1.0
# TODO: separate pick and place for move
init = [('Obs', obs) for obs in observations] + \
[('Location', l) for l in locations]
for o, d in initial_bel.items():
init += [('Dist', o, d), ('BLoc', o, d)]
for (o, l, p) in belief_problem.goal:
init += [('Location', l), ('GoalProb', l, p)]
goal_literals = [('BLocGE', o, l, p) for (o, l, p) in belief_problem.goal]
goal = And(*goal_literals)
directory = os.path.dirname(os.path.abspath(__file__))
domain_pddl = read(os.path.join(directory, 'domain.pddl'))
stream_pddl = read(os.path.join(directory, 'stream.pddl'))
constant_map = {}
stream_map = {
'BCollision':
get_collision_test(max_p_collision),
'GE':
from_test(ge_fn),
'prob-after-move':
from_fn(get_move_fn(belief_problem.p_move_s)),
'MoveCost':
move_cost_fn,
'prob-after-look':
from_fn(get_look_fn(belief_problem.p_look_fp,
belief_problem.p_look_fn)),
'LookCost':
get_look_cost_fn(belief_problem.p_look_fp, belief_problem.p_look_fn),
#'PCollision': from_fn(prob_occupied), # Then can use GE
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert (initial.holding is None)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
init = [
('CanMove',),
('Conf', initial.conf),
('AtConf', initial.conf),
('HandEmpty',),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', b, p) for b, p in initial.block_poses.items()] + \
[('Region', r) for r in tamp_problem.regions.keys()] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND) for b in initial.block_poses.keys()] + \
[('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]
goal_literals = [('HandEmpty',)] + \
[('In', b, r) for b, r in tamp_problem.goal_regions.items()]
if tamp_problem.goal_conf is not None:
goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal = And(*goal_literals)
stream_map = {
'plan-motion': from_fn(plan_motion),
'sample-pose': from_gen_fn(get_pose_gen(tamp_problem.regions)),
'test-region': from_test(get_region_test(tamp_problem.regions)),
'inverse-kinematics': from_fn(inverse_kin_fn),
'collision-free': from_test(lambda *args: not collision_test(*args)),
'cfree': lambda *args: not collision_test(*args),
'posecollision': collision_test,
'trajcollision': lambda *args: False,
'distance': distance_fn,
#'Valid': valid_state_fn,
}
#stream_map = 'debug'
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert (initial.holding is None)
known_poses = list(initial.block_poses.values()) + \
list(tamp_problem.goal_poses.values())
directory = os.path.dirname(os.path.abspath(__file__))
domain_pddl = read(os.path.join(directory, 'domain.pddl'))
stream_pddl = read(os.path.join(directory, 'stream.pddl'))
q100 = np.array([100, 100])
constant_map = {
'q100': q100,
}
init = [
#Type(q100, 'conf'),
('CanMove',),
('Conf', q100),
('Conf', initial.conf),
('AtConf', initial.conf),
('HandEmpty',),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', p) for p in known_poses] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()]
# [('Pose', p) for p in known_poses + tamp_problem.poses] + \
goal = And(*[('AtPose', b, p) for b, p in tamp_problem.goal_poses.items()])
def collision_test(p1, p2):
return np.linalg.norm(p2 - p1) <= 1e-1
def distance_fn(q1, q2):
ord = 1 # 1 | 2
return int(math.ceil(np.linalg.norm(q2 - q1, ord=ord)))
# TODO: convert to lower case
stream_map = {
#'sample-pose': from_gen_fn(lambda: ((np.array([x, 0]),) for x in range(len(poses), n_poses))),
'sample-pose':
from_gen_fn(lambda: ((p, ) for p in tamp_problem.poses)),
'inverse-kinematics': from_fn(lambda p: (p + GRASP, )),
#'inverse-kinematics': IKGenerator,
#'inverse-kinematics': IKFactGenerator,
'collision-free': from_test(lambda *args: not collision_test(*args)),
'collision': collision_test,
#'constraint-solver': None,
'distance': distance_fn,
}
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_belief():
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
'inv-kin': from_fn(ik_fn),
'motion': from_fn(motion_fn),
}
# Options
# - observation produces one of several poses
# - always at the pose, observation just makes it observable
# - object has a unobserved fluent
block = 'block1'
#pose = None # Unknown
pose = Uncertain()
#pose = np.array([1, 0])
conf = np.array([0, 1])
booth = np.array([0, 2])
init = [
('Initial',), # Forces move first
('Conf', conf),
#('Booth', conf),
#('Conf', booth),
#('Booth', booth),
('AtConf', conf),
('HandEmpty',),
('Block', block),
('Pose', pose),
('AtPose', block, pose),
#('Observable', pose),
#('Latent', pose),
]
goal = ('Holding', block)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert(not initial.holding) # is None
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
# TODO: can always make the state a set of fluents
init = [
('State', initial),
('AtState', initial)] + \
[('Robot', r) for r in initial.robot_confs.keys()] + \
[('Conf', q) for q in initial.robot_confs.values()] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', b, p) for b, p in initial.block_poses.items()] + \
[('Grasp', b, GRASP) for b in initial.block_poses.keys()] + \
[('Region', r) for r in tamp_problem.regions.keys()] + \
[('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \
[('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]
# TODO: could populate AtConf, AtPose, HandEmpty, Holding
goal = ('AtGoal',)
stream_map = {
'plan-motion': from_fn(plan_motion),
'sample-pose': from_gen_fn(get_pose_gen(tamp_problem.regions)),
'test-region': from_test(get_region_test(tamp_problem.regions)),
'inverse-kinematics': from_fn(inverse_kin_fn),
#'posecollision': collision_test,
#'trajcollision': lambda *args: False,
'forward-move': from_fn(move_fn),
'forward-pick': from_fn(pick_fn),
'forward-place': from_fn(place_fn),
'test-goal': from_test(get_goal_test(tamp_problem)),
}
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def pddlstream_from_problem(robot, movable):
domain_pddl = read('tamp/domain_stacking.pddl')
stream_pddl = read('tamp/stream_stacking.pddl')
constant_map = {}
fixed = misc.get_fixed(robot, movable)
stream_map = {
'sample-pose-table':
from_gen_fn(primitives.get_stable_gen_table(fixed)),
'sample-pose-block':
from_fn(primitives.get_stable_gen_block(fixed)),
'sample-grasp':
from_gen_fn(primitives.get_grasp_gen(robot)),
'inverse-kinematics':
from_fn(primitives.get_ik_fn(robot, fixed)),
'plan-free-motion':
from_fn(primitives.get_free_motion_gen(robot, fixed)),
'plan-holding-motion':
from_fn(primitives.get_holding_motion_gen(robot, fixed)),
}
return domain_pddl, constant_map, stream_pddl, stream_map
def get_problem1(n=5):
# TODO: consider even/odd and version with infinite generator
# TODO: replicate the effect where a stream fails
constant_map = {}
stream_map = {
'increment':
from_fn(lambda x: (x + 1, )), # Increment by different amounts
'decrement': from_fn(lambda x: (x - 1, )),
'test-large': from_test(lambda x: n <= x),
'test-small': from_test(lambda x: x <= -n),
}
init = [
('Integer', 0),
]
goal = Or(
('Goal', )
#Exists(['?x'], ('Large', '?x')),
#Exists(['?x'], ('Small', '?x')),
)
# TODO: sort actions when renaming to make deterministic
return PDDLProblem(DOMAIN_PDDL, constant_map, STREAM_PDDL, stream_map,
init, goal)
def pddlstream_from_tamp(tamp_problem):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
stream_map = {
#'s-motion': from_fn(plan_motion),
't-reachable': from_test(test_reachable),
's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)),
't-region': from_test(get_region_test(tamp_problem.regions)),
's-ik': from_fn(lambda b, p, g: (inverse_kin(p, g),)),
'dist': distance_fn,
}
init, goal = create_problem(tamp_problem)
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert(initial.holding is None)
known_poses = list(initial.block_poses.values()) + \
list(tamp_problem.goal_poses.values())
directory = os.path.dirname(os.path.abspath(__file__))
domain_pddl = read(os.path.join(directory, 'domain.pddl'))
stream_pddl = read(os.path.join(directory, 'stream.pddl'))
q100 = np.array([100, 100])
constant_map = {
'q100': q100, # As an example
}
init = [
#Type(q100, 'conf'),
('CanMove',),
('Conf', q100),
('Conf', initial.conf),
('AtConf', initial.conf),
('HandEmpty',),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', p) for p in known_poses] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()]
# [('Pose', p) for p in known_poses + tamp_problem.poses] + \
goal = And(*[
('AtPose', b, p) for b, p in tamp_problem.goal_poses.items()
])
# TODO: convert to lower case
stream_map = {
#'sample-pose': from_gen_fn(lambda: ((np.array([x, 0]),) for x in range(len(poses), n_poses))),
'sample-pose': from_gen_fn(lambda: ((p,) for p in tamp_problem.poses)),
'inverse-kinematics': from_fn(lambda p: (p + GRASP,)),
'test-cfree': from_test(lambda *args: not collision_test(*args)),
'collision': collision_test,
'distance': distance_fn,
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def get_pddlstream_problem():
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
#'test-feasible': from_test(test_feasible),
'test-feasible': from_fn(feasibility_fn),
}
init = [
('Block', 'b1'),
('Block', 'b2'),
('OnTable', 'b1'),
('OnTable', 'b2'),
]
goal = ('Holding', 'b1')
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def __init__(self, name, gen_fn, inputs, domain, outputs, certified, info):
if info is None:
info = StreamInfo(p_success=None, overhead=None)
for p, c in Counter(outputs).items():
if c != 1:
raise ValueError(
'Output [{}] for stream [{}] is not unique'.format(
p, name))
for p in set(inputs) & set(outputs):
raise ValueError(
'Parameter [{}] for stream [{}] is both an input and output'.
format(p, name))
parameters = {
a
for i in certified for a in get_args(i) if is_parameter(a)
}
for p in (parameters - set(inputs + outputs)):
raise ValueError(
'Parameter [{}] for stream [{}] is not included within outputs'
.format(p, name))
super(Stream, self).__init__(name, info, inputs, domain)
# Each stream could certify a stream-specific fact as well
if gen_fn == DEBUG:
#gen_fn = from_fn(lambda *args: tuple(object() for _ in self.outputs))
gen_fn = from_fn(lambda *args: tuple(
DebugValue(name, args, o) for o in self.outputs))
self.gen_fn = gen_fn
self.outputs = tuple(outputs)
self.certified = tuple(certified)
self.constants.update(a for i in certified for a in get_args(i)
if not is_parameter(a))
# TODO: generalize to a hierarchical sequence
always_unique = False
if always_unique:
self.num_opt_fns = 0
#self.opt_list_fn = get_unique_fn(self)
self.opt_gen_fn = get_constant_gen_fn(self, None)
else:
self.num_opt_fns = 1
self.opt_gen_fn = get_shared_gen_fn(self) if (
self.info.opt_gen_fn is None) else self.info.opt_gen_fn
def create_static_stream(stream, evaluations, fluent_predicates, future_fn):
def static_fn(*input_values):
instance = stream.get_instance(
tuple(map(Object.from_value, input_values)))
if all(
evaluation_from_fact(f) in evaluations
for f in instance.get_domain()):
return None
return tuple(
FutureValue(stream.name, input_values, o) for o in stream.outputs)
#opt_evaluations = None
def static_opt_gen_fn(*input_values):
instance = stream.get_instance(
tuple(map(Object.from_value, input_values)))
if all(
evaluation_from_fact(f) in evaluations
for f in instance.get_domain()):
return
for output_values in stream.opt_gen_fn(*input_values):
yield output_values
# TODO: need to replace regular opt_gen_fn to update opt_evaluations
# if I want to prevent switch from normal to static in opt
# Focused algorithm naturally biases against using future because of axiom layers
fluent_domain = list(
filter(lambda a: get_prefix(a) in fluent_predicates, stream.domain))
static_domain = list(
filter(lambda a: a not in fluent_domain, stream.domain))
new_domain = list(map(future_fn, static_domain))
stream_atom = ('{}-result'.format(
stream.name), ) + tuple(stream.inputs + stream.outputs)
new_certified = [stream_atom] + list(map(future_fn, stream.certified))
static_stream = FutureStream(stream, new_domain, fluent_domain,
new_certified)
if REPLACE_STREAM:
static_stream.gen_fn = from_fn(static_fn)
static_stream.opt_gen_fn = static_opt_gen_fn
return static_stream
def pddlstream_from_problem(robot,
movable=[],
teleport=False,
movable_collisions=False,
grasp_name='top'):
#assert (not are_colliding(tree, kin_cache))
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
print('Robot:', robot)
conf = BodyConf(robot, get_configuration(robot))
init = [('CanMove', ), ('Conf', conf), ('AtConf', conf), ('HandEmpty', )]
fixed = get_fixed(robot, movable)
print('Movable:', movable)
print('Fixed:', fixed)
for body in movable:
pose = BodyPose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', body, pose)]
for surface in fixed:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
for body in fixed:
name = get_body_name(body)
if 'sink' in name:
init += [('Sink', body)]
if 'stove' in name:
init += [('Stove', body)]
body = movable[0]
goal = (
'and',
('AtConf', conf),
#('Holding', body),
#('On', body, fixed[1]),
#('On', body, fixed[2]),
#('Cleaned', body),
('Cooked', body),
)
stream_map = {
'sample-pose':
from_gen_fn(get_stable_gen(fixed)),
'sample-grasp':
from_gen_fn(get_grasp_gen(robot, grasp_name)),
'inverse-kinematics':
from_fn(get_ik_fn(robot, fixed, teleport)),
'plan-free-motion':
from_fn(get_free_motion_gen(robot, fixed, teleport)),
#'plan-free-motion': empty_gen(),
'plan-holding-motion':
from_fn(get_holding_motion_gen(robot, fixed, teleport)),
#'plan-holding-motion': empty_gen(),
'TrajCollision':
get_movable_collision_test(),
}
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_state(state, teleport=False):
task = state.task
robot = task.robot
# TODO: infer open world from task
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'base': 'base',
'left': 'left',
'right': 'right',
'head': 'head',
}
#base_conf = state.poses[robot]
base_conf = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
scan_cost = 1
init = [
('BConf', base_conf),
('AtBConf', base_conf),
Equal(('MoveCost', ), 1),
Equal(('PickCost', ), 1),
Equal(('PlaceCost', ), 1),
Equal(('ScanCost', ), scan_cost),
Equal(('RegisterCost', ), 1),
]
holding_arms = set()
holding_bodies = set()
for attach in state.attachments.values():
holding_arms.add(attach.arm)
holding_bodies.add(attach.body)
init += [('Grasp', attach.body, attach.grasp),
('AtGrasp', attach.arm, attach.body, attach.grasp)]
for arm in ARM_NAMES:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('AtAConf', arm, conf)]
if arm in task.arms:
init += [('Controllable', arm)]
if arm not in holding_arms:
init += [('HandEmpty', arm)]
for body in task.get_bodies():
if body in holding_bodies:
continue
# TODO: no notion whether observable actually corresponds to the correct thing
pose = state.poses[body]
init += [
('Pose', body, pose),
('AtPose', body, pose),
('Observable', pose),
]
init += [('Scannable', body) for body in task.rooms + task.surfaces]
init += [('Registerable', body) for body in task.movable]
init += [('Graspable', body) for body in task.movable]
for body in task.get_bodies():
supports = task.get_supports(body)
if supports is None:
continue
for surface in supports:
p_obs = state.b_on[body].prob(surface)
cost = revisit_mdp_cost(0, scan_cost,
p_obs) # TODO: imperfect observation model
init += [('Stackable', body, surface),
Equal(('LocalizeCost', surface, body), clip_cost(cost))]
#if is_placement(body, surface):
if is_center_stable(body, surface):
if body in holding_bodies:
continue
pose = state.poses[body]
init += [('Supported', body, pose, surface)]
for body in task.get_bodies():
if state.is_localized(body):
init.append(('Localized', body))
else:
init.append(('Uncertain', body))
if body in state.registered:
init.append(('Registered', body))
goal = And(*[('Holding', a, b) for a, b in task.goal_holding] + \
[('On', b, s) for b, s in task.goal_on] + \
[('Localized', b) for b in task.goal_localized] + \
[('Registered', b) for b in task.goal_registered])
stream_map = {
'sample-pose':
get_stable_gen(task),
'sample-grasp':
from_list_fn(get_grasp_gen(task)),
'inverse-kinematics':
from_gen_fn(get_ik_ir_gen(task, teleport=teleport)),
'plan-base-motion':
from_fn(get_motion_gen(task, teleport=teleport)),
'test-vis-base':
from_test(get_in_range_test(task, VIS_RANGE)),
'test-reg-base':
from_test(get_in_range_test(task, REG_RANGE)),
'sample-vis-base':
accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, VIS_RANGE)),
max_attempts=25),
'sample-reg-base':
accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, REG_RANGE)),
max_attempts=25),
'inverse-visibility':
from_fn(get_inverse_visibility_fn(task)),
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def get_debug_gen_fn(stream):
return from_fn(lambda *args: tuple(
DebugValue(stream.name, args, o) for o in stream.outputs))
def get_pddlstream(robot,
brick_from_index,
obstacle_from_name,
collisions=True,
teleport=False):
domain_pddl = read(get_file_path(__file__, 'retired.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
conf = np.array(get_configuration(robot))
init = [
('CanMove', ),
('Conf', conf),
('AtConf', conf),
('HandEmpty', ),
]
goal_literals = [
('AtConf', conf),
#('HandEmpty',),
]
#indices = brick_from_index.keys()
#indices = range(2, 5)
indices = [4]
for index in list(indices):
indices.append(index + 6)
for index in indices:
brick = brick_from_index[index]
init += [
('Graspable', index),
('Pose', index, brick.initial_pose),
('AtPose', index, brick.initial_pose),
('Pose', index, brick.goal_pose),
]
goal_literals += [
#('Holding', index),
('AtPose', index, brick.goal_pose),
]
for index2 in brick.goal_supports:
brick2 = brick_from_index[index2]
init += [
('Supported', index, brick.goal_pose, index2,
brick2.goal_pose),
]
goal = And(*goal_literals)
if not collisions:
obstacle_from_name = {}
stream_map = {
'sample-grasp':
from_gen_fn(get_grasp_gen_fn(brick_from_index)),
'inverse-kinematics':
from_gen_fn(get_ik_gen_fn(robot, brick_from_index,
obstacle_from_name)),
'plan-motion':
from_fn(
get_motion_fn(robot,
brick_from_index,
obstacle_from_name,
teleport=teleport)),
'TrajCollision':
get_collision_test(robot, brick_from_index, collisions=collisions),
}
#stream_map = 'debug'
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_pddlstream_problem(task, context, collisions=True):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
plant = task.mbp
robot = task.robot
robot_name = get_model_name(plant, robot)
world = plant.world_frame() # mbp.world_body()
robot_joints = get_movable_joints(plant, robot)
robot_conf = Conf(robot_joints, get_configuration(plant, context, robot))
init = [
('Robot', robot_name),
('CanMove', robot_name),
('Conf', robot_name, robot_conf),
('AtConf', robot_name, robot_conf),
('HandEmpty', robot_name),
]
goal_literals = []
if task.reset_robot:
goal_literals.append(('AtConf', robot_name, robot_conf), )
for obj in task.movable:
obj_name = get_model_name(plant, obj)
#obj_frame = get_base_body(mbp, obj).body_frame()
obj_pose = Pose(plant, world, obj,
get_world_pose(plant, context,
obj)) # get_relative_transform
init += [
('Graspable', obj_name),
('Pose', obj_name, obj_pose),
#('InitPose', obj_name, obj_pose),
('AtPose', obj_name, obj_pose)
]
for surface in task.surfaces:
init += [('Stackable', obj_name, surface)]
# TODO: detect already stacked
for surface in task.surfaces:
surface_name = get_model_name(plant, surface.model_index)
if 'sink' in surface_name:
init += [('Sink', surface)]
if 'stove' in surface_name:
init += [('Stove', surface)]
for door in task.doors:
door_body = plant.tree().get_body(door)
door_name = door_body.name()
door_joints = get_parent_joints(plant, door_body)
door_conf = Conf(door_joints,
get_joint_positions(door_joints, context))
init += [
('Door', door_name),
('Conf', door_name, door_conf),
('AtConf', door_name, door_conf),
]
for positions in [get_door_positions(door_body, DOOR_OPEN)]:
conf = Conf(door_joints, positions)
init += [('Conf', door_name, conf)]
if task.reset_doors:
goal_literals += [('AtConf', door_name, door_conf)]
for obj, transform in task.goal_poses.items():
obj_name = get_model_name(plant, obj)
obj_pose = Pose(plant, world, obj, transform)
init += [('Pose', obj_name, obj_pose)]
goal_literals.append(('AtPose', obj_name, obj_pose))
for obj in task.goal_holding:
goal_literals.append(
('Holding', robot_name, get_model_name(plant, obj)))
for obj, surface in task.goal_on:
goal_literals.append(('On', get_model_name(plant, obj), surface))
for obj in task.goal_cooked:
goal_literals.append(('Cooked', get_model_name(plant, obj)))
goal = And(*goal_literals)
print('Initial:', init)
print('Goal:', goal)
stream_map = {
#'sample-pose': from_gen_fn(get_stable_gen(task, context, collisions=collisions)),
'sample-reachable-pose':
from_gen_fn(
get_reachable_pose_gen_fn(task, context, collisions=collisions)),
'sample-grasp':
from_gen_fn(get_grasp_gen_fn(task)),
'plan-ik':
from_gen_fn(get_ik_gen_fn(task, context, collisions=collisions)),
'plan-motion':
from_fn(get_motion_fn(task, context, collisions=collisions)),
'plan-pull':
from_fn(get_pull_fn(task, context, collisions=collisions)),
'TrajPoseCollision':
get_collision_test(task, context, collisions=collisions),
'TrajConfCollision':
get_collision_test(task, context, collisions=collisions),
}
#stream_map = 'debug' # Runs PDDLStream with "placeholder streams" for debugging
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_debug_gen_fn(stream, shared=True):
if shared:
return from_fn(lambda *args, **kwargs: tuple(
SharedDebugValue(stream.name, o) for o in stream.outputs))
return from_fn(lambda *args, **kwargs: tuple(
DebugValue(stream.name, args, o) for o in stream.outputs))
def pddlstream_from_problem(problem, collisions=True, teleport=False):
#rovers = problem.rovers
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
camera = 'RGBD'
mode = 'color' # highres | lowres | color | depth
init = []
goal_literals = [
#('Calibrated', camera, problem.rovers[0])
#('Analyzed', problem.rovers[0], problem.rocks[0])
#('ReceivedAnalysis', problem.rocks[0])
]
init += [('Lander', l) for l in problem.landers]
init += [('Camera', camera), ('Supports', camera, mode), ('Mode', mode)]
for rover in problem.rovers:
base_joints = get_group_joints(rover, 'base')
bq0 = Conf(rover, base_joints)
head_joints = get_group_joints(rover, 'head')
hq0 = Conf(rover, head_joints)
init += [('Rover', rover), ('OnBoard', camera, rover),
('BConf', bq0), ('AtBConf', rover, bq0),
('HConf', hq0), ('AtHConf', rover, hq0)]
for name in problem.stores:
init += [('Store', name)]
init += [('Free', rover, name) for rover in problem.rovers]
for name in problem.objectives:
init += [('Objective', name)]
#initial_atoms += [IsClass(name, cl) for cl in CLASSES if cl in name]
goal_literals += [('ReceivedImage', name, mode)]
for name in problem.rocks + problem.soils:
# pose = Pose(name, get_pose(env.GetKinBody(name)))
init += [('Rock', name)] # , IsPose(name, pose), AtPose(name, pose)]
#init += [('IsClass', name, cl) for cl in CLASSES if cl in name]
#if problem.rocks:
# goal_literals.append(Exists(['?rock'], And(('Rock', '?rock'),
# ('ReceivedAnalysis', '?rock'))))
#if problem.soils:
# goal_literals.append(Exists(['?soil'], And(('Soil', '?soil'),
# ('ReceivedAnalysis', '?soil')))) # TODO: soil class
goal_formula = And(*goal_literals)
custom_limits = {}
if problem.limits is not None:
for rover in problem.rovers:
custom_limits.update(get_base_custom_limits(rover, problem.limits))
stream_map = {
'test-reachable': from_test(get_reachable_test(problem)),
'obj-inv-visible': from_gen_fn(get_inv_vis_gen(problem, custom_limits=custom_limits)),
'com-inv-visible': from_gen_fn(get_inv_com_gen(problem, custom_limits=custom_limits)),
'sample-above': from_gen_fn(get_above_gen(problem, custom_limits=custom_limits)),
'sample-base-motion': from_fn(get_base_motion_fn(problem, custom_limits=custom_limits, teleport=teleport)),
'sample-head-motion': from_fn(get_head_motion_fn(problem, teleport=teleport)),
}
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
def pddlstream_from_problem(problem, teleport=False):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {'{}'.format(name).lower(): name
for name in problem.initial_confs.keys()}
edges = set()
init = [
]
for name, conf in problem.initial_confs.items():
init += [
('Safe',),
('Robot', name),
('Conf', conf),
('AtConf', name, conf),
Equal(('Speed', name), DIST_PER_TIME),
Equal(('BatteryCapacity', name), MAX_ENERGY),
Equal(('RechargeRate', name), CHARGE_PER_TIME),
Equal(('ConsumptionRate', name), BURN_PER_TIME),
Equal(('Energy', name), INITIAL_ENERGY),
]
goal_literals = [
#('Safe',),
#('Unachievable',),
]
for name, base_values in problem.goal_confs.items():
body = problem.get_body(name)
joints = get_base_joints(body)
#extend_fn = get_extend_fn(body, joints, resolutions=5*BASE_RESOLUTIONS)
#q_init = problem.initial_confs[name]
#path = [q_init] + [Conf(body, joints, q) for q in extend_fn(q_init.values, base_values)]
#edges.update(zip(path, path[1:]))
#q_goal = path[-1]
q_goal = Conf(body, joints, base_values)
init += [('Conf', q_goal)]
goal_literals += [('AtConf', name, q_goal)]
goal_formula = And(*goal_literals)
robot = list(map(problem.get_body, problem.initial_confs))[0]
with LockRenderer():
init += [('Conf', q) for _, n, q in create_vertices(problem, robot, [], samples_per_ft2=6)]
#vertices = {v for edge in edges for v in edge}
#handles = []
#for vertex in vertices:
# handles.extend(draw_point(point_from_conf(vertex.values), size=0.05))
#for conf1, conf2 in edges:
# traj = linear_trajectory(conf1, conf2)
# init += [
# ('Conf', conf1),
# ('Traj', traj),
# ('Conf', conf2),
# ('Motion', conf1, traj, conf2),
# ]
#draw_edges(edges)
cfree_traj_traj_test = get_test_cfree_traj_traj(problem)
cfree_traj_traj_list_gen_fn = from_test(cfree_traj_traj_test)
traj_traj_collision_test = negate_test(cfree_traj_traj_test)
stream_map = {
'test-cfree-conf-conf': cfree_traj_traj_list_gen_fn,
'test-cfree-traj-conf': cfree_traj_traj_list_gen_fn,
'test-cfree-traj-traj': cfree_traj_traj_list_gen_fn,
'ConfConfCollision': traj_traj_collision_test,
'TrajConfCollision': traj_traj_collision_test,
'TrajTrajCollision': traj_traj_collision_test,
'compute-motion': from_fn(get_motion_fn2(problem)),
'TrajDistance': get_distance_fn(),
}
#stream_map = 'debug'
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
return problem, edges
def main(display=True, simulate=False, teleport=False):
parser = argparse.ArgumentParser()
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
#parser.add_argument('-display', action='store_true', help='displays the solution')
args = parser.parse_args()
connect(use_gui=args.viewer)
problem_fn = cooking_problem
# holding_problem | stacking_problem | cleaning_problem | cooking_problem
# cleaning_button_problem | cooking_button_problem
with HideOutput():
problem = problem_fn()
state_id = save_state()
#saved_world = WorldSaver()
#dump_world()
pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport)
stream_info = {
'sample-pose': StreamInfo(PartialInputs('?r')),
'inverse-kinematics': StreamInfo(PartialInputs('?p')),
'plan-base-motion': StreamInfo(PartialInputs('?q1 ?q2')),
'MoveCost': FunctionInfo(opt_move_cost_fn),
}
synthesizers = [
StreamSynthesizer(
'safe-base-motion', {
'plan-base-motion': 1,
'TrajPoseCollision': 0,
'TrajGraspCollision': 0,
'TrajArmCollision': 0,
}, from_fn(get_base_motion_synth(problem, teleport))),
] if USE_SYNTHESIZERS else []
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', synthesizers)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
synthesizers=synthesizers,
success_cost=INF)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
if (not display) or (plan is None):
disconnect()
return
commands = post_process(problem, plan)
if args.viewer:
restore_state(state_id)
else:
disconnect()
connect(use_gui=True)
with HideOutput():
problem_fn() # TODO: way of doing this without reloading?
if simulate:
enable_gravity()
control_commands(commands)
else:
step_commands(commands, time_step=0.01)
user_input('Finish?')
disconnect()
def main(display=True, teleport=False, partial=False, defer=False):
parser = argparse.ArgumentParser()
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
#parser.add_argument('-display', action='store_true', help='displays the solution')
args = parser.parse_args()
connect(use_gui=args.viewer)
problem_fn = cooking_problem
# holding_problem | stacking_problem | cleaning_problem | cooking_problem
# cleaning_button_problem | cooking_button_problem
with HideOutput():
problem = problem_fn()
state_id = save_state()
#saved_world = WorldSaver()
#dump_world()
pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport)
stream_info = {
'sample-pose':
StreamInfo(PartialInputs('?r')),
'inverse-kinematics':
StreamInfo(PartialInputs('?p')),
'plan-base-motion':
StreamInfo(PartialInputs('?q1 ?q2'),
defer_fn=defer_shared if defer else never_defer),
'MoveCost':
FunctionInfo(opt_move_cost_fn),
} if partial else {
'sample-pose': StreamInfo(from_fn(opt_pose_fn)),
'inverse-kinematics': StreamInfo(from_fn(opt_ik_fn)),
'plan-base-motion': StreamInfo(from_fn(opt_motion_fn)),
'MoveCost': FunctionInfo(opt_move_cost_fn),
}
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
pr = cProfile.Profile()
pr.enable()
with LockRenderer():
#solution = solve_incremental(pddlstream_problem, debug=True)
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
success_cost=INF,
debug=False)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
if (not display) or (plan is None):
disconnect()
return
with LockRenderer():
commands = post_process(problem, plan)
if args.viewer:
restore_state(state_id)
else:
disconnect()
connect(use_gui=True)
with HideOutput():
problem_fn() # TODO: way of doing this without reloading?
if args.simulate:
control_commands(commands)
else:
apply_commands(State(), commands, time_step=0.01)
user_input('Finish?')
disconnect()
def get_problem(optimize=True):
initial_atm = 30
initial_person = 2
min_take = 1
#max_take = 10
max_take = initial_atm
target = 50 # 1 | 3 | 50
num_atms = 2
domain_pddl = read_pddl(__file__, 'domain0.pddl')
constant_map = {
#'@min': min_take,
#'@max': max_take,
'@amount': target,
}
if optimize:
stream_pddl = read_pddl(__file__, 'optimizer.pddl')
else:
stream_pddl = read_pddl(__file__, 'stream.pddl')
# TODO: store history of withdraws to ensure not taking too much
#stream_pddl = None
stream_map = {
#'s-cash': from_gen((c,) for c in randomize(irange(min_take, max_take + 1))),
#'s-cash': from_gen((c,) for c in reversed(list(irange(min_take, max_take+1)))),
's-cash':
from_sampler(lambda: (round(uniform(min_take, max_take), 2), )),
't-ge':
from_test(lambda c1, c2: c1 >= c2),
'add':
from_fn(lambda c1, c2: (c1 + c2, )),
'subtract':
from_fn(lambda c3, c2: (c3 - c2, ) if c3 - c2 >= 0 else None),
'withdraw':
from_fn(lambda wc, pc1, mc1: (pc1 + wc, mc1 - wc)
if mc1 - wc >= 0 else None),
'withdrawcost':
lambda c: c, # TODO: withdraw fee
'gurobi':
create_optimizer(min_take, max_take),
}
person = 'Emre'
#initial_people = {'Emre': initial_person}
#initial_atms = {'Emre': initial_person}
#amounts = [min_take, max_take, target, initial_atm, initial_person]
init = [
('person', person),
('pcash', initial_person),
('pcash', target),
('tcash', target),
('inpocket', person, initial_person),
] # + [('cash', amount) for amount in amounts]
for num in range(num_atms):
name = 'atm{}'.format(num + 1)
init.extend([
('machine', name),
('mcash', initial_atm),
('maxwithdraw', name, initial_atm),
])
#goal = Exists(['?c1'], And(('person', person), ('ge', '?c1', target),
# ('inpocket', person, '?c1')))
goal = ('finished', ) # TODO: focused bug when goal initially satisfied
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def pddlstream_from_problem(problem, teleport=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
init = [
('CanMove',),
('BConf', initial_bq),
('AtBConf', initial_bq),
Equal(('PickCost',), 1),
Equal(('PlaceCost',), 1),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
for arm in ARM_NAMES:
#for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm),
('AtAConf', arm, conf)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', body, pose)]
for surface in problem.surfaces:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
goal = [AND]
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
init += [('BConf', goal_conf)]
goal += [('AtBConf', goal_conf)]
goal += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('On', b, s) for b, s in problem.goal_on] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
stream_map = {
'sample-pose':
get_stable_gen(problem),
'sample-grasp':
from_list_fn(get_grasp_gen(problem)),
'inverse-kinematics':
from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)),
'plan-base-motion':
from_fn(get_motion_gen(problem, teleport=teleport)),
'MoveCost':
move_cost_fn,
'TrajPoseCollision':
fn_from_constant(False),
'TrajArmCollision':
fn_from_constant(False),
'TrajGraspCollision':
fn_from_constant(False),
}
if USE_SYNTHESIZERS:
stream_map['plan-base-motion'] = empty_gen(),
# get_press_gen(problem, teleport=teleport)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_problem(problem, teleport=False, movable_collisions=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'world': 'world',
}
world = 'world'
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
init = [
('CanMove',),
('BConf', initial_bq), # TODO: could make pose as well...
('AtBConf', initial_bq),
('AtAConf', world, None),
('AtPose', world, world, None),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
#for arm in ARM_JOINT_NAMES:
for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm),
('AtAConf', arm, conf), ('AtPose', arm, arm, None)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', world, body, pose)]
for surface in problem.surfaces:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
goal = ['and']
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
init += [('BConf', goal_conf)]
goal += [('AtBConf', goal_conf)]
goal += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('On', b, s) for b, s in problem.goal_on] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
stream_map = {
'sample-pose':
get_stable_gen(problem),
'sample-grasp':
from_list_fn(get_grasp_gen(problem)),
'inverse-kinematics':
from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)),
'plan-base-motion':
from_fn(get_motion_gen(problem, teleport=teleport)),
#'plan-base-motion': empty_gen(),
'BTrajCollision':
fn_from_constant(False),
}
# get_press_gen(problem, teleport=teleport)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
