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

fixed guidance only pulling things towards the middle and added CA QP CBF code

Browse Source
This commit is contained in:
Kevin Dee
2025-12-05 16:04:02 -08:00
parent 96c91c3988
commit 95ea19e546
31 changed files with 342 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
@agent/agent.m
View File

@@ -1,5 +1,5 @@
classdef agent
properties (SetAccess = private, GetAccess = public)
properties (SetAccess = public, GetAccess = public)
% Identifiers
index = NaN;
label = "";

24
@agent/run.m
View File

@@ -9,12 +9,6 @@ function obj = run(obj, domain, partitioning, t)
obj (1, 1) {mustBeA(obj, 'agent')};
end
% Update collision barrier function
% first part evaluates to +/-1 if the point is outside/inside the collision geometry
% Second part determines the distance from the point to the boundary of the collision geometry
obj.barrierFunction = @(x) (1 - 2 * obj.collisionGeometry.contains(x)) * obj.collisionGeometry.distance(x); % x is 1x3
obj.dBarrierFunction = @(x) obj.collisionGeometry.distanceGradient(x); % x is 1x3
% Collect objective function values across partition
partitionMask = partitioning == obj.index;
objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
@@ -85,6 +79,11 @@ function obj = run(obj, domain, partitioning, t)
r = randi([1, size(x, 1)]);
x = x(r); y = y(r);
% switch them
temp = x;
x = y;
y = temp;
% find objective location in discrete domain
[~, xIdx] = find(domain.objective.groundPos(1) == domain.objective.X);
xIdx = unique(xIdx);
@@ -104,12 +103,17 @@ function obj = run(obj, domain, partitioning, t)
% Use largest grad(C) value to find the direction of the next position
[xNextIdx, yNextIdx] = find(nGradC == max(nGradC, [], 'all'));
% switch them
temp = xNextIdx;
xNextIdx = yNextIdx;
yNextIdx = temp;
roundingScale = 10^-log10(domain.objective.discretizationStep);
pNext = [floor(roundingScale .* mean(unique(domain.objective.X(:, xNextIdx))))./roundingScale, floor(roundingScale .* mean(unique(domain.objective.Y(yNextIdx, :))))./roundingScale, obj.pos(3)]; % have to do some unfortunate rounding here soemtimes
% Determine next position
vDir = (pNext - obj.pos)./norm(pNext - obj.pos, 2);
rate = 0.2 - 0.004 * t;
rate = 0.1 - 0.0004 * t; % slow down as you get closer, coming to a stop by the end
nextPos = obj.pos + vDir * rate;
% Move to next position
@@ -118,5 +122,11 @@ function obj = run(obj, domain, partitioning, t)
% Reinitialize collision geometry in the new position
d = obj.pos - obj.collisionGeometry.center;
if isa(obj.collisionGeometry, 'rectangularPrism')
obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
elseif isa(obj.collisionGeometry, 'spherical')
obj.collisionGeometry = obj.collisionGeometry.initialize(obj.collisionGeometry.center + d, obj.collisionGeometry.radius, obj.collisionGeometry.tag, obj.collisionGeometry.label);
else
error("?");
end
end

61
@miSim/constrainMotion.m Normal file
View File

@@ -0,0 +1,61 @@
function [obj] = constrainMotion(obj)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'miSim')};
end
if size(obj.agents, 1) < 2
return;
% this doesn't work right now with only one agent
end
agents = [obj.agents{:}];
v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
h = NaN(size(obj.agents, 1));
h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
nCon = nchoosek(size(obj.agents, 1), 2);
kk = 1;
A = zeros(nCon, 3 * size(obj.agents, 1));
b = zeros(nCon, 1);
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(jj, ii) = h(ii, jj);
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - agents(jj).pos);
A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
b(kk) = obj.barrierGain * h(ii, jj)^3;
kk = kk + 1;
end
end
% Solve QP program generated earlier
vhat = reshape(v', 3 * size(obj.agents, 1), 1);
H = 2 * eye(3 * size(obj.agents, 1));
f = -2 * vhat;
% Update solution based on constraints
opt = optimoptions('quadprog', 'Display', 'off');
[vNew, ~, exitflag] = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opt);
vNew = reshape(vNew, 3, size(obj.agents, 1))';
if exitflag <= 0
warning("QP failed, continuing with unconstrained solution...")
vNew = v;
end
% Update the "next position" that was previously set by unconstrained
% GA using the constrained solution produced here
for ii = 1:size(vNew, 1)
obj.agents{ii}.pos = obj.agents{ii}.lastPos + vNew(ii, :) * obj.timestep;
end
% Here we run this at the simulation level, but in reality there is no
% parent level, so this would be run independently on each agent.
% Running at the simulation level is just meant to simplify the
% simulation
end

