Motion Planning on a Spherical Shell

I am trying to create a spherical surface to use as the occupancy grid for the RRT motion planner and plan between two points on the surface.

I have successfully used the occupancyMap3D() function to create a set of data points along a the surface of a unit circle. Additionaly, I have created two sets of data points at a radius of just less that one and just more than one to act as boundaries in the occupancy grid. They should ensure the planner travels along the surface of the sphere and doesn't cut straight across the inside of the sphere.

My issue occurs when I try to run the planner in SE3 space. I receive this error after checking that my start and goal states are included within my stateSpaceSE3.StateBounds().

***Error using plannerRRT/validateStartGoal (line 497)

Start state is either not valid based on the planner's state validator, or is not finite.***

I may be stretching the capabilities of the motion planner and Navigation Toolbox. Let me know. Any help or advice would be much appreciated.

% Create sphere surface
[X,Y,Z] = sphere(100); 

% Create points on sphere surface
x = X(:);  
y = Y(:);
z = Z(:);

% Create bounding points inside sphere surface
x_i = 0.999*X(:); 
y_i = 0.999*Y(:);
z_i = 0.999*Z(:);

% Create bounding points outside sphere surface
x_o = 1.001*X(:);  
y_o = 1.001*Y(:);
z_o = 1.001*Z(:);

% This section visualizes the three regions of points. Feel free to ignore.
S = ones(numel(X),1);
s = S(:);
s_i = S(:);

%figure
%scatter3(x,y,z,s,'.k')
%axis square off
%hold on
%scatter3(x_i,y_i,z_i,'.w')
%scatter3(x_o,y_o,z_o,'w')

% Group sets of point to easily set occupancy level
X_bound = [x_i,x_o]; 
Y_bound = [y_i,y_o];
Z_bound = [z_i,z_o];

pts_surf = [x(:) y(:) z(:)];
pts_bound = [X_bound(:) Y_bound(:) Z_bound(:)];

% Create an empty occupancy map
map = occupancyMap3D(100);

% Set occupancy value of different regions
setOccupancy(map,pts_surf,0);
setOccupancy(map,pts_bound,1);

% See the map now
figure
show(map)
axis square off

% Set start and goal states
start = [0 0 -1 1 0 0 0]; % [x y z qw qx qy qz]
goal  = [0 0 1 1 0 0 0];

% Specify state space and state bounds
ss = stateSpaceSE3; % [x y z qw qx qy qz]
%ss.StateBounds = [x x; y y; z z; Inf Inf; Inf Inf; Inf Inf; Inf Inf];
ss.StateBounds = [-1 1; -1 1; -1 1; Inf Inf; Inf Inf; Inf Inf; Inf Inf];

% Create collision detector and sets discretization distance
stateValidator = validatorOccupancyMap3D(ss); 
stateValidator.Map = map;
stateValidator.ValidationDistance = 0.05;

% Create path planner and set connection distance to connect more states
planner = plannerRRT(ss,stateValidator);
planner.MaxConnectionDistance = 1.0;
planner.MaxIterations = 30000;

% Plan path
rng default % Reset number generator for reproducable result

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

Error using plannerRRT/validateStartGoal (line 497)
Start state is either not valid based on the planner's state validator, or is not finite.

Error in plannerRRT/plan (line 236)
[startState, goalState] = validateStartGoal( ...

% Plot search tree. Then interpolate and overlay final path.
show(map)
hold on

% Plot entire search tree
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-');

% Interpolate and plot path.
interpolate(pthObj,300)
plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2)

% Show start and goal in grid map
plot(start(1),start(2),'ro')
plot(goal(1),goal(2),'mo')
hold off

[Cameron Stabile]

Hi Nash,

First off, thanks for including your code, it helps immensely when trying to debug the issue! There are a few layers to this problem, so let's start with the error message and then move on to workarounds/concepts.

1) Error message

The error you are seeing comes from the start point laying on the boundary of an occupied cell. We can see this by plotting the start point atop the visualized occupancy map (see the tiny red X at the bottom of the sphere):

First thing to note here is that you've marked both the inner and outter cells as being occupied, which guarantees that your initial start point, with radius=1, will lie inside the max discretized radius, 1+1/mapResolution. The second thing to note is that even if the outer points are removed from pts_bound, that initial state will still be invalid because it lies on the boundary of an occupied cell (red X at corner of the center cell):

2) StateSpace/StateValidator Workaround

The second issue is that the bounds of the stateSpace are pretty restrictive. Even if you wanted to back the Z-value of your start point off so that it was not on the boundary of an occupied cell, the validator would report the state as invalid because your state-limits are [-1 1] in each cartesian dimension. Increasing these limits slightly and offsetting your initial/final states would allow the planner to begin exploring the space, but we should ask ourselves whether this state-space is the right choice for your problem

