def get_valid_predicate(self, operator):
name = '_'.join([self.valid_prefix, self.name, operator.name])
parameters = [
param for param in self.parameters if param in operator.parameters
]
return Predicate(name, [param.type
for param in parameters]) # TODO - save this?
def rename_variables(assignments):
for name, value in assignments.iteritems():
if type(value) == VarType:
value.name = name.lower()
elif type(value) == ConstraintType:
value.name = name.lower()
value.predicate = Predicate(value.name, value.types)
elif type(value) == FreeParameter:
FreeParameter.__init__(value, name, value.type)
def __init__(self, types=(), test=None, domain=(), satisfying=(), name=None, **kwargs):
self.types = tuple(types)
self.test = test
self.n = ConstraintType.num
ConstraintType.num += 1
self.name = name if name is not None else self.prefix % self.n
self.domain = tuple(domain)
self.satisfying = tuple(satisfying)
self.predicate = Predicate(self.name, self.types)
self.kwargs = kwargs
def __init__(
self,
refinement,
partial_orders=set(),
eager=False
): # TODO - be careful about named arguments and which ones match
self.refinement = refinement # NOTE - only allowing a single refinement. Can make multiple operator copies if there is another
self.partial_orders = partial_orders # TODO - partial order constraints for sequence refinement?
self.eager = eager
assert not isinstance(self, STRIPSAction) or self.is_consistent()
self.RefinedPredicate = Predicate(
'_'.join([self.refined_prefix, self.name]),
[arg.type for arg in self.parameters])
def create_axiom(con_form, var_names, params, inputs, var_map, axiom_map):
key = (con_form, tuple(var_names), tuple(params), tuple(inputs))
if key not in axiom_map:
conditions = []
for name, nested in zip(var_names, params):
if name is not None:
conditions.append(var_map[name].predicate(*nested))
con_params = [nested[-1] for nested in params]
conditions.append(con_form.predicate(*con_params))
DerPred = Predicate('_der_%s' % len(axiom_map), [
par.type for par in inputs])
effect = DerPred(*inputs)
axiom_map[key] = STRIPSAxiom(conditions, [effect])
return axiom_map[key].effects[0]
def __init__(self,
types=(),
test=None,
domain=(),
satisfying=(),
name=None,
**kwargs):
self.types = tuple(types)
self.test = test
self.n = ConstraintType.num
ConstraintType.num += 1
self.name = name if name is not None else self.prefix % self.n
self.domain = tuple(
domain
) # TODO - how do I specify the domain constraints if I don't have variables?
self.satisfying = tuple(satisfying)
self.predicate = Predicate(self.name, self.types)
self.kwargs = kwargs
from math import ceil
from stripstream.algorithms.plan import substitute_axioms
from stripstream.pddl.logic.atoms import Atom, Cost, Initialize
from stripstream.pddl.logic.connectives import And
from stripstream.pddl.logic.predicates import Predicate, Function
from stripstream.pddl.objects import Constant, OBJECT, Object
from stripstream.utils import INF
Concrete = Predicate('_concrete', [OBJECT])
FUNCTION_TEMPLATE = '_%s_cost'
def is_concrete(atom):
return atom.predicate is Concrete
def constant_from_concrete(atom):
assert is_concrete(atom)
[constant] = atom.args
return constant
def partition_values(values):
atoms, objects = set(), set()
for atom in values:
if isinstance(atom, Atom):
objects.update(atom.args)
if not is_concrete(atom):
atoms.add(atom)
for obj in values:
def __init__(self, name, dtype, args=[]):
self.name = name
self.dtype = dtype
self.args = args
assert all(arg.domain is not None for arg in args)
self.predicate = Predicate('%s_at' % name, args + [dtype])
EAGER_TESTS = True
EAGER_SAMPLE = False
EAGER_COLLISION = False
P = Parameter
C = Constant
CONF = EuclideanType('conf', norm=lambda a, b: 0)
STEER = EuclideanType(
'steer', norm=lambda a, b: 0
) # NOTE - not used within the planner, but guide the expansion
REGION = FiniteType('region')
OBSTACLE = FixedType('obs', [])
AtConfig = Predicate('at_conf', [CONF])
IsCollisionFree = Predicate('is_collision_free', [CONF, CONF])
InRegion = Predicate('in_region', [CONF])
MoveCost = Function('move_cost', [CONF, CONF])
##################################################
# TODO - specialized PRM planner that only connects to new states and dynamically represents the state-space
# TODO - specialized RRT planner which takes advantage of metric space to do things
##################################################
class Conf(Constant):
dictionary = {}
def equal(var, value): # TODO - function that produces these atoms?
predicate = Predicate(var.name + '_eq', [var])
return predicate(value)
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)
FindCost = Function('find_cost', [OBJ, LOC])
InspectLocCost = Function('inspect_loc_cost', [LOC])
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)
#IsBelief = Predicate(OBJ, BELIEF)
LookCost = Function('look_cost', [BELIEF, LOC])
MoveCost = Function('move_cost', [LOC, BELIEF])
InferAtCost = Function('infer_at_cost', [OBJ, LOC])
##########
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])
####################
#Holding = Predicate('holding', [BLOCK]) # NOTE - used to use an axiom, but now I update it explicitly
InRegion = Predicate('in_region', [BLOCK, REGION])
Safe = Predicate('safe', [BLOCK, TRAJ])
####################
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 stripstream.pddl.examples.belief.utils import *
from stripstream.pddl.examples.belief.unknown import OPERATOR_MAP
from toyTest import glob, makeOperators, Bd, ObjState, ObjLoc
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
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
rename_easy(locals())
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])
IsContained = Predicate('is_contained', [BLOCK, POSE, REGION])
##################################################
# TODO - make a hashable object dictionary for each of these
CONFIG = Type('conf')
def assign(var, value): # TODO - function that produces these atoms?
return var.name + '_eq_' + value
def equal(var, value): # TODO - function that produces these atoms?
predicate = Predicate(var.name + '_eq', [var])
return predicate(value)
Assign = Predicate('assign', [OBJECT, OBJECT])
Equal = Predicate('equal',
[OBJECT, OBJECT]) # TODO - make a separate one per variable?
obj_params = {} # NOTE - I can either construct these online or ahead of time
"""
class Move(object):
#def __init__(self, objects):
def __init__(self, start, end): # Start and end maps from type to object
self.constraints = [
Motion(Q1, Q2, T),
Equal(H1, H2),
Equal(H1, 0),
] + [
Equal(o1, o2) for o in objects
]
#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])
AtOrbit = Predicate('at_orbit', [SATELLITE, POSITION])
Carrying = Predicate('carrying', [SATELLITE])
Flying = Predicate('flying', [])
Landed = Predicate('landed', [])
######
#IsBurst = Predicate('is_burst', [POSITION, VELOCITY, ACCELERATION, TIME, POSITION, VELOCITY]) # Alternatively constant of zero
IsBurst = Predicate('is_burst', [STATE, ACCELERATION, TIME, STATE])
IsGlide = Predicate('is_glide', [STATE, TIME, STATE])
#IsFloat = Predicate('at_state', [POSITION, VELOCITY, TIME, POSITION, VELOCITY])
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.examples.countable_tamp.countable_tamp_viewer import CountableTMPViewer
from stripstream.pddl.examples.countable_tamp.countable_tamp_utils import NUM_POSES
P = Parameter
C = Constant
BASE = True
COLLISIONS = True
ACTION_COST = 1
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])
Unsafe = Predicate('unsafe', [POSE]) # The opposite of clear
Safe = Predicate('safe', [BLOCK, POSE])
InRegion = Predicate('in_region', [BLOCK, REGION])
IsBlock = Predicate('is_block', [BLOCK])
IsPose = Predicate('is_pose', [POSE])
IsConfig = Predicate('is_config', [CONFIG])
IsIK = Predicate('is_ik', [POSE, CONFIG])
IsCollisionFree = Predicate('is_collision_free', [POSE, POSE])
IsContained = Predicate('is_contained', [REGION, POSE])
