stateSpaceSE3

SE(3) state space

expand all in page

    Description

    The stateSpaceSE3 object stores parameters and states in the SE(3) state space, which is composed of state vectors represented by [x, y, z, qw, qx, qy, qz]. x, y, and z are Cartesian coordinates. qw, qx, qy, and qz represent the orientation in a quaternion. The object uses Euclidean distance calculation and linear interpolation for the translation component of the state. The object uses quaternion distance calculation and spherical linear interpolation for the rotation component of the state.

    Creation

    Syntax

    space = stateSpaceSE3
    space = stateSpaceSE3(bounds)

    Description

    space = stateSpaceSE3 creates an SE(3) state space object with default state bounds for x, y, and z. The state variables qw, qx, qy, and qz corresponding to orientation are not bounded.

    example

    space = stateSpaceSE3(bounds) creates an SE(3) state space object with state bounds specified as a 7-by-2 matrix. Each row specifies the minimum and maximum value for a dimension of the state in the order x, y, z, qw, qx, qy, and qz. The input bounds sets the StateBounds property.

    Properties

    expand all

    This property is read-only.

    Name of state space, specified as a character vector.

    Data Types: char

    This property is read-only.

    Number of state space dimensions, specified dimensions, returned as a positive integer.

    Data Types: double

    Bounds of state variables, specified as a 7-by-2 matrix of real values.

    • The first row specifies the lower and upper bounds of the x state in meters.

    • The second row specifies the lower and upper bounds of the y state in meters.

    • The third row specifies the lower and upper bounds of the z state in meters.

    • The fourth through the seventh rows specify the lower and upper bounds of the state variables qw, qx, qy, and qz respectively, corresponding to orientation as a quaternion.

    Note

    The StateBounds property only affect the Cartesian components of the state. The state variables corresponding to orientation are not bounded.

    Example: stateSpaceSE3([-10 10; -10 10; -10 10; Inf Inf; Inf Inf; Inf Inf; Inf Inf])

    Example: space.StateBounds = [-10 10; -10 10; -10 10; Inf Inf; Inf Inf; Inf Inf; Inf Inf]

    Data Types: double

    Weight applied to the x, y, and z distance calculation, specified as a positive real scalar. By default, the weight for translation is chosen to be greater than the weight for rotation.

    The object calculates distance as:

    d=wxyz(dx2+dy2+dz2)+wqdq2

    ,

    where wxyz is the weight applied to x, y, and z coordinates, and wq is the weight applied to the orientation in quaternion. dx, dy, and dz are the distances in the x, y, and z directions, respectively. dq is the quaternion distance.

    Example: space.WeightXYZ = 2

    Data Types: double

    Weight applied to quaternion distance calculation, specified as a positive real scalar. By default, the weight for rotation is chosen to be less than the weight for translation.

    The object calculates distance as:

    d=wxyz(dx2+dy2+dz2)+wqdq2

    ,

    where wxyz is weight applied to x, y, and z coordinates, and wq is the weight applied to the orientation in quaternion. dx, dy, and dz are the distances in the x, y, and z direction, respectively. dq is the quaternion distance.

    Example: space.WeightQuaternion = 0.5

    Data Types: double

    Object Functions

    copyCreate deep copy of state space object
    distanceDistance between two states
    enforceStateBoundsReduce state to state bounds
    interpolateInterpolate between states
    sampleUniformSample state using uniform distribution

    Examples

    collapse all

    Open Live Script

    This example shows how to validate paths through a 3-D occupancy map evironment.

    Load and Assign Map to State Validator

    Load the 3-D occupancy map of a city block. Specify the threshold to consider cells as obstacle-free.

    mapData = load('dMapCityBlock.mat');
omap = mapData.omap;
omap.FreeThreshold = 0.5;

    Inflate the map to add a buffer zone for safe operation around the obstacles.

    inflate(omap,5);

    Create an SE(3) state space object with bounds for state variables.

    ss = stateSpaceSE3([-20 220;-20 220;-10 100; inf inf; inf inf; inf inf; inf inf]);

    Create an occupancyMap3D based state validator using the created state space.

    sv = validatorOccupancyMap3D(ss);

    Assign the map to the state validator object. Specify the ValidationDistance property.

    sv.Map = omap;
sv.ValidationDistance = 0.1;

    Plan and Visualize Path

    Create the path planner and increase the maximum connection distance. Reduce the maximum number of iterations.

    planner = plannerRRT(ss,sv);
planner.MaxConnectionDistance = 50;
planner.MaxIterations = 1000;

    Create a user-defined goal reached evaluation function. Specify the GoalBias property.

    planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3))<5);
planner.GoalBias = 0.1;

    Set the start and goal states.

    start = [40 180 25 0.7 0.2 0 0.1];
goal = [150 33 35 0.3 0 0.1 0.6];

    Plan a path with default settings.

    [pthObj,solnInfo] = plan(planner,start,goal);

    Check the points of the path are valid states.

    isValid = isStateValid(sv,pthObj.States)
    isValid = 6×1 logical array

   1
   1
   1
   1
   1
   1

    Check the motion between each sequential path states.

    isPathValid = zeros(size(pthObj.States,1)-1,1,'logical');
for i = 1:size(pthObj.States,1)-1
    [isPathValid(i), ~] = isMotionValid(sv,pthObj.States(i,:),pthObj.States(i+1,:));
end
isPathValid
    isPathValid = 5×1 logical array

   1
   1
   1
   1
   1

    Visualize the results.

    show(omap)
hold on
scatter3(start(1,1),start(1,2),start(1,3),'g','filled') % draw start state
scatter3(goal(1,1),goal(1,2),goal(1,3),'r','filled') % draw goal state
plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3),'r-','LineWidth',2) % draw path

    Extended Capabilities

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

    See Also

    |

    Introduced in R2020b

    Navigation Toolbox Documentation

    Support