Add obstacles to 3-D capsule list
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.
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
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);
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. 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.
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
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);
obstacleStruct— Obstacle parameters
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:
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
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,
FixedTransform field as a fixed transform relative
to the local frame of the capsule. To keep the default capsule origin, specify the
status— Result of adding obstacles
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 (
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.