update to "waiting" motion grammar with new parser

src/main/jastadd/cleanup/Cleanup.jrag

@@ -4,16 +4,17 @@ aspect CleanupAttributes {
   inh String PickUpObject.object();
   inh String RightPlace.object();
   inh String DropObjectAtRightPlace.object();
-  eq MoveObjectToCorrectPlace.getPickUpObject().object() = getObjectAtWrongPlace().getObject();
-  eq MoveObjectToCorrectPlace.getDropObjectAtRightPlace().object() = getObjectAtWrongPlace().getObject();
-  eq Tidy.getMoveObjectToCorrectPlace(int i).object() = getMoveObjectToCorrectPlace(i).getObjectAtWrongPlace().getObject();
+  eq MoveObjectToCorrectPlace.getPickUpObject().object() = getMisplacedObject().getAvailableObject().getObject();
+  eq MoveObjectToCorrectPlace.getDropObjectAtRightPlace().object() = getMisplacedObject().getAvailableObject().getObject();
+  eq Tidy.getMoveObjectToCorrectPlace(int i).object() = getMoveObjectToCorrectPlace(i).getMisplacedObject().getAvailableObject().getObject();
   eq Tidy.getChild().object() {
     throw new UnsupportedOperationException("Invalid use of attribute object():String");
   }
 
-  syn String ASTNode.place(); // TODO can this be avoided?
-  eq DropObjectAtRightPlace.place() = getRightPlace().getPlace();
-  eq ASTNode.place() {
+  inh String ASTNode.place(); // TODO can this be avoided?
+  inh String DropObjectAtRightPlace.place();
+  eq MoveObjectToCorrectPlace.getDropObjectAtRightPlace().place() = getMisplacedObject().getRightPlace().getPlace();
+  eq ASTNode.getChild().place() {
     throw new UnsupportedOperationException("Invalid use of attribute place():String");
   }
 

src/main/jastadd/cleanup/Cleanup.relast

 Tidy : MotionGrammarElement ::= NotEmptyTable MoveObjectToCorrectPlace* EmptyTable;
-MoveObjectToCorrectPlace : MotionGrammarElement ::= ObjectAtWrongPlace/*provides object*/ PickUpObject/*uses object*/ DropObjectAtRightPlace/*uses object*/;
-PickUpObject/*requires object*/ : MotionGrammarElement ::= RobotIsReadyToPickToken;
-DropObjectAtRightPlace/*requires object*/ : MotionGrammarElement ::= RobotIsReadyToDropToken RightPlace/*uses object, provides place*/;
+MoveObjectToCorrectPlace : MotionGrammarElement ::= MisplacedObject/*provides object, place*/ PickUpObject/*uses object*/ DropObjectAtRightPlace/*uses object*/ RobotIsReadyToPickToken;
+PickUpObject/*requires object*/ : MotionGrammarElement;
+DropObjectAtRightPlace/*requires object, place*/ : MotionGrammarElement ::= RobotIsReadyToDropToken;
 
 // Tokens
 EmptyTable                        : Token;
 NotEmptyTable                     : Token; // TODO should be improved to express EmptyTable can not be parsed
-ObjectAtWrongPlace                : Token ::= <Object:String>; // the Object is a variable of the token
+AvailableObject                   : Token ::= <Object:String>; // the object is a variable of the token
 RobotIsReadyToPickToken           : Token ::= RobotIsIdle RobotHasNoItemAttached; // combined token. both individual tokens are parsed in parallel
 RobotIsNotReadyToPickToken        : Token ::= [RobotIsNotIdle] /*or*/ [RobotHasItemAttached]; // TODO negated RobotIsReadyToPickToken
 RobotIsReadyToDropToken           : Token ::= RobotIsIdle RobotHasItemAttached;   // combined token. both individual tokens are parsed in parallel
@@ -16,3 +16,4 @@ RobotIsNotIdle                    : Token; // TODO negated RobotIsIdle
 RobotHasItemAttached              : Token;
 RobotHasNoItemAttached            : Token; // TODO negated RobotHasItemAttached
 RightPlace/*requires object*/     : Token ::= <Place:String>;
+MisplacedObject                   : Token ::= AvailableObject RightPlace <Object:String> <Place:String>; // TODO not so nice that the tokens depend on each other; also, object and place propagation...

src/main/jastadd/cleanup/DecisionTree.jadd

+aspect DecisionTree {
+
+  public Node Node.getOrCreateColorSelection(String color) {
+    throw new UnsupportedOperationException("Unable to create a color selection in a node that is not a Selection node");
+  }
+  public Node SelectionNode.getOrCreateColorSelection(String color) {
+    for (Node child : getNodeList()) {
+      if (child.isColorSelection() && child.asColorSelection().getColor().equals(color)) {
+        return child;
+      }
+    }
+    ColorSelection newColorSelection = new ColorSelection();
+    newColorSelection.setColor(color);
+    addNode(newColorSelection);
+    return newColorSelection;
+  }
+
+  public Node Node.getOrCreateSizeSelection(String size) {
+    throw new UnsupportedOperationException("Unable to create a size selection in a node that is not a Selection node");
+  }
+  public Node SelectionNode.getOrCreateSizeSelection(String size) {
+    for (Node child : getNodeList()) {
+      if (child.isSizeSelection() && child.asSizeSelection().getSize().equals(size)) {
+        return child;
+      }
+    }
+    SizeSelection newSizeSelection = new SizeSelection();
+    newSizeSelection.setSize(size);
+    addNode(newSizeSelection);
+    return newSizeSelection;
+  }
+
+  public Node Node.getOrCreateDecision(String decision) {
+    throw new UnsupportedOperationException("Unable to create a decision selection in a node that is not a Selection node");
+  }
+  public Node SelectionNode.getOrCreateDecision(String decision) {
+    for (Node child : getNodeList()) {
+      if (child.isDecision()) {
+        child.asDecision().setTarget(decision);
+        return child;
+      }
+    }
+    Decision newDecision = new Decision();
+    newDecision.setTarget(decision);
+    addNode(newDecision);
+    return newDecision;
+  }
+
+}

src/main/jastadd/cleanup/DecisionTree.jrag

+aspect DecisionTree {
+
+  syn boolean Node.isColorSelection() = false;
+  eq ColorSelection.isColorSelection() = true;
+
+  syn ColorSelection Node.asColorSelection() = null;
+  eq ColorSelection.asColorSelection() = this;
+
+  syn boolean Node.isSizeSelection() = false;
+  eq SizeSelection.isSizeSelection() = true;
+
+  syn SizeSelection Node.asSizeSelection() = null;
+  eq SizeSelection.asSizeSelection() = this;
+
+  syn boolean Node.isDecision() = false;
+  eq Decision.isDecision() = true;
+
+  syn Decision Node.asDecision() = null;
+  eq Decision.asDecision() = this;
+
+  inh DecisionTree PhysicalObject.decisionTree();
+  eq RobotWorld.getTable().decisionTree() = getDecisionTree();
+
+  syn String MovableObject.rightPlace() = decisionTree().processElement(this).orElse(null);
+
+  syn java.util.Optional<String> Node.processElement(MovableObject object);
+  eq Decision.processElement(MovableObject object) = java.util.Optional.of(getTarget());
+  eq SelectionNode.processElement(MovableObject object) {
+    for (Node child: getNodeList()) {
+      if (child.check(object)) {
+        return child.processElement(object);
+      }
+    }
+    return java.util.Optional.empty();
+  }
+  syn boolean Node.check(MovableObject object) = true;
+  eq ColorSelection.check(MovableObject object) = getColor().equals(object.getColor());
+  eq SizeSelection.check(MovableObject object) {
+    if (object.getName().startsWith("big") | object.getName().startsWith("large")) {
+      return getSize() == "large";
+    } else if (object.getName().startsWith("small")) {
+      return getSize() == "small";
+    } else {
+      return getSize() == "medium";
+    }
+  }
+
+}

src/main/jastadd/cleanup/RobotWorld.connect

 // --- receiving ---
-receive RobotWorld.Table using ParseScene, ConvertScene ;
+receive RobotWorld.Table using ParseScene, ConvertScene;
 
 ParseScene maps byte[] bytes to de.tudresden.inf.st.ceti.Scene {:
   return de.tudresden.inf.st.ceti.Scene.parseFrom(bytes);
@@ -34,3 +34,16 @@ ConvertScene maps de.tudresden.inf.st.ceti.Scene pbScene to Table {:
   }
   return result;
 :}
+
+receive indexed with add RobotWorld.Selection using ParseSelection, ConvertSelection;
+
+ParseSelection maps byte[] bytes to de.tudresden.inf.st.ceti.Selection {:
+  return de.tudresden.inf.st.ceti.Selection.parseFrom(bytes);
+:}
+
+ConvertSelection maps de.tudresden.inf.st.ceti.Selection pbSelection to Selection {:
+  var result = new Selection();
+  result.setObject(pbSelection.getId());
+  result.setTimeStamp(java.time.Instant.now());
+  return result;
+:}

src/main/jastadd/cleanup/RobotWorld.jadd

@@ -13,7 +13,7 @@ aspect RobotWorld {
     Robot r = new Robot();
     r.setIsIdle(true);
     t.setRobot(r);
-    return new RobotWorld(t);
+    return new RobotWorld(t, t.treeCopy(), new JastAddList<>());
   }
 
   public static RobotWorld RobotWorld.initialWorld() {
@@ -21,14 +21,14 @@ aspect RobotWorld {
     r.setIsIdle(true);
     Table t = new Table();
     t.setRobot(r);
-    RobotWorld result = new RobotWorld(t);
+    RobotWorld result = new RobotWorld(t, new Table(), new JastAddList<>());
     result.simulated = false;
     return result;
   }
 
   public static TokenType EmptyTable.type()                 { return TokenType.of("EMPTY_TABLE"); }
   public static TokenType NotEmptyTable.type()              { return TokenType.of("NOT_EMPTY_TABLE"); }
-  public static TokenType ObjectAtWrongPlace.type()         { return TokenType.of("OBJECT_AT_WRONG_PLACE"); }
+  public static TokenType AvailableObject.type()            { return TokenType.of("AVAILABLE_OBJECT"); }
   public static TokenType RobotIsReadyToPickToken.type()    { return TokenType.of("ROBOT_IS_READY_TO_PICK_TOKEN"); }
   public static TokenType RobotIsNotReadyToPickToken.type() { return TokenType.of("ROBOT_IS_NOT_READY_TO_PICK_TOKEN"); }
   public static TokenType RobotIsReadyToDropToken.type()    { return TokenType.of("ROBOT_IS_READY_TO_DROP_TOKEN"); }
@@ -38,6 +38,7 @@ aspect RobotWorld {
   public static TokenType RobotHasItemAttached.type()       { return TokenType.of("ROBOT_HAS_ITEM_ATTACHED"); }
   public static TokenType RobotHasNoItemAttached.type()     { return TokenType.of("ROBOT_HAS_NO_ITEM_ATTACHED"); }
   public static TokenType RightPlace.type()                 { return TokenType.of("RIGHT_PLACE"); }
+  public static TokenType MisplacedObject.type()            { return TokenType.of("MISPLACED_OBJECT"); }
 
   public static Pose Pose.of(double x, double y, double z) {
     return new Pose(x, y, z, 0, 0, 0, 1);
@@ -80,17 +81,13 @@ aspect RobotWorld {
     }
   }
 
-  public ObjectAtWrongPlace World.parseObjectAtWrongPlace(Tidy context) {
+  public AvailableObject World.parseAvailableObject() {
     return null;
   }
-  public ObjectAtWrongPlace RobotWorld.parseObjectAtWrongPlace(Tidy context) {
-    System.out.print("Trying to parse token <ObjectAtWrongPlace>... ");
-    // TODO get a *random* object?
-    for (PhysicalObject o : getTable().getPhysicalObjectList()) {
-      if (!context.inPlan(o.getName()) && o.isMovableObject() && !o.asMovableObject().isInBin()) {
-        System.out.println("success");
-        return new ObjectAtWrongPlace(o.getName());
-      }
+  public AvailableObject RobotWorld.parseAvailableObject() {
+    System.out.print("Trying to parse token <AvailableObject>... ");
+    if (getTable().getNumPhysicalObject() > 0) {
+      return new AvailableObject(getTable().getPhysicalObject(new java.util.Random().nextInt(getTable().getNumPhysicalObject())).getName());
     }
     System.out.println("failure");
     return null;
@@ -224,17 +221,35 @@ aspect RobotWorld {
     MovableObject o = getTable().getMovableObjectByName(objectName);
 
     if (o != null) {
-      for (PhysicalObject b : getTable().getPhysicalObjectList()) {
-        if (b.isBin() && o.fitsIn(b.asBin())) {
-          System.out.println("success");
-          return new RightPlace(b.getName());
-        }
+      Bin rightPlace = getTable().getBinByName(o.rightPlace());
+      if (rightPlace == null) {
+        System.out.println("failure (no matching bin found)");
+        return null;
       }
-      System.out.println("failure (no matching bin found)");
-      return null;
+      return new RightPlace(rightPlace.getName());
     } else {
       System.out.println("failure (object does not exist)");
       return null;
     }
   }
+
+  public MisplacedObject World.parseMisplacedObject() {
+    return null;
+  }
+  public MisplacedObject RobotWorld.parseMisplacedObject() {
+    System.out.println("Trying to parse token <MisplacedObject> by parsing ALL sub-tokens...");
+    AvailableObject availableObject = parseAvailableObject();
+    if (availableObject == null) {
+      System.out.println("Trying to parse token <RobotIsReadyToDropToken> by parsing ALL sub-tokens... failure");
+      return null;
+    }
+    RightPlace rightPlace = parseRightPlace(availableObject.getObject());
+    if (rightPlace != null) {
+      System.out.println("Trying to parse token <RobotIsReadyToDropToken> by parsing ALL sub-tokens... success");
+      return new MisplacedObject(availableObject, rightPlace, availableObject.getObject(), rightPlace.getPlace());
+    } else {
+      System.out.println("Trying to parse token <RobotIsReadyToDropToken> by parsing ALL sub-tokens... failure");
+      return null;
+    }
+  }
 }

src/main/jastadd/cleanup/RobotWorld.jrag

@@ -78,7 +78,89 @@ aspect RobotWorld {
     }
   }
 
-  syn boolean MovableObject.fitsIn(Bin b) {
-    return getColor().equals(b.getColor());
+  syn boolean ASTNode.inDemonstrationTable() = false;
+  inh boolean Pose.inDemonstrationTable();
+  inh boolean Robot.inDemonstrationTable();
+  inh boolean MovableObject.inDemonstrationTable();
+  eq RobotWorld.getDemonstrationTable().inDemonstrationTable() = true;
+  eq RobotWorld.getTable().inDemonstrationTable() = false;
+
+  syn nta DecisionTree RobotWorld.getDecisionTree() {
+    DecisionTree t = new DecisionTree();
+
+    MovableObject objectSelected = null;
+    Bin targetSelected = null;
+
+    for (Selection selection : getSelectionList()) {
+
+      PhysicalObject newSelection = getDemonstrationTable().getPhysicalObjectByName(selection.getObject());
+
+      if (newSelection == null) {
+        System.out.println("Selected unknown object " + selection.getObject());
+      } else if (objectSelected == null && targetSelected == null) {
+        if (newSelection.isMovableObject()) {
+          objectSelected = newSelection.asMovableObject();
+        } else if (newSelection.isBin()) {
+          targetSelected = newSelection.asBin();
+        }
+      } else if (objectSelected != null && targetSelected == null) {
+        if (newSelection.isMovableObject()) {
+          if (objectSelected == newSelection) {
+            // unselect
+            objectSelected = null;
+          } else {
+            // change selection
+            objectSelected = newSelection.asMovableObject();
+          }
+        } else if (newSelection.isBin()) {
+          // we have a full selection, add it to the tree
+          targetSelected = newSelection.asBin();
+
+          String color = objectSelected.getColor();
+          String size = "medium";
+          if (objectSelected.getName().startsWith("big") | objectSelected.getName().startsWith("large")) {
+            size = "large";
+          } else if (objectSelected.getName().startsWith("small")) {
+            size = "small";
+          }
+          Node n = t.getOrCreateSizeSelection(size)
+              .getOrCreateColorSelection(color)
+              .getOrCreateDecision(targetSelected.getName());
+
+          objectSelected = null;
+          targetSelected = null;
+        }
+      } else if (objectSelected == null && targetSelected != null) {
+        if (newSelection.isBin()) {
+          if (targetSelected == newSelection) {
+            // unselect
+            targetSelected = null;
+          } else {
+            // change selection
+            targetSelected = newSelection.asBin();
+          }
+        } else if (newSelection.isMovableObject()) {
+          // we have a full selection, add it to the tree
+          objectSelected = newSelection.asMovableObject();
+
+          String color = objectSelected.getColor();
+          String size = "medium";
+          if (objectSelected.getName().startsWith("big") | objectSelected.getName().startsWith("large")) {
+            size = "large";
+          } else if (objectSelected.getName().startsWith("small")) {
+            size = "small";
+          }
+          Node n = t.getOrCreateSizeSelection(size)
+              .getOrCreateColorSelection(color)
+              .getOrCreateDecision(targetSelected.getName());
+
+          objectSelected = null;
+          targetSelected = null;
+        }
+      } else {
+        // this should not happen
+      }
+    }
+    return t;
   }
 }

src/main/jastadd/cleanup/RobotWorld.relast

 // World Model of the Motion Grammar
 
-RobotWorld : World ::= Table;
+RobotWorld : World ::= Table DemonstrationTable:Table Selection* /DecisionTree/;
+
+abstract Node;
+abstract SelectionNode : Node ::= Node*;
+abstract DecisionNode : Node;
+DecisionTree : SelectionNode;
+ColorSelection : SelectionNode ::= <Color>;
+SizeSelection : SelectionNode ::= <Size>;
+Decision : DecisionNode ::= <Target>;
+
 
 Table ::= PhysicalObject* Robot;
 abstract PhysicalObject ::= <Name> Pose <Color>;
@@ -10,4 +19,6 @@ Bin : PhysicalObject;
 Pose ::= <X:double> <Y:double> <Z:double> <QX:double> <QY:double> <QZ:double> <QW:double>;
 
 Robot ::= <IsIdle:boolean>;
-rel Robot.attachedItem? <-> MovableObject.attachedRobot?;
+rel Robot.AttachedItem? <-> MovableObject.AttachedRobot?;
+
+Selection ::= <Object> <TimeStamp:java.time.Instant>;

src/main/jastadd/cleanup/SemanticActions.jadd

@@ -2,7 +2,7 @@ aspect SemanticActions {
 
     public void PickUpObject.action(World world) {
         RobotWorld scene = world.asRobotWorld();
-        System.out.println("performing semantic action for element Pick");
+        System.out.println("performing semantic action for element PickUpObject");
         String objectName = object();
 
         if (scene.simulated) {
@@ -31,7 +31,7 @@ aspect SemanticActions {
 
     public void DropObjectAtRightPlace.action(World world) {
         RobotWorld scene = world.asRobotWorld();
-        System.out.println("performing semantic action for element Pick");
+        System.out.println("performing semantic action for element DropObjectAtRightPlace");
         String objectName = object();
         String placeName = place();
         if (scene.simulated) {
@@ -42,8 +42,8 @@ aspect SemanticActions {
                 } catch (InterruptedException e) { /* ignore */ }
                 MovableObject object = scene.getTable().getMovableObjectByName(objectName);
                 Bin bin = scene.getTable().getBinByName(placeName);
-                object.setPose(Pose.of(bin.getPose()));
                 scene.getTable().getRobot().setAttachedItem(null);
+                object.removeSelf();
                 try {
                     Thread.sleep(3000);
                 } catch (InterruptedException e) { /* ignore */ }

src/main/jastadd/common/Helpers.jadd

+aspect Helpers {
+  public void ASTNode.removeSelf() {
+    if (getParent() == null) {
+      throw new RuntimeException("Unable to remove object (no parent)");
+    }
+    for (int i=0; i<getParent().numChildren();i++) {
+      if (getParent().getChild(i) == this) {
+        getParent().removeChild(i);
+        return;
+      }
+    }
+    throw new RuntimeException("Unable to remove object (wrong parent relation)");
+  }
+}

src/main/jastadd/common/Tracing.jadd

 aspect Tracing {
 
-  public static void World.printContextOf(String step, ASTNode<?> highlightNode, String highlightColor) {
+  synchronized public static void World.printContextOf(String step, ASTNode<?> highlightNode, String highlightColor) {
 
     final String DEFAULT_COLOR = de.tudresden.inf.st.mg.common.MotionGrammarConfig.DEFAULT_COLOR;
 
@@ -17,7 +17,11 @@ aspect Tracing {
       de.tudresden.inf.st.jastadd.dumpAst.ast.Dumper.read(world)
           .setNameMethod(o -> o == null ? "null" : o.getClass().getSimpleName())
           .setBackgroundColorMethod(n -> (n == highlightNode ? highlightColor : DEFAULT_COLOR))
-          .excludeTokens("Q.") // FIXME remove domain-specific code
+          .includeTokensWhen((node, tokenName, value) -> !tokenName.matches("Q.") && !tokenName.equals("TimeStamp")) // FIXME remove domain-specific code
+          .includeAttributeWhen((node, attributeName, isNTA, value) -> isNTA)
+          .<ASTNode>includeChildWhen((parentNode, childNode, contextName) -> !((contextName.equals("Pose") || contextName.equals("Robot")) && childNode.inDemonstrationTable()))
+          .<ASTNode>includeRelationsWhen((sourceNode, targetNode, roleName) -> !roleName.equals("AttachedRobot") && !(sourceNode != null && sourceNode.inDemonstrationTable()) && !(targetNode != null && targetNode.inDemonstrationTable())) // !sourceNode.inDemonstrationTable()
+//          .excludeRelations("AttachedRobot")
           .dumpAsSVG(de.tudresden.inf.st.mg.common.MotionGrammarConfig.astDiagramDir.resolve("Context-" + worldName + "-" + new java.text.SimpleDateFormat("yyyy.MM.dd.HH.mm.ss.SSS").format(new java.util.Date()) + "-" + step + ".svg"));
     } catch (java.io.IOException e) {
       e.printStackTrace();

src/main/java/de/tudresden/inf/st/mg/RobotParser.java

@@ -75,23 +75,23 @@ public final class RobotParser extends MotionGrammarParser<Tidy> {
     printAST("parseNotEmptyTable", result);
   }
 
-  private ObjectAtWrongPlace peekedObjectAtWrongPlace_ = null;
+  private AvailableObject peekedAvailableObject_ = null;
 
-  private boolean peekObjectAtWrongPlace(Tidy context) {
-    peekedObjectAtWrongPlace_ = getWorld().parseObjectAtWrongPlace(context);
-    return peekedObjectAtWrongPlace_ != null;
+  private boolean peekAvailableObject() {
+    peekedAvailableObject_ = getWorld().parseAvailableObject();
+    return peekedAvailableObject_ != null;
   }
 
-  private void parseObjectAtWrongPlace(ASTNode<?> parent, int index, Tidy context) throws ParseException {
-    ObjectAtWrongPlace result;
+  private void parseAvailableObject(ASTNode<?> parent, int index, Tidy context) throws ParseException {
+    AvailableObject result;
 
-    if (peekedObjectAtWrongPlace_ != null) {
-      result = peekedObjectAtWrongPlace_;
-      peekedObjectAtWrongPlace_ = null; // TODO check if all peeked values are actually parsed afterwards
+    if (peekedAvailableObject_ != null) {
+      result = peekedAvailableObject_;
+      peekedAvailableObject_ = null; // TODO check if all peeked values are actually parsed afterwards
     } else {
-      result = getWorld().parseObjectAtWrongPlace(context);
+      result = getWorld().parseAvailableObject();
       if (result == null) {
-        throw new ParseException(ObjectAtWrongPlace.type());
+        throw new ParseException(AvailableObject.type());
       }
     }
 
@@ -99,7 +99,7 @@ public final class RobotParser extends MotionGrammarParser<Tidy> {
 
     // semantic action for ObjectAtWrongPlace
     result.action(getWorld());
-    printAST("parseObjectAtWrongPlace", result);
+    printAST("parseAvailableObject", result);
   }
 
   private RobotIsReadyToPickToken peekedRobotIsReadyToPickToken_ = null;
@@ -333,6 +333,32 @@ public final class RobotParser extends MotionGrammarParser<Tidy> {
     printAST("parseRightPlace", result);
   }
 
+  private MisplacedObject peekedMisplacedObject_ = null;
+  private boolean peekMisplacedObject() {
+    peekedMisplacedObject_ = getWorld().parseMisplacedObject();
+    return peekedMisplacedObject_ != null;
+  }
+
+  private void parseMisplacedObject(ASTNode<?> parent, int index) throws ParseException {
+    MisplacedObject result;
+
+    if (peekedMisplacedObject_ != null) {
+      result = peekedMisplacedObject_;
+      peekedMisplacedObject_ = null; // TODO check if all peeked values are actually parsed afterwards
+    } else {
+      result = getWorld().parseMisplacedObject();
+      if (result == null) {
+        throw new ParseException(MisplacedObject.type());
+      }
+    }
+
+    parent.setChild(result, index);
+
+    // semantic action for MisplacedObject
+    result.action(getWorld());
+    printAST("parseMisplacedObject", result);
+  }
+
   private void parseTidy(ASTNode<?> parent, int index) throws ParseException {
     Tidy result = new Tidy();
     result.parser = this;
@@ -341,12 +367,13 @@ public final class RobotParser extends MotionGrammarParser<Tidy> {
     peekNotEmptyTable();
     while (peekedNotEmptyTable_ == null) {
       waitSomeTime();
+      peekNotEmptyTable();
     }
     parseNotEmptyTable(result, 0);
 
     while (true) {
-      peekObjectAtWrongPlace(result);
-      if (peekedObjectAtWrongPlace_ != null) {
+      peekMisplacedObject();
+      if (peekedMisplacedObject_ != null) {
         int i = result.getNumMoveObjectToCorrectPlace();
         result.addMoveObjectToCorrectPlace(null);
         parseMoveObjectToCorrectPlace(result.getMoveObjectToCorrectPlaceList(), i);
@@ -372,9 +399,15 @@ public final class RobotParser extends MotionGrammarParser<Tidy> {
     result.parser = this;
     parent.setChild(result, index);
 
-    parseObjectAtWrongPlace(result, 0, parent.containingTidy());
+    parseMisplacedObject(result, 0);
     parsePickUpObject(result, 1);
     parseDropObjectAtRightPlace(result, 2);
+    peekRobotIsReadyToPickToken();
+    while (peekedRobotIsReadyToPickToken_ == null) {
+      waitSomeTime();
+      peekRobotIsReadyToPickToken();
+    }
+    parseRobotIsReadyToPickToken(result, 3);
 
     // semantic action for MoveObjectToCorrectPlace
     result.action(getWorld());
@@ -390,11 +423,10 @@ public final class RobotParser extends MotionGrammarParser<Tidy> {
     peekRobotIsReadyToDropToken();
     while (peekedRobotIsReadyToDropToken_ == null) {
       waitSomeTime();
+      peekRobotIsReadyToDropToken();
     }
     parseRobotIsReadyToDropToken(result, 0);
 
-    parseRightPlace(result, 1);
-
     // semantic action for DropObjectAtRightPlace
     result.action(getWorld());
     printAST("parseDropObjectAtRightPlace", result);
@@ -405,12 +437,6 @@ public final class RobotParser extends MotionGrammarParser<Tidy> {
     result.parser = this;
     parent.setChild(result, index);
 
-    peekRobotIsReadyToPickToken();
-    while (peekedRobotIsReadyToPickToken_ == null) {
-      waitSomeTime();
-    }
-    parseRobotIsReadyToPickToken(result, 0);
-
     // semantic action for PickUpObject
     result.action(getWorld());
     printAST("parsePickUpObject", result);

src/main/java/de/tudresden/inf/st/mg/common/MotionGrammarParser.java

@@ -22,7 +22,7 @@ public abstract class MotionGrammarParser<T extends MotionGrammarElement> {
 
   protected static void waitSomeTime() {
     try {
-      Thread.sleep(100/*ms*/);
+      Thread.sleep(1000/*ms*/);
     } catch(InterruptedException ignored) {
     }
   }

src/test/java/de/tudresden/inf/st/mg/ParserTest.java

 package de.tudresden.inf.st.mg;
 
 import de.tudresden.inf.st.mg.common.MotionGrammarParser;
-import de.tudresden.inf.st.mg.jastadd.model.Container;
-import de.tudresden.inf.st.mg.jastadd.model.RobotWorld;
-import de.tudresden.inf.st.mg.jastadd.model.T1;
-import de.tudresden.inf.st.mg.jastadd.model.Tidy;
+import de.tudresden.inf.st.mg.jastadd.model.*;
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
 
@@ -57,6 +54,16 @@ public class ParserTest {
 
     // for some reason, the best random seed value here is 1 and not 0???
     RobotWorld world = RobotWorld.initialWorld(new Random(1));
+    World.printContextOf("initial", null, "");
+
+    world.addSelection(new Selection().setObject("boxRed").setTimeStamp(java.time.Instant.now()))
+        .addSelection(new Selection().setObject("boxGreen").setTimeStamp(java.time.Instant.now()))
+        .addSelection(new Selection().setObject("binGreen").setTimeStamp(java.time.Instant.now()))
+        .addSelection(new Selection().setObject("binRed").setTimeStamp(java.time.Instant.now()))
+        .addSelection(new Selection().setObject("boxRed").setTimeStamp(java.time.Instant.now()))
+        .addSelection(new Selection().setObject("boxRed").setTimeStamp(java.time.Instant.now()));
+
+    World.printContextOf("initial-with-selection", null, "");
 
     // create a parser using the world
     RobotParser parser = new RobotParser(world);