diff --git a/@agent/agent.m b/@agent/agent.m index 88fe5f6..6f75803 100644 --- a/@agent/agent.m +++ b/@agent/agent.m @@ -17,8 +17,8 @@ classdef agent fovGeometry; % Communication - commsGeometry = spherical; - lesserNeighbors = []; + commsGeometry; + lesserNeighbors = zeros(1, 0); % Performance performance = 0; @@ -34,6 +34,17 @@ classdef agent end methods (Access = public) + function obj = agent() + arguments (Input) + end + arguments (Output) + obj (1, 1) agent + end + obj.collisionGeometry = spherical; + obj.sensorModel = sigmoidSensor; + obj.fovGeometry = cone; + obj.commsGeometry = spherical; + end [obj] = initialize(obj, pos, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label); [obj] = run(obj, domain, partitioning, t, index, agents); [partitioning] = partition(obj, agents, objective) diff --git a/@agent/initialize.m b/@agent/initialize.m index e1bd17b..37ada2b 100644 --- a/@agent/initialize.m +++ b/@agent/initialize.m @@ -23,7 +23,9 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma obj.stepDecayRate = obj.initialStepSize / maxIter; % Initialize performance vector - obj.performance = [0, NaN(1, maxIter), 0]; + if coder.target('MATLAB') + obj.performance = [0, NaN(1, maxIter), 0]; + end % Add spherical geometry based on com range obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label)); diff --git a/@agent/partition.m b/@agent/partition.m index b5c3020..1d0b32b 100644 --- a/@agent/partition.m +++ b/@agent/partition.m @@ -7,29 +7,33 @@ function [partitioning] = partition(obj, agents, objective) arguments (Output) partitioning (:, :) double; end - - % Assess sensing performance of each agent at each sample point - % in the domain - agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, [objective.X(:), objective.Y(:), zeros(size(objective.X(:)))]), size(objective.X)), agents, "UniformOutput", false); - agentPerformances{end + 1} = objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton - agentPerformances = cat(3, agentPerformances{:}); - % Get highest performance value at each point - [~, idx] = max(agentPerformances, [], 3); + nAgents = size(agents, 1); + gridM = size(objective.X, 1); + gridN = size(objective.X, 2); + nPoints = gridM * gridN; - % Collect agent indices in the same way as performance - indices = 1:size(agents, 1); - agentInds = squeeze(tensorprod(indices, ones(size(objective.X)))); - if size(agentInds, 1) ~= size(agents, 1) - agentInds = reshape(agentInds, [size(agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive + % Assess sensing performance of each agent at each sample point. + % agentPerf is (nPoints x nAgents+1): the extra column is the + % minimum threshold that must be exceeded for any assignment. + agentPerf = zeros(nPoints, nAgents + 1); + for aa = 1:nAgents + p = agents{aa}.sensorModel.sensorPerformance(agents{aa}.pos, ... + [objective.X(:), objective.Y(:), zeros(nPoints, 1)]); + agentPerf(:, aa) = p(:); end - agentInds = num2cell(agentInds, 2:3); - agentInds = cellfun(@(x) squeeze(x), agentInds, "UniformOutput", false); - agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment - agentInds = cat(3, agentInds{:}); + agentPerf(:, nAgents + 1) = objective.sensorPerformanceMinimum; - % Use highest performing agent's index to form partitions - [m, n, ~] = size(agentInds); - [jj, kk] = ndgrid(1:m, 1:n); - partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx)); -end \ No newline at end of file + % Find which agent has highest performance at each point. + % If the threshold column wins (idx == nAgents+1) the point is unassigned (0). + [~, idx] = max(agentPerf, [], 2); + + assignedAgent = zeros(nPoints, 1); + for pp = 1:nPoints + if idx(pp) <= nAgents + assignedAgent(pp) = idx(pp); + end + end + + partitioning = reshape(assignedAgent, gridM, gridN); +end diff --git a/@agent/run.m b/@agent/run.m index 83637cf..3df8627 100644 --- a/@agent/run.m +++ b/@agent/run.m @@ -13,7 +13,7 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents) % Collect objective function values across partition partitionMask = partitioning == index; - if ~unique(partitionMask) + if ~any(partitionMask(:)) % This agent has no partition, maintain current state return; end @@ -32,10 +32,10 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents) pos = obj.pos + delta * deltaApplicator(ii, 1:3); % Compute performance values on partition - if ii < 5 + if ii < 6 % Compute sensing performance sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n - % Objective performance does not change for 0, +/- X, Y steps. + % Objective performance does not change for 0, +/- X, +/- Y steps. % Those values are computed once before the loop and are only % recomputed when +/- Z steps are applied else @@ -64,17 +64,26 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents) end % Store agent performance at current time and place - obj.performance(timestepIndex + 1) = C_delta(1); + if coder.target('MATLAB') + obj.performance(timestepIndex + 1) = C_delta(1); + end % Compute gradient by finite central differences gradC = [(C_delta(2)-C_delta(3))/(2*delta), (C_delta(4)-C_delta(5))/(2*delta), (C_delta(6)-C_delta(7))/(2*delta)]; % Compute scaling factor targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer - rateFactor = targetRate / norm(gradC); + gradNorm = norm(gradC); - % Compute unconstrained next position - pNext = obj.pos + rateFactor * gradC; + % Compute unconstrained next position. + % Guard against near-zero gradient: when sensor performance is saturated + % or near-zero across the whole partition, rateFactor -> Inf and pNext + % explodes. Stay put instead. + if gradNorm < 1e-10 + pNext = obj.pos; + else + pNext = obj.pos + (targetRate / gradNorm) * gradC; + end % Move to next position obj.lastPos = obj.pos; diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m index ca8b0ec..a8fccaa 100644 --- a/@miSim/constrainMotion.m +++ b/@miSim/constrainMotion.m @@ -6,138 +6,152 @@ function [obj] = constrainMotion(obj) obj (1, 1) {mustBeA(obj, "miSim")}; end - if size(obj.agents, 1) < 2 + nAgents = size(obj.agents, 1); + + if nAgents < 2 nAAPairs = 0; else - nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs + nAAPairs = nchoosek(nAgents, 2); % unique agent/agent pairs end - agents = [obj.agents{:}]; - v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))'; - if all(isnan(v), "all") || all(v == zeros(size(obj.agents, 1), 3), "all") + % Compute velocity matrix from unconstrained gradient-ascent step + v = zeros(nAgents, 3); + for ii = 1:nAgents + v(ii, :) = (obj.agents{ii}.pos - obj.agents{ii}.lastPos) ./ obj.timestep; + end + if all(isnan(v), "all") || all(v == zeros(nAgents, 3), "all") % Agents are not attempting to move, so there is no motion to be % constrained return; end % Initialize QP based on number of agents and obstacles - nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs - nADPairs = size(obj.agents, 1) * 5; % agents x (4 walls + 1 ceiling) - nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - size(obj.agents, 1); + nAOPairs = nAgents * size(obj.obstacles, 1); % unique agent/obstacle pairs + nADPairs = nAgents * 6; % agents x (4 walls + 1 floor + 1 ceiling) + nLNAPairs = sum(obj.constraintAdjacencyMatrix, "all") - nAgents; total = nAAPairs + nAOPairs + nADPairs + nLNAPairs; kk = 1; - A = zeros(total, 3 * size(obj.agents, 1)); + A = zeros(total, 3 * nAgents); b = zeros(total, 1); % Set up collision avoidance constraints - h = NaN(size(obj.agents, 1)); - h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0 - for ii = 1:(size(obj.agents, 1) - 1) - for jj = (ii + 1):size(obj.agents, 1) - h(ii, jj) = norm(agents(ii).pos - agents(jj).pos)^2 - (agents(ii).collisionGeometry.radius + agents(jj).collisionGeometry.radius)^2; + h = NaN(nAgents, nAgents); + h(logical(eye(nAgents))) = 0; % self value is 0 + for ii = 1:(nAgents - 1) + for jj = (ii + 1):nAgents + h(ii, jj) = norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2; h(jj, ii) = h(ii, jj); - - A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - agents(jj).pos); + + A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - obj.agents{jj}.pos); A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii)); - b(kk) = obj.barrierGain * h(ii, jj)^obj.barrierExponent; + b(kk) = obj.barrierGain * max(0, h(ii, jj))^obj.barrierExponent; kk = kk + 1; end end - hObs = NaN(size(obj.agents, 1), size(obj.obstacles, 1)); + hObs = NaN(nAgents, size(obj.obstacles, 1)); % Set up obstacle avoidance constraints - for ii = 1:size(obj.agents, 1) + for ii = 1:nAgents for jj = 1:size(obj.obstacles, 1) % find closest position to agent on/in obstacle - cPos = obj.obstacles{jj}.closestToPoint(agents(ii).pos); + cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.pos); - hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2; + hObs(ii, jj) = dot(obj.agents{ii}.pos - cPos, obj.agents{ii}.pos - cPos) - obj.agents{ii}.collisionGeometry.radius^2; + + A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - cPos); + b(kk) = obj.barrierGain * max(0, hObs(ii, jj))^obj.barrierExponent; - A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - cPos); - b(kk) = obj.barrierGain * hObs(ii, jj)^obj.barrierExponent; - kk = kk + 1; end end - + % Set up domain constraints (walls and ceiling only) % Floor constraint is implicit with an obstacle corresponding to the % minimum allowed altitude, but I included it anyways - for ii = 1:size(obj.agents, 1) + h_xMin = 0.0; h_xMax = 0.0; h_yMin = 0.0; h_yMax = 0.0; h_zMin = 0.0; h_zMax = 0.0; + for ii = 1:nAgents % X minimum - h_xMin = (agents(ii).pos(1) - obj.domain.minCorner(1)) - agents(ii).collisionGeometry.radius; + h_xMin = (obj.agents{ii}.pos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0]; - b(kk) = obj.barrierGain * h_xMin^obj.barrierExponent; + b(kk) = obj.barrierGain * max(0, h_xMin)^obj.barrierExponent; kk = kk + 1; - + % X maximum - h_xMax = (obj.domain.maxCorner(1) - agents(ii).pos(1)) - agents(ii).collisionGeometry.radius; + h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.pos(1)) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0]; - b(kk) = obj.barrierGain * h_xMax^obj.barrierExponent; + b(kk) = obj.barrierGain * max(0, h_xMax)^obj.barrierExponent; kk = kk + 1; - + % Y minimum - h_yMin = (agents(ii).pos(2) - obj.domain.minCorner(2)) - agents(ii).collisionGeometry.radius; + h_yMin = (obj.agents{ii}.pos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0]; - b(kk) = obj.barrierGain * h_yMin^obj.barrierExponent; + b(kk) = obj.barrierGain * max(0, h_yMin)^obj.barrierExponent; kk = kk + 1; - + % Y maximum - h_yMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius; + h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.pos(2)) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0]; - b(kk) = obj.barrierGain * h_yMax^obj.barrierExponent; + b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent; kk = kk + 1; - + % Z minimum - h_zMin = (agents(ii).pos(3) - obj.domain.minCorner(3)) - agents(ii).collisionGeometry.radius; + h_zMin = (obj.agents{ii}.pos(3) - obj.domain.minCorner(3)) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1]; - b(kk) = obj.barrierGain * h_zMin^obj.barrierExponent; + b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent; kk = kk + 1; - + % Z maximum - h_zMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius; + h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.pos(3)) - obj.agents{ii}.collisionGeometry.radius; A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1]; - b(kk) = obj.barrierGain * h_zMax^obj.barrierExponent; + b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent; kk = kk + 1; end - % Save off h function values (ignoring network constraints which may evolve in time) - obj.h(:, obj.timestepIndex) = [h(triu(true(size(obj.agents, 1)), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;]; + if coder.target('MATLAB') + % Save off h function values (logging only — not needed in compiled mode) + obj.h(:, obj.timestepIndex) = [h(triu(true(nAgents), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;]; + end % Add communication network constraints - hComms = NaN(size(obj.agents, 1)); - hComms(logical(eye(size(obj.agents, 1)))) = 0; - for ii = 1:(size(obj.agents, 1) - 1) - for jj = (ii + 1):size(obj.agents, 1) + hComms = NaN(nAgents, nAgents); + hComms(logical(eye(nAgents))) = 0; + for ii = 1:(nAgents - 1) + for jj = (ii + 1):nAgents if obj.constraintAdjacencyMatrix(ii, jj) - hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(agents(ii).pos - agents(jj).pos)^2; + hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2; - A(kk, (3 * ii - 2):(3 * ii)) = 2 * (agents(ii).pos - agents(jj).pos); + A(kk, (3 * ii - 2):(3 * ii)) = 2 * (obj.agents{ii}.pos - obj.agents{jj}.pos); A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii)); - b(kk) = obj.barrierGain * hComms(ii, jj)^obj.barrierExponent; + b(kk) = obj.barrierGain * max(0, hComms(ii, jj))^obj.barrierExponent; - kk = kk + 1; + kk = kk + 1; end end end % Solve QP program generated earlier - vhat = reshape(v', 3 * size(obj.agents, 1), 1); - H = 2 * eye(3 * size(obj.agents, 1)); + vhat = reshape(v', 3 * nAgents, 1); + H = 2 * eye(3 * nAgents); f = -2 * vhat; - + % Update solution based on constraints - assert(size(A,2) == size(H,1)) - assert(size(A,1) == size(b,1)) - assert(size(H,1) == length(f)) + if coder.target('MATLAB') + assert(size(A,2) == size(H,1)) + assert(size(A,1) == size(b,1)) + assert(size(H,1) == length(f)) + end opt = optimoptions("quadprog", "Display", "off", "Algorithm", "active-set", "UseCodegenSolver", true); x0 = zeros(size(H, 1), 1); [vNew, ~, exitflag, m] = quadprog(H, double(f), A, b, [], [], [], [], x0, opt); - assert(exitflag == 1, sprintf("quadprog failure... %s%s", newline, m.message)); - vNew = reshape(vNew, 3, size(obj.agents, 1))'; + if coder.target('MATLAB') + assert(exitflag == 1, sprintf("quadprog failure... %s%s", newline, m.message)); + end + vNew = reshape(vNew, 3, nAgents)'; if exitflag <= 0 - warning("QP failed, continuing with unconstrained solution...") + if coder.target('MATLAB') + warning("QP failed, continuing with unconstrained solution...") + end vNew = v; end diff --git a/@miSim/initialize.m b/@miSim/initialize.m index 0abb98c..16e1cd9 100644 --- a/@miSim/initialize.m +++ b/@miSim/initialize.m @@ -20,14 +20,20 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m obj.makePlots = makePlots; if ~obj.makePlots if makeVideo - warning("makeVideo set to true, but makePlots set to false. Setting makeVideo to false."); + if coder.target('MATLAB') + warning("makeVideo set to true, but makePlots set to false. Setting makeVideo to false."); + end makeVideo = false; end end obj.makeVideo = makeVideo; % Generate artifact(s) name - obj.artifactName = strcat(string(datetime("now"), "yyyy_MM_dd_HH_mm_ss")); + if coder.target('MATLAB') + obj.artifactName = strcat(string(datetime("now"), "yyyy_MM_dd_HH_mm_ss")); + else + obj.artifactName = ""; % Generate no artifacts from simulation in codegen + end % Define simulation time parameters obj.timestep = timestep; @@ -37,14 +43,24 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m % Define domain obj.domain = domain; - % Add geometries representing obstacles within the domain - obj.obstacles = obstacles; + % Add geometries representing obstacles within the domain, pre-allocating + % one extra slot for the minimum altitude floor obstacle if needed + numInputObs = size(obstacles, 1); + if minAlt > 0 + obj.obstacles = repmat({rectangularPrism}, numInputObs + 1, 1); + else + obj.obstacles = repmat({rectangularPrism}, numInputObs, 1); + end + for kk = 1:numInputObs + obj.obstacles{kk} = obstacles{kk}; + end - % Add an additional obstacle spanning the domain's footprint to + % Add an additional obstacle spanning the domain's footprint to % represent the minimum allowable altitude if minAlt > 0 - obj.obstacles{end + 1, 1} = rectangularPrism; - obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint"); + minAltObstacle = rectangularPrism; + minAltObstacle = minAltObstacle.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint"); + obj.obstacles{numInputObs + 1} = minAltObstacle; end % Define agents @@ -56,12 +72,12 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m for ii = 1:size(obj.agents, 1) % Agent if isempty(char(obj.agents{ii}.label)) - obj.agents{ii}.label = sprintf("Agent %d", ii); + 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", ii); + obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", int8(ii)); end end @@ -76,22 +92,26 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m % Set up times to iterate over obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)'; - % Prepare performance data store (at t = 0, all have 0 performance) - obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)]; + if coder.target('MATLAB') + % Prepare performance data store (at t = 0, all have 0 performance) + obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)]; - % Prepare h function data store - obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1)); + % Prepare h function data store + obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1)); + end % Create initial partitioning obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective); - % Initialize variable that will store agent positions for trail plots - obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3); - obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3); + if coder.target('MATLAB') + % Initialize variable that will store agent positions for trail plots + obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3); + obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3); - % Set up plots showing initialized state - obj = obj.plot(); + % Set up plots showing initialized state + obj = obj.plot(); - % Run validations - obj.validate(); + % Run validations + obj.validate(); + end end \ No newline at end of file diff --git a/@miSim/lesserNeighbor.m b/@miSim/lesserNeighbor.m index 8461dfc..1dd21b4 100644 --- a/@miSim/lesserNeighbor.m +++ b/@miSim/lesserNeighbor.m @@ -9,42 +9,48 @@ function obj = lesserNeighbor(obj) % initialize solution with self-connections only constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1))); - for ii = 1:size(obj.agents, 1) + nAgents = size(obj.agents, 1); + for ii = 1:nAgents % Find lesser neighbors of each agent % Lesser neighbors of ii are jj < ii in range of ii - lesserNeighbors = []; + lnBuf = zeros(1, nAgents); + lnCount = 0; for jj = 1:(ii - 1) if obj.adjacency(ii, jj) - lesserNeighbors = [lesserNeighbors, jj]; + lnCount = lnCount + 1; + lnBuf(lnCount) = jj; end end - obj.agents{ii}.lesserNeighbors = lesserNeighbors; - + obj.agents{ii}.lesserNeighbors = lnBuf(1:lnCount); + % Early exit for isolated agents - if isempty(obj.agents{ii}.lesserNeighbors) + if lnCount == 0 continue end % Focus on subgraph defined by lesser neighbors subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors); - % Find connected components in each agent's subgraph - % TODO: rewrite this using matlab "conncomp" function? - visited = false(size(subgraphAdjacency, 1), 1); - components = {}; - for jj = 1:size(subgraphAdjacency, 1) + % Find connected components; store only the max global index per + % component (the only value needed below) to avoid a cell array + visited = false(1, lnCount); + maxInComponent = zeros(1, lnCount); + numComponents = 0; + for jj = 1:lnCount if ~visited(jj) reachable = bfs(subgraphAdjacency, jj); visited(reachable) = true; - components{end+1} = obj.agents{ii}.lesserNeighbors(reachable); + numComponents = numComponents + 1; + maxInComponent(numComponents) = max(obj.agents{ii}.lesserNeighbors(reachable)); end end % Connect to the greatest index in each connected component in the % lesser neighborhood of this agent - for jj = 1:size(components, 2) - constraintAdjacencyMatrix(ii, max(components{jj})) = true; - constraintAdjacencyMatrix(max(components{jj}), ii) = true; + for jj = 1:numComponents + maxIdx = maxInComponent(jj); + constraintAdjacencyMatrix(ii, maxIdx) = true; + constraintAdjacencyMatrix(maxIdx, ii) = true; end end obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix | constraintAdjacencyMatrix'; @@ -53,24 +59,34 @@ end function cComp = bfs(subgraphAdjacency, startIdx) n = size(subgraphAdjacency, 1); visited = false(1, n); - queue = startIdx; - cComp = startIdx; + + % Pre-allocated queue and component buffer with head/tail pointers + % to avoid element deletion and dynamic array growth + queue = zeros(1, n); + cCompBuf = zeros(1, n); + qHead = 1; + qTail = 2; + queue(1) = startIdx; + cCompBuf(1) = startIdx; + cSize = 1; visited(startIdx) = true; - - while ~isempty(queue) - current = queue(1); - queue(1) = []; - + + while qHead < qTail + current = queue(qHead); + qHead = qHead + 1; + % Find all neighbors of current node in the subgraph neighbors = find(subgraphAdjacency(current, :)); - - for neighbor = neighbors + for kk = 1:numel(neighbors) + neighbor = neighbors(kk); if ~visited(neighbor) visited(neighbor) = true; - cComp = [cComp, neighbor]; - queue = [queue, neighbor]; + cCompBuf(cSize + 1) = neighbor; + cSize = cSize + 1; + queue(qTail) = neighbor; + qTail = qTail + 1; end end end - cComp = sort(cComp); + cComp = sort(cCompBuf(1:cSize)); end \ No newline at end of file diff --git a/@miSim/miSim.m b/@miSim/miSim.m index 7fd9f97..d0b4560 100644 --- a/@miSim/miSim.m +++ b/@miSim/miSim.m @@ -2,17 +2,17 @@ classdef miSim % multiagent interconnection simulation % Simulation parameters - properties (SetAccess = private, GetAccess = public) + properties (SetAccess = public, GetAccess = public) timestep = NaN; % delta time interval for simulation iterations timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays) maxIter = NaN; % maximum number of simulation iterations - domain = rectangularPrism; - objective = sensingObjective; - obstacles = cell(0, 1); % geometries that define obstacles within the domain - agents = cell(0, 1); % agents that move within the domain - adjacency = NaN; % Adjacency matrix representing communications network graph - constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections - partitioning = NaN; + domain; + objective; + obstacles; % geometries that define obstacles within the domain + agents; % agents that move within the domain + adjacency = false(0, 0); % Adjacency matrix representing communications network graph + constraintAdjacencyMatrix = false(0, 0); % Adjacency matrix representing desired lesser neighbor connections + partitioning = zeros(0, 0); perf; % sensor performance timeseries array performance = 0; % simulation performance timeseries vector barrierGain = NaN; % CBF gain parameter @@ -54,6 +54,15 @@ classdef miSim end methods (Access = public) + function obj = miSim() + arguments (Output) + obj (1, 1) miSim + end + obj.domain = rectangularPrism; + obj.objective = sensingObjective; + obj.obstacles = {rectangularPrism}; + obj.agents = {agent}; + end [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo); [obj] = run(obj); [obj] = lesserNeighbor(obj); diff --git a/@miSim/run.m b/@miSim/run.m index a759da7..cc4a280 100644 --- a/@miSim/run.m +++ b/@miSim/run.m @@ -6,21 +6,24 @@ function [obj] = run(obj) obj (1, 1) {mustBeA(obj, "miSim")}; end - % Start video writer - if obj.makeVideo - v = obj.setupVideoWriter(); - v.open(); + if coder.target('MATLAB') + % Start video writer + if obj.makeVideo + v = obj.setupVideoWriter(); + v.open(); + end end - + for ii = 1:size(obj.times, 1) % Display current sim time obj.t = obj.times(ii); obj.timestepIndex = ii; - fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1); + if coder.target('MATLAB') + fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1); - % Before moving - % Validate current simulation configuration - obj.validate(); + % Validate current simulation configuration + obj.validate(); + end % Update partitioning before moving (this one is strictly for % plotting purposes, the real partitioning is done by the agents) @@ -39,28 +42,31 @@ function [obj] = run(obj) % CBF constraints solved by QP obj = constrainMotion(obj); - % After moving - % Update agent position history array - obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3); + if coder.target('MATLAB') + % Update agent position history array + obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, "UniformOutput", false)), size(obj.agents, 1), 1, 3); - % Update total performance - obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))]; + % Update total performance + obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))]; - % Update adjacency matrix - obj = obj.updateAdjacency(); + % Update adjacency matrix + obj = obj.updateAdjacency(); - % Update plots - obj = obj.updatePlots(); + % Update plots + obj = obj.updatePlots(); - % Write frame in to video - if obj.makeVideo - I = getframe(obj.f); - v.writeVideo(I); + % Write frame in to video + if obj.makeVideo + I = getframe(obj.f); + v.writeVideo(I); + end end end - if obj.makeVideo - % Close video file - v.close(); + if coder.target('MATLAB') + if obj.makeVideo + % Close video file + v.close(); + end end end diff --git a/@sensingObjective/initialize.m b/@sensingObjective/initialize.m index a1a8669..8096ba2 100644 --- a/@sensingObjective/initialize.m +++ b/@sensingObjective/initialize.m @@ -30,8 +30,7 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr [obj.X, obj.Y] = meshgrid(xGrid, yGrid); % Evaluate function over grid points - obj.objectiveFunction = objectiveFunction; - obj.values = reshape(obj.objectiveFunction(obj.X, obj.Y), size(obj.X)); + obj.values = reshape(objectiveFunction(obj.X, obj.Y), size(obj.X)); % Normalize obj.values = obj.values ./ max(obj.values, [], "all"); diff --git a/@sensingObjective/initializeWithValues.m b/@sensingObjective/initializeWithValues.m new file mode 100644 index 0000000..3a5354b --- /dev/null +++ b/@sensingObjective/initializeWithValues.m @@ -0,0 +1,42 @@ +function obj = initializeWithValues(obj, values, domain, discretizationStep, protectedRange, sensorPerformanceMinimum) + arguments (Input) + obj (1,1) {mustBeA(obj, "sensingObjective")}; + values (:,:) double; + domain (1, 1) {mustBeGeometry}; + discretizationStep (1, 1) double = 1; + protectedRange (1, 1) double = 1; + sensorPerformanceMinimum (1, 1) double = 1e-6; + end + arguments (Output) + obj (1,1) {mustBeA(obj, "sensingObjective")}; + end + + obj.discretizationStep = discretizationStep; + obj.sensorPerformanceMinimum = sensorPerformanceMinimum; + obj.protectedRange = protectedRange; + + % Extract footprint limits + xMin = min(domain.footprint(:, 1)); + xMax = max(domain.footprint(:, 1)); + yMin = min(domain.footprint(:, 2)); + yMax = max(domain.footprint(:, 2)); + + xGrid = unique([xMin:obj.discretizationStep:xMax, xMax]); + yGrid = unique([yMin:obj.discretizationStep:yMax, yMax]); + + % Store grid points + [obj.X, obj.Y] = meshgrid(xGrid, yGrid); + + % Use pre-computed values (caller must evaluate on same grid) + obj.values = reshape(values, size(obj.X)); + + % Normalize + obj.values = obj.values ./ max(obj.values, [], "all"); + + % Store ground position (peak of objective) + idx = obj.values == 1; + obj.groundPos = [obj.X(idx), obj.Y(idx)]; + obj.groundPos = obj.groundPos(1, 1:2); + + assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective") +end diff --git a/@sensingObjective/sensingObjective.m b/@sensingObjective/sensingObjective.m index 6456d07..46b41a8 100644 --- a/@sensingObjective/sensingObjective.m +++ b/@sensingObjective/sensingObjective.m @@ -4,7 +4,6 @@ classdef sensingObjective label = ""; groundPos = [NaN, NaN]; discretizationStep = NaN; - objectiveFunction = @(x, y) NaN; % define objective functions over a grid in this manner X = []; Y = []; values = []; @@ -14,7 +13,8 @@ classdef sensingObjective methods (Access = public) [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum); - [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange); + [obj] = initializeWithValues(obj, values, domain, discretizationStep, protectedRange, sensorPerformanceMinimum); + [obj] = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange); [f ] = plot(obj, ind, f); end end \ No newline at end of file diff --git a/aerpaw/MESSAGE_TYPE.m b/aerpaw/MESSAGE_TYPE.m index 338c5ee..9637edc 100644 --- a/aerpaw/MESSAGE_TYPE.m +++ b/aerpaw/MESSAGE_TYPE.m @@ -3,7 +3,10 @@ classdef MESSAGE_TYPE < uint8 TARGET (1) % Server->Client: target coordinates follow (3 doubles) ACK (2) % Client->Server: command received READY (3) % Both: ready for next command / mission complete - RTL (4) % Server->Client: return to launch - LAND (5) % Server->Client: land now + RTL (4) % Server->Client: return to launch + LAND (5) % Server->Client: land now + GUIDANCE_TOGGLE (6) % Server->Client: toggle guidance mode on/off + REQUEST_POSITION (7) % Server->Client: respond with current ENU position + POSITION (8) % Client->Server: current ENU position (3 doubles) end end diff --git a/aerpaw/client/uav_runner.py b/aerpaw/client/uav_runner.py index f2741c6..9204bb1 100644 --- a/aerpaw/client/uav_runner.py +++ b/aerpaw/client/uav_runner.py @@ -36,11 +36,14 @@ from aerpawlib.vehicle import Drone # Message types - must match MESSAGE_TYPE.m enum class MessageType(IntEnum): - TARGET = 1 - ACK = 2 - READY = 3 - RTL = 4 - LAND = 5 + TARGET = 1 + ACK = 2 + READY = 3 + RTL = 4 + LAND = 5 + GUIDANCE_TOGGLE = 6 + REQUEST_POSITION = 7 + POSITION = 8 AERPAW_DIR = Path(__file__).parent.parent @@ -177,6 +180,7 @@ class UAVRunner(BasicRunner): return log_task = None + nav_task = None try: # Takeoff to above AERPAW minimum altitude print("[UAV] Taking off...") @@ -186,32 +190,64 @@ class UAVRunner(BasicRunner): # Start GPS logging in background log_task = asyncio.create_task(_gps_log_loop(drone)) - # Command loop - handle TARGET, RTL, LAND, READY from controller + # Command loop - handle all messages from controller waypoint_num = 0 + in_guidance = False while True: msg_type = await recv_message_type(reader) print(f"[UAV] Received: {msg_type.name}") - if msg_type == MessageType.TARGET: + if msg_type == MessageType.GUIDANCE_TOGGLE: + in_guidance = not in_guidance + print(f"[UAV] Guidance mode: {'ON' if in_guidance else 'OFF'}") + if not in_guidance: + # Exiting guidance: wait for current navigation to finish + # before resuming sequential (ACK/READY) mode + if nav_task and not nav_task.done(): + print("[UAV] Waiting for current navigation to complete...") + await nav_task + nav_task = None + # Acknowledge that we are ready for sequential commands + await send_message_type(writer, MessageType.ACK) + print("[UAV] Sent ACK (guidance mode exited, ready for sequential commands)") + + elif msg_type == MessageType.REQUEST_POSITION: + # Respond immediately with current ENU position relative to origin + pos = drone.position + enu = pos - self.origin # VectorNED(north, east, down) + await send_message_type(writer, MessageType.POSITION) + writer.write(struct.pack(' + + int32 + + + + + int32 + + + + + int32 + + + + + int32 + + + @@ -88,71 +108,965 @@ /home/kdee/Desktop/miSim/aerpaw/codegen - /home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/RemoveDependentEq_.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller.h + /home/kdee/Desktop/miSim/aerpaw/codegen/RemoveDependentEq_.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h + /home/kdee/Desktop/miSim/aerpaw/codegen/RemoveDependentIneq_.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/RemoveDependentIneq_.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h + /home/kdee/Desktop/miSim/aerpaw/codegen/abs.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/abs.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h + /home/kdee/Desktop/miSim/aerpaw/codegen/addBoundToActiveSetMatrix_.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h + /home/kdee/Desktop/miSim/aerpaw/codegen/addBoundToActiveSetMatrix_.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h + /home/kdee/Desktop/miSim/aerpaw/codegen/agent.cpp GENERATED_SOURCE - /home/kdee/matlab/R2025a/extern/include/tmwtypes.h + /home/kdee/Desktop/miSim/aerpaw/codegen/agent.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/all.cpp GENERATED_SOURCE + + /home/kdee/Desktop/miSim/aerpaw/codegen/all.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/any1.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/any1.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/applyToMultipleDims.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/applyToMultipleDims.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/atan2d.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/atan2d.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/coder_array.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/coder_bounded_array.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/colon.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/colon.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFirstOrderOpt.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFirstOrderOpt.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval_ReuseHx.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval_ReuseHx.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/computeGrad_StoreHx.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/computeGrad_StoreHx.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/computeQ_.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/computeQ_.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/compute_deltax.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/compute_deltax.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/compute_lambda.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/compute_lambda.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/cone.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/cone.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/controller.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_internal_types.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/countsort.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/countsort.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/dot.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/dot.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/driver.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/driver.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/exp.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/exp.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/eye.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/eye.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/factoryConstruct.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/factoryConstruct.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/find.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/find.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/norm.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/norm.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/partition.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/partition.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/rt_defines.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/run.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/run.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sort.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/string1.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/string1.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/sum.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/unique.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h + + GENERATED_SOURCE + + + + /home/kdee/matlab/R2025a/extern/include/tmwtypes.h + + GENERATED_SOURCE + + + + /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp + + GENERATED_SOURCE + + /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.h @@ -160,7 +1074,7 @@ true - 2026-02-13T18:04:24 + 2026-02-24T18:31:43 diff --git a/aerpaw/controller.m b/aerpaw/controller.m index 922937f..35db136 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -83,7 +83,69 @@ for w = 1:numWaypoints end end -% Wait for user input before closing experiment +% ---- Phase 2: miSim guidance loop ---------------------------------------- +% Guidance parameters (adjust here and recompile as needed) +MAX_GUIDANCE_STEPS = int32(100); % number of guidance iterations +GUIDANCE_RATE_MS = int32(5000); % ms between iterations (0.2 Hz default) + +% Wait for user input to start guidance loop +if coder.target('MATLAB') + input('Press Enter to start guidance loop: ', 's'); +else + coder.ceval('waitForUserInput'); +end + +% Enter guidance mode on all clients +if ~coder.target('MATLAB') + coder.ceval('sendGuidanceToggle', int32(numClients)); +end + +% Request initial GPS positions and initialise guidance algorithm +positions = zeros(MAX_CLIENTS, 3); +if ~coder.target('MATLAB') + coder.ceval('sendRequestPositions', int32(numClients)); + coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS)); +end +guidance_step(positions(1:numClients, :), true); + +% Main guidance loop +for step = 1:MAX_GUIDANCE_STEPS + % Query current GPS positions from all clients + if ~coder.target('MATLAB') + coder.ceval('sendRequestPositions', int32(numClients)); + coder.ceval('recvPositions', int32(numClients), coder.ref(positions), int32(MAX_CLIENTS)); + end + + % Run one guidance step: feed GPS positions in, get targets out + nextPositions = guidance_step(positions(1:numClients, :), false); + + % Send target to each client (no ACK/READY expected in guidance mode) + for i = 1:numClients + target = nextPositions(i, :); + if ~coder.target('MATLAB') + coder.ceval('sendTarget', int32(i), coder.ref(target)); + else + disp(['[guidance] target UAV ', num2str(i), ': ', num2str(target)]); + end + end + + % Wait for the guidance rate interval before the next iteration + if ~coder.target('MATLAB') + coder.ceval('sleepMs', int32(GUIDANCE_RATE_MS)); + end +end + +% Exit guidance mode on all clients (second toggle) +if ~coder.target('MATLAB') + coder.ceval('sendGuidanceToggle', int32(numClients)); + % Wait for ACK from all clients: confirms each client has finished its + % last guidance navigation and is back in sequential (ACK/READY) mode. + coder.ceval('waitForAllMessageType', int32(numClients), ... + int32(MESSAGE_TYPE.ACK)); +end +% -------------------------------------------------------------------------- + +% Wait for user input before closing experiment (RTL + LAND) if coder.target('MATLAB') input('Press Enter to close experiment (RTL + LAND): ', 's'); else diff --git a/aerpaw/guidance_step.m b/aerpaw/guidance_step.m new file mode 100644 index 0000000..b8489b4 --- /dev/null +++ b/aerpaw/guidance_step.m @@ -0,0 +1,179 @@ +function nextPositions = guidance_step(currentPositions, isInit) +% guidance_step One step of the miSim sensing coverage guidance algorithm. +% +% Wraps the miSim gradient-ascent + CBF motion algorithm for AERPAW. +% Holds full simulation state in a persistent variable between calls. +% +% Usage (from controller.m): +% guidance_step(initPositions, true) % first call: initialise state +% nextPos = guidance_step(gpsPos, false) % subsequent calls: run one step +% +% Inputs: +% currentPositions (MAX_CLIENTS × 3) double ENU [east north up] metres +% isInit (1,1) logical true on first call only +% +% Output: +% nextPositions (MAX_CLIENTS × 3) double guidance targets, ENU metres +% +% Codegen notes: +% - Persistent variable 'sim' holds the miSim object between calls. +% - Plotting/video are disabled (makePlots=false, makeVideo=false) for +% deployment. The coder.target('MATLAB') guards in the miSim/agent +% class files must be in place before codegen will succeed. +% - objectiveFunctionWrapper returns a function handle which is not +% directly codegen-compatible; the MATLAB path uses it normally. The +% compiled path requires an equivalent C impl (see TODO below). + +coder.extrinsic('disp', 'objectiveFunctionWrapper'); + +% ========================================================================= +% Tunable guidance parameters — adjust here and recompile as needed. +% ========================================================================= +MAX_CLIENTS = int32(4); % must match MAX_CLIENTS in controller.m + +% Domain bounds in ENU metres [east, north, up] +DOMAIN_MIN = [ 0.0, 0.0, 25.0]; +DOMAIN_MAX = [ 20.0, 20.0, 55.0]; +MIN_ALT = 25.0; % hard altitude floor (m) + +% Sensing objective: bivariate Gaussian centred at [east, north] +OBJECTIVE_GROUND_POS = [10.0, 10.0]; +DISCRETIZATION_STEP = 0.1; % objective grid step (m) — coarser = faster +PROTECTED_RANGE = 1.0; % objective centre must be >this from domain edge + +% Agent safety geometry +COLLISION_RADIUS = 3.0; % spherical collision radius (m) +COMMS_RANGE = 60.0; % communications range (m) + +% Gradient-ascent parameters +INITIAL_STEP_SIZE = 1; % step size at iteration 0 (decays to 0 at MAX_ITER) +MAX_ITER = 100; % guidance steps (sets decay rate) + +% Sensor model (sigmoidSensor) +ALPHA_DIST = 60.0; % effective sensing distance (m) — set beyond max domain slant range (~53 m) +BETA_DIST = 0.2; % distance sigmoid steepness — gentle, tilt drives the coverage gradient +ALPHA_TILT = 10.0; % max useful tilt angle (degrees) +BETA_TILT = 1.0; % tilt sigmoid steepness + +% Safety filter — Control Barrier Function (CBF/QP) +BARRIER_GAIN = 100; +BARRIER_EXPONENT = 3; + +% Simulation timestep (s) — should match GUIDANCE_RATE_MS / 1000 in controller.m +TIMESTEP = 5.0; +% ========================================================================= + +persistent sim; +if isempty(sim) + sim = miSim; +end + +% Pre-allocate output with known static size (required for codegen) +nextPositions = zeros(MAX_CLIENTS, 3); + +numAgents = int32(size(currentPositions, 1)); + +if isInit + if coder.target('MATLAB') + disp('[guidance_step] Initialising simulation...'); + end + + % --- Build domain geometry --- + dom = rectangularPrism; + dom = dom.initialize([DOMAIN_MIN; DOMAIN_MAX], REGION_TYPE.DOMAIN, "Guidance Domain"); + + % --- Build sensing objective --- + dom.objective = sensingObjective; + if coder.target('MATLAB') + % objectiveFunctionWrapper returns a function handle — MATLAB only. + objFcn = objectiveFunctionWrapper(OBJECTIVE_GROUND_POS, 3*eye(2)); + dom.objective = dom.objective.initialize(objFcn, dom, ... + DISCRETIZATION_STEP, PROTECTED_RANGE); + else + % Evaluate bivariate Gaussian inline (codegen-compatible; no function handle). + % Must build the same grid that initializeWithValues uses internally. + 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); + objValues = exp(-0.5 * (dx .* dx + dy .* dy)); + dom.objective = dom.objective.initializeWithValues(objValues, dom, ... + DISCRETIZATION_STEP, PROTECTED_RANGE); + end + + % --- Build shared sensor model --- + sensor = sigmoidSensor; + sensor = sensor.initialize(ALPHA_DIST, BETA_DIST, ALPHA_TILT, BETA_TILT); + + % --- Initialise agents from GPS positions --- + agentList = cell(numAgents, 1); + for ii = 1:numAgents + pos = currentPositions(ii, :); + geom = spherical; + geom = geom.initialize(pos, COLLISION_RADIUS, REGION_TYPE.COLLISION, ... + sprintf("UAV %d Collision", ii)); + ag = agent; + ag = ag.initialize(pos, geom, sensor, COMMS_RANGE, MAX_ITER, ... + INITIAL_STEP_SIZE, sprintf("UAV %d", ii)); + agentList{ii} = ag; + end + + % --- Initialise simulation (plots and video disabled) --- + sim = miSim; + sim = sim.initialize(dom, agentList, BARRIER_GAIN, BARRIER_EXPONENT, ... + MIN_ALT, TIMESTEP, MAX_ITER, cell(0, 1), false, false); + + if coder.target('MATLAB') + disp('[guidance_step] Initialisation complete.'); + end + + % On the init call return current positions unchanged + for ii = 1:numAgents + nextPositions(ii, :) = currentPositions(ii, :); + end + +else + % ===================================================================== + % One guidance step + % ===================================================================== + + % 1. Inject actual GPS positions (closed-loop feedback) + for ii = 1:size(sim.agents, 1) + sim.agents{ii}.lastPos = sim.agents{ii}.pos; + sim.agents{ii}.pos = currentPositions(ii, :); + + % Re-centre collision geometry at new position + d = currentPositions(ii, :) - sim.agents{ii}.collisionGeometry.center; + sim.agents{ii}.collisionGeometry = sim.agents{ii}.collisionGeometry.initialize( ... + sim.agents{ii}.collisionGeometry.center + d, ... + sim.agents{ii}.collisionGeometry.radius, ... + REGION_TYPE.COLLISION); + end + + % 2. Advance timestep counter + sim.timestepIndex = sim.timestepIndex + 1; + + % 3. Update communications topology (Lesser Neighbour Assignment) + sim = sim.lesserNeighbor(); + + % 4. Compute Voronoi partitioning + sim.partitioning = sim.agents{1}.partition(sim.agents, sim.domain.objective); + + % 5. Unconstrained gradient-ascent step for each agent + for ii = 1:size(sim.agents, 1) + sim.agents{ii} = sim.agents{ii}.run(sim.domain, sim.partitioning, ... + sim.timestepIndex, ii, sim.agents); + end + + % 6. Apply CBF safety filter (collision / comms / domain constraints via QP) + sim = sim.constrainMotion(); + + % 7. Return constrained next positions + for ii = 1:size(sim.agents, 1) + nextPositions(ii, :) = sim.agents{ii}.pos; + end + +end + +end diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp index 190f4b5..f8246b7 100644 --- a/aerpaw/impl/controller_impl.cpp +++ b/aerpaw/impl/controller_impl.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #define SERVER_PORT 5000 #define SERVER_IP "127.0.0.1" @@ -120,6 +121,9 @@ static const char* messageTypeName(uint8_t msgType) { case 3: return "READY"; case 4: return "RTL"; case 5: return "LAND"; + case 6: return "GUIDANCE_TOGGLE"; + case 7: return "REQUEST_POSITION"; + case 8: return "POSITION"; default: return "UNKNOWN"; } } @@ -190,7 +194,17 @@ int waitForAllMessageType(int numClients, int expectedType) { if (!completed[i] && FD_ISSET(clientSockets[i], &readfds)) { uint8_t msgType; int len = recv(clientSockets[i], &msgType, 1, 0); - if (len != 1) return 0; + if (len == 0) { + std::cerr << "waitForAllMessageType: client " << (i + 1) + << " disconnected while waiting for " + << messageTypeName(expected) << "\n"; + return 0; + } + if (len < 0) { + std::cerr << "waitForAllMessageType: recv error for client " << (i + 1) + << " while waiting for " << messageTypeName(expected) << "\n"; + return 0; + } std::cout << "Received " << messageTypeName(msgType) << " from client " << (i + 1) << "\n"; @@ -207,6 +221,66 @@ int waitForAllMessageType(int numClients, int expectedType) { // Wait for user to press Enter void waitForUserInput() { - std::cout << "Press Enter to close experiment (RTL + LAND)...\n"; + std::cout << "Press Enter to continue...\n"; std::cin.ignore(std::numeric_limits::max(), '\n'); } + +// Broadcast GUIDANCE_TOGGLE to all clients +void sendGuidanceToggle(int numClients) { + for (int i = 1; i <= numClients; i++) { + sendMessageType(i, 6); // GUIDANCE_TOGGLE = 6 + } +} + +// Send REQUEST_POSITION to all clients +int sendRequestPositions(int numClients) { + for (int i = 1; i <= numClients; i++) { + if (!sendMessageType(i, 7)) return 0; // REQUEST_POSITION = 7 + } + return 1; +} + +// Receive POSITION response (1 byte type + 24 bytes ENU) from all clients. +// Stores results in column-major order: positions[client + 0*maxClients] = x (east), +// positions[client + 1*maxClients] = y (north), +// positions[client + 2*maxClients] = z (up) +int recvPositions(int numClients, double* positions, int maxClients) { + if (numClients <= 0 || numClients > (int)clientSockets.size()) return 0; + + for (int i = 0; i < numClients; i++) { + // Expect: POSITION byte (1) + 3 doubles (24) + uint8_t msgType; + if (recv(clientSockets[i], &msgType, 1, MSG_WAITALL) != 1) { + std::cerr << "recvPositions: failed to read msg type from client " << (i + 1) << "\n"; + return 0; + } + if (msgType != 8) { // POSITION = 8 + std::cerr << "recvPositions: expected POSITION(8), got " << (int)msgType + << " from client " << (i + 1) << "\n"; + return 0; + } + + double coords[3]; + if (recv(clientSockets[i], coords, sizeof(coords), MSG_WAITALL) != (ssize_t)sizeof(coords)) { + std::cerr << "recvPositions: failed to read coords from client " << (i + 1) << "\n"; + return 0; + } + + // Store column-major (MATLAB layout): col 0 = east, col 1 = north, col 2 = up + positions[i + 0 * maxClients] = coords[0]; // east (x) + positions[i + 1 * maxClients] = coords[1]; // north (y) + positions[i + 2 * maxClients] = coords[2]; // up (z) + + std::cout << "Position from client " << (i + 1) << ": " + << coords[0] << "," << coords[1] << "," << coords[2] << "\n"; + } + return 1; +} + +// Sleep for the given number of milliseconds +void sleepMs(int ms) { + struct timespec ts; + ts.tv_sec = ms / 1000; + ts.tv_nsec = (ms % 1000) * 1000000L; + nanosleep(&ts, nullptr); +} diff --git a/aerpaw/impl/controller_impl.h b/aerpaw/impl/controller_impl.h index 5f6ac05..2fac077 100644 --- a/aerpaw/impl/controller_impl.h +++ b/aerpaw/impl/controller_impl.h @@ -23,6 +23,12 @@ int sendMessageType(int clientId, int msgType); int sendTarget(int clientId, const double* coords); int waitForAllMessageType(int numClients, int expectedType); +// Guidance loop operations +void sendGuidanceToggle(int numClients); +int sendRequestPositions(int numClients); +int recvPositions(int numClients, double* positions, int maxClients); // column-major maxClients x 3 +void sleepMs(int ms); + #ifdef __cplusplus } #endif diff --git a/aerpaw/results/plotGpsCsvs.m b/aerpaw/results/plotGpsCsvs.m index bdc8561..8c193a8 100644 --- a/aerpaw/results/plotGpsCsvs.m +++ b/aerpaw/results/plotGpsCsvs.m @@ -6,9 +6,10 @@ c = ["r", "g", "b", "m", "c", "y", "k"]; % plotting colors seaToGroundLevel = 110; % meters, measured approximately from USGS national map viewer % Paths to logs -gpsCsvs = ["GPS_DATA_0c8d904aa159_2026-02-16_13:26:33.csv"; ... - "GPS_DATA_8e4f52dac04d_2026-02-16_13:26:33.csv"; ... - ]; +gpsCsvs = fullfile ("sandbox", "test1", ... + ["GPS_DATA_0c8d904aa159_2026-02-24_21:33:25.csv"; ... + "GPS_DATA_8e4f52dac04d_2026-02-24_21:33:25.csv"; ... + ]); G = cell(size(gpsCsvs)); for ii = 1:size(gpsCsvs, 1) @@ -16,7 +17,7 @@ for ii = 1:size(gpsCsvs, 1) G{ii} = readGpsCsv(gpsCsvs(ii)); % Plot recorded trajectory - geoplot3(gf, G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 3, "MarkerSize", 5); + geoplot3(gf, G{ii}.Latitude, G{ii}.Longitude, G{ii}.Altitude + seaToGroundLevel, c(mod(ii, length(c))), 'LineWidth', 2, "MarkerSize", 5); end hold(gf, "off"); \ No newline at end of file diff --git a/geometries/@cone/plot.m b/geometries/@cone/plot.m index 8076e92..83098db 100644 --- a/geometries/@cone/plot.m +++ b/geometries/@cone/plot.m @@ -31,7 +31,7 @@ function [obj, f] = plot(obj, ind, f, maxAlt) o = surf(f.CurrentAxes, X, Y, Z); else hold(f.Children(1).Children(ind(1)), "on"); - o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(obj.tag.color, 1, 1, 3), "FaceAlpha", 0.25, "EdgeColor", "none"); + o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(regionTypeColor(obj.tag), 1, 1, 3), "FaceAlpha", 0.25, "EdgeColor", "none"); hold(f.Children(1).Children(ind(1)), "off"); end diff --git a/geometries/@rectangularPrism/plotWireframe.m b/geometries/@rectangularPrism/plotWireframe.m index 1e6e9c6..14d69a8 100644 --- a/geometries/@rectangularPrism/plotWireframe.m +++ b/geometries/@rectangularPrism/plotWireframe.m @@ -19,10 +19,10 @@ function [obj, f] = plotWireframe(obj, ind, f) % Plot the boundaries of the geometry into 3D view if isnan(ind) - o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 2); + o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2); else hold(f.Children(1).Children(ind(1)), "on"); - o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 2); + o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2); hold(f.Children(1).Children(ind(1)), "off"); end diff --git a/geometries/@rectangularPrism/rectangularPrism.m b/geometries/@rectangularPrism/rectangularPrism.m index bb0dd07..4c4192d 100644 --- a/geometries/@rectangularPrism/rectangularPrism.m +++ b/geometries/@rectangularPrism/rectangularPrism.m @@ -28,6 +28,12 @@ classdef rectangularPrism end methods (Access = public) + function obj = rectangularPrism() + arguments (Output) + obj (1, 1) rectangularPrism + end + obj.objective = sensingObjective; + end [obj ] = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep); [obj ] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain); [r ] = random(obj); diff --git a/geometries/@spherical/plotWireframe.m b/geometries/@spherical/plotWireframe.m index 0a8bcf8..47bc8aa 100644 --- a/geometries/@spherical/plotWireframe.m +++ b/geometries/@spherical/plotWireframe.m @@ -25,10 +25,10 @@ function [obj, f] = plotWireframe(obj, ind, f) % Plot the boundaries of the geometry into 3D view if isnan(ind) - o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 2); + o = plot3(f.CurrentAxes, X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 2); else hold(f.Children(1).Children(ind(1)), "on"); - o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", obj.tag.color, "LineWidth", 1); + o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, "-", "Color", regionTypeColor(obj.tag), "LineWidth", 1); hold(f.Children(1).Children(ind(1)), "off"); end diff --git a/geometries/@spherical/spherical.m b/geometries/@spherical/spherical.m index 8e3b618..fd33742 100644 --- a/geometries/@spherical/spherical.m +++ b/geometries/@spherical/spherical.m @@ -2,12 +2,12 @@ classdef spherical % Rectangular prism geometry properties (SetAccess = private, GetAccess = public) % Spatial - center = NaN; + center = NaN(1, 3); radius = NaN; diameter = NaN; - vertices; % fake vertices - edges; % fake edges + vertices = NaN(6, 3); % fake vertices + edges = NaN(8, 2); % fake edges % Plotting lines; @@ -22,6 +22,12 @@ classdef spherical end methods (Access = public) + function obj = spherical() + arguments (Output) + obj (1, 1) spherical + end + obj.objective = sensingObjective; + end [obj ] = initialize(obj, center, radius, tag, label); [r ] = random(obj); [c ] = contains(obj, pos); diff --git a/geometries/REGION_TYPE.m b/geometries/REGION_TYPE.m index 8a15dd1..6850229 100644 --- a/geometries/REGION_TYPE.m +++ b/geometries/REGION_TYPE.m @@ -1,20 +1,10 @@ classdef REGION_TYPE < uint8 - properties - id - color - end enumeration - INVALID (0, [255, 127, 255]); % default value - DOMAIN (1, [0, 0, 0]); % domain region - OBSTACLE (2, [255, 127, 127]); % obstacle region - COLLISION (3, [255, 255, 128]); % collision avoidance region - FOV (4, [255, 165, 0]); % field of view region - COMMS (5, [0, 255, 0]); % comunications region + INVALID (0) % default value + DOMAIN (1) % domain region + OBSTACLE (2) % obstacle region + COLLISION (3) % collision avoidance region + FOV (4) % field of view region + COMMS (5) % comunications region end - methods - function obj = REGION_TYPE(id, color) - obj.id = id; - obj.color = color./ 255; - end - end -end \ No newline at end of file +end diff --git a/geometries/regionTypeColor.m b/geometries/regionTypeColor.m new file mode 100644 index 0000000..6edab6f --- /dev/null +++ b/geometries/regionTypeColor.m @@ -0,0 +1,24 @@ +function color = regionTypeColor(tag) +% regionTypeColor Return the RGB color (0-1 range) for a REGION_TYPE value. +arguments (Input) + tag (1, 1) REGION_TYPE +end +arguments (Output) + color (1, 3) double +end + +switch tag + case REGION_TYPE.DOMAIN + color = [0, 0, 0] / 255; + case REGION_TYPE.OBSTACLE + color = [255, 127, 127] / 255; + case REGION_TYPE.COLLISION + color = [255, 255, 128] / 255; + case REGION_TYPE.FOV + color = [255, 165, 0] / 255; + case REGION_TYPE.COMMS + color = [0, 255, 0] / 255; + otherwise % INVALID + color = [255, 127, 255] / 255; +end +end \ No newline at end of file diff --git a/resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/LiklXLmEZberq-e28LLA86kEkNMd.xml b/resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/LiklXLmEZberq-e28LLA86kEkNMd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/LiklXLmEZberq-e28LLA86kEkNMd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/LiklXLmEZberq-e28LLA86kEkNMp.xml b/resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/LiklXLmEZberq-e28LLA86kEkNMp.xml new file mode 100644 index 0000000..be12718 --- /dev/null +++ b/resources/project/Gnz6T47dAsmf4YcBHB3EkpeZeYA/LiklXLmEZberq-e28LLA86kEkNMp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/M5zOD2wqz2AjI0NiSW1HbszatFMd.xml b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/M5zOD2wqz2AjI0NiSW1HbszatFMd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/M5zOD2wqz2AjI0NiSW1HbszatFMd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/M5zOD2wqz2AjI0NiSW1HbszatFMp.xml b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/M5zOD2wqz2AjI0NiSW1HbszatFMp.xml new file mode 100644 index 0000000..2fb7c37 --- /dev/null +++ b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/M5zOD2wqz2AjI0NiSW1HbszatFMp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/qFRcMsci5bTUnTBevgtb2pITyQU/l6p0lRpsuYLVyH-ULL9p4PIRTu4d.xml b/resources/project/qFRcMsci5bTUnTBevgtb2pITyQU/l6p0lRpsuYLVyH-ULL9p4PIRTu4d.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/qFRcMsci5bTUnTBevgtb2pITyQU/l6p0lRpsuYLVyH-ULL9p4PIRTu4d.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/qFRcMsci5bTUnTBevgtb2pITyQU/l6p0lRpsuYLVyH-ULL9p4PIRTu4p.xml b/resources/project/qFRcMsci5bTUnTBevgtb2pITyQU/l6p0lRpsuYLVyH-ULL9p4PIRTu4p.xml new file mode 100644 index 0000000..092ec7e --- /dev/null +++ b/resources/project/qFRcMsci5bTUnTBevgtb2pITyQU/l6p0lRpsuYLVyH-ULL9p4PIRTu4p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/test/test_miSim.m b/test/test_miSim.m index 2f7531e..3072220 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -207,6 +207,12 @@ classdef test_miSim < matlab.unittest.TestCase break; end end + + % Make sure candidate clears domain floor + if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt + violation = true; + end + if violation continue; end @@ -340,6 +346,12 @@ classdef test_miSim < matlab.unittest.TestCase break; end end + + % Make sure candidate clears domain floor + if newAgent.pos(3) - newAgent.collisionGeometry.radius <= tc.minAlt + violation = true; + end + if violation continue; end diff --git a/util/validators/arguments/mustBeGeometry.m b/util/validators/arguments/mustBeGeometry.m index 095f90e..d333fe4 100644 --- a/util/validators/arguments/mustBeGeometry.m +++ b/util/validators/arguments/mustBeGeometry.m @@ -1,10 +1,11 @@ function mustBeGeometry(geometry) - validGeometries = ["rectangularPrism"; "spherical"]; - if isa(geometry, "cell") + if isa(geometry, 'cell') for ii = 1:size(geometry, 1) - assert(any(arrayfun(@(x) isa(geometry{ii}, x), validGeometries)), "Geometry in index %d is not a valid geometry class", ii); + assert(isa(geometry{ii}, 'rectangularPrism') || isa(geometry{ii}, 'spherical'), ... + 'Geometry in index %d is not a valid geometry class', ii); end else - assert(any(arrayfun(@(x) isa(geometry, x), validGeometries)), "Geometry is not a valid geometry class"); + assert(isa(geometry, 'rectangularPrism') || isa(geometry, 'spherical'), ... + 'Geometry is not a valid geometry class'); end end \ No newline at end of file diff --git a/util/validators/arguments/mustBeSensor.m b/util/validators/arguments/mustBeSensor.m index 6d03935..6166995 100644 --- a/util/validators/arguments/mustBeSensor.m +++ b/util/validators/arguments/mustBeSensor.m @@ -1,10 +1,11 @@ function mustBeSensor(sensorModel) - validSensorModels = ["fixedCardinalSensor"; "sigmoidSensor";]; - if isa(sensorModel, "cell") + if isa(sensorModel, 'cell') for ii = 1:size(sensorModel, 1) - assert(any(arrayfun(@(x) isa(sensorModel{ii}, x), validSensorModels)), "Sensor in index %d is not a valid sensor class", ii); + assert(isa(sensorModel{ii}, 'sigmoidSensor'), ... + 'Sensor in index %d is not a valid sensor class', ii); end else - assert(any(arrayfun(@(x) isa(sensorModel, x), validSensorModels)), "Sensor is not a valid sensor class"); + assert(isa(sensorModel, 'sigmoidSensor'), ... + 'Sensor is not a valid sensor class'); end end \ No newline at end of file