diff --git a/.gitignore b/.gitignore index dfac210..1fd7ddb 100644 --- a/.gitignore +++ b/.gitignore @@ -4,7 +4,6 @@ *.autosave *.slx.r* *.mdl.r* -*.bak # Derived content-obscured files *.p diff --git a/@miSim/initialize.m b/@miSim/initialize.m index 91c5fe6..060bd5f 100644 --- a/@miSim/initialize.m +++ b/@miSim/initialize.m @@ -72,18 +72,16 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m obj.constraintAdjacencyMatrix = logical(eye(size(agents, 1))); % Set labels for agents and collision geometries in cases where they - % were not provieded at the time of their initialization. - % Guarded by coder.target: label is a fixed-size "" in codegen, and - % sprintf returns variable-length — incompatible even in a dead branch. - % On the compiled path agents always receive explicit labels from the caller. - if coder.target('MATLAB') - for ii = 1:size(obj.agents, 1) - if isempty(char(obj.agents{ii}.label)) - obj.agents{ii}.label = sprintf("Agent %d", int8(ii)); - end - if isempty(char(obj.agents{ii}.collisionGeometry.label)) - obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii)); - end + % were not provieded at the time of their initialization + for ii = 1:size(obj.agents, 1) + % Agent + if isempty(char(obj.agents{ii}.label)) + obj.agents{ii}.label = sprintf("Agent %d", int8(ii)); + end + + % Collision geometry + if isempty(char(obj.agents{ii}.collisionGeometry.label)) + obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii)); end end diff --git a/aerpaw/config/scenario.csv b/aerpaw/config/scenario.csv index 4f685c0..f155cfc 100644 --- a/aerpaw/config/scenario.csv +++ b/aerpaw/config/scenario.csv @@ -1,2 +1,2 @@ timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology -1, 80, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "15.0, 15.0, 50.0, 40.0, 10.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1 \ No newline at end of file +1, 100, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 80.0", "0.25, 0.25", "8.0, 8.0", "0.1, 0.1", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "66.6, 66.6", "55, 35, 35, 55", 0.15, "15.0, 15.0, 50.0, 40.0, 15.0, 50.0", 1, "0.0, 35.0, 0.0", "50, 40.0, 60", 1, 2.0, 1 \ No newline at end of file diff --git a/aerpaw/config/scenario_columns.csv b/aerpaw/config/scenario_columns.csv index 2f8e7db..1e11351 100644 --- a/aerpaw/config/scenario_columns.csv +++ b/aerpaw/config/scenario_columns.csv @@ -1,2 +1,2 @@ timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology -1, 80, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "15.0, 15.0, 50.0, 40.0, 10.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1 +1, 50, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "60.0, 80.0, 45.0, 70.0", "70, 15, 15, 20, 20, 15, 15, 70", 0.15, "10.0, 10.0, 50.0, 40.0, 15.0, 45.0", 8, "0.0, 30.0, 0.0, 42.0, 30.0, 0.0, 84.0, 30.0, 0.0, 13.0, 60.0, 0.0, 55.0, 60.0, 0.0, 0.0, 90, 0.0, 42.0, 90.0, 0.0, 84.0, 90.0, 0.0", "16.0, 40.0, 100.0, 58.0, 40.0, 100.0, 100.0, 40.0, 100.0, 29.0, 70.0, 100.0, 71.0, 70.0, 100.0, 16.0, 100.0, 100.0, 58.0, 100.0, 100.0, 100.0, 100.0, 100.0", 0, 2.0, 1 \ No newline at end of file diff --git a/aerpaw/config/scenario_lock.csv b/aerpaw/config/scenario_lock.csv index f44d623..3dbf3b4 100644 --- a/aerpaw/config/scenario_lock.csv +++ b/aerpaw/config/scenario_lock.csv @@ -1,2 +1,2 @@ timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology -1, 125, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "65.0, 15.0, 65.0, 65.0, 15.0, 45.0", 3, "0.0, 25.0, 55.0, 40.0, 10.0, 0.0, 40.0, 45.0, 60.0", "100.0, 70.0, 60.0, 45.0, 80.0, 55.0, 100.0, 50.0, 100.0", 0, 2.0, 1 +1, 65, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "65.0, 15.0, 65.0, 65.0, 15.0, 45.0", 3, "0.0, 25.0, 55.0, 40.0, 10.0, 0.0, 40.0, 45.0, 60.0", "100.0, 70.0, 60.0, 45.0, 80.0, 55.0, 100.0, 50.0, 100.0", 0, 2.0, 1 \ No newline at end of file diff --git a/aerpaw/config/scenario_walls.csv b/aerpaw/config/scenario_walls.csv index 7a8926c..1dd65d4 100644 --- a/aerpaw/config/scenario_walls.csv +++ b/aerpaw/config/scenario_walls.csv @@ -1,2 +1,2 @@ timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology -1, 125, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "90.0, 10.0, 50.0, 65.0, 15.0, 45.0", 4, "0.0, 30.0, 0.0, 70.0, 30.0, 0.0, 0.0, 60.0, 0.0, 55.0, 60.0, 0.0", "50.0, 40.0, 100.0, 100.0, 40.0, 100.0, 35.0, 70.0, 100.0, 100.0, 70.0, 100.0", 0, 2.0, 1 +1, 65, 35.0, 0.1, 2.0, 6, 1, 1, "8.0, 8.0", "35.0, 35.0", "80.0, 50.0", "0.25, 1.0", "8.0, 25.0", "0.1, 0.02", "0.0, 0.0, 0.0", "100.0, 100.0, 100.0", "30.0, 80.0", "60, 20, 20, 30", 0.15, "90.0, 10.0, 50.0, 65.0, 15.0, 45.0", 4, "0.0, 30.0, 0.0, 70.0, 30.0, 0.0, 0.0, 60.0, 0.0, 55.0, 60.0, 0.0", "50.0, 40.0, 100.0, 100.0, 40.0, 100.0, 35.0, 70.0, 100.0, 100.0, 70.0, 100.0", 0, 2.0, 1 \ No newline at end of file diff --git a/aerpaw/controller.m b/aerpaw/controller.m index e105862..a62197f 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -7,43 +7,54 @@ coder.extrinsic('disp', 'readScenarioCsv'); % Maximum clients supported (one initial position per UAV) MAX_CLIENTS = 4; -% Three waypoints per UAV: one axis-aligned move per dimension (taxicab flyout/flyback) -MAX_TARGETS = MAX_CLIENTS * 3; - -% Taxicab flyout/flyback only supports exactly 2 UAVs -if numClients ~= int32(2) - error('Taxicab flyout/flyback requires exactly 2 UAVs'); -end +% Two waypoints per UAV: altitude-staggered transit + final position +MAX_TARGETS = MAX_CLIENTS * 2; % Allocate targets array (MAX_TARGETS x 3) targets = zeros(MAX_TARGETS, 3); numWaypoints = int32(0); totalLoaded = int32(0); % pre-declare type for coder.ceval %#ok -% Experiment start positions from scenario CSV (N x 3) -scenarioPositions = zeros(MAX_CLIENTS, 3); - -% Load experiment start positions from scenario CSV +% Load initial UAV positions from scenario CSV if coder.target('MATLAB') disp('Loading initial positions from scenario.csv (simulation)...'); tmpSim = miSim; sc = tmpSim.readScenarioCsv('aerpaw/config/scenario.csv'); flatPos = double(sc.initialPositions); % 1×(3*N) flat vector - posMatrix = reshape(flatPos, 3, [])'; % N×3 + posMatrix = reshape(flatPos, 3, [])'; % N×3, same layout as initializeFromCsv totalLoaded = int32(size(posMatrix, 1)); - scenarioPositions(1:totalLoaded, :) = posMatrix; - % MATLAB path: send directly to scenario positions in one waypoint targets(1:totalLoaded, :) = posMatrix; numWaypoints = int32(1); disp(['Loaded ', num2str(double(totalLoaded)), ' initial positions']); else coder.cinclude('controller_impl.h'); filename = ['config/scenario.csv', char(0)]; - % Load into targets temporarily; copy to scenarioPositions below totalLoaded = coder.ceval('loadInitialPositions', coder.ref(filename), ... coder.ref(targets), int32(MAX_TARGETS)); - scenarioPositions(1:totalLoaded, :) = targets(1:totalLoaded, :); - numWaypoints = int32(3); + numWaypoints = totalLoaded / int32(numClients); +end + +% In the compiled path, inject altitude-staggered transit waypoints so UAVs +% are vertically separated while flying horizontally to their start positions. +% ArduPilot reaches target altitude before horizontal movement, so UAV i is at +% altitude (TRANSIT_ALT_BASE + (i-1)*TRANSIT_ALT_STEP) throughout its transit, +% preventing collisions regardless of horizontal path geometry. +% TRANSIT_ALT_STEP must exceed 2 * max(collisionRadius). +% Waypoint 1: each UAV flies to (finalX, finalY) at its unique transit altitude. +% Waypoint 2: each UAV adjusts to its actual target altitude. +% These constants are also used for the altitude-staggered return before RTL. +TRANSIT_ALT_BASE = 25.0; % must match drone.takeoff() altitude in uav_runner.py +TRANSIT_ALT_STEP = 25; % vertical separation per UAV (m); must exceed 2*collisionRadius +if ~coder.target('MATLAB') + for ii = double(totalLoaded):-1:1 + transitRow = (ii - 1) * 2 + 1; + finalRow = (ii - 1) * 2 + 2; + finalPos = targets(ii, :); + transitAlt = TRANSIT_ALT_BASE + (ii - 1) * TRANSIT_ALT_STEP; + targets(finalRow, :) = finalPos; + targets(transitRow, :) = [finalPos(1), finalPos(2), transitAlt]; + end + numWaypoints = int32(2); end % Load guidance scenario from CSV (parameters for guidance_step) @@ -81,46 +92,6 @@ for i = 1:numClients end end -% Query takeoff-pad GPS positions before sending any waypoints. -% These are also the flyback targets so each UAV lands where it took off. -initialPositions = zeros(MAX_CLIENTS, 3); -if ~coder.target('MATLAB') - coder.ceval('sendRequestPositions', int32(numClients)); - coder.ceval('recvPositions', int32(numClients), coder.ref(initialPositions), int32(MAX_CLIENTS)); -else - % Simulation: treat scenario positions as the takeoff locations - initialPositions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :); -end - -% ---- Build taxicab flyout waypoints ------------------------------------------ -% Determine which UAV has the higher final altitude; it moves Z first so it -% clears vertical separation before the lower UAV converges on the same X/Y. -% Higher UAV order: Z → Y → X -% Lower UAV order: X → Y → Z -if ~coder.target('MATLAB') - if scenarioPositions(1, 3) >= scenarioPositions(2, 3) - higherIdx = int32(1); - lowerIdx = int32(2); - else - higherIdx = int32(2); - lowerIdx = int32(1); - end - - hBase = double(higherIdx - 1) * double(numWaypoints); - lBase = double(lowerIdx - 1) * double(numWaypoints); - - % Higher UAV: Z first - targets(hBase + 1, :) = [initialPositions(higherIdx,1), initialPositions(higherIdx,2), scenarioPositions(higherIdx,3)]; - targets(hBase + 2, :) = [initialPositions(higherIdx,1), scenarioPositions(higherIdx,2), scenarioPositions(higherIdx,3)]; - targets(hBase + 3, :) = scenarioPositions(higherIdx, :); - - % Lower UAV: X first - targets(lBase + 1, :) = [scenarioPositions(lowerIdx,1), initialPositions(lowerIdx,2), initialPositions(lowerIdx,3)]; - targets(lBase + 2, :) = [scenarioPositions(lowerIdx,1), scenarioPositions(lowerIdx,2), initialPositions(lowerIdx,3)]; - targets(lBase + 3, :) = scenarioPositions(lowerIdx, :); -end -% ------------------------------------------------------------------------------ - % Waypoint loop: send each waypoint to all clients, wait for all to arrive for w = 1:numWaypoints % Send TARGET for waypoint w to each client @@ -156,13 +127,8 @@ for w = 1:numWaypoints end % ---- Phase 2: miSim guidance loop ---------------------------------------- -% Number of guidance steps comes from maxIter in scenario.csv (scenarioParams(2)) -% so the controller and the agent step-decay schedule stay in sync. -if coder.target('MATLAB') - MAX_GUIDANCE_STEPS = int32(sc.maxIter); -else - MAX_GUIDANCE_STEPS = int32(scenarioParams(2)); -end +% Guidance parameters (adjust here and recompile as needed) +MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations % Enter guidance mode on all clients if ~coder.target('MATLAB') @@ -175,8 +141,8 @@ if ~coder.target('MATLAB') coder.ceval('sendRequestPositions', int32(numClients)); coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS)); else - % Simulation: seed positions from scenario positions so agents don't start at origin - positions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :); + % Simulation: seed positions from CSV waypoints so agents don't start at origin + positions(1:totalLoaded, :) = targets(1:totalLoaded, :); end guidance_step(positions(1:numClients, :), true, ... scenarioParams, obstacleMin, obstacleMax, numObstacles); @@ -231,48 +197,20 @@ if ~coder.target('MATLAB') end % -------------------------------------------------------------------------- -% ---- Taxicab flyback: return each UAV to its takeoff-pad position --------- -% The UAV that ended guidance at the higher altitude moves Z last (X → Y → Z) -% so it stays high while the lower UAV descends first, maintaining separation -% as both converge back on their respective home X/Y positions. -NUM_RETURN_WP = int32(3); -returnTargets = zeros(MAX_TARGETS, 3); - +% Altitude-staggered return: separate UAVs vertically before issuing RTL, +% mirroring the initial positioning stagger so UAVs transit laterally at +% unique altitudes and cannot collide during the return flight. if ~coder.target('MATLAB') - if positions(1, 3) >= positions(2, 3) - higherRetIdx = int32(1); - lowerRetIdx = int32(2); - else - higherRetIdx = int32(2); - lowerRetIdx = int32(1); - end - - hRetBase = double(higherRetIdx - 1) * double(NUM_RETURN_WP); - lRetBase = double(lowerRetIdx - 1) * double(NUM_RETURN_WP); - - % Higher post-guidance UAV: X → Y → Z (descend last) - returnTargets(hRetBase + 1, :) = [initialPositions(higherRetIdx,1), positions(higherRetIdx,2), positions(higherRetIdx,3)]; - returnTargets(hRetBase + 2, :) = [initialPositions(higherRetIdx,1), initialPositions(higherRetIdx,2), positions(higherRetIdx,3)]; - returnTargets(hRetBase + 3, :) = initialPositions(higherRetIdx, :); - - % Lower post-guidance UAV: Z → Y → X (descend first) - returnTargets(lRetBase + 1, :) = [positions(lowerRetIdx,1), positions(lowerRetIdx,2), initialPositions(lowerRetIdx,3)]; - returnTargets(lRetBase + 2, :) = [positions(lowerRetIdx,1), initialPositions(lowerRetIdx,2), initialPositions(lowerRetIdx,3)]; - returnTargets(lRetBase + 3, :) = initialPositions(lowerRetIdx, :); - - for w = 1:NUM_RETURN_WP - for i = 1:numClients - retIdx = double(i - 1) * double(NUM_RETURN_WP) + w; - retTarget = returnTargets(retIdx, :); - coder.ceval('sendTarget', int32(i), coder.ref(retTarget)); - end - coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.ACK)); - coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY)); + for i = 1:numClients + transitAlt = TRANSIT_ALT_BASE + (double(i) - 1) * TRANSIT_ALT_STEP; + target = [positions(i, 1), positions(i, 2), transitAlt]; + coder.ceval('sendTarget', int32(i), coder.ref(target)); end + coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.ACK)); + coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY)); else - disp('Taxicab return (simulation): UAVs commanded back to takeoff positions.'); + disp('Altitude-staggered return (simulation): UAVs commanded to transit altitudes.'); end -% -------------------------------------------------------------------------- % Send RTL command to all clients for i = 1:numClients diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m index 4b27caf..e7b0cc1 100644 --- a/test/parametricTestSuite.m +++ b/test/parametricTestSuite.m @@ -42,7 +42,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase objectivePos = params.objectivePos; end tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper(objectivePos, objectiveSigma), tc.domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum, objectivePos, objectiveSigma); - + agents = cell(size(params.initialPositions, 2) / 3, 1); for ii = 1:size(agents, 1) agents{ii} = agent; diff --git a/util/objectiveFunctionWrapper.m b/util/objectiveFunctionWrapper.m index 5d678ff..c7cd743 100644 --- a/util/objectiveFunctionWrapper.m +++ b/util/objectiveFunctionWrapper.m @@ -13,7 +13,6 @@ function f = objectiveFunctionWrapper(center, sigma) if size(sigma, 1) == 1 && size(center, 1) > 1 sigma = repmat(sigma, size(center, 1), 1, 1); end - assert(size(center, 1) == size(sigma, 1)); f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), squeeze(sigma(i, :, :))), 1:size(center,1), "UniformOutput", false)), 2); end \ No newline at end of file