diff --git a/@agent/agent.m b/@agent/agent.m index 9337f55..dda8294 100644 --- a/@agent/agent.m +++ b/@agent/agent.m @@ -1,5 +1,5 @@ classdef agent - properties (SetAccess = private, GetAccess = public) + properties (SetAccess = public, GetAccess = public) % Identifiers index = NaN; label = ""; diff --git a/@agent/run.m b/@agent/run.m index 1668fb0..ae68fcd 100644 --- a/@agent/run.m +++ b/@agent/run.m @@ -8,12 +8,6 @@ function obj = run(obj, domain, partitioning, t) arguments (Output) 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; @@ -84,6 +78,11 @@ function obj = run(obj, domain, partitioning, t) % just pick one 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); @@ -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; - obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label); + 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 \ No newline at end of file diff --git a/@miSim/constrainMotion.m b/@miSim/constrainMotion.m new file mode 100644 index 0000000..05333c0 --- /dev/null +++ b/@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 \ No newline at end of file diff --git a/@miSim/miSim.m b/@miSim/miSim.m index c69bac1..4742c84 100644 --- a/@miSim/miSim.m +++ b/@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); diff --git a/@miSim/run.m b/@miSim/run.m index f00e4e5..391057d 100644 --- a/@miSim/run.m +++ b/@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))]; diff --git a/geometries/@rectangularPrism/initialize.m b/geometries/@rectangularPrism/initialize.m index 66df445..5d3f21a 100644 --- a/geometries/@rectangularPrism/initialize.m +++ b/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 \ No newline at end of file diff --git a/geometries/@rectangularPrism/rectangularPrism.m b/geometries/@rectangularPrism/rectangularPrism.m index 901aa06..1a28a7b 100644 --- a/geometries/@rectangularPrism/rectangularPrism.m +++ b/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) diff --git a/geometries/@spherical/contains.m b/geometries/@spherical/contains.m new file mode 100644 index 0000000..af1442a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/geometries/@spherical/containsLine.m b/geometries/@spherical/containsLine.m new file mode 100644 index 0000000..68edd2f --- /dev/null +++ b/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 \ No newline at end of file diff --git a/geometries/@spherical/initialize.m b/geometries/@spherical/initialize.m new file mode 100644 index 0000000..a35418d --- /dev/null +++ b/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 \ No newline at end of file diff --git a/geometries/@spherical/plotWireframe.m b/geometries/@spherical/plotWireframe.m new file mode 100644 index 0000000..9917ba7 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/geometries/@spherical/random.m b/geometries/@spherical/random.m new file mode 100644 index 0000000..d1f7599 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/geometries/@spherical/spherical.m b/geometries/@spherical/spherical.m new file mode 100644 index 0000000..6fb66b3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/OJQQt9oLZ-3u4fU3OVlfDGdcdxcd.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/OJQQt9oLZ-3u4fU3OVlfDGdcdxcd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/OJQQt9oLZ-3u4fU3OVlfDGdcdxcd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/OJQQt9oLZ-3u4fU3OVlfDGdcdxcp.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/OJQQt9oLZ-3u4fU3OVlfDGdcdxcp.xml new file mode 100644 index 0000000..b5d4f51 --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/OJQQt9oLZ-3u4fU3OVlfDGdcdxcp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcd.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcp.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcp.xml new file mode 100644 index 0000000..de71410 --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBId.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBId.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBId.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBIp.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBIp.xml new file mode 100644 index 0000000..e743aec --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBIp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/kCBcxuevUMNp2KwPOqqCVMO5GE8d.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/kCBcxuevUMNp2KwPOqqCVMO5GE8d.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/kCBcxuevUMNp2KwPOqqCVMO5GE8d.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/kCBcxuevUMNp2KwPOqqCVMO5GE8p.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/kCBcxuevUMNp2KwPOqqCVMO5GE8p.xml new file mode 100644 index 0000000..01cb34e --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/kCBcxuevUMNp2KwPOqqCVMO5GE8p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUd.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUp.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUp.xml new file mode 100644 index 0000000..7ae9478 --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwd.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwp.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwp.xml new file mode 100644 index 0000000..844d632 --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakd.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakp.xml b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakp.xml new file mode 100644 index 0000000..fd703d9 --- /dev/null +++ b/resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8d.xml b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8d.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8d.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8p.xml b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8p.xml new file mode 100644 index 0000000..f2da550 --- /dev/null +++ b/resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8p.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/test/test_miSim.m b/test/test_miSim.m index 5fe400c..d94d9ee 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -279,9 +279,11 @@ 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; sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin)); @@ -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; diff --git a/util/validators/arguments/mustBeGeometry.m b/util/validators/arguments/mustBeGeometry.m index 870e109..93ed9c2 100644 --- a/util/validators/arguments/mustBeGeometry.m +++ b/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);