def __init__(self, conditions, effects):
"""
:param conditions: a list of :class:`.Atom` or :class:`.Not`
:param effects: a list of :class:`.Atom` or :class:`.Not`
"""
if len(effects) != 1:
raise NotImplementedError(
'Currently only support a single effect: %s' % effects)
if not all(is_literal(c) for c in conditions):
raise ValueError('Conditions must all be literals: %s' %
conditions)
if not all(is_literal(e) for e in effects):
raise ValueError('Effects must all be literals: %s' % effects)
self.conditions, self.effects = conditions, effects
self.free_parameters = list(
set(flatten(c.get_parameters() for c in conditions)) -
set(effects[0].get_parameters()))
super(STRIPSAxiom,
self).__init__(effects[0],
Exists(self.free_parameters, And(*conditions)))
def dantam_distract(env, n_obj):
env.Load(os.path.join(ENVIRONMENTS_DIR, 'empty.xml'))
m, n = 3, 3
side_dim = .07
height_dim = .1
box_dims = (side_dim, side_dim, height_dim)
separation = (side_dim, side_dim)
coordinates = list(product(range(m), range(n)))
assert n_obj <= len(coordinates)
obj_coordinates = sample(coordinates, n_obj)
length = m * (box_dims[0] + separation[0])
width = n * (box_dims[1] + separation[1])
height = .7
table = box_body(env, 'table', length, width, height, color=TAN)
set_pose(table, pose_from_quat_point(unit_quat(), np.array([0, 0, 0])))
env.Add(table)
robot = env.GetRobots()[0]
set_manipulator_conf(robot.GetManipulator('leftarm'), TOP_HOLDING_LEFT_ARM)
open_gripper(robot.GetManipulator('leftarm'))
set_manipulator_conf(robot.GetManipulator('rightarm'),
mirror_arm_config(robot, REST_LEFT_ARM))
close_gripper(robot.GetManipulator('rightarm'))
robot.SetDOFValues([.15], [robot.GetJointIndex('torso_lift_joint')])
set_base_conf(robot, (-.75, .2, -math.pi / 2))
poses = []
z = height + SURFACE_Z_OFFSET
for r in range(m):
row = []
x = -length / 2 + (r + .5) * (box_dims[0] + separation[0])
for c in range(n):
y = -width / 2 + (c + .5) * (box_dims[1] + separation[1])
row.append(Pose(pose_from_quat_point(
unit_quat(), np.array([x, y, z]))))
poses.append(row)
initial_poses = {}
goal_poses = {}
for i, (r, c) in enumerate(obj_coordinates):
row_color = np.zeros(4)
row_color[2 - r] = 1.
if i == 0:
name = 'goal%d-%d' % (r, c)
color = BLUE
goal_poses[name] = poses[m / 2][n / 2]
else:
name = 'block%d-%d' % (r, c)
color = RED
initial_poses[name] = poses[r][c]
obj = box_body(env, name, *box_dims, color=color)
set_pose(obj, poses[r][c].value)
env.Add(obj)
known_poses = list(flatten(poses))
object_names = initial_poses.keys()
known_grasps = list(
map(Grasp, top_grasps(env.GetKinBody(object_names[0]))))
return ManipulationProblem(object_names=object_names, table_names=[table.GetName()],
goal_poses=goal_poses, initial_poses=initial_poses, known_poses=known_poses, known_grasps=known_grasps)
def get_literals(self):
return flatten(f.get_literals() for f in self.formulas)
def delete(self, atoms, constants):
return flatten(
formula.delete(atoms, constants) for formula in self.formulas)
def add(self, atoms, constants):
return flatten(
formula.add(atoms, constants) for formula in self.formulas)
def convert_axioms_to_effects(self):
derived_predicates = self.get_derived_predicates()
fluent_predicates = self.get_fluent_predicates()
constants = defaultdict(set)
for const in self.get_constants():
constants[const.type].add(const)
mapping = []
for op in self.operators:
if isinstance(op, Axiom):
[conditions
] = op.condition.dequantify(constants).get_literals()
assert not filter(lambda a: a.predicate in derived_predicates,
conditions)
print conditions
[fluent] = filter(lambda a: a.predicate in fluent_predicates,
conditions)
others = filter(lambda a: a != fluent, conditions)
mapping.append((fluent, others, op.effect))
new_operators = []
for op in self.operators:
if isinstance(op, Action):
new_op = op.clone()
new_operators.append(new_op)
[literals] = new_op.effect.get_literals()
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):
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
]
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))))
return new_operators
def get_parameters(self):
return set(flatten(atom.get_parameters() for atom in self.get_atoms()))
def get_objects(self):
return set(flatten(atom.args for atom in self.get_atoms()))
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