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

fixed data logging codegen logic

Browse Source
This commit is contained in:
Kevin Dee
2026-04-05 13:41:43 -07:00
parent 31e88a8535
commit bd452b7180
1 changed files with 16 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
@miSim/constrainMotion.m
+16 -7
View File
@@ -60,8 +60,10 @@ function [obj] = constrainMotion(obj)
end end
end end
idx = length(h(triu(true(size(h)), 1))); idx = length(h(triu(true(size(h)), 1)));
obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1)); if coder.target('MATLAB')
obj.barriers(1:idx, obj.timestepIndex) = h(triu(true(size(h)), 1));
end
idx = idx + 1; idx = idx + 1;
hObs = NaN(nAgents, size(obj.obstacles, 1)); hObs = NaN(nAgents, size(obj.obstacles, 1));
@@ -84,7 +86,9 @@ function [obj] = constrainMotion(obj)
end end
end end
obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1); if coder.target('MATLAB')
obj.barriers(idx:(idx + numel(hObs) - 1), obj.timestepIndex) = reshape(hObs, [], 1);
end
idx = idx + numel(hObs); idx = idx + numel(hObs);
% Set up domain constraints (walls and ceiling only) % Set up domain constraints (walls and ceiling only)
@@ -128,8 +132,10 @@ function [obj] = constrainMotion(obj)
b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent; b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
kk = kk + 1; kk = kk + 1;
obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax]; if coder.target('MATLAB')
idx = idx + 6; obj.barriers(idx:(idx + 5), obj.timestepIndex) = [h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax];
end
idx = idx + 6;
end end
% Add communication network constraints % Add communication network constraints
@@ -159,7 +165,10 @@ function [obj] = constrainMotion(obj)
end end
end end
end end
obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
if coder.target('MATLAB')
obj.barriers(idx:(idx + length(hComms(triu(true(size(hComms)), 1))) - 1), obj.timestepIndex) = hComms(triu(true(size(hComms)), 1));
end
% Double-integrator: transform QP from velocity to acceleration space. % Double-integrator: transform QP from velocity to acceleration space.
% Single-integrator constraint: A * v <= b % Single-integrator constraint: A * v <= b
@@ -219,4 +228,4 @@ function [obj] = constrainMotion(obj)
% Running at the simulation level is just meant to simplify the % Running at the simulation level is just meant to simplify the
% simulation % simulation
end end