3) StateSpace/StateValidator Concept

If we go back to your original problem statement, you are trying to plan on a spherical surface. The behavior of the planner you have chosen, plannerRRT, is determined by the state-space and state-validator provided to it.

Since your problem is constrained to a sphere, it might be worth creating a statespace which already captures this constraint. This can be done by creating a new class which either inherits from nav.StateSpace, or perhaps inherits from stateSpaceSE3 so that you retain some of the convenience functions.

What you want, essentially, is to plan in spherical coordinates, and then convert those coordinates to SE3 to check for collisions with obstacles on your surface. Therefore, your state-space methods could look something like the following:

classdef myCustomSS < stateSpaceSE3

    properties
        %SphericalLimits
        SphericalLimits = [1 1; -pi/2 pi/2; -pi pi];
    end
    
    methods
        function obj = myCustomSS(sphereRadius)
            % Create XYZ bounds with slight breathing room
            bounds = [repmat(sphereRadius*1.01); inf(4,1).*[-1 1]];
            
            % Construct parent SE3 space
            obj = obj@stateSpaceSE3(bounds);
            
            % Update spherical bounds
            obj.SphericalLimits(1,:) = sphereRadius;
        end
        
        function randomSE3State = sampleUniform(obj,...)
            % Randomly sample a state in spherical coordinates
            sphereState = obj.sampleSherical(1); % [rho, theta, sci]
            
            % Randomly sample an orientation using the SE3 sampler
            randomSE3State = obj.sampleUniform@stateSpaceSE3(1);
            
            % Replace XYZ elements of SE3 state with the spherical coordinates converted to XYZ
            randomSE3State(1:3) = obj.sphere2xyz(sphereState) % <YOUR IMPL>
        end
        
        function dist = distance(obj,se3state1,se3state2)
            % Spheri-Linear connection: Calculate the straight-line distance 
            % along the spherical surface, essentially d=rho*angle, 
            % between your two states
            angleDiff = obj.myAngDiff(se3state1,se3state2);
            dist = obj.SphericalLimits(1)*angleDiff;
        end
        
        function se3States = interpolate(obj,se3state1,se3state2,ratio)
            % Interpolate points along the path connecting your states. For
            % Spheri-Linear, you would just perform a linear interpolation
            % of your state in spherical coords, then convert the points
            % back to XYZ.
            sphereState1 = obj.xyz2sphere(se3State1);
            sphereState2 = obj.xyz2sphere(se3State2);
            iSphereState = obj.sphereInterp(sphereState1, sphereState2, ratio);
            
            % Your choice of how to handle orientation.
            iOrientation = obj.orientationInterp(se3State1, se3State2, ratio);
            
            % Convert back to SE3 state
            se3States = [obj.sphere2xyz(iSphereStates) iOrientation];
        end
        function XYZ = sphere2xyz(sphereState)
            error('Implement me');
        end
        function rhoThetaPsi = xyz2sphere(se3State)
            error('Implement me');
        end
        function sphereStates = sphereInterp(se3State1,se3State2,ratios)
            error('Implement me');
        end
        function sphereStates = orientationInterp(se3State1,se3State2,ratios)
            error('Implement me');
        end
        function angDiff = myAngDiff(se3State1,se3State2,ratios)
            error('Implement me');
        end
        function sphereState = sampleSpherical(obj, N)
            error('Implement me');
        end
    end

I will leave the choice of how to handle the implementations of these pieces to you, as there are many different ways to define distance, perform interpolation, sample points, etc... which determine the behavior of your planner, but in general, a state-space like the one outlined above may prove a better solution in the long run.

Again, a nice benefit to this space is that your sphere's surface is implicitly defined by your space. This means you can just define obstacles which lay on the surface using your occupancyMap3D and the planner should respect them.

Let us know if you have any additional questions. In the meantime I will capture this use-case as an enhancement request. We will review this alongside other such requests and you might see this exposed as a feature in a future release.

Best,

Cameron

[Nash Reimer]

Cameron,

Thank you so much for the help. The require path now seems obvious. I was trying to avoid defining my own planner and state space, but that now seems necessary.

Answer 1

[Cameron Stabile]

Adding this to the Answers section.

_________________________________________________________________________________________

Hi Nash,

First off, thanks for including your code, it helps immensely when trying to debug the issue! There are a few layers to this problem, so let's start with the error message and then move on to workarounds/concepts.

1) Error message

The error you are seeing comes from the start point laying on the boundary of an occupied cell. We can see this by plotting the start point atop the visualized occupancy map (see the tiny red X at the bottom of the sphere):

