Lis spot/wmcclinton lin demo

domain.pddl

@@ -0,0 +1,55 @@
+(define (domain mydomain)
+  (:requirements :typing)
+  (:types block robot)
+
+  (:predicates
+    (Clear ?x0 - block)
+    (GripperOpen ?x0 - robot)
+    (Holding ?x0 - block)
+    (On ?x0 - block ?x1 - block)
+    (OnTable ?x0 - block)
+  )
+
+  (:action PickFromTable
+    :parameters (?block - block ?robot - robot)
+    :precondition (and (Clear ?block)
+        (GripperOpen ?robot)
+        (OnTable ?block))
+    :effect (and (Holding ?block)
+        (not (Clear ?block))
+        (not (GripperOpen ?robot))
+        (not (OnTable ?block)))
+  )
+
+  (:action PutOnTable
+    :parameters (?block - block ?robot - robot)
+    :precondition (and (Holding ?block))
+    :effect (and (Clear ?block)
+        (GripperOpen ?robot)
+        (OnTable ?block)
+        (not (Holding ?block)))
+  )
+
+  (:action Stack
+    :parameters (?block - block ?otherblock - block ?robot - robot)
+    :precondition (and (Clear ?otherblock)
+        (Holding ?block))
+    :effect (and (Clear ?block)
+        (GripperOpen ?robot)
+        (On ?block ?otherblock)
+        (not (Clear ?otherblock))
+        (not (Holding ?block)))
+  )
+
+  (:action Unstack
+    :parameters (?block - block ?otherblock - block ?robot - robot)
+    :precondition (and (Clear ?block)
+        (GripperOpen ?robot)
+        (On ?block ?otherblock))
+    :effect (and (Clear ?otherblock)
+        (Holding ?block)
+        (not (Clear ?block))
+        (not (GripperOpen ?robot))
+        (not (On ?block ?otherblock)))
+  )
+)

predicators/cogman.py

@@ -18,7 +18,7 @@
 from predicators.settings import CFG
 from predicators.structs import Action, Dataset, EnvironmentTask, GroundAtom, \
     InteractionRequest, InteractionResult, LowLevelTrajectory, Metrics, \
-    Observation, State, Task, Video
+    Observation, State, Task, Video, Object
 
 
 class CogMan:
@@ -38,6 +38,8 @@ def __init__(self, approach: BaseApproach, perceiver: BasePerceiver,
         self._episode_action_history: List[Action] = []
         self._episode_images: Video = []
         self._episode_num = -1
+        self._last_goal = None
+        self._last_act = None
 
     def reset(self, env_task: EnvironmentTask) -> None:
         """Start a new episode of environment interaction."""
@@ -60,6 +62,9 @@ def reset(self, env_task: EnvironmentTask) -> None:
     def step(self, observation: Observation) -> Optional[Action]:
         """Receive an observation and produce an action, or None for done."""
         state = self._perceiver.step(observation)
+        print("State: ", state, "\n")
+        print("Abstract State:", utils.abstract(state, self._approach._initial_predicates))
+        print("\n\n")
         if CFG.make_cogman_videos:
             assert self._current_env_task is not None
             imgs = self._perceiver.render_mental_images(
@@ -80,9 +85,18 @@ def step(self, observation: Observation) -> Optional[Action]:
             logging.info("[CogMan] Termination triggered.")
             return None
         # Check if we should replan.
+        self._exec_monitor.perceiver = self._perceiver
+        self._exec_monitor.env_task = self._current_env_task
         if self._exec_monitor.step(state):
             logging.info("[CogMan] Replanning triggered.")
+            self._last_goal = self._current_goal
+            try:
+                self._last_act = self._current_policy(state)
+            except Exception as e:
+                import ipdb; ipdb.set_trace()
+            self._current_goal = self._exec_monitor._curr_goal
             assert self._current_goal is not None
+            state = self._perceiver.step(observation)
             task = Task(state, self._current_goal)
             self._reset_policy(task)
             self._exec_monitor.reset(task)
@@ -94,7 +108,11 @@ def step(self, observation: Observation) -> Optional[Action]:
             if self._override_policy is None:
                 assert not self._exec_monitor.step(state)
         assert self._current_policy is not None
-        act = self._current_policy(state)
+        try:
+            act = self._current_policy(state)
+        except Exception as e:
+            assert self._last_act is not None
+            act = self._last_act
         self._perceiver.update_perceiver_with_action(act)
         self._exec_monitor.update_approach_info(
             self._approach.get_execution_monitoring_info())