Commit 1d0572b8 authored by Alexander Ryndin's avatar Alexander Ryndin
Browse files

autopilot modeling

parent 4e429fdd
package de.monticore.lang.monticar.generator.cpp;
import de.monticore.lang.math.math._symboltable.expression.*;
import de.monticore.lang.math.math._symboltable.expression.MathArithmeticExpressionSymbol;
import de.monticore.lang.math.math._symboltable.expression.MathExpressionSymbol;
import de.monticore.lang.math.math._symboltable.expression.MathNameExpressionSymbol;
import de.monticore.lang.math.math._symboltable.expression.MathParenthesisExpressionSymbol;
import de.monticore.lang.math.math._symboltable.expression.MathValueExpressionSymbol;
import de.monticore.lang.math.math._symboltable.matrix.MathMatrixAccessSymbol;
import de.monticore.lang.math.math._symboltable.matrix.MathMatrixExpressionSymbol;
import de.monticore.lang.math.math._symboltable.matrix.MathMatrixNameExpressionSymbol;
......@@ -8,7 +12,6 @@ import de.monticore.lang.monticar.generator.FileContent;
import de.monticore.lang.monticar.generator.Variable;
import de.monticore.lang.monticar.generator.cpp.converter.ComponentConverter;
import de.monticore.lang.monticar.generator.cpp.converter.ExecuteMethodGenerator;
import de.monticore.lang.monticar.generator.cpp.converter.MathConverter;
import de.se_rwth.commons.logging.Log;
import java.util.ArrayList;
......@@ -153,7 +156,8 @@ public class OctaveHelper {
return "Double";
else if (mathExpressionSymbol.isNameExpression()) {
Variable variable = ComponentConverter.currentBluePrint.getMathInformationRegister().getVariable(((MathNameExpressionSymbol) mathExpressionSymbol).getNameToResolveValue());
return variable.getVariableType().getTypeNameTargetLanguage();
String typeName = variable.getVariableType().getTypeNameTargetLanguage();
return typeName.substring(0, 1).toUpperCase() + typeName.substring(1);
} else {
Log.error("OctaveHelper Case not handled!");
}
......
......@@ -56,11 +56,11 @@ component Autopilot {
connect compass -> c2cd.compass;
connect x -> behaviorGeneration.current_position_x;
connect y -> behaviorGeneration.current_position_y;
connect trajectory_length -> behaviorGeneration.planned_trajectory_length;
connect trajectory_x -> behaviorGeneration.planned_trajectory_x;
connect trajectory_y -> behaviorGeneration.planned_trajectory_y;
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.current_direction_x -> behaviorGeneration.current_direction_x;
connect c2cd.current_direction_x -> behaviorGeneration.current_direction_x;
......
......@@ -2,15 +2,15 @@ package de.rwth.armin.modeling.autopilot.behavior;
component BehaviorGeneration {
port
in Q (-oo m : 0.01 m : oo m) current_position_x,
in Q (-oo m : 0.01 m : oo m) current_position_y,
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) current_direction_x,
in Q (-oo m : 0.01 m : oo m) current_direction_y,
in Z (0 : 100) planned_trajectory_length,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} planned_trajectory_x,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} planned_trajectory_y,
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) desired_direction_x,
out Q (-oo m : 0.01 m : oo m) desired_direction_y,
......@@ -20,19 +20,19 @@ component BehaviorGeneration {
instance TrimPath tp;
instance CalculateMotionCommands cmc;
connect current_position_x -> tp.current_position_x;
connect current_position_y -> tp.current_position_y;
connect planned_trajectory_length -> tp.planned_trajectory_length;
connect planned_trajectory_x -> tp.planned_trajectory_x;
connect planned_trajectory_y -> tp.planned_trajectory_y;
connect currentPositionX -> tp.currentPositionX;
connect currentPositionY -> tp.currentPositionY;
connect plannedTrajectoryLength -> tp.plannedTrajectoryLength;
connect plannedTrajectoryX -> tp.plannedTrajectoryX;
connect plannedTrajectoryY -> tp.plannedTrajectoryY;
connect current_position_x -> cmc.current_position_x;
connect current_position_y -> cmc.current_position_y;
connect currentPositionX -> cmc.currentPositionX;
connect currentPositionY -> cmc.currentPositionY;
connect current_direction_x -> cmc.current_direction_x;
connect current_direction_y -> cmc.current_direction_y;
connect tp.trimmed_trajectory_length -> cmc.trimmed_trajectory_length;
connect tp.trimmed_trajectory_x -> cmc.trimmed_trajectory_x;
connect tp.trimmed_trajectory_y -> cmc.trimmed_trajectory_y;
connect tp.trimmedTrajectoryLength -> cmc.trimmedTrajectoryLength;
connect tp.trimmedTrajectoryX -> cmc.trimmedTrajectoryX;
connect tp.trimmedTrajectoryY -> cmc.trimmedTrajectoryY;
connect cmc.desired_direction_x -> desired_direction_x;
connect cmc.desired_direction_y -> desired_direction_y;
......
......@@ -2,85 +2,68 @@ package de.rwth.armin.modeling.autopilot.behavior;
component CalculateMotionCommands {
port
in Q (-oo m : 0.01 m : oo m) current_position_x,
in Q (-oo m : 0.01 m : oo m) current_position_y,
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) current_direction_x,
in Q (-oo m : 0.01 m : oo m) current_direction_y,
in Z (0 : 100) trimmed_trajectory_length,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmed_trajectory_x,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmed_trajectory_y,
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) desired_direction_x,
out Q (-oo m : 0.01 m : oo m) desired_direction_y,
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;
implementation Math {
// maybe it can be moved to parameter later
Q TOO_FAR_FROM_TRAJECTORY_THRESHOLD = 3.0;
Q NUMBER_OF_INTERMEDIATE_INTERPOLATED_POINTS = 10.0;
Q TURN_ANGLE_THRESHOLD = 10.0 * M_PI / 180.0; // 10 degrees in radians
desired_direction_x = 0.0;
desired_direction_y = 0.0;
signedDistanceToTrajectory = 0.0;
desiredVelocity = 0.0;
if trimmed_trajectory_length > 0
// assumption: 1st point of trajectory is the projection of current vehicle's position on trajectory
Q p1x = trimmed_trajectory_x(1,1);
Q p1y = trimmed_trajectory_y(1,1);
Q dist_to_trajectory = sqrt( (p1x - current_position_x) ^ 2 + (p1y - current_position_y) ^ 2 );
Q is_vehicle_went_off_trajectory = dist_to_trajectory > TOO_FAR_FROM_TRAJECTORY_THRESHOLD;
Q is_only_one_position_specified = trimmed_trajectory_length == 1;
Q is_drive_to_first_position = is_vehicle_went_off_trajectory || is_only_one_position_specified;
if is_drive_to_first_position
desired_direction_x = p1x - current_position_x;
desired_direction_y = p1y - current_position_y;
signedDistanceToTrajectory = 0.0;
desiredVelocity = 0.0; // TODO calc. as function of dist_to_trajectory
else
// vehicle is on track AND there is at least two points in trajectory
// TODO calc. signedDistanceToTrajectory
// using current position and first trajectory segment
// see de.rwth.ryndin.modelling.autopilot.component.common.CalculateSignedDistanceToLineImplementation
Q signedDistanceToTrajectory = 0.0;
// points for interpolation
Q interp_p1x = current_position_x;
Q interp_p1y = current_position_y;
Q interp_p2x = trimmed_trajectory_x(1,1);
Q interp_p2y = trimmed_trajectory_y(1,1);
Q interp_p3x = trimmed_trajectory_x(1,2);
Q interp_p3y = trimmed_trajectory_y(1,2);
if trimmed_trajectory_length > 2
// if there is at least 3 points
// then do not use the projection point
// but use 2nd and 3rd trajectory points instead
Q interp_p2x = trimmed_trajectory_x(1,2);
Q interp_p2y = trimmed_trajectory_y(1,2);
Q interp_p3x = trimmed_trajectory_x(1,3);
Q interp_p3y = trimmed_trajectory_y(1,3);
end
// Bezier interpolation (only the 1st intermediate point is taken)
// de.rwth.ryndin.modelling.autopilot.geometry.Interpolation
Q t = 1.0 / (NUMBER_OF_INTERMEDIATE_INTERPOLATED_POINTS+1.0);
Q a = 1.0 - t;
Q b = t;
Q k1 = a * a;
Q k2 = 2 * a * b;
Q k3 = b * b;
Q next_x = k1 * interp_p1x + k2 * interp_p2x + k3 * interp_p3x;
Q next_y = k1 * interp_p1y + k2 * interp_p2y + k3 * interp_p3y;
desired_direction_x = next_x - current_position_x;
desired_direction_y = next_y - current_position_y;
// TODO go along the trajectory
// and calculate the distance d_turn to the next turn.
// if angle >= TURN_ANGLE_THRESHOLD then two segments form a turn
// then calc. desiredVelocity as function of d_turn
// example:
// de.rwth.ryndin.modelling.autopilot.component.behavior.Distance2VelocityFunction
// or use something similar
end
end
}
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.desired_direction_x -> selector.desiredDirectionX1;
connect driveToFirstPosition.desired_direction_y -> selector.desiredDirectionY1;
connect driveToFirstPosition.signedDistanceToTrajectory -> selector.signedDistanceToTrajectory1;
connect d2v1.velocity -> selector.desiredVelocity1;
connect followTrajectory.desired_direction_x -> selector.desiredDirectionX2;
connect followTrajectory.desired_direction_y -> selector.desiredDirectionY2;
connect followTrajectory.signedDistanceToTrajectory -> selector.signedDistanceToTrajectory2;
connect d2v2.velocity -> selector.desiredVelocity2;
connect selector.desiredDirectionX -> desired_direction_x;
connect selector.desiredDirectionY -> desired_direction_y;
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;
static Q V1 = 1.0;
static Q D2 = 100.0;
static Q V2 = 13.0;
static Q COEF_K = log( (V1 / V2) ) / (D2 - D1);
Q v = 0.0;
if distance > D1
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) desired_direction_x,
out Q (-oo m : 0.01 m : oo m) desired_direction_y,
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 {
desired_direction_x = 0.0;
desired_direction_y = 0.0;
signedDistanceToTrajectory = 0.0;
minVelocity = 0.0;
maxVelocity = 13.0;
distance = 0.0;
if (isDriveToFirstPosition) && (trimmedTrajectoryLength > 0)
Q nextX = trimmedTrajectoryX(1,1);
Q nextY = trimmedTrajectoryY(1,1);
desired_direction_x = nextX - currentPositionX;
desired_direction_y = nextY - currentPositionY;
Q sqr_dist = desired_direction_x * desired_direction_x + desired_direction_y * desired_direction_y;
distance = sqrt( sqr_dist );
if (trimmedTrajectoryLength > 1)
minVelocity = 1.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) desired_direction_x,
out Q (-oo m : 0.01 m : oo m) desired_direction_y,
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 NUMBER_OF_INTERMEDIATE_INTERPOLATED_POINTS = 10.0;
static Q TURN_ANGLE_THRESHOLD = 10.0 * M_PI / 180.0; // 10 degrees
desired_direction_x = 0.0;
desired_direction_y = 0.0;
signedDistanceToTrajectory = 0.0;
minVelocity = 0.0;
maxVelocity = 13.0;
distance = 0.0;
if isDriveToFirstPosition
Q doNothing;
elseif (trimmedTrajectoryLength >= 2)
// 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;
if (v12_sqr_norm > 0.00001)
// https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
Q v12_norm = sqrt(v12_sqr_norm);
signedDistanceToTrajectory = -(v12y*currentPositionX - v12x*currentPositionY + p2x*p1y - p1x*p2y) / v12_norm;
end
// DESIRED DIRECTION
Q interp_p1x = currentPositionX;
Q interp_p1y = currentPositionY;
Q interp_p2x = p1x;
Q interp_p2y = p1y;
Q interp_p3x = p2x;
Q interp_p3y = p2y;
if (trimmedTrajectoryLength >= 3)
Q interp_p2x = p2x;
Q interp_p2y = p2y;
Q interp_p3x = trimmedTrajectoryX(1,3);
Q interp_p3y = trimmedTrajectoryY(1,3);
end
// Bezier interpolation
Q t = 1.0 / (NUMBER_OF_INTERMEDIATE_INTERPOLATED_POINTS + 1.0);
Q a = 1.0 - t;
Q b = t;
Q k1 = a * a;
Q k2 = 2 * a * b;
Q k3 = b * b;
Q nextX = k1 * interp_p1x + k2 * interp_p2x + k3 * interp_p3x;
Q nextY = k1 * interp_p1y + k2 * interp_p2y + k3 * interp_p3y;
desired_direction_x = nextX - currentPositionX;
desired_direction_y = nextY - currentPositionY;
// DISTANCE
Q dist = sqrt( (p2x-p1x)*(p2x-p1x) + (p2y-p1y)*(p2y-p1y) );
if (trimmedTrajectoryLength >= 3)
minVelocity = 1.0;
Q is_go_on = 1;
Q lastIndex = trimmedTrajectoryLength - 2;
for i = 1:lastIndex
if is_go_on
Q pt1x = plannedTrajectoryX(1,i);
Q pt1y = plannedTrajectoryY(1,i);
Q pt2x = plannedTrajectoryX(1,i+1);
Q pt2y = plannedTrajectoryY(1,i+1);
Q pt3x = plannedTrajectoryX(1,i+2);
Q pt3y = plannedTrajectoryY(1,i+2);
Q vect1x = pt2x - pt1x;
Q vect1y = pt2y - pt1y;
Q vect2x = pt3x - pt2x;
Q vect2y = pt3y - pt2y;
Q vect1_norm = sqrt( vect1x*vect1x + vect1y*vect1y );
Q vect2_norm = sqrt( vect2x*vect2x + vect2y*vect2y );
Q cos_alpha = (vect1x*vect2x + vect1y*vect2y) / vect1_norm / vect2_norm;
Q alpha = acos(cos_alpha);
if (alpha > TURN_ANGLE_THRESHOLD)
is_go_on = 0;
else
dist += vect2_norm;
end
end
end
end
distance = dist;
end
}
}
package de.rwth.armin.modeling.autopilot.behavior;
component IsDriveToFirstPosition {
port
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 B result;
implementation Math {
// maybe it can be moved to parameter later
Q TOO_FAR_FROM_TRAJECTORY_THRESHOLD = 3.0;
result = 1;
if trimmedTrajectoryLength > 0
Q p1x = trimmedTrajectoryX(1,1);
Q p1y = trimmedTrajectoryY(1,1);
Q dx = p1x - currentPositionX;
Q dy = p1y - currentPositionY;
Q sqr_dist = dx*dx + dy*dy;
Q dist_to_trajectory = sqrt( sqr_dist );
Q is_vehicle_went_off_trajectory = dist_to_trajectory > TOO_FAR_FROM_TRAJECTORY_THRESHOLD;
Q is_only_one_position_specified = trimmedTrajectoryLength <= 1;
result = is_vehicle_went_off_trajectory || is_only_one_position_specified;
end
}
}
package de.rwth.armin.modeling.autopilot.behavior;
component Selector {
port
in B isDriveToFirstPosition,
in Q (-oo m : 0.01 m : oo m) desiredDirectionX1,
in Q (-oo m : 0.01 m : oo m) desiredDirectionY1,
in Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory1,
in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity1,
in Q (-oo m : 0.01 m : oo m) desiredDirectionX2,
in Q (-oo m : 0.01 m : oo m) desiredDirectionY2,
in Q (-oo m : 0.01 m : oo m) signedDistanceToTrajectory2,
in Q (0.0 m/s : 0.01 m/s : 13.0 m/s) desiredVelocity2,
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;
implementation Math {
if isDriveToFirstPosition
desiredDirectionX = desiredDirectionX1;
desiredDirectionY = desiredDirectionY1;
signedDistanceToTrajectory = signedDistanceToTrajectory1;
desiredVelocity = desiredVelocity1;
else
desiredDirectionX = desiredDirectionX2;
desiredDirectionY = desiredDirectionY2;
signedDistanceToTrajectory = signedDistanceToTrajectory2;
desiredVelocity = desiredVelocity2;
end
}
}
......@@ -9,57 +9,55 @@ import de.rwth.armin.modeling.autopilot.common.*;
// of the current position to the planned path
component TrimPath {
port
in Q (-oo m : 0.01 m : oo m) current_position_x,
in Q (-oo m : 0.01 m : oo m) current_position_y,
in Z (0 : 100) planned_trajectory_length,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} planned_trajectory_x,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} planned_trajectory_y,
out Z (0 : 100) trimmed_trajectory_length,
out Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmed_trajectory_x,
out Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmed_trajectory_y;
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
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 Z (0 : 100) trimmedTrajectoryLength,
out Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryX,
out Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryY;
implementation Math {
trimmed_trajectory_length = 0;
if planned_trajectory_length == 1
trimmed_trajectory_length = 1;
trimmed_trajectory_x(1,1) = planned_trajectory_x(1,1);
trimmed_trajectory_y(1,1) = planned_trajectory_y(1,1);
elseif planned_trajectory_length > 1
trimmedTrajectoryLength = 0;
if plannedTrajectoryLength == 1
trimmedTrajectoryLength = 1;
trimmedTrajectoryX(1,1) = plannedTrajectoryX(1,1);
trimmedTrajectoryY(1,1) = plannedTrajectoryY(1,1);
elseif plannedTrajectoryLength > 1
Q closestSegmentIndex = -1;
Q closestSegmentDistance = -1;
//Q last_segment_index = planned_trajectory_length-1;
for i = 1:planned_trajectory_length
Q p1x = planned_trajectory_x(1,i);
Q p1y = planned_trajectory_y(1,i);
Q p2x = planned_trajectory_x(1,i+1);
Q p2y = planned_trajectory_y(1,i+1);
Q lastSegmentIndex = plannedTrajectoryLength - 1;
for i = 1:lastSegmentIndex
Q p1x = plannedTrajectoryX(1,i);
Q p1y = plannedTrajectoryY(1,i);
Q p2x = plannedTrajectoryX(1,i+1);
Q p2y = plannedTrajectoryY(1,i+1);
// projection
Q vx = current_position_x - p1x;
Q vy = current_position_y - p1y;
Q vx = currentPositionX - p1x;
Q vy = currentPositionY - p1y;
Q v12x = p2x - p1x;
Q v12y = p2y - p1y;
Q k = ( vx*v12x + vy*v12y ) / ( v12x*v12x + v12y*v12y );
Q projection_x = p1x + k * v12x;
Q projection_y = p1y + k * v12y;
//B cond1 = (p1x-projection_x) * (p2x-projection_x) <= 0 && (p1y-projection_y) * (p2y-projection_y) <= 0;
Q cond1 = (p1x-projection_x) * (p2x-projection_x) <= 0 && (p1y-projection_y) * (p2y-projection_y) <= 0;
if cond1