2
@miSim/miSim.m
View File

@@ -14,6 +14,7 @@ classdef miSim
sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
partitioning = NaN;
performance = 0; % cumulative sensor performance
barrierGain = 100; % collision avoidance parameter
fPerf; % performance plot figure
end
@@ -43,6 +44,7 @@ classdef miSim
methods (Access = public)
[obj] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles);
[obj] = run(obj);
[obj] = constrainMotion(obj);
[obj] = partition(obj);
[obj] = updateAdjacency(obj);
[obj] = plot(obj);

4
@miSim/run.m
View File

@@ -27,6 +27,10 @@ function [obj] = run(obj)
obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.t);
end
% Adjust motion determined by unconstrained gradient ascent using
% CBF constraints solved by QP
obj = constrainMotion(obj);
% Update total performance
obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(end), obj.agents))];

13
geometries/@rectangularPrism/initialize.m
View File

@@ -24,6 +24,10 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
% Compute center
obj.center = obj.minCorner + obj.dimensions ./ 2;
% Compute a (fake) radius
% fully contains the rectangular prism from the center
obj.radius = (1/2) * sqrt(sum(obj.dimensions.^2));
% Compute vertices
obj.vertices = [obj.minCorner;
obj.maxCorner(1), obj.minCorner(2:3);
@@ -44,4 +48,13 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
if tag == REGION_TYPE.DOMAIN
obj.objective = sensingObjective;
end
% Initialize CBF
% first part evaluates to +/-1 if the point is outside/inside the collision geometry
% Second part determines the distance from the point to the boundary of the collision geometry
obj.barrierFunction = @(x) (1 - 2 * obj.collisionGeometry.contains(x)) * obj.collisionGeometry.distance(x); % x is 1x3
% gradient of barrier function
obj.dBarrierFunction = @(x) obj.collisionGeometry.distanceGradient(x); % x is 1x3
% as long as the collisionGeometry object is updated during runtime,
% these functions never have to be updated again
end

5
geometries/@rectangularPrism/rectangularPrism.m
View File

@@ -11,6 +11,7 @@ classdef rectangularPrism
dimensions = NaN(1, 3);
center = NaN;
footprint = NaN(4, 2);
radius = NaN; % fake radius
% Graph
vertices = NaN(8, 3);
@@ -20,6 +21,10 @@ classdef rectangularPrism
% Plotting
lines;
% collision
barrierFunction;
dBarrierFunction;
end
properties (SetAccess = public, GetAccess = public)
% Sensing objective (for DOMAIN region type only)

10
geometries/@spherical/contains.m Normal file
View File

@@ -0,0 +1,10 @@
function c = contains(obj, pos)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'spherical')};
pos (:, 3) double;
end
arguments (Output)
c (:, 1) logical
end
c = norm(obj.center - pos) <= obj.radius;
end

28
geometries/@spherical/containsLine.m Normal file
View File

@@ -0,0 +1,28 @@
function c = containsLine(obj, pos1, pos2)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'spherical')};
pos1 (1, 3) double;
pos2 (1, 3) double;
end
arguments (Output)
c (1, 1) logical
end
d = pos2 - pos1;
f = pos1 - obj.center;
a = dot(d, d);
b = 2 * dot(f, d);
c = dot(f, f) - obj.radius^2;
disc = b^2 - 4*a*c;
if disc < 0
c = false;
return;
end
t = [(-b - sqrt(disc)) / (2 * a), (-b + sqrt(disc)) / (2 * a)];
c = (t(1) >= 0 && t(1) <= 1) || (t(2) >= 0 && t(2) <= 1);
end

42
geometries/@spherical/initialize.m Normal file
View File

@@ -0,0 +1,42 @@
function obj = initialize(obj, center, radius, tag, label)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'spherical')};
center (1, 3) double;
radius (1, 1) double;
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
label (1, 1) string = "";
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'spherical')};
end
obj.tag = tag;
obj.label = label;
% Define geometry
obj.center = center;
obj.radius = radius;
obj.diameter = 2 * obj.radius;
% Initialize CBF
obj.barrierFunction = @(x) NaN;
% gradient of barrier function
obj.dBarrierFunction = @(x) NaN;
% fake vertices in a cross pattern
obj.vertices = [obj.center + [obj.radius, 0, 0]; ...
obj.center - [obj.radius, 0, 0]; ...
obj.center + [0, obj.radius, 0]; ...
obj.center - [0, obj.radius, 0]; ...
obj.center + [0, 0, obj.radius]; ...
obj.center - [0, 0, obj.radius]];
% fake edges in two perpendicular rings
obj.edges = [1, 3; ...
3, 2; ...
2, 4; ...
4, 1; ...
1, 5; ...
5, 2; ...
2, 6; ...
6, 1];
end

