checkCollision

Check for collisions between ego bodies and obstacles

Syntax

collisionFound = checkCollision(capsuleListObj)

[fullResults,distance] = checkCollision(capsuleListObj,options)

Description

collisionFound = checkCollision(capsuleListObj) checks each ego body for collisions with obstacles in the environment. The function indicates whether each ego body is in collision at each time step..

[fullResults,distance] = checkCollision(capsuleListObj,options) checks each ego body for collisions with obstacles in the environment, and returns the results using additional specified collision detection options options.

Examples

Build Ego Body Paths and Check for Collisions with 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.

Create the 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])

Add Obstacles

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])

Update Obstacles

Alter your obstacle locations and geometry dimensions over time. Use the previously generated structure, modify the fields, and update the obstacles using the updateObstacleGeometry and 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

Collision detected.

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])

Input Arguments

capsuleListObj — Dynamic capsule list

dynamicCapsuleList object | dynamicCapsuleList3D object

Dynamic capsule list, specified as a dynamicCapsuleList or dynamicCapsuleList3D object.

options — Collision detection options

structure

Collision detection options, specified as a structure with these fields:

• FullResults –– Return the collision results for each obstacle separately , specified as a logical 0 (false) or 1 (true). See the fullResults output argument.

• ReturnDistance –– Return the distance calculation from collision checking, specified as a logical 0 (false) or 1 (true). See the distance output argument.

Data Types: struct

Output Arguments

collisionFound — Collision checking results

n-by-e matrix of logical values

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 ego bodies.

Data Types: logical

fullResults — Full collision checking results for each obstacle

n-by-o-by-e array of logical values

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 specified capsuleListobj argument, o is the number of obstacles, and e is the number of ego bodies.

Dependencies

To return the fullResults output argument, specify the options input argument with the FullResults field set to true.

Data Types: logical

distance — Distance from obstacles

n-by-e numeric matrix | n-by-oby-e numeric array

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 of the FullResults field of the options argument

distance Dimensions	FullResults Value	Behavior
n-by-e numeric matrix	false	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 capsuleListObj argument, and e is the number of ego bodies.
n-by-oby-e numeric array	true	Returns the distance between each ego body and each obstacle at each time step. o is the number of obstacles.

Dependencies

To return the distance output argument, specify the options input argument with the ReturnDistance field set to true.

Data Types: single | double

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.