Check for collisions between ego bodies and obstacles
Build an ego body path and maintain obstacle states using the
dynamicCapsuleList object. Visualize the states of all objects in the environment at different timestamps. Validate the path of the ego body by checking for collisions with obstacles in the environment.
dynamicCapsuleList object. Extract the maximum number of steps to use as the number of time stamps for your object paths.
obsList = dynamicCapsuleList; numSteps = obsList.MaxNumSteps;
Add Ego Body
Define an ego body by specifying the ID, geometry, and state together in a structure. The capsule geometry has a length of 3 m and radius of 1 m. Specify the state as a linear path from x = 0m to x = 100m.
egoID1 = 1; geom = struct("Length",3,"Radius",1,"FixedTransform",eye(3)); states = linspace(0,1,obsList.MaxNumSteps)'.*[100 0 0]; egoCapsule1 = struct('ID',egoID1,'States',states,'Geometry',geom); addEgo(obsList,egoCapsule1); show(obsList,"TimeStep",[1:numSteps]); ylim([-20 20])
Specify states for two obstacles that are separated from the ego body by 5 m in opposite directions on the y-axis.. Assume the obstacles have the same geometry
geom as the ego body.
obsState1 = states + [0 5 0]; obsState2 = states + [0 -5 0]; obsCapsule1 = struct('ID',1,'States',obsState1,'Geometry',geom); obsCapsule2 = struct('ID',2,'States',obsState2,'Geometry',geom); addObstacle(obsList,obsCapsule1); addObstacle(obsList,obsCapsule2); show(obsList,"TimeStep",[1:numSteps]); ylim([-20 20])
Alter your obstacle locations and geometry dimensions over time. Use the previously generated structure, modify the fields, and update the obstacles using the
updateObstaclePose object functions. Reduces the radius of the first obstacle to 0.5 m, and change the path to move it towards the ego body.
obsCapsule1.Geometry.Radius = 0.5; obsCapsule1.States = ... [linspace(0,100,numSteps)' ... % x linspace(5,-4,numSteps)' ... % y zeros(numSteps,1)]; % theta updateObstacleGeometry(obsList,1,obsCapsule1); updateObstaclePose(obsList,1,obsCapsule1);
Check for Collisions
Visualize the new paths. Show where collisions between the ego body and an obstacle, which the display highlights in red. Notice that collisions between the obstacles are not checked.
show(obsList,"TimeStep",[1:numSteps],"ShowCollisions",1); ylim([-20 20]) xlabel("X (m)") ylabel("Y (m)")
Programmatically check for collisions by using the
checkCollision object function. The function returns a vector of logical values that indicates the status of each time step. The vector is transposed for display purposes.
collisions = checkCollision(obsList)'
collisions = 1x31 logical array 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0
To validate paths with a large number of steps, use the
any function on the vector of collision values.
if any(collisions) disp("Collision detected.") end
Update Ego Path
Specify a new path for the ego body. Visualize the paths again, displaying collisions.
egoCapsule1.States = ... [linspace(0,100,numSteps)' ... % x 3*sin(linspace(0,2*pi,numSteps))' ... % y zeros(numSteps,1)]; % theta updateEgoPose(obsList,1,egoCapsule1); show(obsList,"TimeStep",[1:numSteps],"ShowCollisions",1); ylim([-20 20])
options— Collision detection options
Collision detection options, specified as a structure with these fields:
FullResults –– Return the collision results for each
obstacle separately , specified as a logical
fullResults output argument.
ReturnDistance –– Return the distance calculation from
collision checking, specified as a logical
distance output argument.
collisionFound— Collision checking results
Collision checking results, returned as an
n-by-e matrix of logical values. By default, the
function checks for any collision between any object, which returns an
n-by-e matrix, where n is the
maximum number of states for ego bodies in the specified
capsuleListobj object, and e is the number of
fullResults— Full collision checking results for each obstacle
Full collision checking results for each obstacle, returned as an
n-by-o-by-e array of logical
values. n is the maximum number of states for ego bodies in the
capsuleListobj argument, o is the
number of obstacles, and e is the number of ego bodies.
To return the
fullResults output argument, specify the
options input argument with the
field set to
distance— Distance from obstacles
Distance from obstacles, returned as an n-by-e
numeric matrix or n-by-oby-e
numeric array. The dimensions and behavior of the distance argument depend on the value
FullResults field of the options argument
|n-by-e numeric matrix||Returns the distance between each ego body and the closest obstacle at each
time step. n is the maximum number of states for ego bodies
specified in the |
|n-by-oby-e numeric array||Returns the distance between each ego body and each obstacle at each time step. o is the number of obstacles.|
To return the
distance output argument, specify the
options input argument with the
ReturnDistance field set to