43
geometries/@spherical/plotWireframe.m Normal file
View File

@@ -0,0 +1,43 @@
function [obj, f] = plotWireframe(obj, ind, f)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'spherical')};
ind (1, :) double = NaN;
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
end
arguments (Output)
obj (1, 1) {mustBeA(obj, 'spherical')};
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
end
% Create axes if they don't already exist
f = firstPlotSetup(f);
% Create plotting inputs
[X, Y, Z] = sphere(8);
% Scale
X = X * obj.radius;
Y = Y * obj.radius;
Z = Z * obj.radius;
% Shift
X = X + obj.center(1);
Y = Y + obj.center(2);
Z = Z + obj.center(3);
% 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);
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);
hold(f.Children(1).Children(ind(1)), "off");
end
% Copy to other requested tiles
if numel(ind) > 1
for ii = 2:size(ind, 2)
o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
end
end
obj.lines = o;
end

15
geometries/@spherical/random.m Normal file
View File

@@ -0,0 +1,15 @@
function r = random(obj)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'spherical')};
end
arguments (Output)
r (1, 3) double
end
y = (rand - 0.5) * 2; % uniform draw on [-1, 1]
R = sqrt(1 - y^2);
lon = (rand - 0.5) * 2 * pi; % uniform draw on [-pi, pi]
s = [R * sin(lon), y, R * cos(lon)]; % random point on surface
r = s * rand^(1/3); % scaled to random normalized radius [0, 1]
r = obj.center + obj.radius * r;
end

37
geometries/@spherical/spherical.m Normal file
View File

@@ -0,0 +1,37 @@
classdef spherical
% Rectangular prism geometry
properties (SetAccess = private, GetAccess = public)
% Meta
tag = REGION_TYPE.INVALID;
label = "";
% Spatial
center = NaN;
radius = NaN;
diameter = NaN;
vertices; % fake vertices
edges; % fake edges
% Plotting
lines;
% collision
barrierFunction;
dBarrierFunction;
end
properties (SetAccess = public, GetAccess = public)
% Sensing objective (for DOMAIN region type only)
objective;
end
methods (Access = public)
[obj ] = initialize(obj, center, radius, tag, label);
[r ] = random(obj);
[c ] = contains(obj, pos);
[d ] = distance(obj, pos);
[g ] = distanceGradient(obj, pos);
[c ] = containsLine(obj, pos1, pos2);
[obj, f] = plotWireframe(obj, ind, f);
end
end

6
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/OJQQt9oLZ-3u4fU3OVlfDGdcdxcd.xml Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="design"/>
</Category>
</Info>

2
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/OJQQt9oLZ-3u4fU3OVlfDGdcdxcp.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="random.m" type="File"/>

6
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcd.xml Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="design"/>
</Category>
</Info>

2
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcp.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="containsLine.m" type="File"/>

6
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBId.xml Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="design"/>
</Category>
</Info>

2
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBIp.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="spherical.m" type="File"/>

2
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/kCBcxuevUMNp2KwPOqqCVMO5GE8d.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info/>

2
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/kCBcxuevUMNp2KwPOqqCVMO5GE8p.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="1" type="DIR_SIGNIFIER"/>

6
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUd.xml Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="design"/>
</Category>
</Info>

2
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUp.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="contains.m" type="File"/>

6
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwd.xml Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="design"/>
</Category>
</Info>

2
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwp.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="initialize.m" type="File"/>

6
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakd.xml Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="design"/>
</Category>
</Info>

2
resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakp.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="plotWireframe.m" type="File"/>

2
resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8d.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info/>

2
resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8p.xml Normal file
View File

@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<Info location="@spherical" type="File"/>

8
test/test_miSim.m
View File

@@ -279,8 +279,10 @@ classdef test_miSim < matlab.unittest.TestCase
end
% Initialize candidate agent collision geometry
candidateGeometry = rectangularPrism;
candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
% candidateGeometry = rectangularPrism;
% candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
candidateGeometry = spherical;
candidateGeometry = candidateGeometry.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
% Initialize candidate agent sensor model
sensor = sigmoidSensor;
@@ -418,7 +420,7 @@ classdef test_miSim < matlab.unittest.TestCase
tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
% make basic sensing objective
tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2)), tc.domain, tc.discretizationStep, tc.protectedRange);
tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [2, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);
% Initialize agent collision geometry
geometry1 = rectangularPrism;

2
util/validators/arguments/mustBeGeometry.m
View File

@@ -1,5 +1,5 @@
function mustBeGeometry(geometry)
validGeometries = ["rectangularPrism";];
validGeometries = ["rectangularPrism"; "spherical"];
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);