diff --git a/libs/relast.jar b/libs/relast.jar
index b194a775113a72e2a0c5ccaf7d175322fdc8eab7..b1a7542048dd1611db7f479307b0285efd8bb1f6 100644
Binary files a/libs/relast.jar and b/libs/relast.jar differ
diff --git a/ros2rag.common/config.yaml b/ros2rag.common/config.yaml
index 889bf22911b2406af758a9083c57a2ebb8d52a45..3c9ae2d959cf77ec1b02b362cda22db49e032542 100644
--- a/ros2rag.common/config.yaml
+++ b/ros2rag.common/config.yaml
@@ -21,16 +21,18 @@ panda_mqtt_connector:
Link4: "panda::panda_link4"
Link5: "panda::panda_link5"
Link6: "panda::panda_link6"
- Link7: "panda::panda_link7"
- # LeftFinger: "panda::panda_leftfinger"
+ EndEffector: "panda::panda_link7"
RightFinger: "panda::panda_rightfinger"
end_effectors:
panda:
- EndEffector: "panda::panda_leftfinger"
+ LeftFinger: "panda::panda_leftfinger"
goal_poses:
- position: "0.6 0 0.6"
- wait: "5000"
+ orientation: "0 1 1 0"
+ work: "5000"
- position: "-0.6 0.0 0.6"
- wait: "3000"
+ orientation: "0 1 1 0"
+ work: "3000"
- position: "0.6 0.0 0.6"
- wait: "4000"
+ orientation: "0 1 1 0"
+ work: "4000"
diff --git a/ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/DataConfiguration.java b/ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/DataConfiguration.java
index de0758b209fa013132163c178d911124629ad1e3..5586c9396d45398f6b2a2c2d6e2f453a71c63da4 100644
--- a/ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/DataConfiguration.java
+++ b/ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/DataConfiguration.java
@@ -34,6 +34,7 @@ public class DataConfiguration {
}
public static class DataWorkPose {
public String position;
- public String wait;
+ public String orientation;
+ public String work;
}
}
diff --git a/ros2rag.goal/src/main/jastadd/Computation.jrag b/ros2rag.goal/src/main/jastadd/Computation.jrag
index da1d21fae8d8891b554c8dec90da3e2d2549a4a9..446dfaa719d21e3579cae3ff821830aace9f152c 100644
--- a/ros2rag.goal/src/main/jastadd/Computation.jrag
+++ b/ros2rag.goal/src/main/jastadd/Computation.jrag
@@ -10,7 +10,17 @@ aspect Computation {
for (WorkPose workPose : getWorkPoseList()) {
MoveToStep moveToStep = new MoveToStep();
moveToStep.setId(index++);
- moveToStep.setPosition(workPose.getPosition());
+ moveToStep.setPosition(plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
+ .setX(workPose.getPositionX())
+ .setY(workPose.getPositionY())
+ .setZ(workPose.getPositionZ())
+ .build());
+ moveToStep.setOrientation(plan.TrajectoryOuterClass.Trajectory.Orientation.newBuilder()
+ .setX(workPose.getOrientationX())
+ .setY(workPose.getOrientationY())
+ .setZ(workPose.getOrientationZ())
+ .setW(workPose.getOrientationW())
+ .build());
lastStep.setSuccessor(moveToStep);
result.addStep(moveToStep);
@@ -60,18 +70,33 @@ aspect Computation {
return getId();
}
- syn DoublePosition Workflow.getNextPosition() {
- return currentStep().nextPosition();
+ syn plan.TrajectoryOuterClass.Trajectory Workflow.getNextTrajectory() {
+ return currentStep().nextTrajectory();
}
- syn DoublePosition Step.nextPosition();
+ syn plan.TrajectoryOuterClass.Trajectory Step.nextTrajectory();
// next position should not be called before step is advanced
- eq StartStep.nextPosition() = DoublePosition.of(Double.NaN, Double.NaN, Double.NaN);
- eq MoveToStep.nextPosition() = getPosition();
- eq WorkStep.nextPosition() = getPredecessor().nextPosition();
- eq FinishedStep.nextPosition() = getPredecessor().nextPosition();
+ eq StartStep.nextTrajectory() = plan.TrajectoryOuterClass.Trajectory.newBuilder()
+ .addPose(plan.TrajectoryOuterClass.Trajectory.Pose.newBuilder()
+ .setPosition(plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
+ .setX(Double.NaN)
+ .setY(Double.NaN)
+ .setZ(Double.NaN)
+ .build())
+ .build())
+ .build();
+ eq MoveToStep.nextTrajectory() = plan.TrajectoryOuterClass.Trajectory.newBuilder()
+ .addPose(plan.TrajectoryOuterClass.Trajectory.Pose.newBuilder()
+ .setPosition(getPosition())
+ .setOrientation(getOrientation())
+ .setMode(plan.TrajectoryOuterClass.Trajectory.PlanningMode.FLUID)
+ .build())
+ .setLoop(true)
+ .build();
+ eq WorkStep.nextTrajectory() = getPredecessor().nextTrajectory();
+ eq FinishedStep.nextTrajectory() = getPredecessor().nextTrajectory();
private static final double MoveToStep.DELTA = 0.1;
- private boolean MoveToStep.isNear(DoublePosition one, DoublePosition other) {
+ private boolean MoveToStep.isNear(plan.TrajectoryOuterClass.Trajectory.Position one, plan.TrajectoryOuterClass.Trajectory.Position other) {
return Math.abs(one.getX() - other.getX()) < DELTA &&
Math.abs(one.getY() - other.getY()) < DELTA &&
Math.abs(one.getZ() - other.getZ()) < DELTA;
diff --git a/ros2rag.goal/src/main/jastadd/GoalModel.relast b/ros2rag.goal/src/main/jastadd/GoalModel.relast
index ada05c63e58672128740f3a1616f2bf5ed20cf59..03ec3fa01051855d8485444d6270591e7671ee9b 100644
--- a/ros2rag.goal/src/main/jastadd/GoalModel.relast
+++ b/ros2rag.goal/src/main/jastadd/GoalModel.relast
@@ -1,13 +1,13 @@
GoalModel ::= WorkPose* /Workflow/ RobotState ;
-RobotState ::= <CurrentPosition:DoublePosition> <LastUpdate:long> ;
+RobotState ::= <CurrentPosition:plan.TrajectoryOuterClass.Trajectory.Position> <LastUpdate:long> ;
-WorkPose ::= <Position:DoublePosition> <Duration:long> ; // Position in [m,m,m], Duration in ms
+WorkPose ::= <PositionX:double> <PositionY:double> <PositionZ:double> <OrientationX:double> <OrientationY:double> <OrientationZ:double> <OrientationW:double> <Duration:long> ; // Positions in m, Duration in ms
-Workflow ::= Step* /<ReadyForThisStep:int>/ /<NextPosition:DoublePosition>/ <CurrentStep:LastUpdatedInteger> ; // NextPosition in [m,m,m]; CurrentStepStart in ms; ReadyForThisStep and CurrentStep are step ids
+Workflow ::= Step* /<ReadyForThisStep:int>/ /<NextTrajectory:plan.TrajectoryOuterClass.Trajectory>/ <CurrentStep:LastUpdatedInteger> ; // ReadyForThisStep and CurrentStep are step ids
abstract Step ::= <Id:int> ; // Started in ms
-MoveToStep : Step ::= <Position:DoublePosition> ; // Position in [m,m,m]
+MoveToStep : Step ::= <Position:plan.TrajectoryOuterClass.Trajectory.Position> <Orientation:plan.TrajectoryOuterClass.Trajectory.Orientation> ; // Position in [m,m,m]
WorkStep : Step ::= <Duration:long> ; // Duration in ms
StartStep : Step ;
FinishedStep : Step ;
diff --git a/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag b/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag
index 8c1da3c6a493ea651fc94180bf110b2957cb794d..448e66a83e17b01f1039f3e36ca17e28a75f3816 100644
--- a/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag
+++ b/ros2rag.goal/src/main/jastadd/GoalModel.ros2rag
@@ -2,20 +2,20 @@
* Version 2020-05-28
*/
// --- update definitions ---
-read RobotState.CurrentPosition using ParseRobotState, RobotStateToDoublePosition ;
+read RobotState.CurrentPosition using ParseRobotState, RobotStateToTrajectoryPosition ;
read RobotState.LastUpdate using TickWhenLinkState ;
// Those two roles together encode an attribute-driven rewrite (via mqtt)
write Workflow.ReadyForThisStep ;
read Workflow.CurrentStep using ParseLastUpdatedInteger ;
-write Workflow.NextPosition using CreateTrajectoryMessage, SerializeTrajectory ;
+write Workflow.NextTrajectory using SerializeTrajectory ;
// --- dependency definitions ---
Workflow.ReadyForThisStep canDependOn RobotState.LastUpdate as LastUpdateDependency ;
Workflow.ReadyForThisStep canDependOn RobotState.CurrentPosition as PositionDependency ;
-Workflow.NextPosition canDependOn Workflow.CurrentStep as OnStepChangeDependency ;
+Workflow.NextTrajectory canDependOn Workflow.CurrentStep as OnStepChangeDependency ;
// --- mapping definitions ---
ParseRobotState maps byte[] bytes to robot.RobotStateOuterClass.RobotState {:
@@ -33,24 +33,29 @@ SerializeTrajectory maps plan.TrajectoryOuterClass.Trajectory t to byte[] {:
return t.toByteArray();
:}
-RobotStateToDoublePosition maps robot.RobotStateOuterClass.RobotState rs to DoublePosition {:
+RobotStateToTrajectoryPosition maps robot.RobotStateOuterClass.RobotState rs to plan.TrajectoryOuterClass.Trajectory.Position {:
// System.out.println("RobotStateToDoublePosition");
robot.RobotStateOuterClass.RobotState.Position p = rs.getPosition();
- return DoublePosition.of(p.getX(), p.getY(), p.getZ());
+ return plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
+ .setX(p.getX())
+ .setY(p.getY())
+ .setZ(p.getZ())
+ .build();
:}
-CreateTrajectoryMessage maps DoublePosition dp to plan.TrajectoryOuterClass.Trajectory {:
- // compute intermediate points at ROS side, not here
- return plan.TrajectoryOuterClass.Trajectory.newBuilder()
- .addPose(plan.TrajectoryOuterClass.Trajectory.Pose.newBuilder()
- .setPosition(plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
- .setX(dp.getX())
- .setY(dp.getY())
- .setZ(dp.getZ())
- .build())
- .build())
- .build();
-:}
+// CreateTrajectoryMessage maps DoublePosition dp to plan.TrajectoryOuterClass.Trajectory {:
+// // compute intermediate points at ROS side, not here
+// return plan.TrajectoryOuterClass.Trajectory.newBuilder()
+// .addPose(plan.TrajectoryOuterClass.Trajectory.Pose.newBuilder()
+// .setPosition(plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
+// .setX(dp.getX())
+// .setY(dp.getY())
+// .setZ(dp.getZ())
+// .build())
+// .build())
+// .setLooping(true);
+// .build();
+// :}
ParseLastUpdatedInteger maps int value to LastUpdatedInteger {:
return LastUpdatedInteger.of(value);
diff --git a/ros2rag.goal/src/main/jastadd/Printing.jrag b/ros2rag.goal/src/main/jastadd/Printing.jrag
index b82655bd949e3d45efa6fdf229ef3d840ef8c628..af358c23b8e52bb141d141a985795f67d458a8b3 100644
--- a/ros2rag.goal/src/main/jastadd/Printing.jrag
+++ b/ros2rag.goal/src/main/jastadd/Printing.jrag
@@ -2,7 +2,9 @@ aspect Printing {
syn String Step.prettyPrint() = getId() + ": " + prettyPrint0() + " ->[" + (getSuccessor() == null ? "/" : getSuccessor().getId()) + "]";
syn String Step.prettyPrint0();
eq StartStep.prettyPrint0() = "Start";
- eq MoveToStep.prettyPrint0() = "Move to " + getPosition();
+ eq MoveToStep.prettyPrint0() = String.format("Move to pos(% .5f,% .5f,% .5f) ori(%.1f, %.1f, %.1f, %.1f)",
+ getPosition().getX(),getPosition().getY(),getPosition().getZ(),
+ getOrientation().getX(),getOrientation().getY(),getOrientation().getZ(),getOrientation().getW());
eq WorkStep.prettyPrint0() = "Work " + getDuration();
eq FinishedStep.prettyPrint0() = "Finish";
}
diff --git a/ros2rag.goal/src/main/java/de/tudresden/inf/st/ros2rag/goal/GoalMain.java b/ros2rag.goal/src/main/java/de/tudresden/inf/st/ros2rag/goal/GoalMain.java
index b8a4c7c29cb3c35a4d5a026fbacb943754b7d328..fc04dc9aeda6cc79222f1042a2d583abeeb86c6d 100644
--- a/ros2rag.goal/src/main/java/de/tudresden/inf/st/ros2rag/goal/GoalMain.java
+++ b/ros2rag.goal/src/main/java/de/tudresden/inf/st/ros2rag/goal/GoalMain.java
@@ -6,9 +6,11 @@ import de.tudresden.inf.st.ros2rag.common.Util;
import de.tudresden.inf.st.ros2rag.goal.ast.*;
import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
+import plan.TrajectoryOuterClass;
import java.io.File;
import java.io.IOException;
+import java.util.Arrays;
import java.util.concurrent.CountDownLatch;
import java.util.concurrent.TimeUnit;
@@ -35,17 +37,26 @@ public class GoalMain {
for (DataWorkPose dataWorkPose : config.goal_poses) {
WorkPose workPose = new WorkPose();
- double[] position = {0, 0, 0};
- String[] positionSplit = dataWorkPose.position.split(" ");
- for (int i = 0; i < positionSplit.length; i++) {
- position[i] = Double.parseDouble(positionSplit[i]);
+ String[] position = dataWorkPose.position.split(" ");
+ String[] orientation = dataWorkPose.orientation.split(" ");
+ if (position.length != 3) {
+ logger.error("Found invalid work pose position: {}", Arrays.toString(position));
}
- workPose.setPosition(DoublePosition.of(position[0], position[1], position[2]));
- workPose.setDuration(Long.parseLong(dataWorkPose.wait));
+ if (orientation.length != 4) {
+ logger.error("Found invalid work pose orientation: {}", Arrays.toString(orientation));
+ }
+ workPose.setPositionX(Double.parseDouble(position[0]));
+ workPose.setPositionY(Double.parseDouble(position[1]));
+ workPose.setPositionZ(Double.parseDouble(position[2]));
+ workPose.setOrientationX(Double.parseDouble(orientation[0]));
+ workPose.setOrientationY(Double.parseDouble(orientation[1]));
+ workPose.setOrientationZ(Double.parseDouble(orientation[2]));
+ workPose.setOrientationW(Double.parseDouble(orientation[3]));
+ workPose.setDuration(Long.parseLong(dataWorkPose.work));
model.addWorkPose(workPose);
}
RobotState robotState = new RobotState();
- robotState.setCurrentPosition(DoublePosition.of(0, 0, 0));
+ robotState.setCurrentPosition(TrajectoryOuterClass.Trajectory.Position.newBuilder().build());
robotState.setLastUpdate(0);
model.setRobotState(robotState);
@@ -76,7 +87,7 @@ public class GoalMain {
}
}, config);
// next position is not initialized, so don't send it
- model.getWorkflow().connectNextPosition(config.topics.trajectory, false);
+ model.getWorkflow().connectNextTrajectory(config.topics.trajectory, false);
model.getWorkflow().connectCurrentStep(config.topics.nextStep);
// initial next step is sent, as soon as this is received, the workflow starts
model.getWorkflow().connectReadyForThisStep(config.topics.nextStep, true);