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;