Commit 44ee5b0f authored by René Schöne's avatar René Schöne
Browse files

Add orientation, changed goal-model to use protobuf types.

- update relast to (currently) inofficial version able to parse FQN for types in NTA tokens
parent e611947f
Pipeline #7332 passed with stages
in 6 minutes and 15 seconds
No preview for this file type
......@@ -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"
......@@ -34,6 +34,7 @@ public class DataConfiguration {
}
public static class DataWorkPose {
public String position;
public String wait;
public String orientation;
public String work;
}
}
......@@ -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;
......
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 ;
......
......@@ -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);
......
......@@ -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";
}
......@@ -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);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment