kdee/miSim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added debug visualization for agent GA

Browse Source
This commit is contained in:
Kevin Dee
2025-11-30 11:00:39 -08:00
parent f9f070e2d0
commit 4e0f213d0c
5 changed files with 88 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
@agent/agent.m
View File

@@ -31,11 +31,13 @@ classdef agent
% Plotting
scatterPoints;
debug = false;
debugFig;
end
methods (Access = public)
[obj] = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
[obj] = run(obj, domain, partitioning);
[obj] = run(obj, domain, partitioning, t);
[obj, f] = plot(obj, ind, f);
updatePlots(obj);
end

29
@agent/initialize.m
View File

@@ -1,4 +1,4 @@
function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label)
function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label, debug)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'agent')};
pos (1, 3) double;
@@ -11,6 +11,7 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
comRange (1, 1) double = NaN;
index (1, 1) double = NaN;
label (1, 1) string = "";
debug (1, 1) logical = false;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'agent')};
@@ -26,6 +27,32 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
obj.comRange = comRange;
obj.index = index;
obj.label = label;
obj.debug = debug;
if obj.debug
obj.debugFig = figure;
tiledlayout(obj.debugFig, "TileSpacing", "tight", "Padding", "compact");
nexttile;
axes(obj.debugFig.Children(1).Children(1));
axis(obj.debugFig.Children(1).Children(1), "image");
xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
title(obj.debugFig.Children(1).Children(1), "Objective View");
nexttile;
axes(obj.debugFig.Children(1).Children(1));
axis(obj.debugFig.Children(1).Children(1), "image");
xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
title(obj.debugFig.Children(1).Children(1), "Sensor Performance View");
nexttile;
axes(obj.debugFig.Children(1).Children(1));
axis(obj.debugFig.Children(1).Children(1), "image");
xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
title(obj.debugFig.Children(1).Children(1), "Gradient Objective View");
nexttile;
axes(obj.debugFig.Children(1).Children(1));
axis(obj.debugFig.Children(1).Children(1), "image");
xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
title(obj.debugFig.Children(1).Children(1), "Gradient Sensor Performance View");
end
% Initialize FOV cone
obj.fovGeometry = cone;

31
@agent/run.m
View File

@@ -1,8 +1,9 @@
function obj = run(obj, domain, partitioning)
function obj = run(obj, domain, partitioning, t)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'agent')};
domain (1, 1) {mustBeGeometry};
partitioning (:, :) double;
t (1, 1) double;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'agent')};
@@ -17,16 +18,16 @@ function obj = run(obj, domain, partitioning)
maskedY = domain.objective.Y(partitionMask);
sensorValues = obj.sensorModel.sensorPerformance(obj.pos, obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
% Find agent's performance
obj.performance = [obj.performance; sum(objectiveValues .* sensorValues, 'all')];
%%
% Put the values back into the form of the partition
F = NaN(size(partitionMask));
F(partitionMask) = objectiveValues;
S = NaN(size(partitionMask));
S(partitionMask) = sensorValues;
% Find agent's performance
C = S.* F;
obj.performance = [obj.performance sum(C(~isnan(C)))];
% Compute gradient on agent's performance
[gradSensorPerformanceX, gradSensorPerformanceY] = gradient(S, domain.objective.discretizationStep); % grad S_n
[gradObjectiveX, gradObjectiveY] = gradient(F, domain.objective.discretizationStep); % grad f
@@ -34,15 +35,31 @@ function obj = run(obj, domain, partitioning)
gradS = cat(3, gradSensorPerformanceX, gradSensorPerformanceY, zeros(size(gradSensorPerformanceX))); % grad S_n
gradF = cat(3, gradObjectiveX, gradObjectiveY, zeros(size(gradObjectiveX))); % grad f
if obj.debug
hold(obj.debugFig.Children(1).Children(4), "on");
imagesc(obj.debugFig.Children(1).Children(4), F);
hold(obj.debugFig.Children(1).Children(4), "off");
hold(obj.debugFig.Children(1).Children(3), "on");
imagesc(obj.debugFig.Children(1).Children(3), S);
hold(obj.debugFig.Children(1).Children(3), "off");
hold(obj.debugFig.Children(1).Children(2), "on");
imagesc(obj.debugFig.Children(1).Children(2), gradF./max(gradF, [], 'all'));
hold(obj.debugFig.Children(1).Children(2), "off");
hold(obj.debugFig.Children(1).Children(1), "on");
imagesc(obj.debugFig.Children(1).Children(1), abs(gradS)./max(gradS, [], 'all'));
hold(obj.debugFig.Children(1).Children(1), "off");
end
% grad(s*f) = grad(f) * s + f * grad(s) - product rule (f scalar field, s vector field)
gradC = S .* gradF + F .* gradS; % second term provides altitude
gradC = S .* gradF + F .* abs(gradS); % second term provides altitude
% normalize in x3 dimension and find the direction which maximizes ascent
nGradC = vecnorm(gradC, 2, 3);
[xNextIdx, yNextIdx] = find(nGradC == max(nGradC, [], 'all')); % find direction of steepest increase
pNext = [floor(mean(unique(domain.objective.X(:, xNextIdx)))), floor(mean(unique(domain.objective.Y(yNextIdx, :)))), obj.pos(3)]; % have to do some unfortunate rounding here soemtimes
vDir = (pNext - obj.pos)./norm(pNext - obj.pos, 2);
nextPos = obj.pos + vDir * 0.2;
rate = 0.1 - 0.004 * t;
nextPos = obj.pos + vDir * rate;
% Move to next position
% (dynamics not modeled at this time)