def create_problem(time_step=.5, fuel_rate=5, start_fuel=10, goal_p=10):
"""
Creates the Tsiolkovsky rocket problem.
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
STATE, RATE, MASS, TIME = Type(), Type(), Type(), Type()
HEIGHT = Type()
# Fluent predicates
AtState = Pred(STATE)
# Derived predicates
Above = Pred(HEIGHT)
# Static predicates
IsBurst = Pred(STATE, RATE, TIME, STATE)
IsAbove = Pred(STATE, HEIGHT)
# Free parameters
X1, X2 = Param(STATE), Param(STATE)
Q, T = Param(RATE), Param(TIME)
H = Param(HEIGHT)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='burst',
parameters=[X1, Q, T, X2],
condition=And(AtState(X1), IsBurst(X1, Q, T, X2)),
effect=And(AtState(X2), Not(AtState(X1)))),
]
axioms = [
Axiom(effect=Above(H),
condition=Exists([X1], And(AtState(X1), IsAbove(X1, H)))),
]
####################
# Conditional stream declarations
cond_streams = [
GeneratorStream(inputs=[X1, Q, T],
outputs=[X2],
conditions=[],
effects=[IsBurst(X1, Q, T, X2)],
generator=forward_burst),
TestStream(inputs=[X1, H],
conditions=[],
effects=[IsAbove(X1, H)],
test=lambda (p, v, m), 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 compile_problem(estimator, task):
# Data types
CONF = Type()
SURFACE = Type() # Difference between fixed and movable objects
ITEM = Type()
POSE = Type()
CLASS = Type()
# Fluent predicates
AtConf = Pred(CONF)
HandEmpty = Pred()
Holding = Pred(ITEM)
AtPose = Pred(ITEM, POSE)
Supported = Pred(POSE, SURFACE) # Fluent
Localized = Pred(OBJECT)
Measured = Pred(OBJECT)
# Static predicates
IsKin = Pred(POSE, CONF)
IsClass = Pred(OBJECT, CLASS)
IsVisible = Pred(SURFACE, CONF)
IsSupported = Pred(POSE, SURFACE) # Static
# Functions
ScanRoom = Func(SURFACE)
#ScanTable = Func(SURFACE, TYPE)
ScanTable = Func(
SURFACE, ITEM) # TODO: could include more specific vantage point costs
Distance = Func(CONF, CONF)
# Derived
On = Pred(ITEM, SURFACE)
ComputableP = Pred(POSE)
ComputableQ = Pred(CONF)
# Free parameters
Q1, Q2 = Param(CONF), Param(CONF)
S1 = Param(SURFACE)
B1, B2 = Param(ITEM), Param(ITEM)
P1, P2 = Param(POSE), Param(POSE)
rename_easy(locals()) # Trick to make debugging easier
# TODO: could just do easier version of this that doesn't require localized to start
actions = [
Action(
name='pick',
parameters=[B1, P1, Q1], # TODO: Visibility constraint
condition=And(Localized(B1), AtPose(B1, P1), HandEmpty(),
AtConf(Q1), IsKin(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), IsKin(P1, Q1)),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=And(AtConf(Q1), ComputableQ(Q2)),
effect=And(AtConf(Q2), Not(AtConf(Q1)), Cost(Distance(Q1,
Q2)))),
Action(name='scan_room',
parameters=[S1],
condition=Not(Localized(S1)),
effect=And(Localized(S1), Cost(ScanRoom(S1)))),
# TODO: need to set later poses to be usable or not to constrain order
Action(name='scan_table',
parameters=[S1, B1, P1, Q1],
condition=And(Localized(S1), AtConf(Q1), IsVisible(S1, Q1),
Not(Localized(B1))),
effect=And(Localized(B1), Measured(P1), Supported(P1, S1),
Cost(ScanTable(S1, B1)))),
]
axioms = [
# TODO: axiom for on? Might need a stream that generates initial fluents for On
# TODO: axiom that says that all fake values depending on a certain one now are usable
# TODO: could use stream predicates as fluents (as long as it doesn't break anything...)
Axiom(effect=On(B1, S1),
condition=Exists([P1],
And(AtPose(B1, P1),
Or(IsSupported(P1, S1), Supported(P1,
S1))))),
# TODO: compile automatically
Axiom(effect=ComputableQ(Q1),
condition=Or(Measured(Q1),
Exists([P1], And(IsKin(P1, Q1), ComputableP(P1))),
Exists([S1], And(IsVisible(S1, Q1),
Localized(S1))))),
Axiom(effect=ComputableP(P1),
condition=Or(
Measured(P1),
Exists([S1], And(IsSupported(P1, S1), Localized(S1))))),
]
#####
surface_types = estimator.surface_octomaps.keys()
item_types = estimator.object_octomaps.keys()
names_from_type = defaultdict(list)
known_poses = {}
holding = None
def add_type(cl):
name = '{}{}'.format(cl, len(names_from_type[cl]))
names_from_type[cl].append(name)
return name
# TODO: this is all very similar to the generic open world stuff
if estimator.holding is not None:
holding = add_type(estimator.holding)
for cl, octomap in estimator.surface_octomaps.items(
): # TODO: generic surface object
for pose in octomap.get_occupied():
known_poses[add_type(cl)] = pose
for cl, octomap in estimator.object_octomaps.items():
for pose in octomap.get_occupied():
known_poses[add_type(cl)] = pose
print dict(names_from_type), known_poses
# Human tells you to move block -> at least one block
# At least one block -> at least one surface
# TODO: generate fake properties about these fake values?
goal_objects, goal_surfaces = entities_from_task(task)
for cl in surface_types:
add_type(cl)
#for cl in goal_surfaces:
# for i in xrange(len(names_from_type[cl]), goal_surfaces[cl]):
# add_type(cl)
#for cl in item_types:
for cl in goal_objects:
for i in xrange(len(names_from_type[cl]), goal_objects[cl]):
add_type(cl)
#####
initial_atoms = [
AtConf(estimator.robot_conf),
Measured(CONF(estimator.robot_conf))
]
if holding is None:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(holding))
class_from_name = {
name: ty
for ty in names_from_type for name in names_from_type[ty]
}
for name, ty in class_from_name.iteritems():
ENTITY = SURFACE if ty in surface_types else ITEM
initial_atoms.append(IsClass(ENTITY(name), ty))
if name in known_poses:
initial_atoms.append(Localized(ENTITY(name)))
if ENTITY == ITEM:
pose = known_poses[name]
initial_atoms += [AtPose(name, pose), Measured(POSE(pose))]
else:
if ENTITY == ITEM:
pose = 'p_init_{}'.format(
name
) # The object should always be at this pose (we just can't do anything about it yet)
initial_atoms += [AtPose(name, pose)]
goal_literals = []
if task.holding is None:
goal_literals.append(HandEmpty())
elif task.holding is not False:
goal_literals.append(
Exists([B1], And(Holding(B1), IsClass(B1, task.holding))))
for obj, surface in task.object_surfaces:
goal_literals.append(
Exists([B1, S1],
And(On(B1, S1), IsClass(B1, obj), IsClass(S1, surface))))
goal_formula = And(*goal_literals)
####################
TOLERANCE = 0.1
def is_visible(table, conf):
x, y = conf
pose = known_poses[table]
#return (pose == x) and (y == 2)
#return (pose == x) and (y == 2)
return (pose == x) and (abs(y - 2) < TOLERANCE)
def is_kinematic(pose, conf):
x, y = conf
#return (pose == x) and (y == 1)
return (pose == x) and (abs(y - 1) < TOLERANCE)
####################
def sample_visible(table): # TODO: could generically do this with poses
if table in known_poses:
y = 2
#y += round(uniform(-TOLERANCE, TOLERANCE), 3)
conf = (known_poses[table], y)
assert is_visible(table, conf)
else:
conf = 'q_vis_{}'.format(table)
yield (conf, )
def inverse_kinematics(pose): # TODO: list stream that uses ending info
# TODO: only do if localized as well?
# TODO: is it helpful to have this even if the raw value is kind of wrong (to steer the search)
if type(pose) != str:
y = 1
#y += round(uniform(-TOLERANCE, TOLERANCE), 3)
conf = (pose, y)
assert is_kinematic(pose, conf)
else:
conf = 'q_ik_{}'.format(pose)
yield (conf, )
def sample_table(table):
if table in known_poses:
pose = known_poses[table]
else:
pose = 'p_{}'.format(table)
yield (pose, )
####################
MAX_DISTANCE = 10
# TODO: maybe I don't need to worry about normalizing. I can just pretend non-parametric again for planning
def scan_surface_cost(
surface, obj): # TODO: what about multiple scans of the belief?
fail_cost = 100
surface_cl = class_from_name[surface]
obj_cl = class_from_name[obj]
prob = 1.0
if obj_cl in estimator.object_prior:
prob *= estimator.object_prior[obj_cl].get(surface_cl, 0)
if surface in known_poses:
prob *= estimator.object_octomaps[obj_cl].get_prob(
known_poses[surface])
else:
prob *= 0.1 # Low chance if you don't even know the table exists
# TODO: could even include the probability the table exists
#return expected_cost(1, fail_cost, prob)
return mdp_cost(1, fail_cost, prob)
def scan_room_cost(surface):
# TODO: try to prove some sort of bound on the cost to recover will suffice?
fail_cost = 100
cl = class_from_name[surface]
occupied_poses = {
known_poses[n]
for n in names_from_type[cl] if n in known_poses
}
p_failure = 1.0
for pose in estimator.poses:
if pose not in occupied_poses:
p_failure *= (1 -
estimator.surface_octomaps[cl].get_prob(pose))
return 1 * (1 - p_failure) + fail_cost * p_failure
def distance_cost(q1, q2):
if str in (type(q1), type(q2)):
return MAX_DISTANCE # TODO: take the max possible pose distance
# TODO: can use the info encoded within these to obtain better bounds
return np.linalg.norm(np.array(q2) - np.array(q1))
####################
# TODO: could add measured as the output to these
streams = [
GeneratorStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[IsKin(P1, Q1)],
generator=inverse_kinematics),
GeneratorStream(inputs=[S1],
outputs=[Q1],
conditions=[],
effects=[IsVisible(S1, Q1)],
generator=sample_visible),
GeneratorStream(inputs=[S1],
outputs=[P1],
conditions=[],
effects=[IsSupported(P1, S1)],
generator=sample_table),
CostStream(inputs=[S1, B1],
conditions=[],
effects=[ScanTable(S1, B1)],
function=scan_surface_cost),
CostStream(inputs=[Q1, Q2],
conditions=[],
effects=[Distance(Q1, Q2),
Distance(Q2, Q1)],
function=distance_cost),
CostStream(inputs=[S1],
conditions=[],
effects=[ScanRoom(S1)],
function=scan_room_cost),
# TODO: make an is original precondition and only apply these to original values?
# I suppose I could apply to all concrete things but that's likely not useful
#TestStream(inputs=[S1, Q1], conditions=[IsOriginal(Q1)], effects=[IsVisible(S1, Q1)],
# test=is_visible, eager=True),
#TestStream(inputs=[P1, Q1], conditions=[IsOriginal(Q1), IsOriginal(Q1)], effects=[IsKin(P1, Q1)],
# test=is_kinematic, eager=True),
#GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[], effects=[IsVisible(P1, Q1)],
# generator=sample_visible),
]
problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms,
streams, [])
def command_from_action((action, args)):
if action.name == 'scan_room':
return simulate_scan, []
if action.name in ('scan_table', 'look_block'):
return simulate_look, []
if action.name == 'move':
q1, q2 = map(get_value, args)
return simulate_move, [q2]
if action.name == 'pick':
o, p, q = map(get_value, args)
return simulate_pick, [class_from_name[o]]
if action.name == 'place':
return simulate_place, []
raise ValueError(action.name)
return problem, command_from_action
holding = None
initial = State(tables, initial_poses, initial_config, holding)
#goal = Goal({}, 'block0')
goal = Goal({
'block0': 'table0',
'block1': 'table2',
'block2': 'table3'
}, None)
return initial, goal
# TODO: later can reincorporate types (instead of assuming all unique)
# Data types
CONF = Type()
BLOCK = Type()
SURFACE = Type()
POSE = Type()
GRASP = Type()
# Fluent predicates
AtConf = Pred(CONF)
AtPose = Pred(BLOCK, POSE)
HasGrasp = Pred(BLOCK, GRASP)
HandEmpty = Pred()
# Derived predicates
On = Pred(BLOCK, SURFACE)
Holding = Pred(BLOCK)
NearSurface = Pred(SURFACE) # Nearby
world_from_mesh = trans_from_pose(pose)
surface_from_mesh = np.linalg.inv(world_from_surface).dot(world_from_mesh)
points_surface = apply_affine(surface_from_mesh, [point_from_pose(pose)])
min_z = np.min(points_surface[:, 2])
return (abs(min_z) < 0.01) and all(
is_point_in_polygon(p, surface.convex_hull) for p in points_surface)
####################
from stripstream.pddl.objects import EasyType as Type, EasyParameter as Param
from stripstream.pddl.logic.predicates import EasyPredicate as Pred
from stripstream.pddl.utils import rename_easy
# Types
CYL, POINT, GRASP = Type(), Type(), Type()
CONF, TRAJ, SURFACE = Type(), Type(), Type()
# Fluents
ConfEq = Pred(CONF)
PointEq = Pred(CYL, POINT)
GraspEq = Pred(CYL, GRASP)
Holding = Pred(CYL)
HandEmpty = Pred()
# Derived
SafePose = Pred(CYL, POINT)
SafeTraj = Pred(CYL, TRAJ)
OnSurface = Pred(CYL, SURFACE)
# Static trajectory
def create_problem(initRobotPos = (0.5, 0.5),
initRobotVar = 0.01,
maxMoveDist = 5.0,
beaconPos = (1, 1),
homePos = (0, 0),
goalPosEps = 0.1,
goalVar = 0.1,
odoErrorRate = 0.1,
obsVarPerDistFromSensor = 10.0,
minObsVar = 0.001,
domainSize = 20,
verboseFns = True):
"""
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
POS = Type() # 2D position
VAR = Type() # 2D variance
DIST = Type() # positive scalar
# Fluent predicates
RobotPos = Pred(POS)
RobotVar = Pred(VAR)
# Derived predicates
KnowYouAreHome = Pred()
# Static predicates
DistBetween = Pred(POS, POS, DIST) # Distance between two positions (function)
LessThanV = Pred(VAR, VAR) # Less than, on distances
LessThanD = Pred(DIST, DIST) # Less than, on distances
OdometryVar = Pred(VAR, VAR, DIST) # How does odometry variance increase?
SensorVar = Pred(VAR, VAR, DIST) # How does an observation decrease variance?
LegalPos = Pred(POS) # Any legal robot pos
# Free parameters
RPOS1, RPOS2, RVAR1, RVAR2 = Param(POS), Param(POS), Param(VAR), Param(VAR)
DIST1, DIST2 = Param(DIST), Param(DIST)
def odoVarFun(rv, d):
odoVar = (d * odoErrorRate)**2
result = rv + odoVar
if verboseFns: print 'ovf:', rv, d, result
return [result]
def sensorVarFun(rv, d):
obsVar = max(d / obsVarPerDistFromSensor, minObsVar)
result = 1.0 / ((1.0 / rv) + (1.0 / obsVar))
if verboseFns: print 'svf:', rv, d, result
return [result]
def randPos():
while True:
result = (random.random() * domainSize, random.random() * domainSize)
print 'rp:', result
yield [result]
def legalTest(rp):
(x, y) = rp
result = (0 <= x <= domainSize) and (0 <= y <= domainSize)
if not result: print 'not legal:', rp
return result
actions = [
Action(name='Move',
parameters=[RPOS1, RPOS2, RVAR1, RVAR2, DIST1],
condition = And(RobotPos(RPOS1),
RobotVar(RVAR1),
LegalPos(RPOS2), # Generate an intermediate pos
DistBetween(RPOS1, RPOS2, DIST1),
LessThanD(DIST1, maxMoveDist),
OdometryVar(RVAR1, RVAR2, DIST1)),
effect = And(RobotPos(RPOS2),
RobotVar(RVAR2),
Not(RobotPos(RPOS1)),
Not(RobotVar(RVAR1)))),
# Action(name='Look',
# parameters=[RPOS1, RVAR1, RVAR2, DIST1],
# condition = And(RobotPos(RPOS1),
# RobotVar(RVAR1),
# DistBetween(RPOS1, beaconPos, DIST1),
# SensorVar(RVAR1, RVAR2, DIST1)),
# effect = And(RobotVar(RVAR2),
# Not(RobotVar(RVAR1))))
]
axioms = [
Axiom(effect = KnowYouAreHome(),
condition = Exists([RPOS1, RVAR1, DIST1],
And(RobotPos(RPOS1),
RobotVar(RVAR1),
DistBetween(RPOS1, homePos, DIST1),
LessThanD(DIST1, goalPosEps),
LessThanV(RVAR1, goalVar))))
]
# Conditional stream declarations
cond_streams = [
TestStream(inputs = [RPOS1],
conditions = [],
effects = [LegalPos(RPOS1)],
test = legalTest,
eager = True),
GeneratorStream(inputs = [],
outputs = [RPOS1],
conditions = [],
effects = [LegalPos(RPOS1)],
generator = randPos),
GeneratorStream(inputs = [RPOS1, RPOS2],
outputs = [DIST1],
conditions = [],
effects = [DistBetween(RPOS1, RPOS2, DIST1)],
generator = lambda rp1, rp2: [distance(rp1, rp2)]),
GeneratorStream(inputs = [RVAR1, DIST1],
outputs = [RVAR2],
conditions = [],
effects = [OdometryVar(RVAR1, RVAR2, DIST1)],
generator = odoVarFun),
# GeneratorStream(inputs = [RVAR1, DIST1],
# outputs = [RVAR2],
# conditions = [],
# effects = [SensorVar(RVAR1, RVAR2, DIST1)],
# generator = sensorVarFun),
TestStream(inputs = [DIST1, DIST2],
conditions = [],
effects = [LessThanD(DIST1, DIST2)],
test = lt,
eager = True),
TestStream(inputs = [RVAR1, RVAR2],
conditions = [],
effects = [LessThanV(RVAR1, RVAR2)],
test = lt, eager = True)
]
####################
constants = [
]
initial_atoms = [
RobotPos(initRobotPos),
RobotVar(initRobotVar),
LegalPos(homePos),
LegalPos((.1, .1)),
LegalPos((3.0, .1)),
LegalPos((6.0, .1))
]
goal_literals = [
KnowYouAreHome()
]
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
return problem
def create_problem(dt, GoalTest, ClearTest, CalcDiffSystem, InitialState):
"""
Creates a generic non-holonomic motion planning problem
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
X, DX, U = Type(), Type(), Type()
# Fluent predicates
AtX = Pred(X)
# Fluent predicates
GoalReached = Pred()
# Static predicates
ComputeDX = Pred(X, U, DX)
Dynamics = Pred(X, DX, X)
AtGoal = Pred(X)
Clear = Pred(X, X)
# Free parameters
X1, X2 = Param(X), Param(X)
DX1 = Param(DX)
U1 = Param(U)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='simulate',
parameters=[X1, U1, X2, DX1],
condition=And(AtX(X1), ComputeDX(X1, U1, DX1),
Dynamics(X1, DX1, X2), Clear(X1, X2)),
effect=And(Not(AtX(X1)), AtX(X2))),
]
axioms = [
Axiom(effect=GoalReached(), condition=Exists([X1], AtGoal(X1))),
]
####################
# Conditional stream declarations
def ComputeDXCalculator(X1F, DX1F):
X2F = X1F[:]
for i in range(len(X2F)):
X2F[i] = X1F[i] + dt * DX1F[i]
return X2F
cond_streams = [
FunctionStream(inputs=[X1, DX1],
outputs=[X2],
conditions=[],
effects=[Dynamics(X1, DX1, X2)],
function=ComputeDXCalculator),
FunctionStream(inputs=[X1, U1],
outputs=[DX1],
conditions=[],
effects=[ComputeDX(X1, U1, DX1)],
function=CalcDiffSystem),
TestStream(inputs=[X1, X2],
conditions=[],
effects=[Clear(X1, X2)],
test=ClearTest,
eager=True),
TestStream(inputs=[X1],
conditions=[],
effects=[AtGoal(X1)],
test=GoalTest,
eager=True),
]
####################
constants = [
# TODO - need to declare control inputs (or make a stream for them)
]
initial_atoms = [
AtX(InitialState),
]
goal_literals = [GoalReached()]
problem = STRIPStreamProblem(initial_atoms, goal_literals,
actions + axioms, cond_streams, constants)
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 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
####################
VALUE = Type()
# Fluent predicates
AtConf = Pred(VALUE)
AtPose = Pred(VALUE, VALUE)
HandEmpty = Pred()
Holding = Pred(VALUE)
# Derived predicates
Safe = Pred(VALUE, VALUE, VALUE)
# Static predicates
IsBlock = Pred(VALUE)
IsPose = Pred(VALUE)
IsConf = Pred(VALUE)
LegalKin = Pred(VALUE, VALUE)
CollisionFree = Pred(VALUE, VALUE, VALUE, VALUE)
# Free parameters
B1, B2 = Param(VALUE), Param(VALUE)
P1, P2 = Param(VALUE), Param(VALUE)
Q1, Q2 = Param(VALUE), Param(VALUE)
rename_easy(locals()) # Trick to make debugging easier
####################
# NOTE - it would be easier to just update an in hand pose
actions = [
STRIPSAction(name='pick', parameters=[B1, P1, Q1],
conditions=[IsBlock(B1), IsPose(P1), IsConf(Q1), LegalKin(P1, Q1),
AtPose(B1, P1), HandEmpty(), AtConf(Q1)],
effects=[Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty())]),
STRIPSAction(name='place', parameters=[B1, P1, Q1],
conditions=[IsBlock(B1), IsPose(P1), IsConf(Q1), LegalKin(P1, Q1),
Holding(B1), AtConf(Q1)] + [Safe(b2, B1, P1) for b2 in blocks],
effects=[AtPose(B1, P1), HandEmpty(), Not(Holding(B1))]),
STRIPSAction(name='move', parameters=[Q1, Q2],
conditions=[IsConf(Q1), IsConf(Q2), AtConf(Q1)],
effects=[AtConf(Q2), Not(AtConf(Q1))]),
]
axioms = [ # TODO - need to combine 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=[IsPose(P1)],
generator=lambda: xrange(num_poses)), # Enumerating all the poses
GeneratorStream(inputs=[P1], outputs=[Q1], conditions=[IsPose(P1)], effects=[IsConf(Q1), LegalKin(P1, Q1)],
generator=lambda p: [p]), # Inverse kinematics
TestStream(inputs=[B1, P1, B2, P2], conditions=[IsBlock(B1), IsPose(P1), IsBlock(B2), IsPose(P2)],
effects=[CollisionFree(B1, P1, B2, P2)], test=lambda b1, p1, b2, p2: p1 != p2, eager=True), # Collision checking
]
####################
constants = []
initial_atoms = [
IsConf(initial_config),
AtConf(initial_config),
HandEmpty()
]
for block, pose in initial_poses.iteritems():
initial_atoms += [
IsBlock(block),
IsPose(pose),
AtPose(block, pose),
]
goal_literals = []
for block, pose in goal_poses.iteritems():
initial_atoms += [
IsBlock(block),
IsPose(pose),
]
goal_literals.append(AtPose(block, pose))
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
return problem
from stripstream.pddl.objects import Type
from stripstream.pddl.objects import Parameter, Constant
from stripstream.pddl.operators import STRIPSAction, STRIPSAxiom, Axiom
from stripstream.pddl.streams import TestStream, FunctionStream, StrictStream
from stripstream.pddl.cond_streams import CondStream, TestCondStream
from stripstream.pddl.problem import STRIPStreamProblem
BASE = True
COLLISIONS = True
DO_MOTION = True
ACTION_COST = 1
EAGER_TESTS = True
####################
CONFIG = Type('conf')
BLOCK = Type('block')
POSE = Type('pose')
GRASP = Type('grasp')
REGION = Type('region')
TRAJ = Type('traj')
####################
AtConfig = Predicate('at_config', [CONFIG])
HandEmpty = Predicate('hand_empty', [])
AtPose = Predicate(
'at_pose', [BLOCK, POSE]) # TODO - probably don't even need block here...
#HasGrasp = Predicate('has_grasp', [BLOCK, GRASP])
HasGrasp = Predicate('has_grasp', [GRASP])
Holding = Predicate('holding', [BLOCK])
from stripstream.pddl.objects import EasyType as Type, EasyParameter as Param
from stripstream.pddl.logic.predicates import EasyPredicate as Predicate
from stripstream.pddl.operators import Action
from stripstream.pddl.logic.connectives import And, Not
from stripstream.algorithms.incremental.incremental_planner import incremental_planner
from stripstream.algorithms.search.fast_downward import get_fast_downward
from stripstream.pddl.utils import convert_plan, rename_easy
from stripstream.pddl.problem import STRIPStreamProblem
from stripstream.pddl.examples.belief.problems import *
from toyTest import glob, makeOperators, Bd, ObjState, ObjLoc
OBJ, LOC = Type(), Type()
At = Predicate(OBJ, LOC)
Clear = Predicate(LOC)
Clean = Predicate(OBJ)
WetPaint = Predicate(OBJ)
DryPaint = Predicate(OBJ)
IsDryer = Predicate(LOC)
IsPainter = Predicate(LOC)
IsWasher = Predicate(LOC)
O, L1, L2 = 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), Clear(L1), Not(At(O, L1)), Not(
from stripstream.pddl.logic.connectives import Not, Or, And from stripstream.pddl.logic.quantifiers import Exists, ForAll from stripstream.pddl.logic.atoms import Equal from stripstream.pddl.operators import Action, Axiom from stripstream.utils import irange, INF from stripstream.pddl.utils import rename_easy from stripstream.pddl.problem import STRIPStreamProblem from stripstream.pddl.cond_streams import EasyGenStream, EasyTestStream from stripstream.pddl.objects import EasyType as Type, EasyParameter as Param from stripstream.pddl.logic.predicates import EasyPredicate as Pred from stripstream.pddl.examples.continuous_tamp.continuous_tamp_utils import are_colliding, in_region, sample_region_pose, inverse_kinematics from stripstream.pddl.utils import get_value EAGER_TESTS = True CONF, BLOCK, POSE, REGION = Type(), Type(), Type(), Type() AtConf = Pred(CONF) HandEmpty = Pred() AtPose = Pred(BLOCK, POSE) Holding = Pred(BLOCK) Safe = Pred(BLOCK, BLOCK, POSE) InRegion = Pred(BLOCK, REGION) LegalKin = Pred(POSE, CONF) CollisionFree = Pred(BLOCK, POSE, BLOCK, POSE) Contained = Pred(BLOCK, POSE, REGION) CanPlace = Pred(BLOCK, REGION) IsSink = Pred(REGION)
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
#import hpn
from stripstream.pddl.objects import EasyType as Type, EasyParameter as Param
from stripstream.pddl.logic.predicates import EasyPredicate as Predicate
from stripstream.pddl.operators import Action, Axiom
from stripstream.pddl.logic.connectives import And, Not
from stripstream.pddl.logic.quantifiers import Exists
from stripstream.pddl.cond_streams import EasyGenStream as GeneratorStream, EasyTestStream as TestStream
from stripstream.algorithms.incremental.incremental_planner import incremental_planner
from stripstream.algorithms.search.fast_downward import get_fast_downward
from stripstream.pddl.utils import convert_plan, rename_easy
from stripstream.pddl.problem import STRIPStreamProblem
from stripstream.pddl.examples.belief.utils import *
OBJ, POSE, BELIEF = Type(), Type(), Type()
DIST = Type()
At = Predicate(OBJ, POSE)
BAt = Predicate(OBJ, POSE, BELIEF)
BAtAbove = Predicate(OBJ, POSE, BELIEF)
Above = Predicate(BELIEF, BELIEF)
IsUpdate = Predicate(BELIEF, BELIEF)
IsPossible = Predicate(BELIEF, BELIEF)
IsClean = Predicate(DIST, DIST)
BClean = Predicate(OBJ, DIST)
BDirty = Predicate(OBJ,
DIST) # TODO - I could just process this as one parameter
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 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
from math import sqrt
from stripstream.pddl.logic.connectives import Not
from stripstream.pddl.logic.predicates import Predicate
from stripstream.pddl.objects import Parameter, Constant, Type, HashableObject
from stripstream.pddl.operators import STRIPSAction, STRIPSAxiom
from stripstream.pddl.streams import GeneratorStream, TestStream, FunctionStream
from stripstream.pddl.cond_streams import CondStream, TestCondStream
from stripstream.pddl.problem import STRIPStreamProblem
P = Parameter
C = Constant
POSITION = Type('pos')
#VELOCITY = Type('vel')
STATE = Type('state')
ACCELERATION = Type('accel')
FORCE = Type('force')
MASS = Type('mass')
TIME = Type('time')
SATELLITE = Type('satellite')
#ROCKET = Type('rocket')
G = -9.8
# TODO - mass of the packages affects things
######
#AtState = Predicate('at_state', [POSITION, VELOCITY])
AtState = Predicate('at_state', [STATE])
Above = Predicate('above', [POSITION])
def create_problem(initRobotPos = (0.5, 0.5),
initRobotVar = 0.01,
maxMoveDist = 5.0,
beaconPos = (1, 1),
homePos = (0, 0),
goalPosEps = 0.1,
goalVar = 0.1,
odoErrorRate = 0.1,
obsVarPerDistFromSensor = 10.0,
minObsVar = 0.001,
domainSize = 20,
verboseFns = False):
"""
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
POS = Type() # 2D position
VAR = Type() # 2D variance
BEACON = Type() # 2D position
# Fluent predicates
RobotPos = Pred(POS)
RobotVar = Pred(VAR)
# Derived predicates
KnowYouAreHome = Pred()
# Static predicates
Odometry = Pred(POS, VAR, POS, VAR)
Sensor = Pred(POS, VAR, BEACON, VAR)
LegalPosVar = Pred(POS, VAR)
NearbyPos = Pred(POS, POS)
NearGoalPos = Pred(POS)
NearGoalVar = Pred(VAR)
# Free parameters
RPOS1, RPOS2 = Param(POS), Param(POS)
RVAR1, RVAR2 = Param(VAR), Param(VAR)
BPOS = Param(BEACON)
rename_easy(locals())
# Helper functions
def distance(p1, p2):
return math.sqrt(sum([(a - b)**2 for (a,b) in zip(p1, p2)]))
def odoVarFun(rp1, rv, rp2):
d = distance(rp1, rp2)
odoVar = (d * odoErrorRate)**2
result = rv + odoVar
if verboseFns: print 'ovf:', rv, d, result
return [result]
def sensorVarFun(rp, rv, bp):
d = distance(rp, bp)
obsVar = max(d / obsVarPerDistFromSensor, minObsVar)
#result = round(1.0 / ((1.0 / rv) + (1.0 / obsVar)), 5) # Causes zero variance which is bad
result = 1.0 / ((1.0 / rv) + (1.0 / obsVar))
if verboseFns: print 'svf:', rv, d, result
return [result]
def randPos():
while True:
result = (random.random() * domainSize, random.random() * domainSize)
print 'rp:', result
yield [result]
def legalTest(rp):
(x, y) = rp
result = (0 <= x <= domainSize) and (0 <= y <= domainSize)
if not result: print 'not legal:', rp
return result
# TODO - combine variance and positions
actions = [
Action(name='Move',
parameters=[RPOS1, RVAR1, RPOS2, RVAR2],
condition = And(RobotPos(RPOS1),
RobotVar(RVAR1),
Odometry(RPOS1, RVAR1, RPOS2, RVAR2)),
effect = And(RobotPos(RPOS2),
RobotVar(RVAR2),
Not(RobotPos(RPOS1)),
Not(RobotVar(RVAR1)))),
Action(name='Look',
parameters=[RPOS1, RVAR1, BPOS, RVAR2],
condition = And(RobotPos(RPOS1),
RobotVar(RVAR1),
Sensor(RPOS1, RVAR1, BPOS, RVAR2)),
effect = And(RobotVar(RVAR2),
Not(RobotVar(RVAR1))))
]
axioms = [
]
# Conditional stream declarations
cond_streams = [
GeneratorStream(inputs = [],
outputs = [RPOS1],
conditions = [],
effects = [],
generator = randPos),
# TODO - the number of variances grows incredibly but we just want to consider variances possible with the move
# Each var only has one pos because moving changes
GeneratorStream(inputs = [RPOS1, RVAR1, RPOS2],
outputs = [RVAR2],
conditions = [LegalPosVar(RPOS1, RVAR1), NearbyPos(RPOS1, RPOS2)],
effects = [Odometry(RPOS1, RVAR1, RPOS2, RVAR2), LegalPosVar(RPOS2, RVAR2)],
generator = odoVarFun),
GeneratorStream(inputs = [RPOS1, RVAR1, BPOS],
outputs = [RVAR2],
conditions = [LegalPosVar(RPOS1, RVAR1)],
effects = [Sensor(RPOS1, RVAR1, BPOS, RVAR2), LegalPosVar(RPOS1, RVAR2)],
generator = sensorVarFun),
TestStream(inputs = [RPOS1, RPOS2],
conditions = [],
effects = [NearbyPos(RPOS1, RPOS2)],
test = lambda rp1, rp2: distance(rp1, rp2) < maxMoveDist,
eager = True),
TestStream(inputs = [RPOS1],
conditions = [],
effects = [NearGoalPos(RPOS1)],
test = lambda rp1: distance(rp1, homePos) < goalPosEps,
eager = True),
TestStream(inputs = [RVAR1],
conditions = [],
effects = [NearGoalVar(RVAR1)],
test = lambda a: a < goalVar,
eager = True)
# NOTE - the problem seems to be the fast that this is eager
]
####################
constants = [
BEACON(beaconPos),
#POS((.1, .1)),
#POS((3., .1)),
#POS((6., .1)),
POS(homePos),
]
initial_atoms = [
RobotPos(initRobotPos),
RobotVar(initRobotVar),
LegalPosVar(initRobotPos, initRobotVar), # TODO - I might as well keep track of pairs. Make this a legal on both
]
goal_formula = Exists([RPOS1, RVAR1],
And(RobotPos(RPOS1),
RobotVar(RVAR1),
LegalPosVar(RPOS1, RVAR1),
NearGoalPos(RPOS1),
NearGoalVar(RVAR1)))
problem = STRIPStreamProblem(initial_atoms, goal_formula, actions + axioms,
cond_streams, constants)
return problem
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
# 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
]
####################
# 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_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
# return sample_block_poses(blocks)
return block_poses
def not_in_region(target_block, block, l, ls, lg):
if ls == lg:
truth_val = False
if ls < lg:
truth_val = (l + block.w <= ls) or (l >= lg + target_block.w)
else:
truth_val = (l + block.w <= lg) or (l >= ls + target_block.w)
return truth_val
# these are types of parameters of the predicates? Or variables?
OBJECT, LOCATION, STOVE_L_S, STOVE_L_G, SINK_L_S, SINK_L_G = Type(), Type(
), Type(), Type(), Type(), Type()
# define predicates
AtPose = Pred('AtPose', [OBJECT, LOCATION])
InStove = Pred('InStove', [OBJECT])
InSink = Pred('InSink', [OBJECT])
Clean = Pred('Clean', [OBJECT])
Cooked = Pred('Cooked', [OBJECT])
EmptySweptVolume = Pred('EmptySweptVolume', [OBJECT, LOCATION, LOCATION])
# define static predicates
Contained = Pred('Contained', [OBJECT, LOCATION, LOCATION, LOCATION])
OutsideRegion = Pred('OutsideRegion',
[OBJECT, OBJECT, LOCATION, LOCATION, LOCATION])
IsStove = Pred('IsStove', [LOCATION, LOCATION])
def create_problem(p_init=0,
v_init=0,
a_init=0,
dt=.5,
max_a=10,
min_a=-10,
p_goal=10):
"""
Creates a 1D car STRIPStream problem.
https://github.com/KCL-Planning/SMTPlan/blob/master/benchmarks/car_nodrag/car_domain_nodrag.pddl
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
STATE, ACCEL, TIME = Type(), Type(), Type()
# Fluent predicates
AtState = Pred(STATE)
AtAccel = Pred(ACCEL)
NewTime = Pred()
# Fluent predicates
Running = Pred()
Stopped = Pred()
EngineBlown = Pred()
TransmissionFine = Pred()
GoalReached = Pred()
# Static predicates
Delta1 = Pred(ACCEL, ACCEL) # A2 - A1 = 1
Dynamics = Pred(STATE, ACCEL, STATE)
Contained = Pred(STATE)
# Free parameters
A1, A2 = Param(ACCEL), Param(ACCEL)
S1 = Param(STATE)
S2 = Param(STATE)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='accelerate',
parameters=[A1, A2],
condition=And(Running(), NewTime(), AtAccel(A1), Delta1(A1,
A2)),
effect=And(AtAccel(A2), Not(NewTime()), Not(AtAccel(A1)))),
Action(name='decelerate',
parameters=[A1, A2],
condition=And(Running(), NewTime(), AtAccel(A1), Delta1(A2,
A1)),
effect=And(AtAccel(A2), Not(NewTime()), Not(AtAccel(A1)))),
Action(name='simulate',
parameters=[S1, A1, S2],
condition=And(AtState(S1), AtAccel(A1), Dynamics(S1, A1, S2)),
effect=And(NewTime(), AtState(S2), Not(AtState(S1)))),
]
axioms = [
Axiom(effect=GoalReached(),
condition=Exists([S1], And(AtState(S1), Contained(S1)))),
]
####################
# Conditional stream declarations
cond_streams = [
FunctionStream(inputs=[S1, A1],
outputs=[S2],
conditions=[],
effects=[Dynamics(S1, A1, S2)],
function=lambda (p1, v1), a1:
(p1 + v1 * dt + .5 * a1 * dt**2, v1 + a1 * dt)),
GeneratorStream(inputs=[A1],
outputs=[A2],
conditions=[],
effects=[Delta1(A1, A2)],
generator=lambda a1: [a1 + 1]
if a1 + 1 <= max_a else []),
GeneratorStream(inputs=[A2],
outputs=[A1],
conditions=[],
effects=[Delta1(A2, A1)],
generator=lambda a2: [a2 + 1]
if a2 - 1 > -min_a else []),
TestStream(inputs=[S1],
conditions=[],
effects=[Contained(S1)],
test=lambda (p1, v1): p1 > p_goal,
eager=True),
]
####################
constants = []
initial_atoms = [
AtState((p_init, v_init)),
AtAccel(a_init),
NewTime(),
Running(),
]
goal_literals = [
GoalReached()
#AtAccel(0),
]
problem = STRIPStreamProblem(initial_atoms, goal_literals,
actions + axioms, cond_streams, constants)
return problem
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
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), 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
from stripstream.pddl.examples.belief.utils import *
from stripstream.pddl.examples.belief.unknown_no_occup import OPERATOR_MAP
from stripstream.pddl.utils import get_value
# NOTE - this is kind of in between forward and unknown
# The key difference is the computation of costs
UNIT = False
FOCUSED = True
COST_SCALE = 100 # TODO - need to adjust default cost
# TODO - can simulate by applying the belief space functions and passing a custom observation or result
# TODO - Encode pose in it
OBJ, LOC, BELIEF = Type(), Type(), Type()
PROB, CONCENTRATION = Type(), Type()
#concentration = CONCENTRATION(('i1', .95))
##########
UnknownAt = Predicate(OBJ)
At = Predicate(OBJ, LOC)
BAt = Predicate(OBJ, BELIEF)
BAtAbove = Predicate(OBJ, LOC, PROB) # TODO - should OBJ go in here?
BSatisfies = Predicate(BELIEF, LOC, PROB)
IsLookUpdate = Predicate(BELIEF, LOC, BELIEF)
IsMoveUpdate = Predicate(LOC, BELIEF, LOC, BELIEF)
def create_problem(goal, obstacles=(), distance=.25, digits=3):
"""
Creates a Probabilistic Roadmap (PRM) motion planning problem.
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
POINT = Type()
REGION = Type()
# Fluent predicates
AtPoint = Pred(POINT)
# Derived predicates
InRegion = Pred(REGION)
# Stream predicates
AreNearby = Pred(POINT, POINT)
IsEdge = Pred(POINT, POINT)
Contained = Pred(POINT, REGION)
# Functions
Distance = Func(POINT, POINT)
# Free parameters
P1, P2 = Param(POINT), Param(POINT)
R = Param(REGION)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
#STRIPSAction(name='move', parameters=[P1, P2],
# conditions=[AtPoint(P1), IsEdge(P1, P2)],
# effects=[AtPoint(P2), Not(AtPoint(P1))], cost=1), # Fixed cost
#STRIPSAction(name='move', parameters=[P1, P2],
# conditions=[AtPoint(P1), IsEdge(P1, P2)],
# effects=[AtPoint(P2), Not(AtPoint(P1)), Cost(Distance(P1, P2))]), # Cost depends on parameters
Action(name='move',
parameters=[P1, P2],
condition=And(AtPoint(P1), IsEdge(P1, P2)),
effect=And(AtPoint(P2), Not(AtPoint(P1))),
costs=[1, Distance(P1, P2)]),
]
axioms = [
#Axiom(effect=GoalReached(), condition=Exists([P1], And(AtPos(P1), Contained(P1)))),
STRIPSAxiom(conditions=[AtPoint(P1), Contained(P1, R)],
effects=[InRegion(R)])
]
####################
# Conditional stream declarations
cond_streams = [
GeneratorStream(
inputs=[],
outputs=[P1],
conditions=[],
effects=[],
generator=lambda: ((sample(digits), ) for _ in inf_sequence())
), # NOTE - version that only generators collision-free points
GeneratorStream(inputs=[R],
outputs=[P1],
conditions=[],
effects=[Contained(P1, R)],
generator=lambda r:
((sample_box(r), ) for _ in inf_sequence())),
#TestStream(inputs=[P1, P2], conditions=[], effects=[AreNearby(P1, P2), AreNearby(P2, P1)],
# test=lambda p1, p2: get_distance(p1, p2) <= distance, eager=True),
#TestStream(inputs=[P1, P2], conditions=[AreNearby(P1, P2)], effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
# test=lambda p1, p2: is_collision_free((p1, p2), obstacles), eager=True),
TestStream(
inputs=[P1, P2],
conditions=[],
effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
#test=lambda p1, p2: is_collision_free((p1, p2), obstacles), eager=True),
test=lambda p1, p2:
(get_distance(p1, p2) <= distance) and is_collision_free(
(p1, p2), obstacles),
eager=True),
#TestStream(inputs=[P1, P2], conditions=[], effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
# test=lambda p1, p2: get_distance(p1, p2) <= distance and is_collision_free((p1, p2), obstacles), eager=True),
#TestStream(inputs=[P1, R], conditions=[], effects=[Contained(P1, R)],
# test=lambda p, r: contains(p, r), eager=True),
CostStream(inputs=[P1, P2],
conditions=[],
effects=[Distance(P1, P2),
Distance(P2, P1)],
function=get_distance,
scale=100,
eager=True),
]
####################
constants = []
initial_atoms = [
AtPoint((0, 0)),
]
goal_literals = []
if is_region(goal):
goal_literals.append(InRegion(goal))
else:
goal_literals.append(AtPoint(goal))
problem = STRIPStreamProblem(initial_atoms, goal_literals,
actions + axioms, cond_streams, constants)
return problem
from toyTest import glob, makeOperators, Bd, ObjState, ObjLoc
import toyTest
OPERATOR_MAP = {
'find': make_look,
'inspect_loc': make_look_clear,
'inspect_state': make_look_state,
'transport': make_transport,
'wash': make_wash,
'paint': make_paint,
'dry': make_dry,
}
COST_SCALE = 10
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)
UnsureLoc = Predicate(OBJ) # NOTE - can also just make these objects
UnsureState = Predicate(OBJ)
UnsureClear = Predicate(LOC)
NotAtLoc = Predicate(OBJ, LOC)
def create_problem(goal, obstacles=(), distance=.25, digits=3):
"""
Creates a Probabilistic Roadmap (PRM) motion planning problem.
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
POINT = Type()
REGION = Type()
# Fluent predicates
AtPoint = Pred(POINT)
# Derived predicates
InRegion = Pred(REGION)
#IsReachable = Pred(POINT, POINT)
IsReachable = Pred(POINT)
# Stream predicates
IsEdge = Pred(POINT, POINT)
Contained = Pred(POINT, REGION)
# Free parameters
P1, P2 = Param(POINT), Param(POINT)
R = Param(REGION)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
Action(name='move',
parameters=[P1, P2],
condition=And(AtPoint(P1), IsReachable(P2)),
effect=And(AtPoint(P2), Not(AtPoint(P1))))
]
axioms = [
Axiom(effect=InRegion(R),
condition=Exists([P1], And(AtPoint(P1), Contained(P1, R)))),
Axiom(effect=IsReachable(P2),
condition=Or(AtPoint(P2),
Exists([P1], And(IsReachable(P1), IsEdge(P1,
P2))))),
]
####################
def sampler():
for _ in inf_sequence():
yield [(sample(digits), ) for _ in range(10)]
roadmap = set()
def test(p1, p2):
if not (get_distance(p1, p2) <= distance and is_collision_free(
(p1, p2), obstacles)):
return False
roadmap.add((p1, p2))
return True
####################
# Conditional stream declarations
cond_streams = [
EasyListGenStream(
inputs=[],
outputs=[P1],
conditions=[],
effects=[],
generator=sampler
), # NOTE - version that only generators collision-free points
GeneratorStream(inputs=[R],
outputs=[P1],
conditions=[],
effects=[Contained(P1, R)],
generator=lambda r:
(sample_box(r) for _ in inf_sequence())),
TestStream(inputs=[P1, P2],
conditions=[],
effects=[IsEdge(P1, P2), IsEdge(P2, P1)],
test=test,
eager=True),
]
####################
constants = []
initial_atoms = [
AtPoint((0, 0)),
]
goal_literals = []
if is_region(goal):
goal_literals.append(InRegion(goal))
else:
goal_literals.append(AtPoint(goal))
problem = STRIPStreamProblem(initial_atoms, goal_literals,
actions + axioms, cond_streams, constants)
return problem, roadmap
def create_problem(p_init=0, v_init=0, a_init=0,
dt=.5, max_a=10, min_a=-10, p_goal=10):
"""
Creates a 1D car STRIPStream problem.
https://github.com/KCL-Planning/SMTPlan/blob/master/benchmarks/car_nodrag/car_domain_nodrag.pddl
:return: a :class:`.STRIPStreamProblem`
"""
# Data types
POS, VEL, ACCEL, TIME = Type(), Type(), Type(), Type()
# Fluent predicates
AtPos = Pred(POS)
AtVel = Pred(VEL)
AtAccel = Pred(ACCEL)
NewTime = Pred()
# Fluent predicates
Running = Pred()
Stopped = Pred()
EngineBlown = Pred()
TransmissionFine = Pred()
GoalReached = Pred()
# Static predicates
Delta1 = Pred(ACCEL, ACCEL) # A2 - A1 = 1
Dynamics = Pred(POS, VEL, ACCEL, POS, VEL)
Contained = Pred(POS)
# Free parameters
A1, A2 = Param(ACCEL), Param(ACCEL)
P1, V1 = Param(POS), Param(VEL)
P2, V2 = Param(POS), Param(VEL)
rename_easy(locals()) # Trick to make debugging easier
####################
actions = [
#Action(name='accelerate', parameters=[A1, A2],
# #condition=And(Running(), NewTime(), AtAccel(A1), Delta1(A1, A2)),
# condition=And(NewTime(), AtAccel(A1), Delta1(A1, A2)),
# effect=And(AtAccel(A2), Not(NewTime()), Not(AtAccel(A1))), cost=5),
STRIPSAction(name='accelerate', parameters=[A1, A2],
conditions=[NewTime(), AtAccel(A1), Delta1(A1, A2)],
effects=[AtAccel(A2), Not(NewTime()), Not(AtAccel(A1))], cost=5),
#Action(name='decelerate', parameters=[A1, A2],
# condition=And(Running(), NewTime(), AtAccel(A1), Delta1(A2, A1)),
# condition=And(NewTime(), AtAccel(A1), Delta1(A2, A1)),
# effect=And(AtAccel(A2), Not(NewTime()), Not(AtAccel(A1)))),
STRIPSAction(name='decelerate', parameters=[A1, A2],
conditions=[NewTime(), AtAccel(A1), Delta1(A2, A1)],
effects=[AtAccel(A2), Not(NewTime()), Not(AtAccel(A1))]),
#Action(name='simulate', parameters=[P1, V1, A1, P2, V2],
# condition=And(AtPos(P1), AtVel(V1), AtAccel(A1), Dynamics(P1, V1, A1, P2, V2)),
# effect=And(NewTime(), AtPos(P2), AtVel(V2), Not(AtPos(P1)), Not(AtVel(V1))), cost=1),
STRIPSAction(name='simulate', parameters=[P1, V1, A1, P2, V2],
conditions=[AtPos(P1), AtVel(V1), AtAccel(A1), Dynamics(P1, V1, A1, P2, V2)],
effects=[NewTime(), AtPos(P2), AtVel(V2), Not(AtPos(P1)), Not(AtVel(V1))], cost=1),
#STRIPSAction(name='goal', parameters=[P1],
# conditions=[AtPos(P1), Contained(P1)],
# effects=[GoalReached()], cost=None),
]
axioms = [
#Axiom(effect=GoalReached(), condition=Exists([P1], And(AtPos(P1), Contained(P1)))),
STRIPSAxiom(conditions=[AtPos(P1), Contained(P1)], effects=[GoalReached()])
]
####################
# Conditional stream declarations
cond_streams = [
FunctionStream(inputs=[P1, V1, A1], outputs=[P2, V2], conditions=[], effects=[Dynamics(P1, V1, A1, P2, V2)],
function=lambda p1, v1, a1: (p1 + v1*dt + .5*a1*dt**2, v1 + a1*dt)),
FunctionStream(inputs=[A1], outputs=[A2], conditions=[], effects=[Delta1(A1, A2)],
function=lambda a1: (a1+1,) if a1+1 <= max_a else None),
#GeneratorStream(inputs=[A1], outputs=[A2], conditions=[], effects=[Delta1(A1, A2)],
# generator=lambda a1: [a1+1] if a1+1 <= max_a else []),
FunctionStream(inputs=[A2], outputs=[A1], conditions=[], effects=[Delta1(A2, A1)],
function=lambda a2: (a2+1,) if a2-1 >= min_a else None),
#GeneratorStream(inputs=[A2], outputs=[A1], conditions=[], effects=[Delta1(A2, A1)],
# generator=lambda a2: [a2+1] if a2-1 >- min_a else []),
TestStream(inputs=[P1], conditions=[], effects=[Contained(P1)],
test=lambda p1: p1 >= p_goal, eager=True),
]
####################
constants = []
initial_atoms = [
AtPos(p_init),
AtVel(v_init),
AtAccel(a_init),
NewTime(),
#Running(),
]
goal_literals = [
GoalReached(),
AtAccel(0),
]
problem = STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms, cond_streams, constants)
return problem
from stripstream.pddl.logic.connectives import Not, Or, And from stripstream.pddl.logic.quantifiers import Exists, ForAll from stripstream.pddl.logic.atoms import Equal, Atom from stripstream.pddl.operators import Action, Axiom from stripstream.utils import irange from stripstream.pddl.utils import rename_easy from stripstream.pddl.problem import STRIPStreamProblem from stripstream.pddl.utils import get_value from stripstream.pddl.cond_streams import EasyGenStream, EasyTestStream from stripstream.pddl.objects import EasyType as Type, EasyParameter as Param from stripstream.pddl.logic.predicates import EasyPredicate as Pred EAGER_TESTS = True ROBOT_ROW = -1 CONF, BLOCK, POSE = Type(), Type(), Type() AtConf = Pred(CONF) AtPose = Pred(BLOCK, POSE) HandEmpty = Pred() Holding = Pred(BLOCK) Safe = Pred(BLOCK, POSE) LegalKin = Pred(POSE, CONF) CollisionFree = Pred(POSE, POSE) rename_easy(locals())
from stripstream.pddl.logic.quantifiers import Exists
from stripstream.pddl.logic.predicates import Predicate
from stripstream.pddl.objects import Parameter, Constant, NamedObject, Type, HashableObject
from stripstream.pddl.operators import STRIPSAction, STRIPSAxiom, Axiom
from stripstream.pddl.streams import GeneratorStream, TestStream, FunctionStream
from stripstream.pddl.cond_streams import CondStream, ConstCondStream, TestCondStream
from stripstream.pddl.problem import STRIPStreamProblem
from stripstream.pddl.examples.continuous_tamp.continuous_tamp_viewer import ContinuousTMPViewer
P = Parameter
C = Constant
USE_BASE = True
EAGER_TESTS = True
COLLISIONS = True
CONFIG = Type('conf')
BLOCK = Type('block')
POSE = Type('pose')
REGION = Type('region')
AtConfig = Predicate('at_config', [CONFIG])
HandEmpty = Predicate('hand_empty')
AtPose = Predicate('at_pose', [BLOCK, POSE])
Holding = Predicate('holding', [BLOCK])
Safe = Predicate('safe', [BLOCK, BLOCK, POSE])
InRegion = Predicate('in_region', [BLOCK, REGION])
#IsPose = Predicate('is_pose', [BLOCK, POSE]) # TODO - verify that the pose is within the interval
IsIK = Predicate('is_ik', [POSE, CONFIG])
IsCollisionFree = Predicate('is_collision_free', [BLOCK, POSE, BLOCK, POSE])