Main Content

stateSpaceSE3

SE(3) state space

    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

    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

    Create a 3-D occupancy map and associated state validator. Plan, validate, and visualize a path through the occupancy map.

    Load and Assign Map to State Validator

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

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

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

    inflate(omap,1)

    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 a 3-D occupancy map state validator using the created state space.

    sv = validatorOccupancyMap3D(ss);

    Assign the occupancy map to the state validator object. Specify the sampling distance interval.

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

    Plan and Visualize Path

    Create a path planner with increased maximum connection distance. Reduce the maximum number of iterations.

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

    Create a user-defined evaluation function for determining whether the path reaches the goal. Specify the probability of choosing the goal state during sampling.

    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 using the specified start, goal, and planner.

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

    Check that the points of the path are valid states.

    isValid = isStateValid(sv,pthObj.States)
    isValid = 7x1 logical array
    
       1
       1
       1
       1
       1
       1
       1
    
    

    Check that the motion between each sequential path state is valid.

    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 = 6x1 logical array
    
       1
       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

    Figure contains an axes object. The axes object with title Occupancy Map contains 4 objects of type patch, scatter, line.

    Extended Capabilities

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

    Introduced in R2020b