Main Content

dynamicCapsuleList3D

Dynamic capsule-based obstacle list

Since R2020b

expand all in page

Description

The dynamicCapsuleList3D object manages two lists of capsule-based collision objects in 3-D space. Collision objects are separated into two lists, ego bodies and obstacles. For ego bodies and obstacles in 2-D, see the dynamicCapsuleList object.

Each collision object in the two lists has three key elements:

  • ID –– Integer that identifies each object, stored in the EgoIDs property for ego bodies and the ObstacleIDs property for obstacles.

  • States –– Location and orientation of the object as an M-by-6 matrix, where each row is of form [x y z qW qX qY qZ], and M is the number of states along the path of the object in the world frame. The list of states assumes each state is separated by a fixed time interval. xyz-positions are in meters, and the orientation is a four-element quaternion vector. The default local origin is located at the center of the left hemisphere of the capsule.

  • Geometry –– Size of the capsule-based object based on the specified length and radius. The radius applies to the spherical ends, and the length applies to the cylinder length. To shift the capsule geometry and local origin relative to the default origin point, specify a 4-by-4 transform relative to the local frame of the capsule. To keep the default transform, specify eye(4).

Capsule geometry image showing the position and orientation of the capsule dimensions. Positive X is the right direction from the world frame. Positive Y is up. Positive Z is into the page, based on right-hand rule. Quaternion is a four-element quaternion relative to the world frame. The capsule geometry has a radius for the spherical ends and a length for the cylindrical section in the middle.

Use the object functions to dynamically add, remove, and update the geometries and states of the various objects in your environment. To add an ego body, see the addEgo object function. To add an obstacle, see the addObstacle object function.

After specifying all the object paths, validate the ego-body paths and check for collisions with obstacles at every step using the checkCollision object function. The function only checks if an ego body collides with an obstacle, ignoring collisions between only obstacles or only ego bodies.

Creation

Syntax

obstacleList = dynamicCapsuleList3D

Description

obstacleList = dynamicCapsuleList3D creates a dynamic capsule-based obstacle list with no ego bodies or obstacles. To begin building an obstacle list, use the addEgo or addObstacle object functions.

example

Properties

expand all

Maximum number of time steps in the obstacle list, specified as a positive integer. The number of steps determines to the maximum length of the States field for a specific ego body or obstacle.

Data Types: double

This property is read-only.

List of identifiers for ego bodies, returned as a vector of positive integers.

Data Types: double

This property is read-only.

List of identifiers for obstacles, returned as a vector of positive integers.

Data Types: double

This property is read-only.

Number of obstacles in list, returned as an integer.

Data Types: double

This property is read-only.

Number of ego bodies in list, returned as an integer.

Data Types: double

Object Functions

addEgoAdd ego bodies to 3D capsule list
addObstacleAdd obstacles to 3-D capsule list
checkCollisionCheck for collisions between ego bodies and obstacles
egoGeometryGeometric properties of ego bodies
egoPosePoses of ego bodies
obstacleGeometryGeometric properties of obstacles
obstaclePosePoses of obstacles
removeEgoRemove ego bodies from capsule list
removeObstacleRemove obstacles from capsule list
showDisplay ego bodies and obstacles in environment
updateEgoGeometryUpdate geometric properties of ego bodies
updateEgoPoseUpdate states of ego bodies
updateObstacleGeometryUpdate geometric properties of obstacles
updateObstaclePoseUpdate states of obstacles

Examples

collapse all

Open Live Script

Build an ego body path and maintain obstacle states using the dynamicCapsuleList3D 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 dynamicCapsuleList3D object. Extract the maximum number of steps to use as the number of time stamps for your object paths.

obsList = dynamicCapsuleList3D;
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 = 0 m to x = 100 m. 

egoID1 = 1;
geom = struct("Length",3,"Radius",1,"FixedTransform",eye(4));
states = linspace(0,1,obsList.MaxNumSteps)'.*[100 0 0];
states = [states ones(numSteps,2) zeros(numSteps,2)];

egoCapsule1 = struct('ID',egoID1,'States',states,'Geometry',geom);
addEgo(obsList,egoCapsule1);

show(obsList,"TimeStep",1:numSteps);
ylim([-20 20])
zlim([-5 20])
view(-45,25)
hold on

Figure contains an axes object. The axes object contains 31 objects of type patch.

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 0 0 0 0];
obsState2 = states + [0 -5 0 0 0 0 0];

obsCapsule1 = struct('ID',1,'States',obsState1,'Geometry',geom);
obsCapsule2 = struct('ID',2,'States',obsState2,'Geometry',geom);

addObstacle(obsList,obsCapsule1);
addObstacle(obsList,obsCapsule2);

cla
show(obsList,"TimeStep",1:numSteps);

Figure contains an axes object. The axes object contains 93 objects of type patch.

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. Reduce 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) ... % z
     ones(numSteps,2) zeros(numSteps,2)]; % quaternion                               % quaternion

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 occur, which the display highlights in red. Notice that collisions between the obstacles are not checked.

cla
show(obsList,"TimeStep",1:numSteps,"ShowCollisions",1);

Figure contains an axes object. The axes object contains 93 objects of type patch.

Programmatically check for collisions bu using the checkCollision object function. The function returns a vector of logical values that indicates the collision status at 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)... % z
    ones(numSteps,2) zeros(numSteps,2)]; %quaternion                                  % quaternion

updateEgoPose(obsList,1,egoCapsule1);

cla
show(obsList,"TimeStep",1:numSteps,"ShowCollisions",1);

Figure contains an axes object. The axes object contains 93 objects of type patch.

Extended Capabilities

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

Version History

Introduced in R2020b

See Also

Objects

Functions

Topics