def convert_axioms_to_effects(
problem
): # NOTE - can always use the previous compilation process for some problematic axioms
derived_predicates = problem.get_derived_predicates()
fluent_predicates = problem.get_fluent_predicates()
constants = defaultdict(set)
for const in problem.get_constants():
constants[const.type].add(const)
mapping = []
for op in problem.operators: # NOTE - can always just convert one
if isinstance(op, Axiom):
[conditions] = op.condition.dequantify(constants).get_literals(
) # TODO - more complicated if multiple ways to achieve
assert not filter(
lambda a: a.predicate in derived_predicates,
conditions) # TODO - difficulty when levels of axioms
print conditions
[fluent] = filter(
lambda a: a.predicate in fluent_predicates,
conditions) # NOTE - could easily expand to conjunctive case
others = filter(lambda a: a != fluent, conditions)
mapping.append((fluent, others, op.effect))
new_operators = []
for op in problem.operators:
if isinstance(op, Action):
new_op = op.clone()
new_operators.append(new_op)
[literals] = new_op.effect.get_literals() # TODO
for literal in literals:
effect = literal.formula if isinstance(literal,
Not) else literal
for fluent, conditions, derived in mapping:
if effect.predicate == fluent.predicate:
free_params = set(
flatten(
map(lambda a: a.args, [derived] +
conditions))) - set(fluent.args)
param_map = dict(zip(fluent.args, effect.args))
for i, param in enumerate(
free_params): # Prevents conflicting names
#param_map[param] = Parameter('_%s'%i, param.type) # NOTE - FF can't parse this
param_map[param] = Parameter('l%s' % i, param.type)
new_params = list(
set(param_map.values()) - set(effect.args))
new_derived = derived.instantiate(param_map)
new_conditions = [
cond.instantiate(param_map) for cond in conditions
] # TODO - could put in the negated version of new_derived
if isinstance(literal, Not):
new_op.add_effects(
ForAll(new_params,
When(And(*new_conditions),
new_derived)))
else:
new_op.add_effects(
ForAll(
new_params,
When(And(*new_conditions),
Not(new_derived)))
) # Not necessary, but it could reduce the number of instances
#op.add_effects(ForAll(new_params, Not(new_derived))) # One failure can break
return new_operators
def compile_problem(tamp_problem):
"""
Constructs a STRIPStream problem for the continuous TMP problem.
:param tamp_problem: a :class:`.TMPProblem`
:return: a :class:`.STRIPStreamProblem`
"""
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
R = Param(REGION)
actions = [
Action(name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1),
LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(name='place',
parameters=[B1, P1, Q1],
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1,
P1)))),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
Action(name='clean',
parameters=[B1, R],
condition=And(InRegion(B1, R), IsSink(R)),
effect=And(Cleaned(B1))),
Action(name='cook',
parameters=[B1, R],
condition=And(InRegion(B1, R), IsStove(R)),
effect=And(Cooked(B1))),
]
axioms = [
Axiom(effect=InRegion(B1, R),
condition=Exists([P1], And(AtPose(B1, P1), Contained(B1, P1,
R)))),
Axiom(effect=Safe(B2, B1, P1),
condition=Exists([P2],
And(AtPose(B2, P2),
CollisionFree(B1, P1, B2, P2)))),
]
cond_streams = [
EasyGenStream(inputs=[R, B1],
outputs=[P1],
conditions=[CanPlace(B1, R)],
effects=[Contained(B1, P1, R)],
generator=lambda r, b:
(sample_region_pose(r, b) for _ in irange(0, INF))),
EasyGenStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[LegalKin(P1, Q1)],
generator=lambda p: iter([inverse_kinematics(p)])),
EasyTestStream(inputs=[R, B1, P1],
conditions=[],
effects=[Contained(B1, P1, R)],
test=in_region,
eager=EAGER_TESTS,
plannable=False),
EasyTestStream(inputs=[B1, P1, B2, P2],
conditions=[],
effects=[CollisionFree(B1, P1, B2, P2)],
test=lambda *args: not are_colliding(*args),
eager=EAGER_TESTS),
]
constants = [
REGION(tamp_problem.env_region),
]
initial_atoms = [
AtConf(tamp_problem.initial_config),
] + [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
] + [
CanPlace(block, tamp_problem.env_region)
for block in tamp_problem.initial_poses
]
if tamp_problem.initial_holding is None:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(tamp_problem.initial_holding))
goal_literals = []
if tamp_problem.goal_config is not None:
goal_literals.append(AtConf(tamp_problem.goal_config))
if tamp_problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif tamp_problem.goal_holding is not None:
goal_literals.append(Holding(tamp_problem.goal_holding))
for block, goal in tamp_problem.goal_poses:
goal_literals.append(AtPose(block, goal))
for block, goal in tamp_problem.goal_regions:
initial_atoms.append(CanPlace(block, goal))
goal_literals.append(InRegion(block, goal))
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
OBJ, LOC, STATE = Type(), Type(), Type()
At = Predicate(OBJ, LOC)
HasState = Predicate(OBJ, STATE)
Clear = Predicate(LOC)
IsDryer = Predicate(LOC)
IsPainter = Predicate(LOC)
IsWasher = Predicate(LOC)
O, L1, L2, S = Param(OBJ), Param(LOC), Param(LOC), Param(STATE)
actions = [
Action(name='transport',
parameters=[O, L1, L2],
condition=And(At(O, L1), Clear(L2)),
effect=And(At(O, L2), Clear(L1), Not(At(O, L1)), Not(
Clear(L2)))), # NOTE - Leslie and Tomas call this Move
Action(name='wash',
parameters=[O, L1, S],
condition=And(At(O, L1), HasState(O, S), IsWasher(L1)),
effect=And(HasState(O, clean), Not(HasState(O, S)))),
Action(name='paint',
parameters=[O, L1],
condition=And(At(O, L1), HasState(O, clean), IsPainter(L1)),
effect=And(HasState(O, wet), Not(HasState(O, clean)))),
Action(name='dry',
parameters=[O, L1],
condition=And(At(O, L1), HasState(O, wet), IsDryer(L1)),
effect=And(HasState(O, dry), Not(HasState(O, wet)))),
]
# NOTE - can add the movable objects here
operators = [
# Without grasp trajectory
#Action(name='pick', parameters=[O, P, G, BQ, MQ],
# condition=And(PoseEq(O, P), HandEmpty(), BaseEq(BQ), ManipEq(MQ), Kin(P, G, BQ, MQ)), #ForAll([OB], Or(Equal(O, OB), Safe(OB, T))))),
# effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
#
#Action(name='place', parameters=[O, P, G, BQ, MQ],
# condition=And(GraspEq(O, G), BaseEq(BQ), ManipEq(MQ), Kin(P, G, BQ, MQ),
# ForAll([O2], Or(Equal(O, O2), SafePose(O2, P)))),
# effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
# With grasp trajectory
Action(name='pick', parameters=[O, P, G, BQ, MQ, GT],
condition=And(PoseEq(O, P), HandEmpty(), BaseEq(BQ), ManipEq(MQ), GraspTraj(P, G, BQ, MQ, GT)),
effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
Action(name='place', parameters=[O, P, G, BQ, MQ, GT],
condition=And(GraspEq(O, G), BaseEq(BQ), ManipEq(MQ), GraspTraj(P, G, BQ, MQ, GT),
ForAll([O2], Or(Equal(O, O2), SafePose(O2, P)))),
effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
# NOTE - I could make actions that backup or move forward instead
Action(name='move_arm', parameters=[MQ, MQ2, BQ, MT],
#condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), ForAll([O2], SafePoseMove(O2, MT))),
condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ), ForAll([O2], SafeMove(O2, MT))),
#condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, BQ, MT), BaseEq(BQ),
# ForAll([O2], Or(SafeGraspMove(O2, MT), SafePoseMove(O2, MT)))), # NOTE - bad idea, creates one per each combo
effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))),
def compile_belief(belief, goal):
locations, _, _ = maximum_likelihood_obs(belief)
constants = map(OBJ, belief.objLoc.keys()) + map(POSE,
belief.occupancies.keys())
initial_atoms = []
#initial_atoms += [At(obj, p) for obj, p in locations.iteritems()]
initial_atoms += [
BAt(obj, p, round(belief.objLocDist(obj).prob(p), 3))
for obj, p in locations.iteritems()
] # NOTE - using the maximum belief here
goal_literals = []
for fluent in goal.fluents:
if isinstance(fluent, Bd):
literal, arg, prob = fluent.args
if isinstance(literal, ObjLoc):
goal_literals.append(
BAtAbove(literal.args[0], literal.value, prob))
elif isinstance(literal, ObjState) and arg == 'clean':
goal_literals.append(BClean(literal.args[0], prob))
else:
raise NotImplementedError(literal)
else:
raise NotImplementedError(fluent)
O, P, B = Param(OBJ), Param(POSE), Param(BELIEF)
B1, B2 = Param(BELIEF), Param(BELIEF)
p_obs_t = 1 - glob.failProbs['Look']
p_obs_f = 1 - p_obs_t
#cost = prob_cost(p_obs_t) # This isn't informative
#cost = prob_cost(float(prior.name)*p_obs_t) # TODO - need to automatically derive the costs
actions = [
#PerfectLook(),
Look(p_obs_t),
#BackwardLook(p_obs_t),
Transport(),
]
axioms = [
Axiom(BAtAbove(O, P, B2),
Exists([B1], And(BAt(O, P, B1), Above(B1, B2)))),
#Axiom(IsPossible(B1, B2), Exists([B], And(IsUpdate(B, B2), Above(B1, B)))), # NOTE - this only uses static facts
]
cond_streams = [
GeneratorStream(inputs=[B1],
outputs=[B2],
conditions=[],
effects=[IsUpdate(B1, B2)],
generator=lambda b1:
[round(forward_belief(b1, p_obs_t, p_obs_f), 3)]),
#GeneratorStream(inputs=[B2], outputs=[B1], conditions=[], effects=[IsUpdate(B1, B2)],
# generator=lambda b2: [round(inverse_belief(b2, p_obs_t, p_obs_f), 3)]), # NOTE - I previously used a Deferred Literal to produce the initial belief
#TestStream(inputs=[O, P, B1], conditions=[], effects=[BAt(O, P, B1)],
# test=lambda o, p, b: belief.objLocDist(o).prob(p) >= b, eager=True),
TestStream(inputs=[B1, B2],
conditions=[],
effects=[Above(B1, B2)],
test=lambda b1, b2: b1 >= b2,
eager=True),
#TestStream(inputs=[B1, B2, B], conditions=[IsUpdate(B, B2), Above(B1, B)], effects=[IsPossible(B1, B2)],
# test=lambda *args: True, eager=True),
]
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
def get_axioms(operators, static_map, objects):
axioms = []
for _, conditions, effects in get_dnf_operators(operators, objects):
[effect] = effects
axioms.append(Axiom(effect, And(*conditions)))
return get_operators(axioms, static_map, objects, PyAxiom)
def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot = env.GetRobots()[0]
set_base_conf(robot, (-.75, .2, -math.pi / 2))
initialize_openrave(env, ARM, min_delta=.01)
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot,
plannername=None,
maxvelmult=None)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
all_bodies = bodies.values()
assert len({body.GetKinematicsGeometryHash()
for body in all_bodies
}) == 1 # NOTE - assuming all objects has the same geometry
body1 = all_bodies[-1] # Generic object 1
body2 = all_bodies[-2] if len(bodies) >= 2 else body1 # Generic object 2
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH,
USE_GRASP_TYPE)[:MAX_GRASPS]
poses = problem.known_poses if problem.known_poses else []
open_gripper(manipulator)
initial_conf = Conf(
robot.GetConfigurationValues()[manipulator.GetArmIndices()])
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G],
outputs=[Q, T],
conditions=[],
effects=[GraspMotion(P, G, Q, T)],
generator=sample_grasp_traj_fn(
env, robot, manipulator, body1, all_bodies)),
# Move trajectory
EasyListGenStream(inputs=[Q, Q2],
outputs=[T],
conditions=[],
effects=[FreeMotion(Q, Q2, T)],
generator=sample_free_motion_fn(
manipulator, base_manip, cspace, all_bodies),
order=1,
max_level=0),
EasyListGenStream(inputs=[Q, Q2, G],
outputs=[T],
conditions=[],
effects=[HoldingMotion(Q, Q2, G, T)],
generator=sample_holding_motion_fn(
robot, manipulator, base_manip, cspace, body1,
all_bodies),
order=1,
max_level=0),
# Collisions
EasyTestStream(inputs=[P, P2],
conditions=[],
effects=[CFreePose(P, P2)],
test=cfree_pose_fn(env, body1, body2),
eager=True),
EasyTestStream(inputs=[T, P],
conditions=[],
effects=[CFreeTraj(T, P)],
test=cfree_traj_fn(env, robot, manipulator, body1,
body2, all_bodies)),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ConfEq(initial_conf),
HandEmpty(),
] + [PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()]
goal_formula = And(
ConfEq(initial_conf),
*(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula,
actions + axioms, cond_streams,
constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
solve = lambda: incremental_planner(
stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
#solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False, max_time=15)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
if viewer and plan is not None:
print SEPARATOR
visualize_solution(env, problem, initial_conf, robot, manipulator,
bodies, plan)
raw_input('Finish?')
cost = int(COST_SCALE * geom_cost(1, prob)) if not UNIT else 1
return [
IsMoveUpdate(loc1, prior, loc2, post),
Initialize(MoveCost(*self.inputs[:2]), cost)
] # TODO - make this a test stream
##################################################
# NOTE - the location here is like a control parameter
actions = [
Action(
name='transport',
parameters=[O, L, L2, B, B2], # NOTE - Leslie and Tomas call this Move
condition=And(BAt(O, B), IsMoveUpdate(L, B, L2, B2)),
effect=And(
BAt(O, B2),
Not(BAt(O, B)), #)),
Cost(MoveCost(L, B)))),
#Action(name='transport', parameters=[O, B, L, B2], # NOTE - I could include belief preconditions for safety or effectiveness
# condition=And(BAt(O, B), BAtAbove(L, B2, ), IsMoveUpdate(B, L, B2)),
# effect=And(BAt(O, B2), Not(BAt(O, B)))), # NOTE - Leslie and Tomas call this Move
Action(
name='find',
parameters=[O, L, B, B2],
condition=And(BAt(O, B), IsLookUpdate(B, L, B2)),
effect=And(
BAt(O, B2),
Not(BAt(O, B)), #)),
def add_conditions(self, *new_conditions):
self.condition = And(self.condition, *new_conditions)
def create_problem(n=40):
"""
Creates the 1D task and motion planning STRIPStream problem.
:return: a :class:`.STRIPStreamProblem`
"""
blocks = ['block%i'%i for i in xrange(n)]
num_poses = pow(10, 10)
initial_config = 0 # the initial robot configuration is 0
initial_poses = {block: i for i, block in enumerate(blocks)} # the initial pose for block i is i
goal_poses = {blocks[0]: 1} # the goal pose for block i is i+1
####################
# Data types
CONF, BLOCK, POSE = Type(), Type(), Type()
# Fluent predicates
AtConf = Pred(CONF)
AtPose = Pred(BLOCK, POSE)
IsPose = Pred(BLOCK, POSE)
HandEmpty = Pred()
Holding = Pred(BLOCK)
# Derived predicates
Safe = Pred(BLOCK, POSE)
Unsafe = Pred(BLOCK, POSE)
# Static predicates
Kin = Pred(POSE, CONF)
CFree = Pred(POSE, POSE)
Collision = Pred(POSE, POSE)
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
rename_easy(locals()) # Trick to make debugging easier
actions = [
Action(name='move', parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = [
#Axiom(effect=Safe(B2, P1),
# condition=Or(Holding(B2), Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Not(Collision(P1, P2)))))),
Axiom(effect=Unsafe(B2, P1),
condition=Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Collision(P1, P2)))),
]
####################
# Conditional stream declarations
cond_streams = [
#GeneratorStream(inputs=[B1], outputs=[P1], conditions=[], effects=[IsPose(B1, P1)],
# generator=lambda b: ((p,) for p in xrange(n, num_poses))),
GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[Kin(P1, Q1)],
generator=lambda p: [(p,)]), # Inverse kinematics
TestStream(inputs=[P1, P2], conditions=[], effects=[Collision(P1, P2)],
test=lambda p1, p2: p1 == p2, eager=True),
]
# TODO: this actually isn't slow at all. What was the problem then?
# Maybe it was that I was creating many predicates?
# It could have also been the lack of IsPose predicates
for b in blocks:
axioms += [
# TODO: to do this, need to make b a parameter and set it using inequality
#Axiom(effect=Unsafe(b, P1),
# condition=Exists([P2], And(AtPose(b, P2), IsPose(b, P2), Collision(P1, P2)))),
]
actions += [
Action(name='pick-{}'.format(b), parameters=[P1, Q1],
condition=And(AtPose(b, P1), HandEmpty(), AtConf(Q1), IsPose(b, P1), Kin(P1, Q1)),
effect=And(Holding(b), Not(AtPose(b, P1)), Not(HandEmpty()))),
Action(name='place-{}'.format(b), parameters=[P1, Q1],
condition=And(Holding(b), AtConf(Q1), IsPose(b, P1), Kin(P1, Q1),
#*[Safe(b2, P1) for b2 in blocks if b2 != b]),
*[Not(Unsafe(b2, P1)) for b2 in blocks if b2 != b]),
effect=And(AtPose(b, P1), HandEmpty(), Not(Holding(b)))),
]
cond_streams += [
GeneratorStream(inputs=[], outputs=[P1], conditions=[], effects=[IsPose(b, P1)],
generator=lambda: ((p,) for p in xrange(n, num_poses))),
]
####################
constants = [
CONF(initial_config) # Any additional objects
]
initial_atoms = [
AtConf(initial_config),
HandEmpty(),
] + [
AtPose(block, pose) for block, pose in initial_poses.items()
] + [
IsPose(block, pose) for block, pose in (initial_poses.items() + goal_poses.items())
]
goal_literals = [AtPose(block, pose) for block, pose in goal_poses.iteritems()]
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
return problem
def create_problem():
STATE = Type()
POSITION = Type()
ACCELERATION = Type()
TIME = Type()
SATELLITE = Type()
#FORCE = Type()
#MASS = Type('mass')
#ROCKET = Type('rocket')
######
#AtState = Predicate(POSITION, VELOCITY)
AtState = Pred(STATE)
Above = Pred(POSITION)
AtOrbit = Pred(SATELLITE, POSITION)
Carrying = Pred(SATELLITE)
Flying = Pred()
Landed = Pred()
######
IsBurst = Pred(STATE, ACCELERATION, TIME, STATE)
IsGlide = Pred(STATE, TIME, STATE)
#IsCrashed = Pred(POSITION)
IsAbove = Pred(STATE, POSITION)
ArePair = Pred(STATE, POSITION)
######
X1, X2 = Param(STATE), Param(STATE)
A, T = Param(ACCELERATION), Param(TIME)
S = Param(SATELLITE)
H = Param(POSITION)
rename_easy(locals()) # Trick to make debugging easier
######
actions = [
Action(name='burst',
parameters=[X1, A, T, X2],
condition=And(AtState(X1), IsBurst(X1, A, T, X2), Flying()),
effect=And(AtState(X2), Not(AtState(X1)))),
Action(name='glide',
parameters=[X1, A, T, X2],
condition=And(AtState(X1), IsGlide(X1, T, X2), Flying()),
effect=And(AtState(X2), Not(AtState(X1)))),
Action(name='deploy',
parameters=[S, X1, H],
condition=And(Carrying(S), AtState(X1), ArePair(X1, H)),
effect=And(AtOrbit(S, H), Not(Carrying(S)))),
Action(name='take_off',
parameters=[],
condition=And(Landed(), AtState((0, 0))),
effect=And(Flying(), Not(Landed()))),
Action(name='land',
parameters=[],
condition=And(Flying(), AtState((0, 0))),
effect=And(Landed(), Not(Flying()))),
]
axioms = [
Axiom(effect=Above(H),
condition=Exists([X1], And(AtState(X1), IsAbove(X1, H)))),
]
cond_streams = [
GeneratorStream(inputs=[X1],
outputs=[A, T, X2],
conditions=[],
effects=[IsBurst(X1, A, T, X2)],
generator=forward_burst),
#GeneratorStream(inputs=[X1, A], outputs=[T, X2], conditions=[], effects=[IsBurst(X1, A, T, X2)],
# generator=fixed_burst),
GeneratorStream(inputs=[X1, X2],
outputs=[A, T],
conditions=[],
effects=[IsBurst(X1, A, T, X2)],
generator=target_burst),
GeneratorStream(inputs=[X1],
outputs=[T, X2],
conditions=[],
effects=[IsGlide(X1, T, X2)],
generator=forward_glide),
TestStream(inputs=[X1, H],
conditions=[],
effects=[IsAbove(X1, H)],
test=lambda (p, v), h: p >= h,
eager=True),
]
def create_problem():
"""
Creates a blocksworld STRIPStream problem.
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
BLOCK = Type()
# Fluent predicates
Clear = Pred(BLOCK)
OnTable = Pred(BLOCK)
ArmEmpty = Pred()
Holding = Pred(BLOCK)
On = Pred(BLOCK, BLOCK)
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
rename_easy(locals())
actions = [
Action(name='pickup',
parameters=[B1],
condition=And(Clear(B1), OnTable(B1), ArmEmpty()),
effect=And(Holding(B1), Not(Clear(B1)), Not(OnTable(B1)),
Not(ArmEmpty()))),
Action(name='putdown',
parameters=[B1],
condition=Holding(B1),
effect=And(Clear(B1), OnTable(B1), ArmEmpty(),
Not(Holding(B1)))),
Action(name='stack',
parameters=[B1, B2],
condition=And(Clear(B2), Holding(B1)),
effect=And(Clear(B1), On(B1, B2), ArmEmpty(), Not(Clear(B2)),
Not(Holding(B1)))),
Action(name='unstack',
parameters=[B1, B2],
condition=And(Clear(B1), On(B1, B2), ArmEmpty()),
effect=And(Clear(B2), Holding(B1), Not(Clear(B1)),
Not(On(B1, B2)), Not(ArmEmpty()))),
]
axioms = []
cond_streams = []
constants = []
initial_atoms = [
OnTable('A'),
OnTable('B'),
OnTable('C'),
Clear('A'),
Clear('B'),
Clear('C'),
ArmEmpty(),
]
goal_literals = [On('A', 'B'), On('B', 'C')]
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot = env.GetRobots()[0]
set_base_values(robot, (-.75, .2, -math.pi/2))
initialize_openrave(env, ARM)
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()}
assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry
all_bodies = bodies.values()
body1 = all_bodies[-1]
body2 = all_bodies[-2] if len(bodies) >= 2 else body1
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1]
poses = problem.known_poses if problem.known_poses else []
open_gripper2(manipulator)
initial_manip = get_arm_conf(manipulator, Config(get_full_config(robot)))
def enable_all(enable):
for body in all_bodies:
body.Enable(enable)
####################
def cfree_pose_pose(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def cfree_gtraj_pose(gt, p):
return cfree_mtraj_pose(gt, p) and cfree_mtraj_grasp_pose(gt, gt.grasp, p)
####################
def cfree_mtraj_grasp(mt, g):
enable_all(False)
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
print 'cfree_mtraj_grasp'
return False
return True
def cfree_mtraj_pose(mt, p):
enable_all(False)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
if env.CheckCollision(robot, body2):
print 'cfree_mtraj_pose'
return False
return True
def cfree_mtraj_grasp_pose(mt, g, p):
enable_all(False)
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1, body2):
print 'cfree_mtraj_grasp_pose'
return False
return True
####################
def sample_grasp_traj(pose, grasp):
enable_all(False)
body1.Enable(True)
set_pose(body1, pose.value)
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_trans)
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
robot.Grab(body1)
grasp_traj = workspace_traj_helper(base_manip, approach_vector)
robot.Release(body1)
if grasp_traj is None: return
grasp_traj.grasp = grasp
yield [(Config(grasp_traj.end()), grasp_traj)]
def sample_manip_motion(mq1, mq2):
enable_all(False)
set_manipulator_values(manipulator, mq1.value)
mt = motion_plan(env, cspace, mq2.value, self_collisions=True)
if not mt: return
yield [(mt,)]
####################
cond_streams = [
# Pick/place trajectory
EasyListGenStream(inputs=[P, G], outputs=[MQ, GT], conditions=[],
effects=[GraspMotion(P, G, MQ, GT)], generator=sample_grasp_traj),
# Move trajectory
EasyListGenStream(inputs=[MQ, MQ2], outputs=[MT], conditions=[],
effects=[ManipMotion(MQ, MQ2, MT)], generator=sample_manip_motion, order=1, max_level=0),
# Pick/place collisions
EasyTestStream(inputs=[P, P2], conditions=[], effects=[CFreePosePose(P, P2)],
test=cfree_pose_pose, eager=True),
EasyTestStream(inputs=[GT, P], conditions=[], effects=[CFreeGTrajPose(GT, P)],
test=cfree_gtraj_pose),
# Move collisions
EasyTestStream(inputs=[MT, P], conditions=[], effects=[CFreeMTrajPose(MT, P)],
test=cfree_mtraj_pose),
EasyTestStream(inputs=[MT, G], conditions=[], effects=[CFreeMTrajGrasp(MT, G)],
test=cfree_mtraj_grasp),
EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[CFreeMTrajGraspPose(MT, G, P)],
test=cfree_mtraj_grasp_pose),
]
####################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_atoms = [
ManipEq(initial_manip),
HandEmpty(),
] + [
PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()
]
goal_formula = And(ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, cond_streams, constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10)
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=False)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i+1, action, args
else:
print 'Failure'
####################
def step_path(traj):
#for j, conf in enumerate(traj.path()):
for j, conf in enumerate([traj.end()]):
set_manipulator_values(manipulator, conf)
raw_input('%s/%s) Step?'%(j, len(traj.path())))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
set_manipulator_values(manipulator, initial_manip.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move':
mq1, mq2, mt = args
step_path(mt)
elif action.name == 'pick':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Grab(bodies[o])
step_path(gt)
elif action.name == 'place':
o, p, g, mq, gt = args
step_path(gt.reverse())
robot.Release(bodies[o])
step_path(gt)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
O, P, G = Param(OBJ), Param(POSE), Param(GRASP)
O2, O3 = Param(OBJ), Param(OBJ)
P2 = Param(POSE)
MQ, MQ2 = Param(MCONF), Param(MCONF)
MT = Param(MTRAJ)
GT = Param(GTRAJ)
rename_easy(locals())
####################
actions = [
Action(name='pick', parameters=[O, P, G, MQ, GT],
condition=And(PoseEq(O, P), HandEmpty(), ManipEq(MQ), GraspMotion(P, G, MQ, GT),
ForAll([O2], Or(Equal(O, O2), SafeGTraj(O2, GT)))),
#ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeGTraj(O2, GT))))),
effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
Action(name='place', parameters=[O, P, G, MQ, GT],
condition=And(GraspEq(O, G), ManipEq(MQ), GraspMotion(P, G, MQ, GT),
ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeGTraj(O2, GT))))),
effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
Action(name='move', parameters=[MQ, MQ2, MT],
condition=And(ManipEq(MQ), ManipMotion(MQ, MQ2, MT), ForAll([O2], SafeMTraj(O2, MT))),
effect=And(ManipEq(MQ2), Not(ManipEq(MQ)))),
]
axioms = [
# Pick/Place collisions
def compile_problem(oracle):
problem = oracle.problem
for obj in problem.goal_poses:
if problem.goal_poses[obj] == 'initial':
problem.goal_poses[obj] = oracle.initial_poses[obj]
elif problem.goal_poses[
obj] in oracle.initial_poses: # Goal names other object (TODO - compile with initial)
problem.goal_poses[obj] = oracle.initial_poses[
problem.goal_poses[obj]]
####################
O, P, G, Q, T = Param(OBJ), Param(POSE), Param(GRASP), Param(CONF), Param(
TRAJ)
Q1, Q2, OB, R = Param(CONF), Param(CONF), Param(OBJ), Param(REG)
#BT = Param(BASE_TRAJ)
rename_easy(locals())
actions = [
Action(
name='pick',
parameters=[O, P, G, Q, T],
condition=And(
PoseEq(O, P),
HandEmpty(),
Manip(O, P, G, Q, T),
ConfEq(Q), # NOTE - can remove ConfEq(Q)
ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
effect=And(PoseEq(O, None), GraspEq(O, G), Not(HandEmpty()),
Not(PoseEq(O, P)))),
Action(
name='place',
parameters=[O, P, G, Q, T],
condition=And(
PoseEq(O, None),
GraspEq(O, G),
Manip(O, P, G, Q, T),
ConfEq(Q), # NOTE - can remove ConfEq(Q)
ForAll([OB], Or(Equal(O, OB), Safe(OB, T)))),
effect=And(PoseEq(O, P), HandEmpty(), Not(PoseEq(O, None)),
Not(GraspEq(O, G)))),
Action('clean',
parameters=[O, R],
condition=And(InRegion(O, R), IsSink(R)),
effect=Cleaned(O)),
Action('cook',
parameters=[O, R],
condition=And(Cleaned(O), InRegion(O, R), IsStove(R)),
effect=And(Cooked(O), Not(Cleaned(O)))),
Action(name='move',
parameters=[Q1, Q2],
condition=ConfEq(Q1),
effect=And(ConfEq(Q2), Not(ConfEq(Q1)))),
]
axioms = [
#Axiom(effect=Holding(O), condition=Exists([G], And(GraspEq(O, G), LegalGrasp(O, G)))),
STRIPSAxiom(conditions=[GraspEq(O, G), LegalGrasp(O, G)],
effects=[Holding(O)]),
Axiom(effect=InRegion(O, R),
condition=Exists([P], And(PoseEq(O, P), Contained(R, O, P)))),
#STRIPSAxiom(conditions=[PoseEq(O, P), ContainedCon(R, O, P)], effects=[InRegion(O, R)]),
Axiom(effect=Safe(O, T),
condition=Exists([P], And(PoseEq(O, P), CFree(O, P, T)))),
#STRIPSAxiom(conditions=[PoseEq(O, P), CFreeCon(O, P, T)], effects=[Safe(O, T)]),
]
# TODO - include parameters in STRIPS axiom?
####################
def sample_poses(o, num_samples=1):
while True:
if AVOID_INITIAL:
oracle.set_all_object_poses(oracle.initial_poses)
else:
oracle.set_all_object_poses({o: oracle.initial_poses[o]})
yield list(
islice(
random_region_placements(oracle,
o,
oracle.get_counters(),
region_weights=True),
num_samples))
def sample_grasps(o):
yield get_grasps(oracle, o)
def sample_region(o, r, num_samples=1):
while True:
oracle.set_all_object_poses({o: oracle.initial_poses[o]})
yield list(
islice(
random_region_placements(oracle,
o, [r],
region_weights=True),
num_samples))
def sample_motion(o, p, g, max_calls=1, max_failures=50):
oracle.set_all_object_poses(
{o: p}) # TODO - saver for the initial state as well?
if oracle.approach_collision(o, p, g):
return
for i in range(max_calls):
pap = PickAndPlace(oracle.get_geom_hash(o), p, g)
if not pap.sample(oracle,
o,
max_failures=max_failures,
sample_vector=DO_ARM_MOTION,
sample_arm=DO_ARM_MOTION,
check_base=CHECK_BASE):
break
pap.obj = o
yield [(pap.approach_config, pap)]
def collision_free(o, p, t):
if p is None or o == t.obj:
return True
holding = ObjGrasp(t.obj, t.grasp)
#holding = Holding(self.oracle.get_body_name(pap.geom_hash), pap.grasp)
if not DO_ARM_MOTION:
return not oracle.holding_collision(t.grasp_config, o, p, holding)
return not oracle.traj_holding_collision(t.approach_config, t.trajs, o,
p, holding)
cond_streams = [
EasyListGenStream(inputs=[O],
outputs=[P],
conditions=[],
effects=[LegalPose(O, P)],
generator=sample_poses),
EasyListGenStream(inputs=[O],
outputs=[G],
conditions=[],
effects=[LegalGrasp(O, G)],
generator=sample_grasps),
EasyListGenStream(inputs=[O, R],
outputs=[P],
conditions=[],
effects=[LegalPose(O, P),
Contained(R, O, P)],
generator=sample_region),
EasyListGenStream(inputs=[O, P, G],
outputs=[Q, T],
conditions=[LegalPose(O, P),
LegalGrasp(O, G)],
effects=[Manip(O, P, G, Q, T)],
generator=sample_motion),
#MultiEasyGenStream(inputs=[Q1, Q2], outputs=[BT], conditions=[],
# effects=[Motion(Q1, BT, Q2)], generator=lambda q1, q2: [[None]]),
EasyTestStream(inputs=[O, P, T],
conditions=[LegalPose(O, P)],
effects=[CFree(O, P, T)],
test=collision_free,
eager=EAGER_TESTS),
]
####################
constants = [POSE(None)]
initial_atoms = [ConfEq(oracle.initial_config)] # TODO - toggle
holding = set()
if problem.start_holding is not False:
obj, grasp = problem.start_holding
initial_atoms += [
GraspEq(obj, grasp),
PoseEq(obj, None),
LegalGrasp(obj, grasp)
]
holding.add(obj)
if not holding:
initial_atoms.append(HandEmpty())
for obj, pose in oracle.initial_poses.iteritems():
if obj not in holding:
initial_atoms += [PoseEq(obj, pose), LegalPose(obj, pose)]
initial_atoms += [IsSink(region) for region in oracle.sinks]
initial_atoms += [IsStove(region) for region in oracle.stoves]
goal_literals = []
if problem.goal_holding is not None:
if problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif isinstance(problem.goal_holding, ObjGrasp):
goal_literals.append(
GraspEq(problem.goal_holding.object_name,
problem.goal_holding.grasp))
elif problem.goal_holding in oracle.get_objects():
goal_literals.append(Holding(problem.goal_holding))
else:
raise Exception()
for obj, pose in problem.goal_poses.iteritems():
goal_literals.append(PoseEq(obj, pose))
initial_atoms.append(LegalPose(obj, pose))
for obj, region in problem.goal_regions.iteritems():
goal_literals.append(InRegion(obj, region))
for obj in problem.goal_cleaned:
goal_literals.append(Cleaned(obj))
for obj in problem.goal_cooked:
goal_literals.append(Cooked(obj))
goal_formula = goal_literals
#goal_formula = And(*goal_literals)
#goal_formula = AbsCondition(goal_literals) # TODO - bug where goals must have And
return STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms,
cond_streams, constants)
def add_effects(self, *new_effects):
self.effect = And(self.effect, *new_effects)
def get_actions(operators, static_map, objects):
actions = []
for action, conditions, effects in get_dnf_operators(operators, objects):
actions.append(Action(action.name, action.parameters, And(*conditions), And(*effects)))
return get_operators(actions, static_map, objects, PyAction)
def create_real_problem(state, goal):
tables, initial_poses, initial_config, holding = state
# TODO: surface parameter here?
# TODO: what happens if I drop preconditions here and things
# TODO: add on here as well? Drop all preconditions mentioning continuous params
# TODO: can then drop all preconditions and effects associated
# Well I won't have these in the effects
actions = [
#Action(name='pick', parameters=[B, P, G, Q],
# condition=And(AtPose(B, P), HandEmpty(), AtConf(Q),
# IsPose(B, P), IsKin(P, G, Q)),
# effect=And(HasGrasp(B, G), Not(AtPose(B, P)), Not(HandEmpty()))),
#Action(name='place', parameters=[B, P, G, Q],
# condition=And(HasGrasp(B, G), AtConf(Q),
# IsPose(B, P), IsKin(P, G, Q)),
# #ForAll([B2], Or(Equal(B, B2), Safe(B2, B, P)))),
# effect=And(AtPose(B, P), HandEmpty(), Not(HasGrasp(B, G)))),
Action(name='pick',
parameters=[B, S, P, G, Q],
condition=And(AtPose(B, P), HandEmpty(), AtConf(Q),
IsPose(B, P), IsKin(P, G, Q), IsSupported(P, S)),
effect=And(HasGrasp(B, G), Holding(B), Not(On(B, S)),
Not(AtPose(B, P)), Not(HandEmpty()))),
Action(
name='place',
parameters=[B, S, P, G, Q],
condition=And(HasGrasp(B, G), AtConf(Q), IsPose(B, P),
IsKin(P, G, Q), IsSupported(P, S)),
# ForAll([B2], Or(Equal(B, B2), Safe(B2, B, P)))),
effect=And(AtPose(B, P), Not(Holding(B)), HandEmpty(), On(B, S),
Not(HasGrasp(B, G)))),
Action(name='move',
parameters=[Q, Q2],
condition=AtConf(Q),
effect=And(AtConf(Q2), Not(AtConf(Q)))),
# TODO: can do a similar thing with is near here
# TODO: should also probably have two costs so one can be dropped
]
# TODO: can relax preconditions as normal to make a strictly easier problem
# If you remove all preconditions, can also remove effects that aren't needed and then parameters
axioms = [
#Axiom(effect=Safe(B2, B, P),
# condition=Exists([P2], And(AtPose(B2, P2), CFree(B, P, B2, P2)))),
#Axiom(effect=On(B, S),
# condition=Exists([P], And(IsPose(B, P), AtPose(B, P), IsSupported(P, S)))),
#Axiom(effect=Holding(B),
# condition=Exists([G], And(IsGrasp(B, G), HasGrasp(B, G)))),
# TODO: one for the robot near a surface as well
]
# TODO: axioms make it less clear what is going on for hierarchy
####################
# Conditional stream declarations
cond_streams = [
GeneratorStream(
inputs=[B, S],
outputs=[P],
conditions=[],
effects=[IsPose(B, P), IsSupported(P, S)],
#generator=lambda b, s: ((Pose(b, randint(*tables[s])),) for _ in xrange(10))),
generator=lambda b, s:
((Pose(b, s, x), ) for x in xrange(*tables[s]))),
GeneratorStream(inputs=[B, P, G],
outputs=[Q],
conditions=[IsPose(B, P), IsGrasp(B, G)],
effects=[IsKin(P, G, Q)],
generator=lambda _, p, g: [(Conf(p.x, g.y), )]),
ListStream(inputs=[B],
outputs=[G],
conditions=[],
effects=[IsGrasp(B, G)],
function=lambda b: [(Grasp(b, v), ) for v in [-1, +1]]),
#TestStream(inputs=[B, P, B2, P2], conditions=[], effects=[CFree(B, P, B2, P2)],
# test=lambda b1, p1, b2, p2: p1 != p2, eager=True), # Collision checking
]
####################
# TODO: need to add surfaces as constants
initial_atoms = [
AtConf(initial_config),
] + [AtPose(block, pose) for block, pose in initial_poses.items()] + [
IsPose(block, pose) for block, pose in initial_poses.items()
] + [IsSupported(pose, pose.sur) for pose in initial_poses.values()
] + [On(block, pose.sur) for block, pose in initial_poses.items()]
if state.holding is None:
initial_atoms.append(HandEmpty())
else:
block, grasp = state.holding
initial_atoms += [HasGrasp(block, grasp), Holding(block)]
goal_literals = [On(b, s) for b, s in goal.on.items()]
if goal.holding is not None:
goal_literals.append(Holding(goal.holding))
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams)
O, P, G = Param(OBJ), Param(POSE), Param(GRASP)
O2, P2 = Param(OBJ), Param(POSE)
Q, Q2 = Param(CONF), Param(CONF)
T = Param(TRAJ)
rename_easy(locals())
####################
actions = [
Action(
name='pick',
parameters=[O, P, G, Q, T],
condition=And(PoseEq(O, P), HandEmpty(), ConfEq(Q),
GraspMotion(P, G, Q, T),
ForAll([O2], Or(Equal(O, O2), SafeTraj(O2, T)))),
#ForAll([O2], Or(Equal(O, O2), And(SafePose(O2, P), SafeGTraj(O2, GT))))),
effect=And(GraspEq(O, G), Not(HandEmpty()), Not(PoseEq(O, P)))),
Action(name='place',
parameters=[O, P, G, Q, T],
condition=And(
GraspEq(O, G), ConfEq(Q), GraspMotion(P, G, Q, T),
ForAll([O2],
Or(Equal(O, O2), And(SafePose(O2, P), SafeTraj(O2,
T))))),
effect=And(PoseEq(O, P), HandEmpty(), Not(GraspEq(O, G)))),
Action(name='move',
parameters=[Q, Q2, T],
condition=And(ConfEq(Q), HandEmpty(), FreeMotion(Q, Q2, T),
ForAll([O2], SafeTraj(O2, T))),
def create_problem2():
"""
Creates the 1D task and motion planning STRIPStream problem.
:return: a :class:`.STRIPStreamProblem`
"""
# How would I specify table position
# From goal specification can derive prior
# Everything of same object type should be one variable? Otherwise, how would I update?
# I do actually have limits on the number of things
# Doing with would relive the strangeness when you have to update the others
# The strange thing is that we would like to distinguish the clusters in space when we do find them
p_table = 0.9
p_hit_exists = 0.99
p_miss_notexists = p_hit_exists
# Could use count based things or could just indicate confidence in sensor model
# Goal, object in hand
# Object starts out with high probability that its on a surface
surfaces = ['table%i'%i for i in range(3)]
# Different predicates for course belief and fine belief?
# Do I want to expose blocks as objects to belief?
# The probability that another table exists drops immensely once we find 3
# I think I always have to fix this number
# I suppose I could make a stream that generates new objects if desired
# Decrease the likelihood of later numbered objects
# Maybe I just use one table and allow it not to integrate to one?
# Why does the online deferral to use objects in the focused algorithm work?
# We often have streams for continuous values and these are the ones we want to defer
# Could I do this for discrete objects as well?
# Sure, just make a stream to generate them
# This is all about hte optimistic, I think there is a pose but I don't actually know it stuff
# Should the imaginary pose be explicit then?
# Maybe I should find a true plan but allow some objects to be imaginary
# Simultaneous actions to look and observe multiple things
blocks = ['block%i'%i for i in range(3)]
num_poses = pow(10, 10)
initial_config = 0 # the initial robot configuration is 0
initial_poses = {block: i for i, block in enumerate(blocks)} # the initial pose for block i is i
goal_poses = {block: i+1 for i, block in enumerate(blocks)} # the goal pose for block i is i+1
####################
# Data types
CONF, BLOCK, POSE = Type(), Type(), Type()
ROOM = Type()
# Fluent predicates
AtConf = Pred(CONF)
AtPose = Pred(BLOCK, POSE)
HandEmpty = Pred()
Holding = Pred(BLOCK)
# Derived predicates
Safe = Pred(BLOCK, BLOCK, POSE)
# Static predicates
LegalKin = Pred(POSE, CONF)
CollisionFree = Pred(BLOCK, POSE, BLOCK, POSE)
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='pick', parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1), LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(name='place', parameters=[B1, P1, Q1],
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1, P1)))), # TODO - convert to finite blocks case?
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move', parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
Action(name='scan', parameters=[Q1], # Looks at a particular object. Discount costs for subsequent looks from that spot
condition=AtConf(Q1),
effect=And()),
Action(name='look', parameters=[Q1, O], # Look at surface vs object
condition=AtConf(Q1),
effect=And()),
]
axioms = [
Axiom(effect=Safe(B2, B1, P1),
condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))), # Infers B2 is at a safe pose wrt B1 at P1
]
####################
# Conditional stream declarations
cond_streams = [
GeneratorStream(inputs=[], outputs=[P1], conditions=[], effects=[],
generator=lambda: ((p,) for p in xrange(num_poses))), # Enumerating all the poses
GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)],
generator=lambda p: [(p,)]), # Inverse kinematics
TestStream(inputs=[B1, P1, B2, P2], conditions=[], effects=[CollisionFree(B1, P1, B2, P2)],
test=lambda b1, p1, b2, p2: p1 != p2, eager=True), # Collision checking
]
####################
constants = [
CONF(initial_config) # Any additional objects
]
initial_atoms = [
AtConf(initial_config),
HandEmpty()
] + [
AtPose(block, pose) for block, pose in initial_poses.iteritems()
]
goal_literals = [AtPose(block, pose) for block, pose in goal_poses.iteritems()]
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
return problem
def create_problem(n=50):
"""
Creates the 1D task and motion planning STRIPStream problem.
:return: a :class:`.STRIPStreamProblem`
"""
blocks = ['block%i' % i for i in xrange(n)]
num_poses = pow(10, 10)
initial_config = 0 # the initial robot configuration is 0
initial_poses = {block: i
for i, block in enumerate(blocks)
} # the initial pose for block i is i
goal_poses = {blocks[1]: 2} # the goal pose for block i is i+1
####################
# TODO: the last version of this would be to make a separate pose type per object (I think I did do this)
CONF = Type()
HandEmpty = Pred()
AtConf = Pred(CONF)
Q1, Q2 = Param(CONF), Param(CONF)
#POSE = Type()
#Kin = Pred(POSE, CONF)
#Collision = Pred(POSE, POSE)
#P1, P2 = Param(POSE), Param(POSE)
#rename_easy(locals()) # Trick to make debugging easier
actions = [
Action(name='move',
parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = []
cond_streams = [
#GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[Kin(P1, Q1)],
# generator=lambda p: [(p,)]), # Inverse kinematics
#TestStream(inputs=[P1, P2], conditions=[], effects=[Collision(P1, P2)],
# test=lambda p1, p2: p1 == p2, eager=True),
]
initial_atoms = [
AtConf(initial_config),
HandEmpty(),
]
goal_literals = []
####################
# TODO: I think thinking invariants gets harder with many predicates. Can cap this time I believe though
#153 initial candidates
#Finding invariants: [2.250s CPU, 2.263s wall - clock]
for b in blocks:
# TODO: can toggle using individual pose types
POSE = Type()
Kin = Pred(POSE, CONF)
P1 = Param(POSE)
AtPose = Pred(POSE)
IsPose = Pred(POSE)
Holding = Pred()
#Unsafe = Pred(BLOCK, POSE)
initial_atoms += [
AtPose(initial_poses[b]),
IsPose(initial_poses[b]),
]
if b in goal_poses:
goal_literals += [AtPose(goal_poses[b])]
initial_atoms += [IsPose(goal_poses[b])]
axioms += [
# TODO: to do this, need to make b a parameter and set it using inequality
#Axiom(effect=Unsafe(b, P1),
# condition=Exists([P2], And(AtPose(b, P2), IsPose(b, P2), Collision(P1, P2)))),
]
actions += [
Action(name='pick-{}'.format(b),
parameters=[P1, Q1],
condition=And(AtPose(P1), HandEmpty(), AtConf(Q1),
IsPose(P1), Kin(P1, Q1)),
effect=And(Holding(), Not(AtPose(P1)), Not(HandEmpty()))),
Action(
name='place-{}'.format(b),
parameters=[P1, Q1],
condition=And(Holding(), AtConf(Q1), IsPose(P1), Kin(P1, Q1)),
#*[Safe(b2, P1) for b2 in blocks if b2 != b]),
#*[Not(Unsafe(b2, P1)) for b2 in blocks if b2 != b]),
effect=And(AtPose(P1), HandEmpty(), Not(Holding()))),
]
cond_streams += [
GeneratorStream(inputs=[],
outputs=[P1],
conditions=[],
effects=[IsPose(P1)],
generator=lambda:
((p, ) for p in xrange(n, num_poses))),
GeneratorStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[Kin(P1, Q1)],
generator=lambda p: [(p, )]), # Inverse kinematics
]
problem = STRIPStreamProblem(initial_atoms, goal_literals,
actions + axioms, cond_streams, [])
return problem
def compile_observable_problem(world, task):
# Data types
CONF = Type()
TABLE = Type() # Difference between fixed and movable objects
BLOCK = Type()
POSE = Type()
# Fluent predicates
AtConf = Pred(CONF)
HandEmpty = Pred()
Holding = Pred(BLOCK)
AtPose = Pred(BLOCK, POSE)
# Static predicates
LegalKin = Pred(POSE, CONF)
# Free parameters
Q1, Q2 = Param(CONF), Param(CONF)
T = Param(TABLE)
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
rename_easy(locals()) # Trick to make debugging easier
actions = [
Action(name='pick', parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1), LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
# Action(name='place', parameters=[B1, P1, Q1], # Localize table?
# condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1)),
# effect=And(AtPoseB(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move', parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = [
# Axiom(effect=InRoom(R),
# condition=Exists([Q1], And(AtConf(Q1), ConfIn(Q1, R)))), # Infers B2 is at a safe pose wrt B1 at P1
]
def inverse_kinematics(pose): # TODO: list stream that uses ending info
yield (pose,)
#def sample_table(table):
# yield (pose,)
streams = [
GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)],
generator=inverse_kinematics),
]
initial_atoms = [AtConf(world.robot_conf)]
if world.holding is None:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(world.holding))
for obj, pose in world.object_poses:
initial_atoms.append(AtPose(obj, pose))
goal_literals = []
if task.robot_conf is not False:
goal_literals.append(AtConf(task.robot_conf))
if task.holding is None:
goal_literals.append(HandEmpty())
elif task.holding:
goal_literals.append(Holding(task.holding))
#for obj, pose in task.object_poses.iteritems():
# goal_literals.append(AtPoseB(obj, pose))
goal_formula = And(*goal_literals)
problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms, streams, [])
return problem
def solve_tamp(env):
viewer = env.GetViewer() is not None
#problem = dantam(env)
problem = dantam2(env)
#problem = move_several_4(env)
robot = env.GetRobots()[0]
set_base_values(robot, (-.75, .2, -math.pi/2))
initialize_openrave(env, 'leftarm')
manipulator = robot.GetActiveManipulator()
cspace = CSpace.robot_arm(manipulator)
base_manip = interfaces.BaseManipulation(robot, plannername=None, maxvelmult=None)
#USE_GRASP_APPROACH = GRASP_APPROACHES.SIDE
USE_GRASP_APPROACH = GRASP_APPROACHES.TOP
#USE_GRASP_TYPE = GRASP_TYPES.TOUCH
USE_GRASP_TYPE = GRASP_TYPES.GRASP
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
geom_hashes = {body.GetKinematicsGeometryHash() for body in bodies.values()}
assert len(geom_hashes) == 1 # NOTE - assuming all objects has the same geometry
all_bodies = bodies.values()
body1 = all_bodies[-1]
body2 = all_bodies[-2] if len(bodies) >= 2 else body1
grasps = get_grasps(env, robot, body1, USE_GRASP_APPROACH, USE_GRASP_TYPE)[:1]
poses = problem.known_poses if problem.known_poses else []
##################################################
def enable_all(enable):
for body in all_bodies:
body.Enable(enable)
def collision_free(pose1, pose2):
body1.Enable(True)
set_pose(body1, pose1.value)
body2.Enable(True)
set_pose(body2, pose2.value)
return not env.CheckCollision(body1, body2)
def grasp_env_cfree(mt, g):
enable_all(False) # TODO - base config?
body1.Enable(True)
for conf in mt.path():
set_manipulator_values(manipulator, conf) # NOTE - can also grab
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1):
return False
return True
def grasp_pose_cfree(mt, g, p):
enable_all(False) # TODO - base config?
body1.Enable(True)
body2.Enable(True)
set_pose(body2, p.value)
for conf in mt.path():
set_manipulator_values(manipulator, conf)
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), g.grasp_trans))
if env.CheckCollision(body1, body2):
return False
return True
##################################################
"""
class CollisionStream(Stream): # TODO - could make an initial state version of this that doesn't need pose2
def get_values(self, **kwargs):
self.enumerated = True
pose1, pose2 = map(get_value, self.inputs)
if collision_free(pose1, pose2):
return [PoseCFree(pose1, pose2)]
return []
#return [Movable()] # NOTE - only make movable when it fails a collision check
CheckedInitial = Pred(POSE) # How should this be handled? The planner will need to revisit on the next state anyways
class EnvCollisionStream(Stream): # NOTE - I could also make an environment OBJECT which I mutate. I'm kind of doing that now
movable = []
def get_values(self, **kwargs):
self.enumerated = True
pose, = map(get_value, self.inputs)
results = [CheckedInitial(pose)]
for obj, pose2 in problem.initial_poses.iteritems():
if obj not in self.movable and collision_free(pose, pose2):
pass
#results.append(PoseCFree(pose, pose2))
else:
self.movable.append(obj)
results.append(PoseEq(obj, pose2))
#results.append(Movable(obj))
if results:
pass # NOTE - I could make this fail if there is a collision
# I could prevent the binding by directly adding CheckedInitial to the universe
# In general, I probably can just mutate the problem however I see fit here
return results
"""
##################################################
# NOTE - can do pose, approach manip, true approach traj, motion plan
# NOTE - can make something that produces approach trajectories
def get_manip_vector(pose, grasp):
manip_trans, approach_vector = manip_from_pose_grasp(pose, grasp)
#enable_all(False)
#if manipulator.CheckEndEffectorCollision(manip_trans):
# return None
return ManipVector(manip_trans, approach_vector)
def sample_ik(pose, grasp, base_conf): # TODO - make this return the grasp
enable_all(False)
set_base_values(robot, base_conf.value)
body1.Enable(True)
set_pose(body1, pose.value)
manip_vector = get_manip_vector(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions
#print manipulator.CheckEndEffectorCollision(manip_trans)
if grasp_config is not None:
yield [Config(grasp_config)]
#traj = workspace_traj_helper(base_manip, approach_vector)
def sample_grasp_traj(pose, grasp, base_conf):
enable_all(False)
set_base_values(robot, base_conf.value)
body1.Enable(True)
set_pose(body1, pose.value)
manip_vector = get_manip_vector(pose, grasp)
grasp_config = inverse_kinematics_helper(env, robot, manip_vector.manip_trans) # NOTE - maybe need to find all IK solutions
if grasp_config is None: return
set_manipulator_values(manipulator, grasp_config)
grasp_traj = workspace_traj_helper(base_manip, manip_vector.approach_vector)
if grasp_config is None: return
yield [(Config(grasp_traj.end()), grasp_traj)]
##################################################
# NOTE - can either include the held object in the traj or have a special condition that not colliding
def sample_arm_traj(mq1, mq2, bq): # TODO - need to add holding back in
yield None
#enable_all(False)
#with robot:
# set_base_values(robot, bq.value)
# pass
# TODO - does it make sense to make a new stream for the biasing or to continuously just pass things
# I suppose I could cache the state or full plan as context
class MotionStream(Stream): # TODO - maybe make this produce the correct values
num = 0
#def get_values(self, **kwargs):
# self.enumerated = True
# mq1, mq2, bq = map(get_value, self.inputs)
# #mt = None
# mt = MotionStream.num
# MotionStream.num += 1 # Ensures all are unique
# return [ManipMotion(mq1, mq2, bq, mt)]
def sample_motion_plan(self, mq1, mq2, bq):
set_manipulator_values(manipulator, mq1.value)
set_base_values(robot, bq.value)
return motion_plan(env, cspace, mq2.value, self_collisions=True)
def get_values(self, universe, dependent_atoms=set(), **kwargs):
mq1, mq2, bq = map(get_value, self.inputs)
collision_atoms = filter(lambda atom: atom.predicate in [MTrajGraspCFree, MTrajPoseCFree], dependent_atoms)
collision_params = {atom: atom.args[0] for atom in collision_atoms}
grasp = None
for atom in collision_atoms:
if atom.predicate is MTrajGraspCFree:
assert grasp is None # Can't have two grasps
_, grasp = map(get_value, atom.args)
placed = []
for atom in collision_atoms:
if atom.predicate is MTrajPoseCFree:
_, pose = map(get_value, atom.args)
placed.append(pose)
#placed, grasp = [], None
print grasp, placed
if placed or grasp:
assert len(placed) <= len(all_bodies) # How would I handle many constraints on the same traj?
enable_all(False)
for b, p in zip(all_bodies, placed):
b.Enable(True)
set_pose(b, p.value)
if grasp:
assert grasp is None or len(placed) <= len(all_bodies)-1
set_pose(body1, object_trans_from_manip_trans(get_trans(manipulator), grasp.grasp_trans))
robot.Grab(body1)
mt = self.sample_motion_plan(mq1, mq2, bq)
if grasp: robot.Release(body1)
if mt:
self.enumerated = True # NOTE - if satisfies all constraints then won't need another. Not true. What if called with different grasps...
# TODO - could always hash this trajectory for the current set of constraints
bound_collision_atoms = [atom.instantiate({collision_params[atom]: MTRAJ(mt)}) for atom in collision_atoms]
#bound_collision_atoms = []
return [ManipMotion(mq1, mq2, bq, mt)] + bound_collision_atoms
raise ValueError()
enable_all(False)
mt = self.sample_motion_plan(mq1, mq2, bq)
if mt:
return [ManipMotion(mq1, mq2, bq, mt)]
return []
##################################################
cond_streams = [
#MultiEasyGenStream(inputs=[O], outputs=[P], conditions=[], effects=[LegalPose(O, P)], generator=sample_poses),
#EasyGenStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[],
# effects=[ManipMotion(MQ, MQ2, BQ, MT)], generator=sample_arm_traj),
ClassStream(inputs=[MQ, MQ2, BQ], outputs=[MT], conditions=[],
effects=[ManipMotion(MQ, MQ2, BQ, MT)], StreamClass=MotionStream, order=1, max_level=0),
#MultiEasyGenStream(inputs=[P, G, BQ], outputs=[MQ], conditions=[],
# effects=[Kin(P, G, BQ, MQ)], generator=sample_ik),
EasyListGenStream(inputs=[P, G, BQ], outputs=[MQ, GT], conditions=[],
effects=[GraspTraj(P, G, BQ, MQ, GT)], generator=sample_grasp_traj),
#EasyTestStream(inputs=[O, P, T], conditions=[], effects=[CFree(O, P, T)],
# test=collision_free, eager=True),
EasyTestStream(inputs=[P, P2], conditions=[], effects=[PoseCFree(P, P2)],
test=collision_free, eager=True),
#ClassStream(inputs=[P, P2], conditions=[], outputs=[],
# effects=[PoseCFree(P, P2)], StreamClass=CollisionStream, eager=True),
EasyTestStream(inputs=[MT, P], conditions=[], effects=[MTrajPoseCFree(MT, P)],
test=lambda mt, p: True, eager=True),
EasyTestStream(inputs=[MT, G], conditions=[], effects=[MTrajGraspCFree(MT, G)],
test=lambda mt, g: True, eager=True),
EasyTestStream(inputs=[MT, G, P], conditions=[], effects=[MTrajGraspPoseCFree(MT, G, P)],
test=lambda mt, g, p: True, eager=True),
#ClassStream(inputs=[P], conditions=[], outputs=[],
# effects=[CheckedInitial(P)], StreamClass=EnvCollisionStream),
]
##################################################
constants = map(GRASP, grasps) + map(POSE, poses)
initial_full = Config(get_full_config(robot))
initial_base = get_base_conf(initial_full)
initial_manip = get_arm_conf(manipulator, initial_full)
initial_atoms = [
BaseEq(initial_base),
ManipEq(initial_manip),
HandEmpty(),
] + [
PoseEq(obj, pose) for obj, pose in problem.initial_poses.iteritems()
]
goal_formula = And(ManipEq(initial_manip), *(PoseEq(obj, pose) for obj, pose in problem.goal_poses.iteritems()))
stream_problem = STRIPStreamProblem(initial_atoms, goal_formula, operators, cond_streams, constants)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager')
#plan, universe = incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, _ = focused_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False) # 1 | 20 | 100 | INF
#plan, universe = simple_focused(stream_problem, search=search_fn, max_level=INF, debug=False, verbose=False) # 1 | 20 | 100 | INF
#plan, _ = plan_focused(stream_problem, search=search_fn, debug=False) # 1 | 20 | 100 | INF
from misc.profiling import run_profile, str_profile
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=INF, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem, search=search_fn, max_level=INF, shared=False, debug=False, verbose=True)
#with env:
env.Lock()
(plan, universe), prof = run_profile(solve)
env.Unlock()
print SEPARATOR
universe.print_domain_statistics()
universe.print_statistics()
print SEPARATOR
print str_profile(prof)
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i+1, action, args
else:
print 'Failure'
##################################################
def step_path(traj):
#for j, conf in enumerate(traj.path()):
for j, conf in enumerate([traj.end()]):
set_manipulator_values(manipulator, conf)
raw_input('%s/%s) Step?'%(j, len(traj.path())))
if viewer and plan is not None:
print SEPARATOR
# Resets the initial state
open_gripper2(manipulator)
set_base_values(robot, initial_base.value)
set_manipulator_values(manipulator, initial_manip.value)
for obj, pose in problem.initial_poses.iteritems():
set_pose(bodies[obj], pose.value)
for i, (action, args) in enumerate(plan):
raw_input('\n%s/%s) Next?'%(i, len(plan)))
if action.name == 'move_arm':
mq1, mq2, bq, mt = args
#set_manipulator_values(manipulator, mq2.value)
step_path(mt)
elif action.name == 'pick':
#o, p, q, mq, bq = args
o, p, g, mq, bq, gt = args
step_path(gt.reverse())
#grasp_gripper2(manipulator, g) # NOTE - g currently isn't a real grasp
robot.Grab(bodies[o])
step_path(gt)
elif action.name == 'place':
#o, p, q, mq, bq = args
o, p, g, mq, bq, gt = args
step_path(gt.reverse())
robot.Release(bodies[o])
#open_gripper2(manipulator)
step_path(gt)
else:
raise ValueError(action.name)
env.UpdatePublishedBodies()
raw_input('Finish?')
def create_problem():
table = 'table'
room = 'room'
block = 'block'
table_pose = None
block_pose = None
if table_pose is None:
table_belief = frozenset({(room, 0.5), (None, 0.5)}) # Discrete distribution over poses
else:
table_belief = frozenset({(table_pose, 1.0)}) # Gaussian
if block_pose is None:
block_belief = frozenset({(table, 0.5), (None, 0.5)}) # Particle filter
else:
block_belief = frozenset({(table_pose, 1.0)}) # Gaussian
# I definitely am implicitly using belief conditions by asserting we will know the resultant pose
# Tables and Objects have three beliefs
# 1) Unknown
# 2) Coarse
# 3) Fine (with respect to base pose). Or could just add LowVariance condition when true
# Data types
CONF = Type()
ROOM = Type()
TABLE = Type() # Difference between fixed and movable objects
BLOCK = Type()
POSE = Type()
# Fluent predicates
AtConf = Pred(CONF)
HandEmpty = Pred()
Holding = Pred(BLOCK)
# Static predicates
LegalKin = Pred(POSE, CONF)
# Know that each block is at one pose at once (but don't know which one). Well
# Tables can be at only one pose. Only need to have argument for whether localized
UncertainT = Pred(TABLE)
UncertainB = Pred(BLOCK) # Has an internal distribution in it
AtPoseT = Pred(TABLE) # Has a fixed pose / convex hull in it
AtPoseB = Pred(BLOCK, POSE)
LocalizedT = Pred(TABLE)
LocalizedB = Pred(BLOCK)
#Scanned = Pred(ROOM)
#IsReal = Pred(POSE) # Could also specify all the fake values upfront
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
R, T = Param(ROOM), Param(TABLE)
rename_easy(locals()) # Trick to make debugging easier
actions = [
Action(name='pick', parameters=[B1, P1, Q1],
condition=And(AtPoseB(B1, P1), LocalizedB(B1), HandEmpty(), AtConf(Q1), LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPoseB(B1, P1)), Not(HandEmpty()), Not(LocalizedB(B1)))),
Action(name='place', parameters=[B1, P1, Q1], # Localize table?
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1)),
effect=And(AtPoseB(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move_base', parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)), ForAll([B1], Not(LocalizedB(B1))))), # Set all known poses to be high uncertainty
#Action(name='scan', parameters=[R, T],
# condition=And(InRoom(R), AtConf(Q1)), # Should have a trajectory really
# condition=And(Believe(T), Not(Scanned(R))), # Scan from anywhere in the room
# effect=And(T)),
Action(name='move_head', parameters=[Q1, Q2], # Head conf, base conf, manip conf?
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))), # Should I undo localized if I move the head at all?
Action(name='scan_room', parameters=[T],
condition=UncertainT(T),
effect=And(AtPoseT(T), Not(UncertainT(T)))),
Action(name='scan_table', parameters=[T, B1, P1, Q1],
condition=And(AtPoseT(T), AtConf(Q1)),
effect=And(AtPoseB(B1, P1), Not(UncertainB(B1)))),
Action(name='look_table', parameters=[T, Q1],
condition=And(AtPoseT(T), AtConf(Q1)),
effect=LocalizedT(T)),
Action(name='look_block', parameters=[B1, P1, Q1],
condition=And(AtPoseB(B1, P1), AtConf(Q1)), # Visibility constraint
effect=LocalizedB(B1)),
#Action(name='stop', parameters=[T, Q1],
# condition=And(AtPoseT(T), AtConf(Q1)),
# effect=LocalizedT(T)),
]
axioms = [
#Axiom(effect=InRoom(R),
# condition=Exists([Q1], And(AtConf(Q1), ConfIn(Q1, R)))), # Infers B2 is at a safe pose wrt B1 at P1
]
# TODO: partially observable version of this
def inverse_kinematics(pose): # TODO: list stream that uses ending info
if type(pose) == str:
yield (pose + '_conf',) # Represents a hypothetical
yield (pose,)
def sample_table(table):
if not localized:
yield # Stuff
yield (pose,)
streams = [
GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[LegalKin(P1, Q1)],
generator=inverse_kinematics),
]
constants = [
POSE('pose'), # Strings denote fake values
]
initial_atoms = [
AtConf(1),
HandEmpty(),
UncertainT(table),
UncertainB(block),
]
goal_literals = [Holding(block)]
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, streams, constants)
return problem
def compile_problem(tamp_problem):
"""
Constructs a STRIPStream problem for the countable TMP problem.
:param tamp_problem: a :class:`.TMPProblem`
:return: a :class:`.STRIPStreamProblem`
"""
# NOTE - the simple focused algorithm gives "Could not find instantiation for PNE!" when this is moved outside
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
actions = [
Action(name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1),
LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(
name='place',
parameters=[B1, P1, Q1],
condition=And(
Holding(B1),
AtConf(Q1),
LegalKin(P1, Q1),
#ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1, P1)))),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))),
#*[Or(Equal(B1, BLOCK(b2)), Safe(b2, P1)) for b2 in tamp_problem.initial_poses]),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = [
#Axiom(effect=Safe(B2, B1, P1), condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))),
Axiom(effect=Safe(B2, P1),
condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(P1,
P2)))),
]
# NOTE - this needs to be inside the method so you make new streams each time
cond_streams = [
#EasyGenStream(inputs=[P2], outputs=[P1], conditions=[], effects=[],
# generator=lambda a: irange(0, NUM_POSES)),
EasyGenStream(inputs=[],
outputs=[P1],
conditions=[],
effects=[],
generator=lambda: irange(0, NUM_POSES)),
EasyGenStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[LegalKin(P1, Q1)],
generator=lambda p: iter([p])),
#EasyTestStream(inputs=[B1, P1, B2, P2], conditions=[], effects=[CollisionFree(B1, P1, B2, P2)],
# test=lambda b1, p1, b2, p2: p1 != p2, eager=EAGER_TESTS),
EasyTestStream(inputs=[P1, P2],
conditions=[],
effects=[CollisionFree(P1, P2)],
test=lambda p1, p2: p1 != p2,
eager=EAGER_TESTS),
]
constants = []
initial_atoms = [
AtConf(tamp_problem.initial_config),
] + [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
]
if tamp_problem.initial_holding is False:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(tamp_problem.initial_holding, BLOCK))
goal_literals = []
if tamp_problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif tamp_problem.goal_holding is not None:
goal_literals.append(Holding(tamp_problem.goal_holding))
for block, goal in tamp_problem.goal_poses.iteritems():
goal_literals.append(AtPose(block, goal))
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
def create_problem():
"""
Creates the 1D task and motion planning STRIPStream problem.
:return: a :class:`.STRIPStreamProblem`
"""
blocks = ['block%i' % i for i in range(3)]
num_poses = pow(10, 10)
initial_config = 0 # the initial robot configuration is 0
initial_poses = {block: i
for i, block in enumerate(blocks)
} # the initial pose for block i is i
goal_poses = {block: i + 1
for i, block in enumerate(blocks)
} # the goal pose for block i is i+1
####################
# Data types
CONF, BLOCK, POSE = Type(), Type(), Type()
# Fluent predicates
AtConf = Pred(CONF)
AtPose = Pred(BLOCK, POSE)
HandEmpty = Pred()
Holding = Pred(BLOCK)
# Derived predicates
Safe = Pred(BLOCK, BLOCK, POSE)
# Static predicates
LegalKin = Pred(POSE, CONF)
CollisionFree = Pred(BLOCK, POSE, BLOCK, POSE)
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1),
LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(name='place',
parameters=[B1, P1, Q1],
condition=And(
Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
ForAll([B2],
Or(
Equal(B1, B2),
Exists([P2],
And(AtPose(B2, P2),
CollisionFree(B1, P1, B2, P2)))))),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
####################
# Conditional stream declarations
cond_streams = [
GeneratorStream(
inputs=[],
outputs=[P1],
conditions=[],
effects=[],
generator=lambda:
((p, ) for p in xrange(num_poses))), # Enumerating all the poses
GeneratorStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[LegalKin(P1, Q1)],
generator=lambda p: [(p, )]), # Inverse kinematics
TestStream(inputs=[B1, P1, B2, P2],
conditions=[],
effects=[CollisionFree(B1, P1, B2, P2)],
test=lambda b1, p1, b2, p2: p1 != p2,
eager=True), # Collision checking
]
####################
constants = [
CONF(initial_config) # Any additional objects
]
initial_atoms = [
AtConf(initial_config), HandEmpty()
] + [AtPose(block, pose) for block, pose in initial_poses.iteritems()]
goal_literals = [
AtPose(block, pose) for block, pose in goal_poses.iteritems()
]
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions,
cond_streams, constants)
return problem
WetPaint = Predicate(OBJ)
DryPaint = Predicate(OBJ)
Clear = Predicate(LOC)
Safe = Predicate(OBJ, LOC)
IsDryer = Predicate(LOC)
IsPainter = Predicate(LOC)
IsWasher = Predicate(LOC)
O, O2, L1, L2 = Param(OBJ), Param(OBJ), Param(LOC), Param(LOC)
actions = [
Action(name='transport',
parameters=[O, L1, L2],
condition=And(At(O, L1), Clear(L2)),
effect=And(At(O, L2),
Not(At(O,
L1)))), # NOTE - Leslie and Tomas call this Move
Action(
name='wash',
parameters=[O, L1],
condition=And(At(O, L1), IsWasher(L1)),
#effect=And(Clean(O)))
effect=And(Clean(O), Not(WetPaint(O)), Not(DryPaint(O)))),
Action(
name='paint',
parameters=[O, L1],
condition=And(At(O, L1), Clean(O), IsPainter(L1)),
#effect=And(WetPaint(O))),
effect=And(WetPaint(O), Not(Clean(O)))),
def create_problem():
"""
Creates the 1D task and motion planning STRIPStream problem.
This models the same problem as :module:`.run_tutorial` but does so without using any streams.
:return: a :class:`.STRIPStreamProblem`
"""
num_blocks = 3
blocks = ['block%i' % i for i in range(num_blocks)]
num_poses = num_blocks + 1
initial_config = 0 # the initial robot configuration is 0
initial_poses = {block: i
for i, block in enumerate(blocks)
} # the initial pose for block i is i
# the goal pose for block i is i+1
goal_poses = {block: i + 1 for i, block in enumerate(blocks)}
####################
# Data types
CONF, BLOCK, POSE = Type(), Type(), Type()
# Fluent predicates
AtConf = Pred(CONF)
AtPose = Pred(BLOCK, POSE)
HandEmpty = Pred()
Holding = Pred(BLOCK)
# Derived predicates
Safe = Pred(BLOCK, BLOCK, POSE)
# Static predicates
LegalKin = Pred(POSE, CONF)
CollisionFree = Pred(BLOCK, POSE, BLOCK, POSE)
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1),
LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(
name='place',
parameters=[B1, P1, Q1],
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
ForAll([B2], Or(Equal(B1, B2), Safe(
B2, B1,
P1)))), # TODO - convert to finite blocks case?
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = [
Axiom(effect=Safe(B2, B1, P1),
condition=Exists(
[P2], And(AtPose(B2, P2), CollisionFree(
B1, P1, B2,
P2)))), # Infers B2 is at a safe pose wrt B1 at P1
]
####################
cond_streams = []
constants = []
initial_atoms = [
AtConf(initial_config), HandEmpty()
] + [AtPose(block, pose) for block, pose in initial_poses.iteritems()
] + [LegalKin(i, i) for i in range(num_poses)] + [
CollisionFree(b1, p1, b2, p2)
for b1, p1, b2, p2 in product(blocks, range(
num_poses), blocks, range(num_poses)) if p1 != p2 and b1 != b2
]
goal_literals = [
AtPose(block, pose) for block, pose in goal_poses.iteritems()
]
problem = STRIPStreamProblem(initial_atoms, goal_literals,
actions + axioms, cond_streams, constants)
return problem
def create_problem(n=50):
"""
Creates the 1D task and motion planning STRIPStream problem.
:return: a :class:`.STRIPStreamProblem`
"""
blocks = ['block%i' % i for i in xrange(n)]
num_poses = pow(10, 10)
initial_config = 0 # the initial robot configuration is 0
initial_poses = {block: i
for i, block in enumerate(blocks)
} # the initial pose for block i is i
#goal_poses = {block: i+1 for i, block in enumerate(blocks)} # the goal pose for block i is i+1
goal_poses = {blocks[0]: 1} # the goal pose for block i is i+1
#goal_poses = {blocks[0]: 100} # the goal pose for block i is i+1
####################
# Data types
CONF, BLOCK, POSE = Type(), Type(), Type()
# Fluent predicates
AtConf = Pred(CONF)
AtPose = Pred(BLOCK, POSE)
IsPose = Pred(BLOCK, POSE)
HandEmpty = Pred()
Holding = Pred(BLOCK)
Moved = Pred() # Prevents double movements
# Derived predicates
Safe = Pred(BLOCK, POSE)
#Unsafe = Pred(BLOCK, BLOCK, POSE)
Unsafe = Pred(BLOCK, POSE)
#Unsafe = Pred(POSE)
# Static predicates
Kin = Pred(POSE, CONF)
CFree = Pred(POSE, POSE)
Collision = Pred(POSE, POSE)
# Free parameters
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
rename_easy(locals()) # Trick to make debugging easier
####################
# TODO: drp_pddl_adl/domains/tmp.py has conditional effects When(Colliding(pose, trajectory), Not(Safe(obj, trajectory))))
# TODO: maybe this would be okay if the effects really are sparse (i.e. not many collide)
# http://www.fast-downward.org/TranslatorOutputFormat
# FastDownward will always make an axiom for the quantified expressions
# I don't really understand why FastDownward does this... It doesn't seem to help
# It creates n "large" axioms that have n-1 conditions (removing the Equal)
# universal conditions: Universal conditions in preconditions, effect conditions and the goal are internally compiled into axioms by the planner.
# Therefore, heuristics that do not support axioms (see previous point) do not support universal conditions either.
# http://www.fast-downward.org/PddlSupport
# TODO: the compilation process actually seems to still make positive axioms for things.
# The default value is unsafe and it creates positive axioms...
# A heuristic cost of 4 is because it does actually move something out the way
# drp_pddl/domains/tmp_separate.py:class CollisionAxiom(Operator, Refinable, Axiom):
# TODO: maybe I didn't actually try negative axioms like I thought?
# See also 8/24/16 and 8/26/16 notes
# Maybe the translator changed sometime making it actually invert these kinds of axioms
# TODO: maybe this would be better if I did a non-boolean version that declared success if at any pose other than this one
# It looks like temporal fast downward inverts axioms as well
actions = [
Action(
name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), IsPose(B1, P1),
Kin(P1, Q1)), # AtConf(Q1),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()),
Not(Moved()))),
Action(
name='place',
parameters=[B1, P1, Q1],
condition=And(
Holding(B1),
IsPose(B1, P1),
Kin(P1, Q1), # AtConf(Q1),
#*[Safe(b, P1) for b in blocks]),
*[Not(Unsafe(b, P1)) for b in blocks]),
#*[Not(Unsafe(b, B1, P1)) for b in blocks]),
#*[Or(Equal(b, B1), Not(Unsafe(b, B1, P1))) for b in blocks]),
#ForAll([B2], Or(Equal(B1, B2), Not(Unsafe(B2, P1))))),
#ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))),
#ForAll([B2], Not(Unsafe(B2, B1, P1)))),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)),
Not(Moved()))),
# Action(name='place', parameters=[B1, P1, Q1],
# condition=And(Holding(B1), AtConf(Q1), IsPose(B1, P1), Kin(P1, Q1),
# #ForAll([B2], Or(Equal(B1, B2),
# # Exists([P2], And(AtPose(B2, P2), CFree(P1, P2)))))),
# ForAll([B2], Or(Equal(B1, B2), # I think this compiles to the forward axioms that achieve things...
# Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Not(Collision(P1, P2))))))),
# #ForAll([B2], Or(Equal(B1, B2),
# # Not(Exists([P2], And(AtPose(B2, P2), Not(CFree(P1, P2)))))))),
# #ForAll([B2], Or(Equal(B1, B2), # Generates a ton of axioms...
# # Not(Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Collision(P1, P2))))))),
# effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)), Not(Moved()))),
#Action(name='place', parameters=[B1, P1, Q1],
# condition=And(Holding(B1), AtConf(Q1), IsPose(B1, P1), Kin(P1, Q1), Not(Unsafe(P1))),
# effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)), Not(Moved()))),
#Action(name='move', parameters=[Q1, Q2],
# condition=And(AtConf(Q1), Not(Moved())),
# effect=And(AtConf(Q2), Moved(), Not(AtConf(Q1)))),
# TODO: a lot of the slowdown is because of the large number of move axioms
# Inferred Safe
#Translator operators: 1843
#Translator axioms: 3281
#Search Time: 10.98
# Explicit Safe
#Translator operators: 1843
#Translator axioms: 3281
#Search Time: 9.926
]
# TODO: translate_strips_axiom in translate.py
# TODO: maybe this is bad because of shared poses...
# 15*15*15*15 = 50625
# Takeaways: using the implicit collision is good because it results in fewer facts
# The negated axiom does better than the normal axiom by a little bit for some reason...
axioms = [
# For some reason, the unsafe version of this is much better than the safe version in terms of making axioms?
# Even with one collision recorded, it makes a ton of axioms
#Axiom(effect=Safe(B2, P1),
# condition=Or(Holding(B2), Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Not(Collision(P1, P2)))))),
#Axiom(effect=Unsafe(B2, B1, P1),
# condition=And(Not(Equal(B1, B2)),
# # Exists([P2], And(AtPose(B2, P2), Not(CFree(P1, P2)))))),
# Exists([P2], And(AtPose(B2, P2), Collision(P1, P2))))),
#Axiom(effect=Unsafe(B2, B1, P1),
# condition=Exists([P2], And(AtPose(B2, P2), Not(CFree(P1, P2))))),
# TODO: I think the inverting is implicitly doing the same thing I do where I don't bother making an axiom if always true
Axiom(effect=Unsafe(B2, P1),
condition=Exists([P2],
And(AtPose(B2, P2), IsPose(B2, P2),
Collision(P1,
P2)))), # Don't even need IsPose?
# This is the best config. I think it is able to work well because it can prune the number of instances when inverting
# It starts to take up a little time when there are many possible placements for things though
# TODO: the difference is that it first instantiates axioms and then inverts!
#Axiom(effect=Unsafe(B2, P1),
# condition=Exists([P2], And(AtPose(B2, P2), IsPose(B2, P2), Not(CFree(P1, P2)))))
# This doesn't result in too many axioms but takes a while to instantiate...
#Axiom(effect=Unsafe(P1), # Need to include the not equal thing
# condition=Exists([B2, P2], And(AtPose(B2, P2), IsPose(B2, P2), Collision(P1, P2)))),
# TODO: Can turn off options.filter_unreachable_facts
]
####################
# Conditional stream declarations
cond_streams = [
#GeneratorStream(inputs=[], outputs=[P1], conditions=[], effects=[],
# generator=lambda: ((p,) for p in xrange(n, num_poses))),
GeneratorStream(
inputs=[B1],
outputs=[P1],
conditions=[],
effects=[IsPose(B1, P1)],
#generator=lambda b: ((p,) for p in xrange(n, num_poses))),
generator=lambda b: iter([(n + blocks.index(b), )])
), # Unique placements
GeneratorStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[Kin(P1, Q1)],
generator=lambda p: [(p, )]), # Inverse kinematics
#TestStream(inputs=[P1, P2], conditions=[], effects=[CFree(P1, P2)],
# test=lambda p1, p2: p1 != p2, eager=True),
# #test = lambda p1, p2: True, eager = True),
TestStream(inputs=[P1, P2],
conditions=[],
effects=[Collision(P1, P2)],
test=lambda p1, p2: p1 == p2,
eager=False,
sign=False),
]
####################
constants = [
CONF(initial_config) # Any additional objects
]
initial_atoms = [
AtConf(initial_config),
HandEmpty(),
] + [AtPose(block, pose) for block, pose in initial_poses.items()] + [
IsPose(block, pose)
for block, pose in (initial_poses.items() + goal_poses.items())
]
goal_literals = [
AtPose(block, pose) for block, pose in goal_poses.iteritems()
]
problem = STRIPStreamProblem(initial_atoms, goal_literals,
actions + axioms, cond_streams, constants)
return problem
class STRIPStreamProblem(object):
def __init__(self,
initial_atoms,
goal_literals,
operators,
cond_streams,
objects=[]):
# TODO - warnings for improperly passed types here
self.initial_atoms = list(initial_atoms)
if isinstance(goal_literals, Atom):
self.goal_literals = And(goal_literals)
elif isinstance(goal_literals, Iterable):
self.goal_literals = And(*goal_literals)
elif isinstance(
goal_literals,
Formula): # or isinstance(goal_literals, AbsCondition):
self.goal_literals = goal_literals
else:
raise ValueError(goal_literals)
# TODO - assert goals aren't empty
self.operators = operators
self.cond_streams = cond_streams
self.objects = objects
def get_constants(self):
objects = set(self.objects)
for atom in self.initial_atoms:
objects.update(atom.args)
if isinstance(self.goal_literals, Formula):
for atom in self.goal_literals.get_atoms():
objects.update(atom.args)
#elif isinstance(self.goal_literals, AbsCondition):
# for formula in self.goal_literals.conditions:
# for atom in formula.get_atoms():
# objects.update(atom.args)
for operator in self.operators:
for atom in operator.condition.get_atoms(
) | operator.effect.get_atoms():
objects.update(atom.args)
for cs in self.cond_streams:
for atom in cs.conditions + cs.effects:
objects.update(atom.args)
return filter(lambda o: isinstance(o, Constant), objects)
def get_functions(self):
from stripstream.pddl.operators import get_function_atoms
functions = set()
for operator in self.operators:
if isinstance(operator, Action):
functions.update(
{atom.predicate
for atom in get_function_atoms(operator)})
return functions
def get_fluent_predicates(
self
): # TODO - enforce consistency for these and make them a property
from stripstream.pddl.operators import Action
fluents = set()
for operator in self.operators:
if isinstance(operator, Action):
fluents.update(
{atom.predicate
for atom in operator.effect.get_atoms()})
return fluents
def get_derived_predicates(self):
from stripstream.pddl.operators import Axiom
return set(operator.effect.predicate for operator in self.operators
if isinstance(operator, Axiom))
def get_stream_predicates(self):
return {
atom.predicate
for cs in self.cond_streams for atom in cs.conditions + cs.effects
}
def get_predicates(self):
return {atom.predicate for op in self.operators for atom in op.condition.get_atoms() | op.effect.get_atoms()} | \
{atom.predicate for cs in self.cond_streams for atom in cs.conditions+cs.effects} | \
{atom.predicate for atom in self.initial_atoms} | \
{atom.predicate for atom in self.goal_literals.get_atoms()}
def get_static_predicates(self):
return self.get_predicates() - self.get_fluent_predicates() - \
self.get_derived_predicates() - self.get_stream_predicates()
def __repr__(self):
return 'STRIPStream Problem\n' \
'Initial: %s\n' \
'Goal: %s\n' \
'Operators: %s\n' \
'Streams: %s\n' \
'Constants: %s'%(self.initial_atoms, self.goal_literals,
self.operators, self.cond_streams, self.objects)