First thing to note here is that you've marked both the inner and outter cells as being occupied, which guarantees that your initial start point, with radius=1, will lie inside the max discretized radius, 1+1/mapResolution. The second thing to note is that even if the outer points are removed from pts_bound, that initial state will still be invalid because it lies on the boundary of an occupied cell (red X at corner of the center cell):

2) StateSpace/StateValidator Workaround

The second issue is that the bounds of the stateSpace are pretty restrictive. Even if you wanted to back the Z-value of your start point off so that it was not on the boundary of an occupied cell, the validator would report the state as invalid because your state-limits are [-1 1] in each cartesian dimension. Increasing these limits slightly and offsetting your initial/final states would allow the planner to begin exploring the space, but we should ask ourselves whether this state-space is the right choice for your problem

3) StateSpace/StateValidator Concept

If we go back to your original problem statement, you are trying to plan on a spherical surface. The behavior of the planner you have chosen, plannerRRT, is determined by the state-space and state-validator provided to it.

Since your problem is constrained to a sphere, it might be worth creating a statespace which already captures this constraint. This can be done by creating a new class which either inherits from nav.StateSpace, or perhaps inherits from stateSpaceSE3 so that you retain some of the convenience functions.

What you want, essentially, is to plan in spherical coordinates, and then convert those coordinates to SE3 to check for collisions with obstacles on your surface. Therefore, your state-space methods could look something like the following:

classdef myCustomSS < stateSpaceSE3
    properties
        %SphericalLimits
        SphericalLimits = [1 1; -pi/2 pi/2; -pi pi];
    end
    
    methods
        function obj = myCustomSS(sphereRadius)
            % Create XYZ bounds with slight breathing room
            bounds = [repmat(sphereRadius*1.01); inf(4,1).*[-1 1]];
            
            % Construct parent SE3 space
            obj = obj@stateSpaceSE3(bounds);
            
            % Update spherical bounds
            obj.SphericalLimits(1,:) = sphereRadius;
        end
        
        function randomSE3State = sampleUniform(obj,...)
                % Randomly sample a state in spherical coordinates
            sphereState = obj.sampleSherical(1); % [rho, theta, sci]
            
            % Randomly sample an orientation using the SE3 sampler
            randomSE3State = obj.sampleUniform@stateSpaceSE3(1);
            
            % Replace XYZ elements of SE3 state with the spherical coordinates converted to XYZ
            randomSE3State(1:3) = obj.sphere2xyz(sphereState) % <YOUR IMPL>
        end
        
        function dist = distance(obj,se3state1,se3state2)
            % Spheri-Linear connection: Calculate the straight-line distance 
            % along the spherical surface, essentially d=rho*angle, 
            % between your two states
            angleDiff = obj.myAngDiff(se3state1,se3state2);
            dist = obj.SphericalLimits(1)*angleDiff;
        end
        
        function se3States = interpolate(obj,se3state1,se3state2,ratio)
            % Interpolate points along the path connecting your states. For
            % Spheri-Linear, you would just perform a linear interpolation
            % of your state in spherical coords, then convert the points
            % back to XYZ.
            sphereState1 = obj.xyz2sphere(se3State1);
            sphereState2 = obj.xyz2sphere(se3State2);
            iSphereState = obj.sphereInterp(sphereState1, sphereState2, ratio);
            
            % Your choice of how to handle orientation.
            iOrientation = obj.orientationInterp(se3State1, se3State2, ratio);
            
            % Convert back to SE3 state
            se3States = [obj.sphere2xyz(iSphereStates) iOrientation];
        end
        function XYZ = sphere2xyz(sphereState)
            error('Implement me');
        end
        function rhoThetaPsi = xyz2sphere(se3State)
            error('Implement me');
        end
        function sphereStates = sphereInterp(se3State1,se3State2,ratios)
            error('Implement me');
        end
        function sphereStates = orientationInterp(se3State1,se3State2,ratios)
            error('Implement me');
        end
        function angDiff = myAngDiff(se3State1,se3State2,ratios)
            error('Implement me');
        end
        function sphereState = sampleSpherical(obj, N)
            error('Implement me');
        end
    end

I will leave the choice of how to handle the implementations of these pieces to you, as there are many different ways to define distance, perform interpolation, sample points, etc... which determine the behavior of your planner, but in general, a state-space like the one outlined above may prove a better solution in the long run.

Again, a nice benefit to this space is that your sphere's surface is implicitly defined by your space. This means you can just define obstacles which lay on the surface using your occupancyMap3D and the planner should respect them.

Let us know if you have any additional questions. In the meantime I will capture this use-case as an enhancement request. We will review this alongside other such requests and you might see this exposed as a feature in a future release.

Best,

Cameron