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
