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

obstacle avoidance

Browse Source
This commit is contained in:
Kevin Dee
2025-12-23 14:57:13 -08:00
parent 4fa942564a
commit 8315b6c511
6 changed files with 53 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
@miSim/constrainMotion.m
View File

@@ -14,12 +14,16 @@ function [obj] = constrainMotion(obj)
agents = [obj.agents{:}];
v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
% Initialize QP based on number of agents and obstacles
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);
nAAPairs = nchoosek(size(obj.agents, 1), 2);
nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1);
kk = 1;
A = zeros(nCon, 3 * size(obj.agents, 1));
b = zeros(nCon, 1);
A = zeros(nAAPairs + nAOPairs, 3 * size(obj.agents, 1));
b = zeros(nAAPairs + nAOPairs, 1);
% Set up collision avoidance constraints
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;
@@ -32,6 +36,23 @@ function [obj] = constrainMotion(obj)
end
end
hObs = NaN(size(obj.agents, 1), size(obj.obstacles, 1));
% Set up obstacle avoidance constraints
for ii = 1:size(obj.agents, 1)
for jj = 1:size(obj.obstacles, 1)
% find closest position to agent on/in obstacle
cPos = obj.obstacles{jj}.closestToPoint(agents(ii).pos);
hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2;
A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - cPos);
b(kk) = obj.barrierGain * hObs(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));

4
@miSim/initialize.m
View File

@@ -32,8 +32,8 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
% Add an additional obstacle spanning the domain's footprint to
% represent the minimum allowable altitude
obj.minAlt = minAlt;
obj.obstacles{end + 1} = rectangularPrism;
obj.obstacles{end} = obj.obstacles{end}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
obj.obstacles{end + 1, 1} = rectangularPrism;
obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
% Define objective
obj.objective = objective;

19
geometries/@rectangularPrism/closestToPoint.m Normal file
View File

@@ -0,0 +1,19 @@
function cPos = closestToPoint(obj, pos)
arguments (Input)
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
pos (:, 3) double;
end
arguments (Output)
cPos (:, 3) double;
end
cPos = NaN(1, 3);
for ii = 1:3
if pos(ii) < obj.minCorner(ii)
cPos(ii) = obj.minCorner(ii);
elseif pos(ii) > obj.maxCorner(ii)
cPos(ii) = obj.maxCorner(ii);
else
cPos(ii) = pos(ii);
end
end
end

2
geometries/@rectangularPrism/distance.m
View File

@@ -4,7 +4,7 @@ function d = distance(obj, pos)
pos (:, 3) double;
end
arguments (Output)
d (:, 1) double
d (:, 1) double;
end
if obj.contains(pos)
% Queried point is inside geometry

1
geometries/@rectangularPrism/rectangularPrism.m
View File

@@ -36,6 +36,7 @@ classdef rectangularPrism
[obj ] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain);
[r ] = random(obj);
[c ] = contains(obj, pos);
[cPos ] = closestToPoint(obj, pos);
[d ] = distance(obj, pos);
[g ] = distanceGradient(obj, pos);
[c ] = containsLine(obj, pos1, pos2);

9
test/test_miSim.m
View File

@@ -503,7 +503,9 @@ classdef test_miSim < matlab.unittest.TestCase
radius = 1.5;
d = [3, 0, 0];
geometry1 = spherical;
geometry1 = geometry1.initialize(tc.domain.center - d, radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
geometry2 = geometry1;
geometry1 = geometry1.initialize(tc.domain.center - d + [0, radius * 1.1, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
geometry2 = geometry2.initialize(tc.domain.center - d - [0, radius * 1.1, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
% Initialize agent sensor model
sensor = sigmoidSensor;
@@ -511,8 +513,9 @@ classdef test_miSim < matlab.unittest.TestCase
sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
% Initialize agents
tc.agents = {agent};
tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d, zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3, 1, sprintf("Agent %d", 1), false);
tc.agents = {agent; agent;};
tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.1, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3, 1, sprintf("Agent %d", 1), false);
tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius * 1.1, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3, 2, sprintf("Agent %d", 2), false);
% Initialize obstacles
obstacleLength = 1;