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 create_static_stream(stream, evaluations, fluent_predicates, get_future):
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
stream_name = 'future-{}'.format(stream.name)
gen_fn = from_fn(static_fn)
static_domain = list(filter(lambda a: get_prefix(a) not in fluent_predicates, stream.domain))
new_domain = list(map(get_future, static_domain))
stream_atom = ('{}-result'.format(stream.name),) + tuple(stream.inputs + stream.outputs)
new_certified = [stream_atom] + list(map(get_future, stream.certified))
static_stream = Stream(stream_name, gen_fn, stream.inputs, new_domain,
stream.outputs, new_certified, stream.info)
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 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)
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,
max_cost=0,
unit_costs=False,
max_time=30)
else:
solution = solve_incremental(pddlstream_problem,
planner=planner,
debug=True,
max_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:', float(cost) / SCALE_COST)
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)
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(lambda p: (p + np.array([0, 1]), )),
'motion':
from_fn(lambda q1, q2:
([t * (q2 - q1) + q1 for t in [0, 1]], )), # linspace, arange
}
# 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 = np.array([1, 0])
conf = np.array([0, 1])
booth = np.array([0, 2])
init = [
('Initial', ), # Forces move first
('Conf', 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 main(focused=True, deterministic=False, unit_costs=False):
np.set_printoptions(precision=2)
if deterministic:
seed = 0
np.random.seed(seed)
print('Seed:', get_random_seed())
problem_fn = get_blocked_problem # get_tight_problem | get_blocked_problem
tamp_problem = problem_fn()
print(tamp_problem)
action_info = {
#'move': ActionInfo(terminal=True),
#'pick': ActionInfo(terminal=True),
#'place': ActionInfo(terminal=True),
}
stream_info = {
#'test-region': StreamInfo(eager=True, p_success=0), # bound_fn is None
#'plan-motion': StreamInfo(p_success=1), # bound_fn is None
#'trajcollision': StreamInfo(p_success=1), # bound_fn is None
#'cfree': StreamInfo(eager=True),
}
dynamic = [
StreamSynthesizer('cfree-motion', {
'plan-motion': 1,
'trajcollision': 0
},
gen_fn=from_fn(cfree_motion_fn)),
#StreamSynthesizer('optimize', {'sample-pose': 1, 'inverse-kinematics': 1,
# 'posecollision': 0, 'distance': 0},
# gen_fn=from_fn(get_optimize_fn(tamp_problem.regions))),
]
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
pr = cProfile.Profile()
pr.enable()
if focused:
solution = solve_focused(pddlstream_problem,
action_info=action_info,
stream_info=stream_info,
synthesizers=dynamic,
max_time=10,
max_cost=INF,
debug=False,
effort_weight=None,
unit_costs=unit_costs,
postprocess=False,
visualize=False)
else:
solution = solve_incremental(pddlstream_problem,
layers=1,
unit_costs=unit_costs)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS))
viewer = ContinuousTMPViewer(tamp_problem.regions, title='Continuous TAMP')
state = tamp_problem.initial
print()
print(state)
draw_state(viewer, state, colors)
for i, action in enumerate(plan):
user_input('Continue?')
print(i, *action)
state = apply_action(state, action)
print(state)
draw_state(viewer, state, colors)
user_input('Finish?')
def main(viewer=False, display=True, simulate=False, teleport=False):
# TODO: fix argparse & FastDownward
#parser = argparse.ArgumentParser() # Automatically includes help
#parser.add_argument('-viewer', action='store_true', help='enable viewer.')
#parser.add_argument('-display', action='store_true', help='enable viewer.')
#args = parser.parse_args()
connect(use_gui=viewer)
problem_fn = cooking_problem
# holding_problem | stacking_problem | cleaning_problem | cooking_problem
# cleaning_button_problem | cooking_button_problem
problem = problem_fn()
state_id = p.saveState()
#saved_world = WorldSaver()
dump_world()
pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport)
_, _, _, stream_map, init, goal = pddlstream_problem
synthesizers = [
StreamSynthesizer(
'safe-base-motion', {
'plan-base-motion': 1,
'TrajPoseCollision': 0,
'TrajGraspCollision': 0,
'TrajArmCollision': 0
}, from_fn(get_base_motion_synth(problem, teleport))),
]
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', synthesizers)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
synthesizers=synthesizers,
max_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):
p.disconnect()
return
if viewer:
p.restoreState(state_id)
else:
disconnect()
connect(use_gui=True)
problem = problem_fn() # TODO: way of doing this without reloading?
commands = post_process(problem, plan)
if simulate:
enable_gravity()
control_commands(commands)
else:
step_commands(commands, time_step=0.01)
user_input('Finish?')
disconnect()
def main(viewer=False, display=True, simulate=False, teleport=False):
# TODO: fix argparse & FastDownward
#parser = argparse.ArgumentParser() # Automatically includes help
#parser.add_argument('-viewer', action='store_true', help='enable viewer.')
#parser.add_argument('-display', action='store_true', help='enable viewer.')
#args = parser.parse_args()
connect(use_gui=viewer)
robot, names, movable = load_world()
saved_world = WorldSaver()
dump_world()
pddlstream_problem = pddlstream_from_problem(robot,
movable=movable,
teleport=teleport,
movable_collisions=True)
_, _, _, stream_map, init, goal = pddlstream_problem
synthesizers = [
StreamSynthesizer(
'safe-free-motion', {
'plan-free-motion': 1,
'trajcollision': 0
}, from_fn(get_free_motion_synth(robot, movable, teleport))),
StreamSynthesizer(
'safe-holding-motion', {
'plan-holding-motion': 1,
'trajcollision': 0
}, from_fn(get_holding_motion_synth(robot, movable, teleport))),
]
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', stream_map.keys())
print(names)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
synthesizers=synthesizers,
max_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):
p.disconnect()
return
paths = []
for name, args in plan:
if name == 'place':
paths += args[-1].reverse().body_paths
elif name in ['move', 'move_free', 'move_holding', 'pick']:
paths += args[-1].body_paths
print(paths)
command = Command(paths)
if not viewer: # TODO: how to reenable the viewer
disconnect()
connect(use_gui=True)
load_world()
saved_world.restore()
user_input('Execute?')
if simulate:
command.control()
else:
#command.step()
command.refine(num_steps=10).execute(time_step=0.001)
#wait_for_interrupt()
user_input('Finish?')
disconnect()
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', ), scale_cost(1)),
Equal(('PickCost', ), scale_cost(1)),
Equal(('PlaceCost', ), scale_cost(1)),
Equal(('ScanCost', ), scale_cost(scan_cost)),
Equal(('RegisterCost', ), scale_cost(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),
]
for body in task.movable:
init += [('Graspable', body)]
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), scale_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)),
'inverse-visibility': from_gen_fn(get_vis_gen(task)),
'plan-scan': from_gen_fn(get_scan_gen(state)),
}
return Problem(domain_pddl, constant_map, stream_pddl, stream_map, init,
goal)
