From 9c65bf78802ba5e7d3fd87ff6309271035dd46f3 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Sun, 24 May 2026 20:28:12 -0700 Subject: [PATCH 1/5] fixed multi-objective support, collected sim data on 3 new experiments --- aerpaw/config/scenario.csv | 2 +- aerpaw/config/scenario_columns.csv | 2 ++ aerpaw/config/scenario_lock.csv | 2 ++ aerpaw/config/scenario_two_around_wall.csv | 2 ++ aerpaw/config/scenario_walls.csv | 2 ++ test/parametricTestSuite.m | 12 +++++++++--- util/objectiveFunctionWrapper.m | 6 +++++- 7 files changed, 23 insertions(+), 5 deletions(-) create mode 100644 aerpaw/config/scenario_columns.csv create mode 100644 aerpaw/config/scenario_lock.csv create mode 100644 aerpaw/config/scenario_two_around_wall.csv create mode 100644 aerpaw/config/scenario_walls.csv diff --git a/aerpaw/config/scenario.csv b/aerpaw/config/scenario.csv index f155cfc..1e11351 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, 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 +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_columns.csv b/aerpaw/config/scenario_columns.csv new file mode 100644 index 0000000..1e11351 --- /dev/null +++ b/aerpaw/config/scenario_columns.csv @@ -0,0 +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, 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 new file mode 100644 index 0000000..3dbf3b4 --- /dev/null +++ b/aerpaw/config/scenario_lock.csv @@ -0,0 +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, 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_two_around_wall.csv b/aerpaw/config/scenario_two_around_wall.csv new file mode 100644 index 0000000..f155cfc --- /dev/null +++ b/aerpaw/config/scenario_two_around_wall.csv @@ -0,0 +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, 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_walls.csv b/aerpaw/config/scenario_walls.csv new file mode 100644 index 0000000..1dd65d4 --- /dev/null +++ b/aerpaw/config/scenario_walls.csv @@ -0,0 +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, 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/test/parametricTestSuite.m b/test/parametricTestSuite.m index 273030c..9b0739f 100644 --- a/test/parametricTestSuite.m +++ b/test/parametricTestSuite.m @@ -34,9 +34,15 @@ classdef parametricTestSuite < matlab.unittest.TestCase % Define scenario according to CSV specification tc.domain = tc.domain.initialize([params.domainMin; params.domainMax], REGION_TYPE.DOMAIN, "Domain"); - objectiveSigma = reshape(params.objectiveVar, [1 2 2]); - tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper(params.objectivePos, objectiveSigma), tc.domain, params.discretizationStep, params.protectedRange, params.sensorPerformanceMinimum, params.objectivePos, objectiveSigma); - + if length(params.objectiveVar) > 4 && length(params.objectivePos) > 2 + objectiveSigma = permute(reshape(params.objectiveVar, [length(params.objectiveVar)/4 2 2]), [3 1 2]); + objectivePos = reshape(params.objectivePos, [length(params.objectivePos)/2, 2])'; + else + objectiveSigma = reshape(params.objectiveVar, [1, 2, 2]); + 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 6846ffe..5d678ff 100644 --- a/util/objectiveFunctionWrapper.m +++ b/util/objectiveFunctionWrapper.m @@ -4,12 +4,16 @@ function f = objectiveFunctionWrapper(center, sigma) % composite objectives in particular arguments (Input) center (:, 2) double; - sigma (:, 2, 2) double = eye(2); + sigma (:, 2, 2) double = reshape(eye(2), 1, 2, 2); end arguments (Output) f (1, 1) {mustBeA(f, "function_handle")}; end + 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 From a30dfe6be77258f3f1f350b2301018bbdf94ce20 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Mon, 25 May 2026 14:07:49 -0700 Subject: [PATCH 2/5] ran 3 new experiments on AERPAW simulation to test before submitting --- .gitignore | 1 + @miSim/run.m | 4 + aerpaw/config/scenario_columns.csv | 2 +- aerpaw/config/scenario_lock.csv | 2 +- aerpaw/config/scenario_walls.csv | 2 +- aerpaw/controller.m | 146 ++++++++++++++++++++--------- 6 files changed, 112 insertions(+), 45 deletions(-) diff --git a/.gitignore b/.gitignore index 1fd7ddb..dfac210 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ *.autosave *.slx.r* *.mdl.r* +*.bak # Derived content-obscured files *.p diff --git a/@miSim/run.m b/@miSim/run.m index 3427ae9..d9d8c46 100644 --- a/@miSim/run.m +++ b/@miSim/run.m @@ -11,6 +11,10 @@ function [obj] = run(obj) if obj.makeVideo v = obj.setupVideoWriter(); v.open(); + + % Write initialization state frame in to video + I = getframe(obj.f); + v.writeVideo(I); end end diff --git a/aerpaw/config/scenario_columns.csv b/aerpaw/config/scenario_columns.csv index 1e11351..4f685c0 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, 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 +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 diff --git a/aerpaw/config/scenario_lock.csv b/aerpaw/config/scenario_lock.csv index 3dbf3b4..a52338f 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, 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 +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 \ No newline at end of file diff --git a/aerpaw/config/scenario_walls.csv b/aerpaw/config/scenario_walls.csv index 1dd65d4..1501aca 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, 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 +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 \ No newline at end of file diff --git a/aerpaw/controller.m b/aerpaw/controller.m index 9d83aa5..1d9a3df 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -7,54 +7,43 @@ coder.extrinsic('disp', 'readScenarioCsv'); % Maximum clients supported (one initial position per UAV) MAX_CLIENTS = 4; -% Two waypoints per UAV: altitude-staggered transit + final position -MAX_TARGETS = MAX_CLIENTS * 2; +% 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 % 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 -% Load initial UAV positions from scenario CSV +% Experiment start positions from scenario CSV (N x 3) +scenarioPositions = zeros(MAX_CLIENTS, 3); + +% Load experiment start 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, same layout as initializeFromCsv + posMatrix = reshape(flatPos, 3, [])'; % N×3 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)); - 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); + scenarioPositions(1:totalLoaded, :) = targets(1:totalLoaded, :); + numWaypoints = int32(3); end % Load guidance scenario from CSV (parameters for guidance_step) @@ -92,6 +81,46 @@ 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 @@ -127,8 +156,13 @@ for w = 1:numWaypoints end % ---- Phase 2: miSim guidance loop ---------------------------------------- -% Guidance parameters (adjust here and recompile as needed) -MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations +% 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 % Enter guidance mode on all clients if ~coder.target('MATLAB') @@ -141,8 +175,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 CSV waypoints so agents don't start at origin - positions(1:totalLoaded, :) = targets(1:totalLoaded, :); + % Simulation: seed positions from scenario positions so agents don't start at origin + positions(1:totalLoaded, :) = scenarioPositions(1:totalLoaded, :); end guidance_step(positions(1:numClients, :), true, ... scenarioParams, obstacleMin, obstacleMax, numObstacles); @@ -197,20 +231,48 @@ if ~coder.target('MATLAB') end % -------------------------------------------------------------------------- -% 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. +% ---- 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); + if ~coder.target('MATLAB') - 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)); + 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)); end - coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.ACK)); - coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY)); else - disp('Altitude-staggered return (simulation): UAVs commanded to transit altitudes.'); + disp('Taxicab return (simulation): UAVs commanded back to takeoff positions.'); end +% -------------------------------------------------------------------------- % Send RTL command to all clients for i = 1:numClients From c9774622f20330caa8488c3ec1eb1742d8ea7ae9 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Mon, 25 May 2026 14:09:14 -0700 Subject: [PATCH 3/5] columns scenario first --- aerpaw/config/scenario.csv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aerpaw/config/scenario.csv b/aerpaw/config/scenario.csv index 1e11351..4f685c0 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, 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 +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 From 4eefe1b9ac3358687c5871f54bd172a731473677 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Mon, 25 May 2026 14:51:39 -0700 Subject: [PATCH 4/5] added 2 objective support for AERPAW --- @miSim/initializeFromCsv.m | 15 +++++++- aerpaw/controller.m | 2 +- aerpaw/guidance_step.m | 33 ++++++++++------- aerpaw/impl/controller_impl.cpp | 65 +++++++++++++++++++++++---------- aerpaw/impl/controller_impl.h | 15 ++++---- 5 files changed, 87 insertions(+), 43 deletions(-) diff --git a/@miSim/initializeFromCsv.m b/@miSim/initializeFromCsv.m index e787bf0..f5c5857 100644 --- a/@miSim/initializeFromCsv.m +++ b/@miSim/initializeFromCsv.m @@ -52,8 +52,19 @@ BETA_TILT_VEC = scenario.betaTilt; % 1×N DOMAIN_MIN = scenario.domainMin; % 1×3 DOMAIN_MAX = scenario.domainMax; % 1×3 -OBJECTIVE_GROUND_POS = scenario.objectivePos; % 1×2 -OBJECTIVE_VAR = reshape(scenario.objectiveVar, 2, 2); % 2×2 covariance matrix + +% objectivePos: 2 values per Gaussian component (1 or 2 components supported) +nObjComponents = numel(scenario.objectivePos) / 2; +assert(mod(numel(scenario.objectivePos), 2) == 0, ... + 'objectivePos must have an even number of values (2 per Gaussian component)'); +assert(nObjComponents >= 1 && nObjComponents <= 2, ... + 'At most 2 objective Gaussian components supported; got %d', nObjComponents); +assert(numel(scenario.objectiveVar) == nObjComponents * 4, ... + 'objectiveVar must have %d values for %d component(s); got %d', ... + nObjComponents * 4, nObjComponents, numel(scenario.objectiveVar)); +OBJECTIVE_GROUND_POS = reshape(scenario.objectivePos, 2, nObjComponents)'; % nObj×2 +OBJECTIVE_VAR = permute(reshape(scenario.objectiveVar, 2, 2, nObjComponents), [3, 1, 2]); % nObj×2×2 + SENSOR_PERFORMANCE_MINIMUM = scenario.sensorPerformanceMinimum; % scalar % Initial UAV positions: flat vector reshaped to N×3 diff --git a/aerpaw/controller.m b/aerpaw/controller.m index 1d9a3df..e105862 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -47,7 +47,7 @@ else end % Load guidance scenario from CSV (parameters for guidance_step) -NUM_SCENARIO_PARAMS = 48; +NUM_SCENARIO_PARAMS = 55; MAX_OBSTACLES_CTRL = int32(8); scenarioParams = zeros(1, NUM_SCENARIO_PARAMS); obstacleMin = zeros(MAX_OBSTACLES_CTRL, 3); diff --git a/aerpaw/guidance_step.m b/aerpaw/guidance_step.m index 25f34df..05384bf 100644 --- a/aerpaw/guidance_step.m +++ b/aerpaw/guidance_step.m @@ -94,29 +94,34 @@ if isInit BETA_TILT_VEC = scenarioParams(29:32); DOMAIN_MIN = scenarioParams(33:35); DOMAIN_MAX = scenarioParams(36:38); - OBJECTIVE_GROUND_POS = scenarioParams(39:40); - OBJECTIVE_VAR = reshape(scenarioParams(41:44), 2, 2); - SENSOR_PERFORMANCE_MINIMUM = scenarioParams(45); - USE_DOUBLE_INTEGRATOR = logical(scenarioParams(46)); - DAMPING_COEFF = scenarioParams(47); - USE_FIXED_TOPOLOGY = logical(scenarioParams(48)); + NUM_OBJ_COMPONENTS = int32(scenarioParams(39)); + OBJECTIVE_POS_FLAT = scenarioParams(40:43); % [x1,y1,x2,y2]; zero-padded if N=1 + OBJECTIVE_VAR_FLAT = scenarioParams(44:51); % [v11,v12,v21,v22 per component] + SENSOR_PERFORMANCE_MINIMUM = scenarioParams(52); + USE_DOUBLE_INTEGRATOR = logical(scenarioParams(53)); + DAMPING_COEFF = scenarioParams(54); + USE_FIXED_TOPOLOGY = logical(scenarioParams(55)); % --- Build domain geometry --- dom = rectangularPrism; dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain"); - % --- Build sensing objective (inline Gaussian; codegen-compatible) --- + % --- Build sensing objective: sum of N bivariate Gaussians (codegen-compatible) --- dom.objective = sensingObjective; xGrid = unique([DOMAIN_MIN(1):DISCRETIZATION_STEP:DOMAIN_MAX(1), DOMAIN_MAX(1)]); yGrid = unique([DOMAIN_MIN(2):DISCRETIZATION_STEP:DOMAIN_MAX(2), DOMAIN_MAX(2)]); [gridX, gridY] = meshgrid(xGrid, yGrid); - dx = gridX - OBJECTIVE_GROUND_POS(1); - dy = gridY - OBJECTIVE_GROUND_POS(2); - % Bivariate Gaussian using objectiveVar covariance matrix (avoids inv()) - ov_a = OBJECTIVE_VAR(1,1); ov_b = OBJECTIVE_VAR(1,2); - ov_c = OBJECTIVE_VAR(2,1); ov_d = OBJECTIVE_VAR(2,2); - ov_det = ov_a * ov_d - ov_b * ov_c; - objValues = exp((-0.5 / ov_det) .* (ov_d .* dx.*dx - (ov_b + ov_c) .* dx.*dy + ov_a .* dy.*dy)); + objValues = zeros(size(gridX)); + for kk = 1:NUM_OBJ_COMPONENTS + pos_k = OBJECTIVE_POS_FLAT((kk-1)*2+1 : (kk-1)*2+2); + var_k = reshape(OBJECTIVE_VAR_FLAT((kk-1)*4+1 : (kk-1)*4+4), 2, 2); + dx = gridX - pos_k(1); + dy = gridY - pos_k(2); + ov_a = var_k(1,1); ov_b = var_k(1,2); + ov_c = var_k(2,1); ov_d = var_k(2,2); + ov_det = ov_a * ov_d - ov_b * ov_c; + objValues = objValues + exp((-0.5 / ov_det) .* (ov_d .* dx.*dx - (ov_b + ov_c) .* dx.*dy + ov_a .* dy.*dy)); + end dom.objective = dom.objective.initializeWithValues(objValues, dom, ... DISCRETIZATION_STEP, PROTECTED_RANGE, SENSOR_PERFORMANCE_MINIMUM); diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp index ded16d9..effe0fe 100644 --- a/aerpaw/impl/controller_impl.cpp +++ b/aerpaw/impl/controller_impl.cpp @@ -222,12 +222,13 @@ static int readScenarioDataRow(const char* filename, char* line, int lineSize) { // 28-31: betaTilt[1:4] // 32-34: domainMin (east, north, up) // 35-37: domainMax (east, north, up) -// 38-39: objectivePos (east, north) -// 40-43: objectiveVar (2x2 col-major: v11, v12, v21, v22) -// 44 : sensorPerformanceMinimum (CSV column 18) -// 45 : useDoubleIntegrator (CSV column 23; 0=single-integrator, 1=double-integrator) -// 46 : dampingCoeff (CSV column 24) -// 47 : useFixedTopology (CSV column 25; 0=dynamic lesser-neighbor, 1=fixed) +// 38 : numObjectiveComponents (1 or 2; inferred from objectivePos field length) +// 39-42: objectivePos flat [x1,y1,x2,y2] (4 slots; zero-padded if N=1) +// 43-50: objectiveVar flat [v11,v12,v21,v22 per component] (8 slots; zero-padded if N=1) +// 51 : sensorPerformanceMinimum (CSV column 18) +// 52 : useDoubleIntegrator (CSV column 23) +// 53 : dampingCoeff (CSV column 24) +// 54 : useFixedTopology (CSV column 25) // Returns 1 on success, 0 on failure. int loadScenario(const char* filename, double* params) { char line[4096]; @@ -305,52 +306,78 @@ int loadScenario(const char* filename, double* params) { } } - // objectivePos: column 16 + // objectivePos: column 16 — 2 values per component (up to 2 components). + // Infer numObjectiveComponents from the number of values parsed. { char tmp[256]; strncpy(tmp, fields[16], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; char* t = trimField(tmp); - if (sscanf(t, "%lf , %lf", ¶ms[38], ¶ms[39]) != 2) { - fprintf(stderr, "loadScenario: failed to parse objectivePos: %s\n", t); + double posVals[4] = {0, 0, 0, 0}; + int posCount = 0; + char* tok = strtok(t, ","); + while (tok && posCount < 4) { + posVals[posCount++] = atof(tok); + tok = strtok(nullptr, ","); + } + // Check for a 5th token — would mean > 2 components + if (tok) { + fprintf(stderr, "loadScenario: at most 2 objective Gaussian components supported (objectivePos has >4 values)\n"); return 0; } + if (posCount == 0 || posCount % 2 != 0) { + fprintf(stderr, "loadScenario: objectivePos must have 2 or 4 values, got %d\n", posCount); + return 0; + } + int nObj = posCount / 2; + params[38] = (double)nObj; + for (int k = 0; k < 4; k++) params[39 + k] = posVals[k]; // zero-padded for nObj=1 } - // objectiveVar: column 17, format "v11, v12, v21, v22" + // objectiveVar: column 17 — 4 values per component (v11,v12,v21,v22). { - char tmp[256]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; + char tmp[512]; strncpy(tmp, fields[17], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; char* t = trimField(tmp); - if (sscanf(t, "%lf , %lf , %lf , %lf", ¶ms[40], ¶ms[41], ¶ms[42], ¶ms[43]) != 4) { - fprintf(stderr, "loadScenario: failed to parse objectiveVar: %s\n", t); + int nObj = (int)params[38]; + double varVals[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + int varCount = 0; + char* tok = strtok(t, ","); + while (tok && varCount < 8) { + varVals[varCount++] = atof(tok); + tok = strtok(nullptr, ","); + } + if (varCount != nObj * 4) { + fprintf(stderr, "loadScenario: objectiveVar has %d values but expected %d (4 per component)\n", + varCount, nObj * 4); return 0; } + for (int k = 0; k < 8; k++) params[43 + k] = varVals[k]; // zero-padded for nObj=1 } // sensorPerformanceMinimum: column 18 { char tmp[64]; strncpy(tmp, fields[18], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; - params[44] = atof(trimField(tmp)); + params[51] = atof(trimField(tmp)); } // useDoubleIntegrator: column 23 { char tmp[64]; strncpy(tmp, fields[23], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; - params[45] = atof(trimField(tmp)); + params[52] = atof(trimField(tmp)); } // dampingCoeff: column 24 { char tmp[64]; strncpy(tmp, fields[24], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; - params[46] = atof(trimField(tmp)); + params[53] = atof(trimField(tmp)); } // useFixedTopology: column 25 { char tmp[64]; strncpy(tmp, fields[25], sizeof(tmp) - 1); tmp[sizeof(tmp)-1] = '\0'; - params[47] = atof(trimField(tmp)); + params[54] = atof(trimField(tmp)); } - printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g]\n", - params[32], params[33], params[34], params[35], params[36], params[37]); + printf("Loaded scenario: domain [%g,%g,%g] to [%g,%g,%g], %d objective component(s)\n", + params[32], params[33], params[34], params[35], params[36], params[37], (int)params[38]); return 1; } diff --git a/aerpaw/impl/controller_impl.h b/aerpaw/impl/controller_impl.h index f751f80..28b81c3 100644 --- a/aerpaw/impl/controller_impl.h +++ b/aerpaw/impl/controller_impl.h @@ -27,13 +27,14 @@ int loadTargets(const char* filename, double* targets, int maxClients); // 28-31 betaTilt[1:4] // 32-34 domainMin // 35-37 domainMax -// 38-39 objectivePos -// 40-43 objectiveVar (2x2 col-major) -// 44 sensorPerformanceMinimum -// 45 useDoubleIntegrator (0=single-integrator, 1=double-integrator) -// 46 dampingCoeff -// 47 useFixedTopology (0=dynamic lesser-neighbor, 1=fixed) -#define NUM_SCENARIO_PARAMS 48 +// 38 numObjectiveComponents (1 or 2; inferred from objectivePos field length) +// 39-42 objectivePos flat [x1,y1,x2,y2] (4 slots; zero-padded if N=1) +// 43-50 objectiveVar flat [v11,v12,v21,v22 per component] (8 slots; zero-padded if N=1) +// 51 sensorPerformanceMinimum +// 52 useDoubleIntegrator (0=single-integrator, 1=double-integrator) +// 53 dampingCoeff +// 54 useFixedTopology (0=dynamic lesser-neighbor, 1=fixed) +#define NUM_SCENARIO_PARAMS 55 #define MAX_CLIENTS_PER_PARAM 4 // Maximum number of obstacles (upper bound for pre-allocated arrays). #define MAX_OBSTACLES 8 From 85d2570c38d76094968c07b9fe0e933fd2c8766e Mon Sep 17 00:00:00 2001 From: Kevin D Date: Mon, 25 May 2026 14:56:14 -0700 Subject: [PATCH 5/5] codegen fix --- @miSim/initialize.m | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/@miSim/initialize.m b/@miSim/initialize.m index 08f045f..de5156f 100644 --- a/@miSim/initialize.m +++ b/@miSim/initialize.m @@ -71,16 +71,18 @@ 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 - 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)); + % 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 end end