addObstacle

Add obstacles to 3-D capsule list

collapse all in page

Syntax

addObstacle(capsuleListObj,obstacleStruct)
status = addObstacle(capsuleListObj,obstacleStruct)

Description

addObstacle(capsuleListObj,obstacleStruct) adds one or more obstacles to the 3-D dynamic capsule list with the specified ID, state, and geometry values given in obstacleStruct.

example

status = addObstacle(capsuleListObj,obstacleStruct) additionally returns an indicator of whether each specified obstacle was added, updated, or a duplicate.

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 = 1×31 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.

Input Arguments

collapse all

Dynamic capsule-list, specified as a dynamicCapsuleList3D object.

Output Arguments

collapse all

Obstacle parameters, specified as an N-element structure or a structure array, where N is the number of added ego bodies. The fields of each structure define the ID, geometry, and states of an obstacle:

  • ID –– Integer that identifies each object. Stored in the ObstacleIDs property of the dynamicCapsuleList3D object specified by the capsuleListObj argument.

  • 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 –– Structure with fields Length, Radius, and FixedTransform. These fields define the size of the capsule-based object using the specified length for the cylinder and hemisphere radius for the end caps. To shift the capsule geometry from the default origin, specify the FixedTransform field as a fixed transform relative to the local frame of the capsule. To keep the default capsule origin, specify the transform as eye(4).

Capsule geometry image showing the position and orientation of the capsule dimensions. Positive X is the right direction in the world frame. Positive Y is up. Positive Z is into the page. The origin of the capsule local frame is at the center of the left hemisphere end-cap. The end-caps have a given radius. The length defines the cylindrical length between ends.

Result of adding obstacles, returned as a N-element column vector of ones, zeros, and negative ones. N is the number of obstacles specified in the obstacleStruct argument. Each value indicates whether the associated body is added (1), updated (0), or a duplicate (-1). While adding obstacles, if multiple structures with the same body ID are found in the structure array obstaclesStruct, then the function marks the previous entry as duplicate and ignores it.

Extended Capabilities

expand all

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

Version History

Introduced in R2020b

See Also

Objects

Functions

Topics