dynamicCapsuleList: only last step of the capsule list is checked for collisions

Hi all,

I am confronted with a problem of the dynamicCapsuleList. I am using the capsule List to detect collisions of future vehicle motions, as done in https://de.mathworks.com/help/nav/ug/highway-trajectory-planning-using-frenet.html .

For some reason during the simulation only the last steps of the capsule List is enabled to be checked for collisions. This is confusing to me, since I simulated the capsule List along the computed frenét trajectories and all of the relevant capsules are displayed in the pop-up figure.

I have written the code as follows:

I start by defining the geometry of the Ego capsules

capList = dynamicCapsuleList;
carLen   = 4.7; % in meters
carWidth = 1.8; % in meters
rearAxleRatio = .25;
egoID = egoVehicle.ActorID;
[egoID, egoGeom] = egoGeometry(capList,egoID);
egoGeom.Geometry.Length = carLen; % in meters
egoGeom.Geometry.Radius = carWidth/2; % in meters
egoGeom.Geometry.FixedTransform(1,end) = -carLen*rearAxleRatio; % in meters
updateEgoGeometry(capList,egoID,egoGeom);

The same is done for the Obstacles

numActors = size(scenario.Actors) -1; % 
VT = numActors (1, 2); % 
actorID = (2:VT+1)'; % 
actorGeom = repelem(egoGeom,VT,1);
updateObstacleGeometry(capList,actorID,actorGeom)

This is followed by a while-loop to simulate the capsules iteratively.

The Capsule for the Obstacles are computed inside the while loop along corresponding frenet trajectories

for i = 1:numel(actorID)  
            ActorPose = scenario.Actors(1, actorID(i)).Position;
            ActorYaw = deg2rad(scenario.Actors(1, actorID(i)).Yaw); %
            ActorSpeed = sqrt(scenario.Actors(1, actorID(i)).Velocity(1)^2 + scenario.Actors(1, actorID(i)).Velocity(2)^2); % 
            ActorAcc = 0;
            ActorKappa = scenario.Actors(1, actorID(i)).AngularVelocity(3)/ActorSpeed/180*pi;

            % global2frenet
            globalActorState = [ActorPose(1:2), ActorYaw, ActorKappa, ActorSpeed, ActorAcc];
            frenetActorState = global2frenet(refPathActor1, globalActorState);
            [~,ActorTrajGlobal] = connect(connector,frenetActorState,TermStates,2);
            
            %     for u = 1:numel(TermStates(:,1))
            capList.MaxNumSteps = numel(ActorTrajGlobal(2).Trajectory(:,1)); 
          
            % CapList States of surrounding vehicles 
            actorPoses(actorID(i)).States = [ActorTrajGlobal(2).Trajectory(:,1:3)];
            
            updateObstaclePose(capList,actorID(:),actorPoses(actorID(:)));
end

and for the Ego-Vehicle with

   for i = 1:numel(idx)
        optimalTrajectory = trajGlobal(idx(i)).Trajectory;
        egoPoses.States = optimalTrajectory(:,1:3);
        updateEgoPose(capList,egoID,egoPoses);
        ...

then the collision would be checked with

        ...
        isColliding = checkCollision(capList);
   end

at every loop the capsules are visualized with

   show(capList, 'TimeStep',[1:capList.MaxNumSteps],'FastUpdate',1, 'ShowCollision', true);

The collision checker does not recognize any capsules before the last state. In other words only the last state of every trajectory is checked for collisions. Other steps before that are ignored for some reason.

For more details:

The code is inside the zip file. The Simulation starts by running the "Main" script.

Would be thankful for any help, that I can get. Thanks!