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

@agent/agent.m

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

@agent/run.m

@@ -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

@miSim/constrainMotion.m

@@ -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

@miSim/miSim.m

@@ -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);

@miSim/run.m

@@ -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))];
 

geometries/@rectangularPrism/initialize.m

@@ -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

geometries/@rectangularPrism/rectangularPrism.m

@@ -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)

geometries/@spherical/contains.m

@@ -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

geometries/@spherical/containsLine.m

@@ -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

geometries/@spherical/initialize.m

@@ -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

geometries/@spherical/plotWireframe.m

@@ -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

geometries/@spherical/random.m

@@ -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

geometries/@spherical/spherical.m

@@ -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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/OJQQt9oLZ-3u4fU3OVlfDGdcdxcd.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/OJQQt9oLZ-3u4fU3OVlfDGdcdxcp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcd.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBId.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBIp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/kCBcxuevUMNp2KwPOqqCVMO5GE8d.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/kCBcxuevUMNp2KwPOqqCVMO5GE8p.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUd.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwd.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakd.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakp.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8d.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8p.xml

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

test/test_miSim.m

@@ -279,8 +279,10 @@ classdef test_miSim < matlab.unittest.TestCase
                     end
 
                     % Initialize candidate agent collision geometry
-                    candidateGeometry = rectangularPrism;
+                    % 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 = 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;

util/validators/arguments/mustBeGeometry.m

@@ -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);