From 4eefe1b9ac3358687c5871f54bd172a731473677 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Mon, 25 May 2026 14:51:39 -0700 Subject: [PATCH] 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