From 8c31af15a0d10e5e8757057ac176ccde49a2568a Mon Sep 17 00:00:00 2001
From: rschoene <rene.schoene@tu-dresden.de>
Date: Thu, 10 Jun 2021 19:27:23 +0200
Subject: [PATCH] using and testing BFS algorithm to compute operations
---
ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag | 1 -
.../src/main/jastadd/WorldModelB.jadd | 72 +++-----
.../tudresden/inf/st/placeB/SimpleMainB.java | 3 -
.../inf/st/placeB/TestComputeOperations.java | 157 ++++++++++++++++++
.../de/tudresden/inf/st/placeB/TestUtils.java | 58 +++++++
5 files changed, 235 insertions(+), 56 deletions(-)
create mode 100644 ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java
create mode 100644 ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java
diff --git a/ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag b/ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag
index e7e1990..13efe23 100644
--- a/ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag
+++ b/ros3rag.placeB/src/main/jastadd/BFS/BFS.jrag
@@ -38,7 +38,6 @@ aspect BFS {
private List<Edge> Vertex.reconstructFrom(Vertex goal, Map<Vertex, Edge> pred) {
List<Edge> result = new ArrayList<>();
- System.out.println(pred);
Vertex current = goal;
while (!current.equals(this)) {
Edge e = pred.get(current);
diff --git a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
index e6fc758..14ada75 100644
--- a/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
+++ b/ros3rag.placeB/src/main/jastadd/WorldModelB.jadd
@@ -62,59 +62,22 @@ aspect Computation {
//--- computeOperations ---
syn List<Operation> Difference.computeOperations();
eq DifferenceObjectAtWrongPlace.computeOperations() {
- // first, find robots that can reach the locations
- // there does not necessarily has to be one robot that can reach both
- Set<Robot> robotsFitForPreviousLocation = new HashSet<>();
- Set<Robot> robotsFitForNewLocation = new HashSet<>();
- for (var robot : worldModelB().getRobotList()) {
- // check if robot can reach both, location (if any) and the object
- if ((!hasPreviousLocation() || robot.canReach(getPreviousLocation().getName())) &&
- robot.canReach(getObject().getName())) {
- robotsFitForPreviousLocation.add(robot);
- }
- if (robot.canReach(getNewLocation().getName())) {
- robotsFitForNewLocation.add(robot);
- }
- }
- if (robotsFitForPreviousLocation.isEmpty()) {
- StringBuilder sb = new StringBuilder("No robot can reach object ");
- sb.append(getObject().getName());
- if (hasPreviousLocation()) {
- sb.append(" and ").append(getPreviousLocation().getName());
- }
- sb.append("!");
- return Collections.singletonList(new ErrorOperation(sb.toString()));
- }
- if (robotsFitForNewLocation.isEmpty()) {
- return Collections.singletonList(new ErrorOperation("No robot can reach new location " + getNewLocation().getName()));
- }
- var result = new ArrayList<Operation>();
- var robotFitForBothLocations = robotsFitForPreviousLocation.stream()
- .filter(robotsFitForNewLocation::contains)
- .findFirst();
- robotFitForBothLocations.ifPresentOrElse(robot -> {
- // there is at least one robot that can reach both locations
- result.add(createPickAndPlace(robot, this.getObject(), this.getNewLocation()));
- },
- () -> {
- // no single robot can reach both locations
- // find a location, that can be used as exchange point reached by two robots
- for (var robotForPrevious : robotsFitForPreviousLocation) {
- for (var robotForNew : robotsFitForNewLocation) {
- for (var canReachOfRobotForPrevious : robotForPrevious.getCanReachObjectOfInterestWrapper().getCanReachObjectOfInterestList()) {
- String objectName = canReachOfRobotForPrevious.getObjectName();
- if (resolveLogicalObjectOfInterest(objectName).isLogicalDropOffLocation() && robotForNew.canReach(objectName)) {
- // add two operations.
- // obj -> transitLoc by robotForPrevious
- // obj -> targetLoc by robotForNew
- result.add(createPickAndPlace(robotForPrevious, this.getObject(), resolveLogicalObjectOfInterest(objectName).asLogicalDropOffLocation()));
- result.add(createPickAndPlace(robotForNew, this.getObject(), this.getNewLocation()));
- }
+ return getPreviousLocation().correspondingVertex().map(previousVertex -> {
+ return getNewLocation().correspondingVertex().map(newVertex -> {
+ List<Edge> shortestPath = previousVertex.BFS(newVertex);
+ if (shortestPath == null || shortestPath.isEmpty()) {
+ return error("No sequence of operations to move " + getObject().getName() + (hasPreviousLocation() ? " from " + getPreviousLocation().getName() : "") + " to " + getNewLocation().getName());
}
- }
- }
- });
- return result;
+ List<Operation> result = new ArrayList<>();
+ Vertex transit = previousVertex;
+ for (Edge edge : shortestPath) {
+ Vertex target = edge.getFrom().equals(transit) ? edge.getTo() : edge.getFrom();
+ result.add(createPickAndPlace(edge.getRobot(), getObject(), target.getLocation()));
+ transit = target;
+ }
+ return result;
+ }).orElseGet(() -> error("Could not resolve graph vertex of previous location " + getNewLocation().getName()));
+ }).orElseGet(() -> error("Could not resolve graph vertex of new location " + getPreviousLocation().getName()));
}
eq DifferenceNewObject.computeOperations() {
// FIXME. stub, may be implemented later
@@ -125,6 +88,11 @@ aspect Computation {
return Collections.emptyList();
}
+ //--- error ---
+ protected static List<Operation> Difference.error(String message) {
+ return Collections.singletonList(new ErrorOperation(message));
+ }
+
//--- createPickAndPlace ---
private static Operation DifferenceObjectAtWrongPlace.createPickAndPlace(Robot robot, LogicalMovableObject obj, LogicalDropOffLocation target) {
var result = new PickAndPlace();
diff --git a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java
index d0449b3..aef3930 100644
--- a/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java
+++ b/ros3rag.placeB/src/main/java/de/tudresden/inf/st/placeB/SimpleMainB.java
@@ -1,6 +1,5 @@
package de.tudresden.inf.st.placeB;
-import com.google.protobuf.util.JsonFormat;
import de.tudresden.inf.st.ceti.Reachability;
import de.tudresden.inf.st.placeB.ast.*;
import de.tudresden.inf.st.ros3rag.common.Configuration;
@@ -9,8 +8,6 @@ import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
import java.io.IOException;
-import java.nio.charset.StandardCharsets;
-import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.List;
diff --git a/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java
new file mode 100644
index 0000000..da21587
--- /dev/null
+++ b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestComputeOperations.java
@@ -0,0 +1,157 @@
+package de.tudresden.inf.st.placeB;
+
+import de.tudresden.inf.st.placeB.ast.*;
+import org.junit.jupiter.api.Test;
+
+import java.util.List;
+
+import static org.assertj.core.api.Assertions.assertThat;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+/**
+ * Testing computing operations from differences.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class TestComputeOperations {
+ @Test
+ public void testNoMovePossible() {
+ WorldModelB model = TestUtils.newModel();
+ TestUtils.addObjects(model, "x");
+ TestUtils.addLocations(model, "red", "blue", "green");
+ TestUtils.addRobots(model, "arm1", "arm2");
+ TestUtils.addReachability(model, "arm1", "red", "blue");
+ TestUtils.addReachability(model, "arm2", "blue");
+
+ LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+ LogicalDropOffLocation green = logicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation();
+ DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, green);
+
+ List<Operation> operations = diff.computeOperations();
+ assertThat(operations).isNotEmpty();
+ assertThat(operations).hasSize(1);
+ Operation op = operations.get(0);
+ assertTrue(op.isErrorOperation());
+ assertEquals("+Error: No sequence of operations to move x from red to green+", op.prettyPrint());
+ }
+
+ @Test
+ public void testDirectMove() {
+ WorldModelB model = TestUtils.newModel();
+ TestUtils.addObjects(model, "x");
+ TestUtils.addLocations(model, "red", "blue", "green");
+ TestUtils.addRobots(model, "arm1", "arm2");
+ TestUtils.addReachability(model, "arm1", "red", "blue");
+ TestUtils.addReachability(model, "arm2", "blue");
+
+ LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+ LogicalDropOffLocation blue = logicalScene.resolveLogicalObjectOfInterest("blue").asLogicalDropOffLocation();
+ Robot arm1 = model.findRobot("arm1").orElseThrow();
+ DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, blue);
+
+ List<Operation> operations = diff.computeOperations();
+ assertThat(operations).isNotEmpty();
+ assertThat(operations).hasSize(1);
+ Operation op = operations.get(0);
+ assertThat(op).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace = (PickAndPlace) op;
+ assertEquals(arm1, pickAndPlace.getRobotToExecute());
+ assertEquals(x, pickAndPlace.getObjectToPick());
+ assertEquals(blue, pickAndPlace.getTargetLocation());
+ }
+
+ @Test
+ public void testIndirectMoveWith2Robots() {
+ WorldModelB model = TestUtils.newModel();
+ TestUtils.addObjects(model, "x");
+ TestUtils.addLocations(model, "red", "blue", "green");
+ TestUtils.addRobots(model, "arm1", "arm2");
+ TestUtils.addReachability(model, "arm1", "red", "blue");
+ TestUtils.addReachability(model, "arm2", "blue", "green");
+
+ LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+ LogicalDropOffLocation blue = logicalScene.resolveLogicalObjectOfInterest("blue").asLogicalDropOffLocation();
+ LogicalDropOffLocation green = logicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation();
+ Robot arm1 = model.findRobot("arm1").orElseThrow();
+ Robot arm2 = model.findRobot("arm2").orElseThrow();
+ DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, green);
+
+ List<Operation> operations = diff.computeOperations();
+ assertThat(operations).isNotEmpty();
+ assertThat(operations).hasSize(2);
+ Operation op1 = operations.get(0);
+ assertThat(op1).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace1 = (PickAndPlace) op1;
+ assertEquals(arm1, pickAndPlace1.getRobotToExecute());
+ assertEquals(x, pickAndPlace1.getObjectToPick());
+ assertEquals(blue, pickAndPlace1.getTargetLocation());
+
+ Operation op2 = operations.get(1);
+ assertThat(op2).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace2 = (PickAndPlace) op2;
+ assertEquals(arm2, pickAndPlace2.getRobotToExecute());
+ assertEquals(x, pickAndPlace2.getObjectToPick());
+ assertEquals(green, pickAndPlace2.getTargetLocation());
+ }
+
+ @Test
+ public void testIndirectMoveWith3Robots() {
+ /*
+ * red -(arm1)-> blue --(arm3)-,
+ * `-(arm2)-> green <-------'
+ * `--(arm4)-> yellow -(arm5)-> purple
+ */
+ WorldModelB model = TestUtils.newModel();
+ TestUtils.addObjects(model, "x");
+ TestUtils.addLocations(model, "red", "blue", "green", "yellow", "purple");
+ TestUtils.addRobots(model, "arm1", "arm2", "arm3", "arm4", "arm5");
+ TestUtils.addReachability(model, "arm1", "red", "blue");
+ TestUtils.addReachability(model, "arm2", "red", "green");
+ TestUtils.addReachability(model, "arm3", "blue", "green");
+ TestUtils.addReachability(model, "arm4", "green", "yellow");
+ TestUtils.addReachability(model, "arm5", "yellow", "purple");
+
+ LogicalScene logicalScene = model.getMyScene().getLogicalScene();
+ LogicalMovableObject x = logicalScene.resolveLogicalObjectOfInterest("x").asLogicalMovableObject();
+ LogicalDropOffLocation red = logicalScene.resolveLogicalObjectOfInterest("red").asLogicalDropOffLocation();
+ LogicalDropOffLocation green = logicalScene.resolveLogicalObjectOfInterest("green").asLogicalDropOffLocation();
+ LogicalDropOffLocation yellow = logicalScene.resolveLogicalObjectOfInterest("yellow").asLogicalDropOffLocation();
+ LogicalDropOffLocation purple = logicalScene.resolveLogicalObjectOfInterest("purple").asLogicalDropOffLocation();
+ Robot arm2 = model.findRobot("arm2").orElseThrow();
+ Robot arm4 = model.findRobot("arm4").orElseThrow();
+ Robot arm5 = model.findRobot("arm5").orElseThrow();
+ DifferenceObjectAtWrongPlace diff = new DifferenceObjectAtWrongPlace(x, red, purple);
+
+ List<Operation> operations = diff.computeOperations();
+ assertThat(operations).isNotEmpty();
+ assertThat(operations).hasSize(3);
+
+ Operation op1 = operations.get(0);
+ assertThat(op1).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace1 = (PickAndPlace) op1;
+ assertEquals(arm2, pickAndPlace1.getRobotToExecute());
+ assertEquals(x, pickAndPlace1.getObjectToPick());
+ assertEquals(green, pickAndPlace1.getTargetLocation());
+
+ Operation op2 = operations.get(1);
+ assertThat(op2).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace2 = (PickAndPlace) op2;
+ assertEquals(arm4, pickAndPlace2.getRobotToExecute());
+ assertEquals(x, pickAndPlace2.getObjectToPick());
+ assertEquals(yellow, pickAndPlace2.getTargetLocation());
+
+ Operation op3 = operations.get(2);
+ assertThat(op3).isInstanceOf(PickAndPlace.class);
+ PickAndPlace pickAndPlace3 = (PickAndPlace) op3;
+ assertEquals(arm5, pickAndPlace3.getRobotToExecute());
+ assertEquals(x, pickAndPlace3.getObjectToPick());
+ assertEquals(purple, pickAndPlace3.getTargetLocation());
+ }
+}
diff --git a/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java
new file mode 100644
index 0000000..befdb39
--- /dev/null
+++ b/ros3rag.placeB/src/test/java/de/tudresden/inf/st/placeB/TestUtils.java
@@ -0,0 +1,58 @@
+package de.tudresden.inf.st.placeB;
+
+import de.tudresden.inf.st.placeB.ast.*;
+
+/**
+ * Utility methods for testing.
+ *
+ * @author rschoene - Initial contribution
+ */
+public class TestUtils {
+ public static WorldModelB newModel() {
+ WorldModelB model = new WorldModelB();
+ Scene scene = new Scene();
+ model.setMyScene(scene);
+ return model;
+ }
+
+ public static void addObjects(WorldModelB model, String... names) {
+ for (String name : names) {
+ int index = model.getMyScene().getNumMovableObject();
+ MovableObject obj = new MovableObject()
+ .setName(name)
+ .setPosition(new Position(-index, -index, -index))
+ .setOrientation(new Orientation())
+ .setSize(new Size(1, 1, 1));
+ model.getMyScene().addMovableObject(obj);
+ }
+ }
+
+ public static void addLocations(WorldModelB model, String... names) {
+ for (String name : names) {
+ int index = model.getMyScene().getNumDropOffLocation();
+ DropOffLocation obj = new DropOffLocation()
+ .setName(name)
+ .setPosition(new Position(+index, +index, +index))
+ .setOrientation(new Orientation())
+ .setSize(new Size(1, 1, 1));
+ model.getMyScene().addDropOffLocation(obj);
+ }
+ }
+
+ public static void addRobots(WorldModelB model, String... names) {
+ for (String name : names) {
+ Robot obj = new Robot().setName(name);
+ model.addRobot(obj);
+ }
+ }
+
+ public static void addReachability(WorldModelB model, String robotName, String... reachableObjects) {
+ Robot robot = model.findRobot(robotName).orElseThrow();
+ CanReachObjectOfInterestWrapper wrapper = new CanReachObjectOfInterestWrapper();
+ for (String reachableObjectName : reachableObjects) {
+ wrapper.addCanReachObjectOfInterest(new CanReachObjectOfInterest().setObjectName(reachableObjectName));
+ }
+ robot.setCanReachObjectOfInterestWrapper(wrapper);
+ }
+
+}
--
GitLab