Added models: Autopilot, Supermario, simulatorModel

Added EvaluationTest.java as base for evaluation
parent ae35f974
Pipeline #102205 failed with stages
in 24 minutes and 10 seconds
package de.monticore.lang.monticar.generator.middleware;
import de.monticore.lang.monticar.generator.middleware.cli.DistributedTargetGeneratorCli;
import org.junit.Ignore;
import org.junit.Test;
public class EvaluationTest {
@Test
public void testPacman() {
DistributedTargetGeneratorCli.main(new String[]{"./src/test/resources/config/evaluation/pacman.json"});
}
@Test
public void testAutopilot() {
DistributedTargetGeneratorCli.main(new String[]{"./src/test/resources/config/evaluation/autopilot.json"});
}
@Test
public void testSupermario() {
DistributedTargetGeneratorCli.main(new String[]{"./src/test/resources/config/evaluation/supermario.json"});
}
@Ignore("Fails on Spectral")
@Test
public void testSimulatorModel() {
DistributedTargetGeneratorCli.main(new String[]{"./src/test/resources/config/evaluation/simulatormodel.json"});
}
}
package de.rwth.armin.modeling.autopilot;
import de.rwth.armin.modeling.autopilot.common.*;
import de.rwth.armin.modeling.autopilot.behavior.*;
import de.rwth.armin.modeling.autopilot.motion.*;
component Autopilot {
port
// length of simulation time frame
// e.g. when simulation frequency is 200 Hz
// this value will be equal to 5ms
in Q (0.0 s : 0.001 s : 1.0 s) timeIncrement,
// current velocity
in Q (0.0 m/s : 0.01 m/s : oo m/s) currentVelocity,
// current vehicle's position
in Q (-oo m : 0.01 m : oo m) x,
in Q (-oo m : 0.01 m : oo m) y,
// orientation angle (also sometimes called yaw angle) in radians
// it is measured from from the positive Y axis direction
// to the actual vehicle's direction counter-clockwise
// Example 1: vehicle has direction (-1.0; -1.0)
// compass = 0.25 * PI (positive)
// Example 2: vehicle has direction (1.0; 0.0) i.e. it is directed along the X axis
// compass = -0.5 * PI (negative)
in Q (-oo : 0.001 : oo) compass,
// current engine actuation
in Q (0.0 : 0.001 : 2.5) currentEngine,
// current steering actuation
// negative value: left turn
// positive value: right turn
in Q (-0.785 : 0.001 : 0.785) currentSteering,
// current brakes actuation
in Q (0.0 : 0.001 : 3.0) currentBrakes,
// planned trajectory (with look ahead 100-200m)
// represented by two arrays trajectory_x[] and trajectory_y[]
// which both have length trajectory_length
in Z (0 : 100) trajectory_length,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trajectory_x,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trajectory_y,
// output actuation commands
out Q (0.0 : 0.001 : 2.5) engine,
out Q (-0.785 : 0.001 : 0.785) steering,
out Q (0.0 : 0.001 : 3.0) brakes;
instance Compass2CurrentDirection c2cd;
instance BehaviorGeneration behaviorGeneration;
instance MotionPlanning motionPlanning;
connect compass -> c2cd.compass;
connect x -> behaviorGeneration.currentPositionX;
connect y -> behaviorGeneration.currentPositionY;
connect trajectory_length -> behaviorGeneration.plannedTrajectoryLength;
connect trajectory_x -> behaviorGeneration.plannedTrajectoryX;
connect trajectory_y -> behaviorGeneration.plannedTrajectoryY;
connect c2cd.currentDirectionX -> behaviorGeneration.currentDirectionX;
connect c2cd.currentDirectionY -> behaviorGeneration.currentDirectionY;
connect currentVelocity -> motionPlanning.currentVelocity;
connect c2cd.currentDirectionX -> motionPlanning.currentDirectionX;
connect c2cd.currentDirectionY -> motionPlanning.currentDirectionY;
connect behaviorGeneration.desiredDirectionX -> motionPlanning.desiredDirectionX;
connect behaviorGeneration.desiredDirectionY -> motionPlanning.desiredDirectionY;
connect behaviorGeneration.signedDistanceToTrajectory -> motionPlanning.signedDistanceToTrajectory;
connect behaviorGeneration.desiredVelocity -> motionPlanning.desiredVelocity;
connect motionPlanning.engine -> engine;
connect motionPlanning.steering -> steering;
connect motionPlanning.brakes -> brakes;
}
package de.rwth.armin.modeling.autopilot;
view Autopilot {
component Autopilot{
ports in ? currentVelocity,
in ? compass,
in ? currentPosX,
in ? currentPosY,
in ? trajectory_length,
in ? trajectory_x,
in ? trajectory_y,
out ? Engine,
out ? Brakes,
out ? Steering;
component Compass2CurrentDirection{
ports in ? ?,
out ? ?,
out ? ?;
}
instance Compass2CurrentDirection comp;
component BehaviorGeneration{
ports in ? ?,
in ? ?,
in ? ?,
in ? ?,
in ? ?,
in ? ?,
in ? ?,
out ? ?;
}
instance BehaviorGeneration behavior;
component CalculateMotionCommands{
ports in ? ?,
in ? ?,
in ? ?,
in ? ?,
in ? ?,
out ? ?;
}
instance CalculateMotionCommands calcMotion;
component TrimPath{
ports in ? ?,
in ? ?,
in ? ?,
in ? ?,
in ? ?,
out ? ?;
}
instance TrimPath trim;
component MotionPlanning{
ports in ? ?,
in ? ?,
out ? ?;
}
instance MotionPlanning motion;
component CalculateEngineAndBrakes{
ports in ? ?,
in ? ?,
out ? ?;
}
instance CalculateEngineAndBrakes calcEngine;
component KeepDirection{
ports in ? ?,
out ? ?;
}
instance KeepDirection keep;
component SteeringAngleCorrection{
ports in ? ?,
out ? ?;
}
instance SteeringAngleCorrection steer;
component EnsureBounds{
ports in ? ?,
out ? ?;
}
instance EnsureBounds ensure;
connect currentVelocity -> motion;
connect compass -> comp;
connect currentPosX -> behavior;
connect currentPosY -> behavior;
connect trajectory_length -> behavior;
connect trajectory_x -> behavior;
connect trajectory_y -> behavior;
connect comp -> behavior;
connect comp -> behavior;
connect behavior -> calcMotion;
connect behavior -> calcMotion;
connect behavior -> calcMotion;
connect behavior -> calcMotion;
connect behavior -> trim;
connect behavior -> trim;
connect behavior -> trim;
connect behavior -> trim;
connect behavior -> trim;
connect trim -> calcMotion;
connect calcMotion -> behavior;
connect behavior -> motion;
connect motion -> calcEngine;
connect motion -> calcEngine;
connect calcEngine -> keep;
connect keep -> steer;
connect steer -> ensure;
connect ensure -> motion;
connect motion -> Engine;
connect motion -> Brakes;
connect motion -> Steering;
}
}
\ No newline at end of file
package de.rwth.armin.modeling.autopilot;
component Tests {
port in B in1, out B out1;
instance de.rwth.armin.modeling.autopilot.common.Tests t1;
instance de.rwth.armin.modeling.autopilot.motion.Tests t2;
instance de.rwth.armin.modeling.autopilot.behavior.Tests t3;
}
package de.rwth.armin.modeling.autopilot.basicComp;
component PID{
port
in Q desiredVel,
in Q currentVel,
in Q timeInc,
out Q engineVal,
out Q breakesVal;
implementation Math {
Q diff = desiredVel - currentVel;
static Q totalTime = 0.0;
totalTime = totalTime + timeInc;
if totalTime > 100
engineVal = 0.0;
breakesVal = 2.5;
elseif diff > 0
engineVal = 2.5;
breakesVal = 0.0;
else
engineVal = 0.0;
breakesVal = 2.5;
end
}
}
package de.rwth.armin.modeling.autopilot.basicComp;
view PID {
component PID{
ports in ? currentSpeed,
in ? desiredSpeed,
out ? Engine,
out ? Brakes;
}
}
\ No newline at end of file
package de.rwth.armin.modeling.autopilot.basicComp;
stream PID1 for PID {
desiredVel: 5 tick 5 tick 5 tick 5;
currentVel: 0 tick 4 tick 5 tick 10;
timeInc : 0.001 tick 0.0001 tick 0.0001 tick 0.0001;
engineVal: 2.5 tick 2.5 tick - tick 0.0;
breakesVal: 0 tick 0 tick - tick 2.5;
}
package de.rwth.armin.modeling.autopilot.behavior;
component BehaviorGeneration {
port
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
in Q (-oo m : 0.01 m : oo m) currentDirectionX,
in Q (-oo m : 0.01 m : oo m) currentDirectionY,
in Z (0 : 100) plannedTrajectoryLength,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} plannedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} plannedTrajectoryY,
out Q (-oo m : 0.01 m : oo m) desiredDirectionX,
out Q (-oo m : 0.01 m : oo m) desiredDirectionY,
out Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity;
instance TrimPath trimPath;
instance CalculateMotionCommands calcMotionCmds;
connect currentPositionX -> trimPath.currentPositionX;
connect currentPositionY -> trimPath.currentPositionY;
connect plannedTrajectoryLength -> trimPath.plannedTrajectoryLength;
connect plannedTrajectoryX -> trimPath.plannedTrajectoryX;
connect plannedTrajectoryY -> trimPath.plannedTrajectoryY;
connect currentPositionX -> calcMotionCmds.currentPositionX;
connect currentPositionY -> calcMotionCmds.currentPositionY;
connect currentDirectionX -> calcMotionCmds.currentDirectionX;
connect currentDirectionY -> calcMotionCmds.currentDirectionY;
connect trimPath.trimmedTrajectoryLength -> calcMotionCmds.trimmedTrajectoryLength;
connect trimPath.trimmedTrajectoryX -> calcMotionCmds.trimmedTrajectoryX;
connect trimPath.trimmedTrajectoryY -> calcMotionCmds.trimmedTrajectoryY;
connect calcMotionCmds.desiredDirectionX -> desiredDirectionX;
connect calcMotionCmds.desiredDirectionY -> desiredDirectionY;
connect calcMotionCmds.signedDistanceToTrajectory -> signedDistanceToTrajectory;
connect calcMotionCmds.desiredVelocity -> desiredVelocity;
}
package de.rwth.armin.modeling.autopilot.behavior;
component CalculateMotionCommands {
port
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
in Q (-oo m : 0.01 m : oo m) currentDirectionX,
in Q (-oo m : 0.01 m : oo m) currentDirectionY,
in Z (0 : 100) trimmedTrajectoryLength,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryY,
out Q (-oo m : 0.01 m : oo m) desiredDirectionX,
out Q (-oo m : 0.01 m : oo m) desiredDirectionY,
out Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity;
instance IsDriveToFirstPosition isDriveToFirstPosition;
instance DriveToFirstPosition driveToFirstPosition;
instance Distance2Velocity d2v1;
instance FollowTrajectory followTrajectory;
instance Distance2Velocity d2v2;
instance Selector selector;
connect currentPositionX -> isDriveToFirstPosition.currentPositionX;
connect currentPositionY -> isDriveToFirstPosition.currentPositionY;
connect trimmedTrajectoryLength -> isDriveToFirstPosition.trimmedTrajectoryLength;
connect trimmedTrajectoryX -> isDriveToFirstPosition.trimmedTrajectoryX;
connect trimmedTrajectoryY -> isDriveToFirstPosition.trimmedTrajectoryY;
connect isDriveToFirstPosition.result -> driveToFirstPosition.isDriveToFirstPosition;
connect currentPositionX -> driveToFirstPosition.currentPositionX;
connect currentPositionY -> driveToFirstPosition.currentPositionY;
connect trimmedTrajectoryLength -> driveToFirstPosition.trimmedTrajectoryLength;
connect trimmedTrajectoryX -> driveToFirstPosition.trimmedTrajectoryX;
connect trimmedTrajectoryY -> driveToFirstPosition.trimmedTrajectoryY;
connect driveToFirstPosition.distance -> d2v1.distance;
connect driveToFirstPosition.minVelocity -> d2v1.minVelocity;
connect driveToFirstPosition.maxVelocity -> d2v1.maxVelocity;
connect isDriveToFirstPosition.result -> followTrajectory.isDriveToFirstPosition;
connect currentPositionX -> followTrajectory.currentPositionX;
connect currentPositionY -> followTrajectory.currentPositionY;
connect trimmedTrajectoryLength -> followTrajectory.trimmedTrajectoryLength;
connect trimmedTrajectoryX -> followTrajectory.trimmedTrajectoryX;
connect trimmedTrajectoryY -> followTrajectory.trimmedTrajectoryY;
connect followTrajectory.distance -> d2v2.distance;
connect followTrajectory.minVelocity -> d2v2.minVelocity;
connect followTrajectory.maxVelocity -> d2v2.maxVelocity;
connect isDriveToFirstPosition.result -> selector.isDriveToFirstPosition;
connect driveToFirstPosition.desiredDirectionX -> selector.desiredDirectionX1;
connect driveToFirstPosition.desiredDirectionY -> selector.desiredDirectionY1;
connect driveToFirstPosition.signedDistanceToTrajectory -> selector.signedDistanceToTrajectory1;
connect d2v1.velocity -> selector.desiredVelocity1;
connect followTrajectory.desiredDirectionX -> selector.desiredDirectionX2;
connect followTrajectory.desiredDirectionY -> selector.desiredDirectionY2;
connect followTrajectory.signedDistanceToTrajectory -> selector.signedDistanceToTrajectory2;
connect d2v2.velocity -> selector.desiredVelocity2;
connect selector.desiredDirectionX -> desiredDirectionX;
connect selector.desiredDirectionY -> desiredDirectionY;
connect selector.signedDistanceToTrajectory -> signedDistanceToTrajectory;
connect selector.desiredVelocity -> desiredVelocity;
}
package de.rwth.armin.modeling.autopilot.behavior;
import de.rwth.armin.modeling.autopilot.common.*;
component Distance2Velocity {
port
in Q (0.0 m : 0.01 m : oo m) distance,
in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) minVelocity,
in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) maxVelocity,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) velocity;
implementation Math {
// exponent
static Q D1 = 1.0 m;
static Q V1 = 1.0 m/s;
static Q D2 = 15.0 m;
static Q V2 = 7.0 m/s;
static Q COEF_K = log( (V1 / V2) ) / (D2 - D1);
Q v = 0.0;
if (distance < D1)
v = 0.0;
elseif (distance >= D2)
v = V2;
else
v = V1 * exp( -COEF_K * (distance - D1) );
end
if (v < minVelocity)
v = minVelocity;
elseif (v > maxVelocity)
v = maxVelocity;
end
velocity = v;
}
}
package de.rwth.armin.modeling.autopilot.behavior;
component DriveToFirstPosition {
port
in B isDriveToFirstPosition,
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
in Z (0 : 100) trimmedTrajectoryLength,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryY,
out Q (-oo m : 0.01 m : oo m) desiredDirectionX,
out Q (-oo m : 0.01 m : oo m) desiredDirectionY,
out Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) minVelocity,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) maxVelocity,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) distance;
implementation Math {
desiredDirectionX = 0.0;
desiredDirectionY = 0.0;
signedDistanceToTrajectory = 0.0;
minVelocity = 0.0;
maxVelocity = 0.0;
distance = 0.0;
if (isDriveToFirstPosition) && (trimmedTrajectoryLength >= 1)
maxVelocity = 7.0;
desiredDirectionX = trimmedTrajectoryX(1,1) - currentPositionX;
desiredDirectionY = trimmedTrajectoryY(1,1) - currentPositionY;
Q sqr_dist = desiredDirectionX * desiredDirectionX + desiredDirectionY * desiredDirectionY;
distance = sqrt( sqr_dist );
if (trimmedTrajectoryLength >= 2)
minVelocity = 3.0;
end
end
}
}
package de.rwth.armin.modeling.autopilot.behavior;
component FollowTrajectory {
port
in B isDriveToFirstPosition,
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
in Z (0 : 100) trimmedTrajectoryLength,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryY,
out Q (-oo m : 0.01 m : oo m) desiredDirectionX,
out Q (-oo m : 0.01 m : oo m) desiredDirectionY,
out Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) minVelocity,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) maxVelocity,
out Q (0.0 m/s : 0.01 m/s : 13.0 m/s) distance;
implementation Math {
// change to params later
static Q BEZIER_COEF = 0.1;
static Q TURN_ANGLE_THRESHOLD = 10.0 * M_PI / 180.0; // 10 degrees
static Q EPSILON = 0.00001;
static Q REACHED_POSITION_THRESHOLD = 5.0;
desiredDirectionX = 0.0;
desiredDirectionY = 0.0;
signedDistanceToTrajectory = 0.0;
minVelocity = 0.0;
maxVelocity = 0.0;
distance = 0.0;
if (isDriveToFirstPosition)
Q doNothing;
elseif (trimmedTrajectoryLength >= 2)
maxVelocity = 7.0;
// SIGNED DISTANCE
Q p1x = trimmedTrajectoryX(1,1);
Q p1y = trimmedTrajectoryY(1,1);
Q p2x = trimmedTrajectoryX(1,2);
Q p2y = trimmedTrajectoryY(1,2);
Q v12x = p2x - p1x;
Q v12y = p2y - p1y;
Q v12_sqr_norm = v12x*v12x + v12y*v12y;
Q v12_norm = sqrt(v12_sqr_norm);
if (v12_norm > EPSILON)
// https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
signedDistanceToTrajectory = -(v12y*currentPositionX - v12x*currentPositionY + p2x*p1y - p1x*p2y) / v12_norm;
end
// DESIRED DIRECTION
Q nextX = currentPositionX;
Q nextY = currentPositionY;
if (trimmedTrajectoryLength >= 3)
Q interp_p1x = trimmedTrajectoryX(1,1);
Q interp_p1y = trimmedTrajectoryY(1,1);
Q interp_p2x = trimmedTrajectoryX(1,2);
Q interp_p2y = trimmedTrajectoryY(1,2);
Q interp_p3x = trimmedTrajectoryX(1,3);
Q interp_p3y = trimmedTrajectoryY(1,3);
// Bezier interpolation
Q t = BEZIER_COEF; // higher value => closer to p3
Q a = 1.0 - t;
Q b = t;
Q k1 = a * a;
Q k2 = 2 * a * b;
Q k3 = b * b;
nextX = k1 * interp_p1x + k2 * interp_p2x + k3 * interp_p3x;
nextY = k1 * interp_p1y + k2 * interp_p2y + k3 * interp_p3y;
Q dist_to_next_sqr = (nextX-currentPositionX)*(nextX-currentPositionX) + (nextY-currentPositionY)*(nextY-currentPositionY);
Q dist_to_next = sqrt(dist_to_next_sqr);
if (dist_to_next <= REACHED_POSITION_THRESHOLD)
// interpolated point is too close, another point needs to be found
Q is_go_on_1 = 1;
for j = 2:trimmedTrajectoryLength
if (is_go_on_1)
Q point_x = trimmedTrajectoryX(1,j);
Q point_y = trimmedTrajectoryY(1,j);
Q dist_to_point_sqr = (point_x-currentPositionX)*(point_x-currentPositionX) + (point_y-currentPositionY)*(point_y-currentPositionY);
Q dist_to_point = sqrt(dist_to_point_sqr);
if (dist_to_point > REACHED_POSITION_THRESHOLD)
nextX = point_x;
nextY = point_y;
is_go_on_1 = 0;
end
end
end
if (is_go_on_1)
// all points are too close
nextX = trimmedTrajectoryX(1,trimmedTrajectoryLength);
nextY = trimmedTrajectoryY(1,trimmedTrajectoryLength);
end
end
else
// only 2 points left
nextX = p2x;
nextY = p2y;
end
desiredDirectionX = nextX - currentPositionX;
desiredDirectionY = nextY - currentPositionY;
// DISTANCE
Q dist = sqrt( (p2x-p1x)*(p2x-p1x) + (p2y-p1y)*(p2y-p1y) );
if (trimmedTrajectoryLength >= 3)
minVelocity = 3.0;
Q is_go_on_2 = 1;
Q lastIndex = trimmedTrajectoryLength - 2;
for i = 1:lastIndex
if (is_go_on_2)
Q pt1x = trimmedTrajectoryX(1,i);
Q pt1y = trimmedTrajectoryY(1,i);