def initialize_fluent_streams(problem):
fluents = problem.fluents()
new_axioms = []
for stream in problem.streams:
stream.fluent_domain = tuple(a for a in stream.domain
if a.head.function in fluents)
stream.domain = tuple(a for a in stream.domain
if a not in stream.fluent_domain)
stream.predicates = []
for i, o in enumerate(stream.outputs):
params = stream.inputs + (X, )
predicate = Predicate(params, name='{}{}'.format(stream.name, i))
stream.predicates.append(predicate)
new_axioms.append(
Axiom(params,
pre=[predicate(*params)] +
[Computable(i) for i in stream.inputs],
eff=Computable(X)))
new_axioms.append(Axiom(X, pre=[Computed(X)], eff=Computable(X)))
for action in problem.actions:
action.preconditions = action.preconditions + \
tuple(map(Computable, action.parameters))
problem.axioms += new_axioms
def solve_streams_new(universe, evaluations, plan, bound_streams, start_time,
max_time, defer, **kwargs):
planner = 'ff-astar'
if plan is None:
return None, plan
if not plan:
return [], plan
OrderPreds = [Predicate([]) for _ in xrange(len(plan) + 1)]
action_from_instance = {}
for i, (action, args) in enumerate(plan):
instance = action.instantiate(args)
instance.preconditions = filter(
lambda p: isinstance(p, Atom) or not universe.is_derived(p),
instance.preconditions) + (OrderPreds[i](), )
instance.effects = list(instance.effects) + \
[~OrderPreds[i](), OrderPreds[i + 1]()]
action_from_instance[instance] = (action, args)
axioms = [
axiom.instantiate(args) for axiom, args in universe.axiom_instances()
]
stream_actions, stream_axioms = stream_action_instances(
evaluations, bound_streams)
goal = [OrderPreds[-1]()]
downward_problem = DownwardProblem(
evaluations | {OrderPreds[0]()}, goal,
action_from_instance.keys() + stream_actions, axioms + stream_axioms)
combined_plan = solve_sas(downward_problem,
planner=planner,
max_time=(max_time - (time.time() - start_time)),
verbose=False,
**kwargs)
if combined_plan is None:
return None, plan
combined_plan = [
action_from_instance.get(op, (op, tuple())) for op in combined_plan
]
combined_plan = defer_streams(
combined_plan) if defer else prioritize_streams(combined_plan)
stream_plan, action_plan = [], []
for i, (action, args) in enumerate(combined_plan):
if (action, args) in plan:
action_plan.append((action, args))
elif action_plan:
action_plan.append((action, args))
else:
stream_plan.append(action.bound_stream)
return stream_plan, action_plan
def make_stream_operators(problem, effort_weight):
stream_actions = []
stream_axioms = []
fluents = problem.fluents()
for stream in problem.streams:
params = stream.inputs + stream.outputs
stream.predicate = Predicate(params)
preconditions = list(stream.domain) + [stream.predicate(*params)]
effects = list(stream.graph)
if effort_weight is not None:
effort = 1
effects.append(Increase(TotalCost(), effort_weight * effort))
action = Action(stream.name, params, preconditions, effects)
action.stream = stream
stream.fluent_domain = tuple(a for a in stream.domain
if a.head.function in fluents)
stream.domain = tuple(a for a in stream.domain
if a not in stream.fluent_domain)
stream_actions.append(action)
return stream_actions, stream_axioms
def create_problem(cupSize=(3, 4),
kettleSize=(5, 6),
cupInitPos=(5, -9, 0),
kettleInitPos=(-5, -9, 0),
faucetPos=(5, 1, 0),
kettleGoalPos=(-5, -9, 0),
goalPosEps=0.5,
maxMoveDist=10.0,
domain=[(-20, 20), (-9, 20), (-np.pi, np.pi)],
verboseFns=True):
CUP = '?cup'
POS1 = '?pos1'
POS2 = '?pos2'
POS3 = '?pos3'
KETTLE = '?kettle'
WATER = '?water'
FAUCET = '?faucet'
IsCup = Predicate([CUP])
IsKettle = Predicate([KETTLE])
IsWater = Predicate([WATER])
IsFaucet = Predicate([FAUCET])
LegalPos = Predicate([POS1])
CanGetWater = Predicate([POS1])
HoldsWater = Predicate([KETTLE])
CanPour = Predicate([POS1, POS2, POS3])
AtPos = Predicate([CUP, POS1])
WaterInKettle = Predicate([WATER, KETTLE])
WaterBoiled = Predicate([WATER, KETTLE])
HasBoiledWater = Predicate([KETTLE])
CanMove = Predicate([POS1, POS2], domain=[LegalPos(POS1), LegalPos(POS2)],
fn=lambda x, y: (distance(x, y) <= maxMoveDist))
CloseTo = Predicate([POS1, POS2], domain=[LegalPos(POS1), LegalPos(POS2)],
fn=lambda x, y: (distance(x, y) <= goalPosEps))
MoveCost = NonNegFunction([POS1, POS2], domain=[LegalPos(POS1), LegalPos(POS2)],
fn=lambda p1, p2: scale_cost(distance(p1, p2)))
def getWaterTest(p):
(x, y, z) = p
result = (faucetPos[0] - 1 <= x <= faucetPos[0] + 1) and (faucetPos[1] -
cupSize[1] - 1 <= y <= faucetPos[1] - cupSize[1]) and (-0.05 <= z <= 0.05)
if verboseFns and not result:
print 'cannot get water from:', p
return result
def fill_cost(pos):
success_cost = 1
fail_cost = 10
p_success = 0.75 if getWaterTest(pos) else 0.25
expected_cost = p_success * success_cost + (1 - p_success) * fail_cost
return scale_cost(expected_cost)
FillCost = NonNegFunction([POS1], domain=[LegalPos(POS1)], fn=fill_cost)
rename_functions(locals())
actions = [
Action(name='Move', param=[CUP, POS1, POS2],
pre=[IsCup(CUP), CanMove(POS1, POS2),
AtPos(CUP, POS1)],
eff=[AtPos(CUP, POS2),
~AtPos(CUP, POS1),
Increase(TotalCost(), MoveCost(POS1, POS2))]),
Action(name='Fill', param=[CUP, FAUCET, POS1, WATER],
pre=[HoldsWater(CUP), IsFaucet(FAUCET), CanGetWater(POS1), IsWater(WATER),
AtPos(CUP, POS1), AtPos(FAUCET, faucetPos)],
eff=[WaterInKettle(WATER, CUP),
Increase(TotalCost(), FillCost(POS1))]),
Action(name='Pour', param=[CUP, KETTLE, POS1, POS3, POS2, WATER],
pre=[HoldsWater(CUP), HoldsWater(KETTLE), CanPour(POS1, POS3, POS2), IsWater(WATER),
AtPos(CUP, POS1), AtPos(KETTLE, POS2), WaterInKettle(WATER, CUP)],
eff=[WaterInKettle(WATER, KETTLE), AtPos(CUP, POS3),
~WaterInKettle(WATER, CUP), ~AtPos(CUP, POS1),
Increase(TotalCost(), scale_cost(1))]),
Action(name='Boil', param=[KETTLE, WATER, POS1],
pre=[IsKettle(KETTLE), IsWater(WATER), CloseTo(kettleGoalPos, POS1),
AtPos(KETTLE, POS1), WaterInKettle(WATER, KETTLE)],
eff=[WaterBoiled(WATER, KETTLE),
Increase(TotalCost(), scale_cost(1))]),
]
axioms = [
Axiom(param=[WATER, KETTLE],
pre=[IsWater(WATER), IsKettle(KETTLE),
WaterInKettle(WATER, KETTLE), WaterBoiled(WATER, KETTLE)],
eff=HasBoiledWater(KETTLE)),
]
def legalTest(rp):
(x, y, z) = rp
result = (domain[0][0] <= x <= domain[0][1]) and (
domain[1][0] <= y <= domain[1][1]) and (domain[2][0] <= z <= domain[2][1])
if verboseFns and not result:
print 'not legal:', rp
return result
def randPos():
while True:
pos = tuple(random.uniform(a[0], a[1]) for a in domain)
if verboseFns:
print 'randPos:', pos
yield (pos,)
def genMove(p):
while True:
pos = tuple(random.uniform(-1, 1) * maxMoveDist + a for a in p)
if distance(pos, p) < maxMoveDist and legalTest(pos):
if verboseFns:
print 'genMove:', pos
yield (pos,)
def genClosePos(p):
while True:
pos = tuple(random.uniform(-1, 1) * goalPosEps + a for a in p)
if (distance(pos, p) < goalPosEps) and legalTest(pos):
if verboseFns:
print 'genClosePos:', pos
yield (pos,)
def genPourPos(kpos):
while True:
x = kpos[0] + random.uniform((cupSize[0] / 2.0 + kettleSize[0] / 2.0),
(cupSize[0] / 2.0 + kettleSize[0] / 2.0 + cupSize[1] / 5.0), )
y = kpos[1] + random.uniform(kettleSize[1] + 2, kettleSize[1] + 5)
initpos = (x, y, 0)
endpos = (x, y, np.pi / 1.4 * np.sign(x - kpos[0]))
if legalTest(initpos) and legalTest(endpos):
if verboseFns:
print 'genPourPos:', initpos, endpos
yield (initpos, endpos)
elif verboseFns:
print 'genPourPos illegal:', initpos, endpos
def genGetWaterPos():
while True:
pos = (random.uniform(-1, 1) * goalPosEps + faucetPos[0],
random.uniform(-cupSize[1] - 1, -cupSize[1]) + faucetPos[1],
0)
if legalTest(pos):
if verboseFns:
print 'genGetWaterPos:', pos
yield (pos,)
streams = [
GenStream(inp=[], domain=[], fn=genGetWaterPos,
out=[POS2], graph=[LegalPos(POS2), CanGetWater(POS2)]),
GenStream(inp=[], domain=[], fn=randPos,
out=[POS1], graph=[LegalPos(POS1)]),
GenStream(inp=[POS1], domain=[LegalPos(POS1)], fn=genMove,
out=[POS2], graph=[LegalPos(POS2), CanMove(POS1, POS2)]),
GenStream(inp=[POS1], domain=[LegalPos(POS1)], fn=genClosePos,
out=[POS2], graph=[LegalPos(POS2), CloseTo(POS1, POS2)]),
GenStream(inp=[POS2], domain=[LegalPos(POS2)], fn=genPourPos,
out=[POS1, POS3], graph=[CanPour(POS1, POS3, POS2), LegalPos(POS1), LegalPos(POS3)]),
TestStream(inp=[POS1], domain=[LegalPos(POS1)], test=getWaterTest,
graph=[CanGetWater(POS1)]),
]
kettle = 'kettle'
cup = 'cup'
faucet = 'faucet'
water = 'water'
initial_atoms = [
IsKettle(kettle),
IsCup(cup),
IsFaucet(faucet),
IsWater(water),
HoldsWater(cup),
HoldsWater(kettle),
LegalPos(cupInitPos),
LegalPos(kettleInitPos),
LegalPos(kettleGoalPos),
LegalPos(faucetPos),
AtPos(kettle, kettleInitPos),
AtPos(faucet, faucetPos),
AtPos(cup, cupInitPos),
initialize(TotalCost(), 0),
]
goal_literals = [
AtPos(cup, cupInitPos),
HasBoiledWater(kettle),
]
objective = TotalCost()
return Problem(initial_atoms, goal_literals, actions, axioms, streams, objective=objective)
def main(n=2, bound='unique'):
initial_conf = CONF(-1)
blocks = ['b{}'.format(i) for i in xrange(n)]
initial_poses = {b: POSE(b, i) for i, b in enumerate(blocks)}
goal_poses = {'b0': POSE('b0', 1)}
Block = Predicate('?b')
IsPose = Predicate('?b ?p')
Conf = Predicate('?q')
Kin = Predicate('?b ?q ?p')
Collision = Predicate('?b ?p ?b2 ?p2',
domain=[IsPose('?b', '?p'),
IsPose('?b2', '?p2')],
fn=lambda b, p, b2, p2: p.value == p2.value,
eager=True,
bound=False)
AtConf = Predicate('?q')
AtPose = Predicate('?b ?p')
Holding = Predicate('?b')
HandEmpty = Predicate('')
Unsafe = Predicate('?b ?p')
rename_functions(locals())
streams = [
Stream(name='placement',
inp=['?b'],
domain=[Block('?b')],
fn=PoseGen,
out='?p',
graph=[IsPose('?b', '?p')],
bound=bound),
FnStream(name='ik',
inp='?b ?p',
domain=[IsPose('?b', '?p')],
fn=lambda b, p: (CONF(p.value), ),
out='?q',
graph=[Kin('?b', '?q', '?p'),
Conf('?q')],
bound=bound),
]
actions = [
Action(name='Move',
param='?q1 ?q2',
pre=[Conf('?q1'), Conf('?q2'),
AtConf('?q1')],
eff=[AtConf('?q2'), ~AtConf('?q1'),
Increase(TotalCost(), 1)]),
Action(name='Pick',
param='?b ?p ?q',
pre=[
Kin('?b', '?q', '?p'),
AtPose('?b', '?p'),
HandEmpty(),
AtConf('?q')
],
eff=[
Holding('?b'), ~AtPose('?b', '?p'), ~HandEmpty(),
Increase(TotalCost(), 1)
]),
Action(name='Place',
param='?b ?p ?q',
pre=[
Kin('?b', '?q', '?p'),
Holding('?b'),
AtConf('?q'), ~Unsafe('?b', '?p')
],
eff=[
AtPose('?b', '?p'),
HandEmpty(), ~Holding('?b'),
Increase(TotalCost(), 1)
]),
]
axioms = [
Axiom(param='?b ?p ?b2 ?p2',
pre=[Collision('?b', '?p', '?b2', '?p2'),
AtPose('?b2', '?p2')],
eff=Unsafe('?b', '?p')),
]
initial_atoms = [
Conf(initial_conf),
AtConf(initial_conf),
HandEmpty(),
initialize(TotalCost(), 0),
]
for b, p in initial_poses.items():
initial_atoms += [Block(b), IsPose(b, p), AtPose(b, p)]
for b, p in goal_poses.items():
initial_atoms += [IsPose(b, p)]
goal_literals = [AtPose(b, p) for b, p in goal_poses.items()]
problem = Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
print problem
pr = cProfile.Profile()
pr.enable()
plan, evaluations = dual_focused(problem,
terminate_cost=INF,
verbose=False,
bind=True,
use_context=True,
temp_dir='fish/',
clean=True)
print plan
pr.disable()
pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
def main(n=5, bound='unique'):
initial_conf = 0
initial_poses = {'b{}'.format(i): i for i in xrange(n)}
goal_poses = {'b1': 2}
Block = Predicate('?b')
Pose = Predicate('?p')
Conf = Predicate('?q')
Kin = Predicate('?q ?p')
CFree = Predicate('?p1 ?p2')
AtConf = Predicate('?q')
AtPose = Predicate('?b ?p')
Holding = Predicate('?b')
HandEmpty = Predicate('')
Safe = Predicate('?b ?p')
rename_functions(locals())
streams = [
Stream(name='placement',
inp=[],
domain=[],
fn=lambda: ([(p, )] for p in xrange(n, 100)),
out='?p',
graph=[Pose('?p')],
bound=bound),
FnStream(name='ik',
inp='?p',
domain=[Pose('?p')],
fn=lambda p: (p, ),
out='?q',
graph=[Kin('?q', '?p'), Conf('?q')],
bound=bound),
TestStream(name='collision',
inp='?p1 ?p2',
domain=[Pose('?p1'), Pose('?p2')],
test=lambda p1, p2: p1 != p2,
graph=[CFree('?p1', '?p2')],
eager=True,
bound=bound),
]
actions = [
Action(name='Move',
param='?q1 ?q2',
pre=[Conf('?q1'), Conf('?q2'),
AtConf('?q1')],
eff=[AtConf('?q2'), ~AtConf('?q1'),
Increase(TotalCost(), 1)]),
Action(name='Pick',
param='?b ?p ?q',
pre=[
Block('?b'),
Kin('?q', '?p'),
AtPose('?b', '?p'),
HandEmpty(),
AtConf('?q')
],
eff=[
Holding('?b'), ~AtPose('?b', '?p'), ~HandEmpty(),
Increase(TotalCost(), 1)
]),
]
for b in initial_poses:
actions += [
Action(name='Place-' + b,
param='?p ?q',
pre=[Block(b),
Kin('?q', '?p'),
Holding(b),
AtConf('?q')] +
[Safe(b2, '?p') for b2 in initial_poses if b2 != b],
eff=[
AtPose(b, '?p'),
HandEmpty(), ~Holding(b),
Increase(TotalCost(), 1)
])
]
axioms = [
Axiom(param='?p1 ?b2 ?p2',
pre=[Block('?b2'),
CFree('?p1', '?p2'),
AtPose('?b2', '?p2')],
eff=Safe('?b2', '?p1')),
]
initial_atoms = [
Conf(initial_conf),
AtConf(initial_conf),
HandEmpty(),
initialize(TotalCost(), 0),
]
for b, p in initial_poses.items():
initial_atoms += [Block(b), Pose(p), AtPose(b, p)]
for b, p in goal_poses.items():
initial_atoms += [Pose(p)]
goal_literals = [AtPose(b, p) for b, p in goal_poses.items()]
problem = Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
print problem
pr = cProfile.Profile()
pr.enable()
plan, evaluations = dual_focused(problem, terminate_cost=INF, verbose=True)
print plan
pr.disable()
pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
GRIPPER = '?gripper' CUP = '?cup' SPOON = '?spoon' KETTLE = '?kettle' BLOCK = '?block' POSE = '?end_pose' POSE2 = '?pose2' POSE3 = '?pose3' POSE4 = '?pose4' GRASP = '?grasp' CONTROL = '?control' ################################################## # Static predicates (predicates that do not change over time) IsGripper = Predicate([GRIPPER]) IsCup = Predicate([CUP]) IsStirrer = Predicate([KETTLE]) IsSpoon = Predicate([KETTLE]) IsBlock = Predicate([CUP]) IsPourable = Predicate([CUP]) IsPose = Predicate([CUP, POSE]) IsGrasp = Predicate([CUP, GRASP]) IsControl = Predicate([CONTROL]) CanGrasp = Predicate([GRIPPER, POSE, CUP, POSE2, GRASP, CONTROL]) BelowFaucet = Predicate([GRIPPER, POSE, CUP, GRASP]) CanPour = Predicate([GRIPPER, POSE, CUP, GRASP, KETTLE, POSE2, CONTROL]) Motion = Predicate([GRIPPER, POSE, POSE2, CONTROL]) MotionH = Predicate([GRIPPER, POSE, CUP, GRASP, POSE2, CONTROL])
def ss_from_problem(robot,
movable=[],
bound='shared',
teleport=False,
movable_collisions=False,
grasp_name='top'):
#assert (not are_colliding(tree, kin_cache))
rigid = [body for body in get_bodies() if body != robot]
fixed = [body for body in rigid if body not in movable]
print('Robot:', robot)
print('Movable:', movable)
print('Fixed:', fixed)
conf = BodyConf(robot, get_configuration(robot))
initial_atoms = [
HandEmpty(),
CanMove(),
IsConf(conf),
AtConf(conf),
initialize(TotalCost(), 0),
]
for body in movable:
pose = BodyPose(body, get_pose(body))
initial_atoms += [
IsMovable(body),
IsPose(body, pose),
AtPose(body, pose)
]
for surface in fixed:
initial_atoms += [Stackable(body, surface)]
if is_placement(body, surface):
initial_atoms += [IsSupported(pose, surface)]
for body in fixed:
name = get_body_name(body)
if 'sink' in name:
initial_atoms.append(Sink(body))
if 'stove' in name:
initial_atoms.append(Stove(body))
body = movable[0]
goal_literals = [
AtConf(conf),
#Holding(body),
#On(body, fixed[0]),
#On(body, fixed[2]),
#Cleaned(body),
Cooked(body),
]
PlacedCollision = Predicate([T, O, P],
domain=[IsTraj(T), IsPose(O, P)],
fn=get_movable_collision_test(),
bound=False)
actions = [
Action(name='pick',
param=[O, P, G, Q, T],
pre=[
IsKin(O, P, G, Q, T),
HandEmpty(),
AtPose(O, P),
AtConf(Q), ~Unsafe(T)
],
eff=[HasGrasp(O, G),
CanMove(), ~HandEmpty(), ~AtPose(O, P)]),
Action(
name='place',
param=[O, P, G, Q, T],
pre=[IsKin(O, P, G, Q, T),
HasGrasp(O, G),
AtConf(Q), ~Unsafe(T)],
eff=[HandEmpty(),
CanMove(),
AtPose(O, P), ~HasGrasp(O, G)]),
# Can just do move if we check holding collisions
Action(name='move_free',
param=[Q, Q2, T],
pre=[
IsFreeMotion(Q, Q2, T),
HandEmpty(),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(name='move_holding',
param=[Q, Q2, O, G, T],
pre=[
IsHoldingMotion(Q, Q2, O, G, T),
HasGrasp(O, G),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(
name='clean',
param=[O, O2], # Wirelessly communicates to clean
pre=[Stackable(O, O2),
Sink(O2), ~Cooked(O),
On(O, O2)],
eff=[Cleaned(O)]),
Action(
name='cook',
param=[O, O2], # Wirelessly communicates to cook
pre=[Stackable(O, O2),
Stove(O2), Cleaned(O),
On(O, O2)],
eff=[Cooked(O), ~Cleaned(O)]),
]
axioms = [
Axiom(param=[O, G],
pre=[IsGrasp(O, G), HasGrasp(O, G)],
eff=Holding(O)),
Axiom(param=[O, P, O2],
pre=[IsPose(O, P),
IsSupported(P, O2),
AtPose(O, P)],
eff=On(O, O2)),
]
if movable_collisions:
axioms += [
Axiom(param=[T, O, P],
pre=[IsPose(O, P),
PlacedCollision(T, O, P),
AtPose(O, P)],
eff=Unsafe(T)),
]
streams = [
GenStream(name='grasp',
inp=[O],
domain=[IsMovable(O)],
fn=get_grasp_gen(robot, grasp_name),
out=[G],
graph=[IsGrasp(O, G)],
bound=bound),
# TODO: test_support
GenStream(name='support',
inp=[O, O2],
domain=[Stackable(O, O2)],
fn=get_stable_gen(fixed=fixed),
out=[P],
graph=[IsPose(O, P), IsSupported(P, O2)],
bound=bound),
FnStream(name='inverse_kin',
inp=[O, P, G],
domain=[IsPose(O, P), IsGrasp(O, G)],
fn=get_ik_fn(robot, fixed=fixed, teleport=teleport),
out=[Q, T],
graph=[IsKin(O, P, G, Q, T),
IsConf(Q), IsTraj(T)],
bound=bound),
FnStream(name='free_motion',
inp=[Q, Q2],
domain=[IsConf(Q), IsConf(Q2)],
fn=get_free_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsFreeMotion(Q, Q2, T),
IsTraj(T)],
bound=bound),
FnStream(name='holding_motion',
inp=[Q, Q2, O, G],
domain=[IsConf(Q), IsConf(Q2),
IsGrasp(O, G)],
fn=get_holding_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsHoldingMotion(Q, Q2, O, G, T),
IsTraj(T)],
bound=bound),
]
return Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
from pybullet_tools.kuka_primitives import BodyPose, BodyConf, Command, get_grasp_gen, get_stable_gen, get_ik_fn, get_free_motion_gen, \
get_holding_motion_gen, get_movable_collision_test
from pybullet_tools.utils import WorldSaver, connect, dump_world, get_pose, set_pose, Pose, Point, set_default_camera, stable_z, \
BLOCK_URDF, get_configuration, SINK_URDF, STOVE_URDF, load_model, wait_for_interrupt, is_placement, get_body_name, \
disconnect, DRAKE_IIWA_URDF, get_bodies, user_input
#######################################################
O, O2 = '?o', '?o2'
P, P2 = '?p', '?p2'
G = '?g'
Q, Q2 = '?q', '?q2'
T = '?t'
IsMovable = Predicate([O])
Stackable = Predicate([O, O2])
Cleaned = Predicate([O])
Cooked = Predicate([O])
Sink = Predicate([O])
Stove = Predicate([O])
IsPose = Predicate([O, P])
IsGrasp = Predicate([O, G])
IsConf = Predicate([Q])
IsTraj = Predicate([T])
IsSupported = Predicate([P, O2])
IsKin = Predicate([O, P, G, Q, T])
IsFreeMotion = Predicate([Q, Q2, T])
IsHoldingMotion = Predicate([Q, Q2, O, G, T])
def evaluate_negative_atoms(universe, evaluations, opt_plan):
success = True
negative_atoms = negative_axioms(universe, opt_plan)
for atom in negative_atoms:
if set(atom.head.domain()) <= evaluations:
eval = atom.head.get_eval()
evaluations.add(eval)
success &= (atom.value == eval.value)
else:
success = False
return success, negative_atoms
X = '?x'
Computed = Predicate([X], name='Computed')
Computable = Predicate([X], name='Computable')
def initialize_fluent_streams(problem):
fluents = problem.fluents()
new_axioms = []
for stream in problem.streams:
stream.fluent_domain = tuple(a for a in stream.domain
if a.head.function in fluents)
stream.domain = tuple(a for a in stream.domain
if a not in stream.fluent_domain)
stream.predicates = []
for i, o in enumerate(stream.outputs):
def ss_from_problem(problem,
bound='shared',
remote=False,
teleport=False,
movable_collisions=False):
robot = problem.robot
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
initial_atoms = [
CanMove(),
IsBConf(initial_bq),
AtBConf(initial_bq),
initialize(TotalCost(), 0),
]
for name in problem.arms:
joints = get_arm_joints(robot, name)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
initial_atoms += [
IsArm(name),
HandEmpty(name),
IsAConf(name, conf),
AtAConf(name, conf)
]
for body in problem.movable:
pose = Pose(body, get_pose(body))
initial_atoms += [
IsMovable(body),
IsPose(body, pose),
AtPose(body, pose),
POSE(pose)
]
for surface in problem.surfaces:
initial_atoms += [Stackable(body, surface)]
if is_placement(body, surface):
initial_atoms += [IsSupported(pose, surface)]
initial_atoms += map(Washer, problem.sinks)
initial_atoms += map(Stove, problem.stoves)
initial_atoms += [IsConnected(*pair) for pair in problem.buttons]
initial_atoms += [IsButton(body) for body, _ in problem.buttons]
goal_literals = []
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
initial_atoms += [IsBConf(goal_conf)]
goal_literals += [AtBConf(goal_conf)]
goal_literals += [Holding(*pair) for pair in problem.goal_holding]
goal_literals += [On(*pair) for pair in problem.goal_on]
goal_literals += map(Cleaned, problem.goal_cleaned)
goal_literals += map(Cooked, problem.goal_cooked)
#GraspedCollision = Predicate([A, G, AT], domain=[IsArm(A), POSE(G), IsArmTraj(AT)],
# fn=lambda a, p, at: False, bound=False)
PlacedCollision = Predicate([AT, P],
domain=[IsArmTraj(AT), POSE(P)],
fn=lambda at, p: False,
bound=False)
actions = [
Action(name='pick',
param=[A, O, P, G, BQ, AT],
pre=[
IsKin(A, O, P, G, BQ, AT),
HandEmpty(A),
AtPose(O, P),
AtBConf(BQ), ~Unsafe(AT)
],
eff=[
HasGrasp(A, O, G),
CanMove(), ~HandEmpty(A), ~AtPose(O, P),
Increase(TotalCost(), 1)
]),
Action(name='place',
param=[A, O, P, G, BQ, AT],
pre=[
IsKin(A, O, P, G, BQ, AT),
HasGrasp(A, O, G),
AtBConf(BQ), ~Unsafe(AT)
],
eff=[
HandEmpty(A),
CanMove(),
AtPose(O, P), ~HasGrasp(A, O, G),
Increase(TotalCost(), 1)
]),
Action(
name='move',
param=[BQ, BQ2, BT],
pre=[IsMotion(BQ, BQ2, BT),
CanMove(),
AtBConf(BQ), ~Unsafe(BT)],
eff=[
AtBConf(BQ2), ~CanMove(), ~AtBConf(BQ),
Increase(TotalCost(), 1)
]),
# TODO: should the robot be allowed to have different carry configs or a defined one?
#Action(name='move_arm', param=[A, BQ, BQ2],
# pre=[IsAConf(A, BQ), IsAConf(A, BQ),
# CanMove(), AtBConf(BQ)],
# eff=[AtAConf(BQ2), ~AtAConf(BQ),
# Increase(TotalCost(), 1)]),
# Why would one want to move to the pick configuration for something anywhere but the goal?
# Stream that generates arm motions as long as one is a shared configuration
# Maybe I should make a base
]
if remote:
actions += [
Action(
name='clean',
param=[O, O2], # Wirelessly communicates to clean
pre=[Stackable(O, O2),
Washer(O2), ~Cooked(O),
On(O, O2)],
eff=[Cleaned(O)]),
Action(
name='cook',
param=[O, O2], # Wirelessly communicates to cook
pre=[Stackable(O, O2),
Stove(O2),
Cleaned(O),
On(O, O2)],
eff=[Cooked(O), ~Cleaned(O)]),
]
else:
actions += [
Action(name='press_clean',
param=[O, O2, A, B, BQ, AT],
pre=[
Stackable(O, O2),
Washer(O2),
IsConnected(B, O2),
IsPress(A, B, BQ, AT), ~Cooked(O),
On(O, O2),
HandEmpty(A),
AtBConf(BQ)
],
eff=[Cleaned(O), CanMove()]),
Action(name='press_cook',
param=[O, O2, A, B, BQ, AT],
pre=[
Stackable(O, O2),
Stove(O2),
IsConnected(B, O2),
IsPress(A, B, BQ, AT),
Cleaned(O),
On(O, O2),
HandEmpty(A),
AtBConf(BQ)
],
eff=[Cooked(O), ~Cleaned(O),
CanMove()]),
]
axioms = [
Axiom(param=[A, O, G],
pre=[IsArm(A), IsGrasp(O, G),
HasGrasp(A, O, G)],
eff=Holding(A, O)),
Axiom(param=[O, P, O2],
pre=[IsPose(O, P),
IsSupported(P, O2),
AtPose(O, P)],
eff=On(O, O2)),
]
if movable_collisions:
axioms += [
Axiom(param=[O, P, AT],
pre=[IsPose(O, P),
PlacedCollision(AT, P),
AtPose(O, P)],
eff=Unsafe(AT)),
]
streams = [
FnStream(name='motion',
inp=[BQ, BQ2],
domain=[IsBConf(BQ), IsBConf(BQ2)],
fn=get_motion_gen(problem, teleport=teleport),
out=[BT],
graph=[IsMotion(BQ, BQ2, BT),
IsBaseTraj(BT)],
bound=bound),
ListStream(name='grasp',
inp=[O],
domain=[IsMovable(O)],
fn=get_grasp_gen(problem),
out=[G],
graph=[IsGrasp(O, G), GRASP(G)],
bound=bound),
# TODO: test_support
Stream(name='support',
inp=[O, O2],
domain=[Stackable(O, O2)],
fn=get_stable_gen(problem),
out=[P],
graph=[IsPose(O, P), IsSupported(P, O2),
POSE(P)],
bound=bound),
GenStream(
name='ik_ir',
inp=[A, O, P, G],
domain=[IsArm(A), IsPose(O, P),
IsGrasp(O, G)],
fn=get_ik_ir_gen(problem, teleport=teleport),
out=[BQ, AT],
graph=[IsKin(A, O, P, G, BQ, AT),
IsBConf(BQ),
IsArmTraj(AT)],
bound=bound),
GenStream(name='press',
inp=[A, B],
domain=[IsArm(A), IsButton(B)],
fn=get_press_gen(problem, teleport=teleport),
out=[BQ, AT],
graph=[IsPress(A, B, BQ, AT),
IsBConf(BQ),
IsArmTraj(AT)],
bound=bound),
]
return Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
from debug_examples.test_json import load_json_problem A = '?a' O = '?o' O2 = '?o2' B = '?b' #S = '?s' # Surface (in case I don't want to worry about stacking for real P = '?p' P2 = '?p2' G = '?g' BQ = '?q' BQ2 = '?q2' AT = '?at' BT = '?bt' IsArm = Predicate([A]) IsMovable = Predicate([O]) Stackable = Predicate([O, O2]) POSE = Predicate([P]) GRASP = Predicate([G]) IsBConf = Predicate([BQ]) IsAConf = Predicate([A, BQ]) IsPose = Predicate([O, P]) IsSupported = Predicate([P, O2]) IsGrasp = Predicate([O, G]) IsArmTraj = Predicate([AT]) IsBaseTraj = Predicate([BT]) IsKin = Predicate([A, O, P, G, BQ, AT]) IsMotion = Predicate([BQ, BQ2, BT])
def main(n=2, verbose=False):
Item = Predicate('?b')
Part = Predicate('?r')
Class = Predicate('?c')
IsMovable = Predicate('?i', domain=[Item('?i')])
IsFixed = Predicate('?i', domain=[Item('?i')])
IsClass = Predicate('?i ?c', domain=[Item('?i'), Class('?c')])
IsArm = Predicate('?r', domain=[Part('?r')])
IsStackable = Predicate('?i1 ?i2', domain=[Item('?i1'), Item('?i2')])
HandHolding = Predicate('?r ?b', domain=[IsArm('?r'), Item('?b')])
HandEmpty = Predicate('?r', domain=[IsArm('?r')])
On = Predicate('?i1 ?i2', domain=[Item('?i1'), Item('?i2')])
Nearby = Predicate('?s', domain=[IsFixed('?s')])
Found = Predicate('?b', domain=[Item('?b')])
Localized = Predicate('?b', domain=[Item('?b')])
Holding = Predicate('?c', domain=[Class('?c')])
rename_functions(locals())
actions = [
Action(
name='Pick',
param='?a ?i ?s',
pre=[
IsArm('?a'),
IsStackable('?i', '?s'),
HandEmpty('?a'),
Nearby('?s'),
On('?i', '?s'),
Localized('?i')
],
eff=[HandHolding('?a', '?i'), ~HandEmpty('?a'), ~On('?i', '?s')]),
Action(name='Place',
param='?a ?i ?s',
pre=[
IsArm('?a'),
IsStackable('?i', '?s'),
Nearby('?s'),
HandHolding('?a', '?i'),
Localized('?s')
],
eff=[HandEmpty('?a'),
On('?i', '?s'), ~HandHolding('?a', '?i')]),
Action(name='ScanRoom',
param='?s',
pre=[IsFixed('?s'), ~Found('?s')],
eff=[Found('?s')]),
Action(name='ScanFixed',
param='?s ?i',
pre=[IsStackable('?i', '?s'), ~Found('?i')],
eff=[Found('?i'),
On('?i', '?s'),
Increase(TotalCost(), 1)]),
Action(name='Look',
param='?i',
pre=[Found('?i')],
eff=[Localized('?i')]),
Action(name='Move',
param='?s',
pre=[IsFixed('?s')],
eff=[Nearby('?s')] + [~Nearby(s)
for s in ['table0', 'table1']] +
[~Localized(s) for s in ['soup0']]),
]
initial_atoms = [
initialize(TotalCost(), 0),
HandEmpty('left'),
HandEmpty('right'),
Found('table0'),
Found('table1'),
IsClass('table0', 'table'),
IsClass('table1', 'table'),
IsFixed('table0'),
IsFixed('table1'),
IsStackable('soup0', 'table0'),
IsStackable('soup0', 'table1'),
IsClass('soup0', 'soup'),
Found('soup0'),
On('soup0', 'table0'),
IsStackable('green0', 'table0'),
IsStackable('green0', 'table1'),
IsClass('green0', 'green'),
Found('green0'),
On('green0', 'table0'),
]
axioms = [
Axiom(param='?a ?i ?c',
pre=[IsArm('?a'),
IsClass('?i', '?c'),
HandHolding('?a', '?i')],
eff=Holding('?c')),
]
goal_literals = [Holding('green'), Holding('soup')]
problem = Problem(initial_atoms,
goal_literals,
actions,
axioms, [],
objective=TotalCost())
print problem
plan, evaluations = incremental(problem, verbose=verbose)
print plan
def main():
initial_bq = BConf(0, 1)
initial_poses = {
'green': [10, 10, 30],
'table': [10, 20, 30],
}
movable = {'green'}
initial_p_from_name = {}
class_from_name = {}
for cl in initial_poses:
for i, x in enumerate(initial_poses[cl]):
name = cl + str(i)
class_from_name[name] = cl
initial_p_from_name[name] = Pose(name, x)
print initial_p_from_name
print class_from_name
O = '?o'
S = '?s'
R = '?r'
P = '?p'
P2 = '?p2'
G = '?g'
Q = '?q'
Q2 = '?q2'
C = '?c'
base = 'base'
head = 'head'
left = 'left'
right = 'right'
IsPose = Predicate([O, P])
IsGrasp = Predicate([O, G])
IsConf = Predicate([P, Q])
IsMovable = Predicate([O])
IsFixed = Predicate([O])
IsKin = Predicate([R, O, P, G, Q])
IsSupported = Predicate([P, P2])
IsArm = Predicate([R])
IsClass = Predicate([O, C])
AtPose = Predicate([O, P])
AtConf = Predicate([R, Q])
HasGrasp = Predicate([R, O, G])
HandEmpty = Predicate([R])
Holding = Predicate([O])
On = Predicate([O, S])
base_constant_cost = 1
def get_circle(value):
if isinstance(value, BConf):
return value.x, 0
if isinstance(value, Pose):
return value.x, 0
if isinstance(value, InputOutputSet):
if value.stream.name == 'IK':
_, _, p, _ = value.inputs
x, r = get_circle(p)
return x, r + 0
if value.stream.name == 'placement':
_, _, p = value.inputs
x, r = get_circle(p)
return x, r + 0
raise ValueError(value)
def distance_bound_fn(q1, q2):
x1, r1 = get_circle(q1)
x2, r2 = get_circle(q2)
return max(abs(x2 - x1) - (r1 + r2), 0) + base_constant_cost
Distance = Function([Q, Q2],
domain=[IsConf(base, Q),
IsConf(base, Q2)],
fn=lambda q1, q2: abs(q2.x - q1.x) + abs(q2.y - q1.y) +
base_constant_cost,
bound=distance_bound_fn)
rename_functions(locals())
bound = 'shared'
streams = [
FnStream(name='grasp',
inp=[O],
domain=[IsMovable(O)],
fn=lambda o: (Grasp(o), ),
out=[G],
graph=[IsGrasp(O, G)],
bound=bound),
ListStream(name='IK',
inp=[R, O, P, G],
domain=[IsArm(R), IsPose(O, P),
IsGrasp(O, G)],
fn=lambda r, o, p, g: [(BConf(p.x, +1), )],
out=[Q],
graph=[IsKin(R, O, P, G, Q),
IsConf(base, Q)],
bound=bound),
FnStream(name='placement',
inp=[O, S, P],
domain=[IsMovable(O), IsFixed(S),
IsPose(S, P)],
fn=lambda o, s, p: (Pose(o, p.x), ),
out=[P2],
graph=[IsPose(O, P2), IsSupported(P2, P)],
bound=bound),
]
actions = [
Action(name='pick',
param=[R, O, P, G, Q],
pre=[
IsKin(R, O, P, G, Q),
HandEmpty(R),
AtPose(O, P),
AtConf(base, Q)
],
eff=[HasGrasp(R, O, G), ~HandEmpty(R)]),
Action(name='place',
param=[R, O, P, G, Q],
pre=[IsKin(R, O, P, G, Q),
HasGrasp(R, O, G),
AtConf(base, Q)],
eff=[HandEmpty(R),
AtPose(O, P), ~HasGrasp(R, O, G)]),
Action(
name='move_base',
param=[Q, Q2],
pre=[IsConf(base, Q),
IsConf(base, Q2),
AtConf(base, Q)],
eff=[AtConf(base, Q2), ~AtConf(base, Q),
Increase(TotalCost(), 1)]),
Action(name='move_head',
param=[Q, Q2],
pre=[IsConf(head, Q),
IsConf(head, Q2),
AtConf(head, Q)],
eff=[AtConf(head, Q2), ~AtConf(head, Q)]),
]
axioms = [
Axiom(param=[R, O, G],
pre=[IsArm(R), IsGrasp(O, G),
HasGrasp(R, O, G)],
eff=Holding(O)),
Axiom(param=[O, P, S, P2],
pre=[
IsPose(O, P),
IsPose(S, P2),
IsSupported(P, P2),
AtPose(O, P),
AtPose(S, P2)
],
eff=On(O, S)),
]
initial_atoms = [
IsArm(left),
HandEmpty(left),
HandEmpty(right),
IsConf(base, initial_bq),
AtConf(base, initial_bq),
initialize(TotalCost(), 0),
]
for n, p in initial_p_from_name.items():
initial_atoms += [IsPose(n, p), AtPose(n, p)]
if class_from_name[n] not in movable:
continue
for n2, p2 in initial_p_from_name.items():
if class_from_name[n2] in movable:
continue
if p.x == p2.x:
initial_atoms.append(IsSupported(p, p2))
for n, cl in class_from_name.items():
if cl in movable:
initial_atoms.append(IsMovable(n))
else:
initial_atoms.append(IsFixed(n))
goal_literals = [On('green0', 'table1'), On('green1', 'table1')]
problem = Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())
print problem
pr = cProfile.Profile()
pr.enable()
plan, _ = dual_focused(problem)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
print plan
return
print 'Length', len(plan)
for i, act in enumerate(plan):
print i, act