Author: jbinney
Date: 2012-05-04 15:01:00 -0700 (Fri, 04 May 2012)
New Revision: 54625
Added:
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hpn.py
Removed:
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/actions.py
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_belief.py
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_object.py
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_table.py
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_world.py
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/planner.py
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/viz_mpl.py
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hip/
Modified:
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/__init__.py
branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/dot_graph.py
Log:
cleaned up hierarchical planning code and added example
Modified: branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/__init__.py
===================================================================
--- branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hip/__init__.py 2012-05-03 02:51:42 UTC (rev 54611)
+++ branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/__init__.py 2012-05-04 22:01:00 UTC (rev 54625)
@@ -1,4 +1,4 @@
-from hip.common import Fluent, ConjunctionOfFluents, AbstractionInfo, Operator, OperatorInstance, HPlanTree, Predicate
-from hip.planner import hplan
-from hip.dot_graph import dot_from_plan_tree
-from hip.exceptions import PlanningFailedError
+from hierarchical_interactive_planning.common import Fluent, ConjunctionOfFluents, AbstractionInfo, Operator, OperatorInstance, HPlanTree, Predicate
+from hierarchical_interactive_planning.hpn import hpn
+from hierarchical_interactive_planning.dot_graph import dot_from_plan_tree
+from hierarchical_interactive_planning.exceptions import PlanningFailedError
Deleted: branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/actions.py
===================================================================
--- branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hip/actions.py 2012-05-03 02:51:42 UTC (rev 54611)
+++ branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/actions.py 2012-05-04 22:01:00 UTC (rev 54625)
@@ -1,21 +0,0 @@
-class FindObjectOfType(HIPOperator):
- def __init__(self, object_class, except_object_ids):
- '''Find an object of the given class (but not one of the objects
- in the given set of object ids).
- '''
-
- def refinements(self, belief):
- self.preconditions = []
- n = NumberOfKnownObjectsOfType(object_type).evaluate(belief)
- self.effects = NumberOfKnownObjectsOfType(object_type) == (n + 1) or NumberOfExistingObjectsOfType(object_type) == n
- self.except_object_ids = except_object_ids
-
- sub_actions = []
- for table in world.get_objects_of_type('table'):
- sub_actions.append(FindObjectOfTypeOnTable(belief, object_type, table, except_object_ids))
- return sub_actions
-
-class FindObjectOfTypeOnTable(HIPOperator):
- def __init__(self, belief, object_type, table, except_object_ids):
- self.preconditions = []
- self.effects
Modified: branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/dot_graph.py
===================================================================
--- branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hip/dot_graph.py 2012-05-03 02:51:42 UTC (rev 54611)
+++ branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/dot_graph.py 2012-05-04 22:01:00 UTC (rev 54625)
@@ -1,4 +1,4 @@
-from hip import ConjunctionOfFluents, OperatorInstance
+from hierarchical_interactive_planning import ConjunctionOfFluents, OperatorInstance
import pygraphviz as pgv
lpk_style = dict(
@@ -24,7 +24,7 @@
G = pgv.AGraph(strict=True, directed=True)
if style is None:
- style = garish_style
+ style = lpk_style
G.add_node(id(tree), label=str(tree.goal), **style['plan_goal'])
if not tree.plan == None:
Deleted: branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_belief.py
===================================================================
--- branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hip/hip_belief.py 2012-05-03 02:51:42 UTC (rev 54611)
+++ branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_belief.py 2012-05-04 22:01:00 UTC (rev 54625)
@@ -1,30 +0,0 @@
-class HIPBelief:
- def __init__(self):
- self._objects = {}
-
- def add_object(self, obj):
- self._objects[self._gen_unique_id()] = obj
-
- def get_objects_of_type(self, object_type):
- return set([obj for obj in self._objects if obj.object_type == object_type])
-
- def _gen_unique_id(self):
- if len(self._objects) == 0:
- return 0
- else:
- return max(self._objects) + 1
-
-class ObjectOnTablePositionBelief:
- def __init__(self, tables):
- self.table_grids = {}
- for table in tables:
- self.table_grids[table] = np.zeros(np.ceil(table.size / self.res), dtype=np.bool)
-
- def point_to_grid(self, p):
- return tuple(np.floor(p / self.res))
-
- def grid_to_point(self, ind):
- p = np.zeros(3)
- p[:2] = (np.array(ind) + 0.5) * self.res
- return p
-
Deleted: branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_object.py
===================================================================
--- branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hip/hip_object.py 2012-05-03 02:51:42 UTC (rev 54611)
+++ branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_object.py 2012-05-04 22:01:00 UTC (rev 54625)
@@ -1,5 +0,0 @@
-class HIPObject:
- def __init__(self, object_type, readable_name):
- self.object_type = object_type
- self.readable_name = readable_name
-
Deleted: branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_table.py
===================================================================
--- branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hip/hip_table.py 2012-05-03 02:51:42 UTC (rev 54611)
+++ branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_table.py 2012-05-04 22:01:00 UTC (rev 54625)
@@ -1,26 +0,0 @@
-import numpy as np
-from hip_object import HIPObject
-
-from pr2_python.geom import Transform
-
-class HIPTable(HIPObject):
- def __init__(self, readable_name, transform, size, res=0.1):
- '''Class that describes a table.
- Args:
- readable_name (str): Human readable object name.
- transform (pr2_python.geom.Transform): Transform from table to world coords.
- size ([x_size, y_size]): Table size in meters.
- delta (float): Discretization granularity in meters of sensed array.
- '''
- HIPObject.__init__(self, 'table', readable_name)
- self.transform = transform
- self.size = np.array(size)
- self.res = res
-
-
- def table_to_world(self, p_table):
- return self.transform.transform_point(p_table)
-
- def world_to_table(self, p_world):
- return self.transform.inverse().transform_point(p_world)
-
Deleted: branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_world.py
===================================================================
--- branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hip/hip_world.py 2012-05-03 02:51:42 UTC (rev 54611)
+++ branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hip_world.py 2012-05-04 22:01:00 UTC (rev 54625)
@@ -1,15 +0,0 @@
-class HIPWorld:
- def __init__(self):
- self._objects = {}
-
- def add_object(self, obj):
- self._objects[self._gen_unique_id()] = obj
-
- def get_objects_of_type(self, object_type):
- return set([obj for obj in self._objects if obj.object_type == object_type])
-
- def _gen_unique_id(self):
- if len(self._objects) == 0:
- return 0
- else:
- return max(self._objects) + 1
Copied: branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hpn.py (from rev 54611, branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hip/planner.py)
===================================================================
--- branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hpn.py (rev 0)
+++ branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/hpn.py 2012-05-04 22:01:00 UTC (rev 54625)
@@ -0,0 +1,106 @@
+import numpy as np
+from hierarchical_interactive_planning import AbstractionInfo, ConjunctionOfFluents, HPlanTree
+from hierarchical_interactive_planning.exceptions import PlanningFailedError
+from hierarchical_interactive_planning.a_star import a_star
+
+def hpn(operators, current_state, goal, world, abs_info=None, maxdepth=np.inf, depth=0, tree=None):
+ '''Implements HPN (Hierarchical Planning in the Now) algorithm of Kaelbing and Lozano-Perez.
+
+ Args:
+ operators (list of Operator): Operators to use in the planning.
+ current_state (ConjunctionOfFluents): Current state, represeneted as a conjunction of fluents.
+ goal (ConjunctionOfFluents): Goal state, described as conjunction of fluents.
+ world: Defined by the domain; used to execute operator instances and get the new state of the world.
+ abs_info: (AbstractionInfo): Keeps track of abstraction level of the operators. Typically not passed in
+ at the top level call.
+ maxdepth (int): Max allowed depth of the planning hierarchy.
+ depth (int): Current depth of the planning hierarchy.
+ tree (HPlanTree): Data structure representing the hierarchical planning tree. Updated as this
+ function recurses, and can be used to visualize the resulting plan.
+ '''
+ if depth > maxdepth:
+ raise RuntimeError('Max recursion depth exceeded')
+
+ if abs_info is None:
+ abs_info = AbstractionInfo()
+
+ plan = a_star(
+ goal, # start from the goal and work backwards
+ lambda s: world.entails(s), # we are done when we reach the current state
+ lambda s: applicable_ops(operators, current_state, s, abs_info), # actions
+ lambda s: num_violated_fluents(world, s) # heuristic
+ )
+ if plan is None:
+ raise PlanningFailedError('A* could not find a plan')
+ plan.reverse()
+
+ tree.plan = []
+ for op, subgoal in plan:
+ tree.plan.append((op, HPlanTree(subgoal)))
+
+ for op, subtree in tree.plan:
+ subgoal = subtree.goal
+
+ if op is None:
+ pass
+ elif op.concrete:
+ print 'Executing:', op
+ current_state = world.execute(op)
+ else:
+ abs_info.inc_abs_level(op.target)
+ hpn(operators, current_state, subgoal, world, abs_info.copy(), maxdepth, depth+1, subtree)
+
+def plan_flat(operators, current_state, goal):
+ '''Uses goal regression to plan without any hierarchy. Useful for testing.
+ '''
+ class ConcreteAbs:
+ def get_abs_level(self, f):
+ return np.inf
+
+ class ZeroAbs:
+ def get_abs_level(self, f):
+ return 0
+
+ plan = a_star(
+ goal, # start from the goal and work backwards
+ lambda s: current_state.entails(s), # we are done when we a state that is already true
+ lambda s: applicable_ops(operators, current_state, s, ConcreteAbs()), # actions
+ lambda s: num_violated_fluents(current_state, s) # heuristic
+ )
+ if plan == None:
+ return None
+ return list(reversed(plan))
+
+def applicable_ops(operators, current_state, goal, abs_info):
+ for op in operators:
+ for op_inst in op.gen_instances(current_state, goal, abs_info):
+ subgoal = regress(goal, op_inst)
+ if subgoal:
+ yield op_inst, subgoal, 1 # cost fixed to 1 for all ops right now
+
+def num_violated_fluents(world, subgoal):
+ '''Computes the "distance" between a conjunction of fluents and
+ the start conjunction of fluents.
+ '''
+ return sum([(not world.entails(f)) for f in subgoal.fluents])
+
+def regress(g, o):
+ '''Constructs the weakest preimage state from which the given operator
+ would achieve all of the fluents in the given state.
+
+ Returns:
+ g_pre (ConjunctionOfFluents or None): Weakest preimage, or None if no preimage exists.
+ '''
+ if o.target.contradicts(g) or o.side_effects.contradicts(g):
+ return None
+
+ g_pre = []
+ for f_g in g.fluents:
+ if not (o.target.entails(f_g) or o.side_effects.entails(f_g)):
+ g_pre.append(f_g)
+
+ for f in o.preconditions.fluents:
+ if f not in g_pre:
+ g_pre.append(f)
+
+ return ConjunctionOfFluents(g_pre)
Deleted: branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/planner.py
===================================================================
--- branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hip/planner.py 2012-05-03 02:51:42 UTC (rev 54611)
+++ branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/planner.py 2012-05-04 22:01:00 UTC (rev 54625)
@@ -1,89 +0,0 @@
-import numpy as np
-from hip import AbstractionInfo, ConjunctionOfFluents, HPlanTree
-from hip.exceptions import PlanningFailedError
-from hip.a_star import a_star
-
-def hplan(operators, current_state, goal, world, abs_info=None, maxdepth=np.inf, depth=0, tree=None):
- if depth > maxdepth:
- raise RuntimeError('Max recursion depth exceeded')
-
- if abs_info is None:
- abs_info = AbstractionInfo()
-
- plan = a_star(
- goal, # start from the goal and work backwards
- lambda s: world.entails(s), # we are done when we reach the current state
- lambda s: applicable_ops(operators, current_state, s, abs_info), # actions
- lambda s: num_violated_fluents(world, s) # heuristic
- )
- if plan is None:
- raise PlanningFailedError('A* could not find a plan')
- plan.reverse()
-
- tree.plan = []
- for op, subgoal in plan:
- tree.plan.append((op, HPlanTree(subgoal)))
-
- for op, subtree in tree.plan:
- subgoal = subtree.goal
- if op is None:
- pass
- elif op.concrete:
- print 'Executing:', op
- current_state = world.execute(op)
- else:
- abs_info.inc_abs_level(op.target)
- hplan(operators, current_state, subgoal, world, abs_info.copy(), maxdepth, depth+1, subtree)
-
-def plan_flat(operators, current_state, goal):
- class ConcreteAbs:
- def get_abs_level(self, f):
- return np.inf
-
- class ZeroAbs:
- def get_abs_level(self, f):
- return 0
-
- plan = a_star(
- goal, # start from the goal and work backwards
- lambda s: current_state.entails(s), # we are done when we a state that is already true
- lambda s: applicable_ops(operators, current_state, s, ConcreteAbs()), # actions
- lambda s: num_violated_fluents(current_state, s) # heuristic
- )
- if plan == None:
- return None
- return list(reversed(plan))
-
-def applicable_ops(operators, current_state, goal, abs_info):
- for op in operators:
- for op_inst in op.gen_instances(current_state, goal, abs_info):
- subgoal = regress(goal, op_inst)
- if subgoal:
- yield op_inst, subgoal, 1 # cost fixed to 1 for all ops right now
-
-def num_violated_fluents(world, subgoal):
- '''Computes the "distance" between a conjunction of fluents and
- the start conjunction of fluents.
- '''
- return sum([(not world.entails(f)) for f in subgoal.fluents])
-
-def regress(g, o):
- '''Constructs the weakest preimage state from which the given operator
- would achieve all of the fluents in the given state.
-
- Returns:
- g_pre (ConjunctionOfFluents or None): Weakest preimage, or None if no preimage exists.
- '''
- if o.target.contradicts(g) or o.side_effects.contradicts(g):
- return None
-
- g_pre = []
- for f_g in g.fluents:
- if not (o.target.entails(f_g) or o.side_effects.entails(f_g)):
- g_pre.append(f_g)
-
- for f in o.preconditions.fluents:
- if f not in g_pre:
- g_pre.append(f)
-
- return ConjunctionOfFluents(g_pre)
Deleted: branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/viz_mpl.py
===================================================================
--- branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hip/viz_mpl.py 2012-05-03 02:51:42 UTC (rev 54611)
+++ branches/trunk_electric/electric_sandbox/hierarchical_interactive_planning/src/hierarchical_interactive_planning/viz_mpl.py 2012-05-04 22:01:00 UTC (rev 54625)
@@ -1,37 +0,0 @@
-import numpy as np
-from matplotlib import pyplot as plt
-from hip.hip_table import HIPTable
-
-from pr2_python.geom import Transform
-
-class VizMPL:
- def __init__(self):
- self._fig = plt.figure()
- self._ax = self._fig.add_subplot(111)
-
- def draw(self, world):
- for obj in world._objects.values():
- if isinstance(obj, HIPTable):
- self.draw_table(obj)
- else:
- print 'Dont know how to draw', obj
-
- def draw_table(self, table):
- sx, sy = table.size
- points_table_frame = [(0.0, 0.0), (sx, 0.0), (sx, sy), (0.0, sy), (0.0, 0.0)]
- points = []
- for x_table, y_table in points_table_frame:
- x, y, z = table.transform.transform_point(np.array((x_table, y_table, 0.0)))
- points.append((x, y))
- points = np.array(points)
- self._ax.plot(points[:,0], points[:,1], 'g-')
-
- for ind in np.reshape(np.indices(table.viewed_grid.shape).T, (-1, 2)):
- if table.viewed_grid[tuple(ind)]:
- c = 'r'
- else:
- c = 'b'
- p_table = table.grid_to_point(ind)
- p_world = table.table_to_world(p_table)
- print p_world
- self._ax.plot([p_world[0]], [p_world[1]], c+'o')
_______________________________________________
Wg-ros-pkg-commits mailing list
Wg-ros-pkg-commits@code.ros.org
https://code.ros.org/mailman/listinfo/wg-ros-pkg-commits
(text/plain)
