I want to create 3d dubins state space,but i am getting an error

This is the code for my 3d dubins state space ,i just want this state space to have a min radius property i.e curvilinear path aside from all properties of SE3 state space but i am getting an error in this ,can you please help ,exactly whats wrong in this?

classdef stateSpaceDubins3D < nav.StateSpace & stateSpaceSE3
    %matlabshared.tracking.internal.CustomDisplay & ...
    %matlabshared.planning.internal.EnforceScalarHandle & ...
    %nav.algs.internal.InternalAccess
    %STATESPACEDUBINS State space for Dubins vehicles
    %   The Dubins state space is based on the SE(2) space with three
    %   state variables: x, y, and theta.
    %
    %   SPACE = stateSpaceDubins creates a Dubins state space object, SPACE,
    %   with default bounds for x, y, and theta. The Dubins state space has
    %   limits on the turning radius, MinTurningRadius, for navigating between
    %   states and uses the shortest feasible curve to connect states.
    %
    %   SPACE = stateSpaceDubins(BOUNDS) creates a state space object
    %   with [min max] BOUNDS for x, y, and theta, specified as a
    %   3-by-2 matrix.
    %
    %   stateSpaceDubins properties:
    %      Name              - Name of state space
    %      NumStateVariables - Number of state variables in space
    %      StateBounds       - Bounds of state variables
    %      MinTurningRadius  - Minimum turning radius
    %
    %   stateSpaceDubins methods:
    %      copy               - Create deep copy of object
    %      distance           - Distance between two states
    %      enforceStateBounds - Ensure state lies within state bounds
    %      interpolate        - Interpolate between two states
    %      sampleUniform      - Sample state using uniform distribution
    %      sampleGaussian     - Sample state using Gaussian distribution
    %
    %   See also stateSpaceReedsShepp, stateSpaceSE2.
    
    %   Copyright 2019-2021 The MathWorks, Inc.
    
    %#codegen
    properties
        %MinTurningRadius Minimum turning radius (in meters)
        %
        %   Default: 1
        MinTurningRadius = 1
    end
    
    methods
        function obj = stateSpaceDubins3D(varargin)
            %STATESPACEDUBINS Construct Dubins state space object
            
            obj@stateSpaceSE3(varargin{:});
            obj.Name = 'SE3 Dubins';
        end
    end
    
    methods
        function dist = distance(obj, state1, state2)
            %DISTANCE Distance between two states
            %   DIST = DISTANCE(SPACE, STATE1, STATE2) computes the distance
            %   along the optimal Dubins curve from STATE1 to STATE2.
            %   These states are M-by-3 matrices, where each row is a different
            %   state. The function calculates the distance between each row in
            %   the two matrices and returns a vector of M distances.
            %
            %   Example:
            %      space = stateSpaceDubins
            %
            %      % Calculate Dubins distance between 2 states
            %      dist = DISTANCE(space, [2 10 -pi], [0 -2.5 -pi/4])
            %
            %      % Decrease the minimum turning radius and calculate again
            %      space.MinTurningRadius = 0.25;
            %      dists = DISTANCE(space, [2 10 -pi; 2 10 0], [0 -2.5 -pi/4; 0 -2.5 -pi/2])
            
            narginchk(3,3);
            
            if ~obj.SkipStateValidation
                % If one of state1 and state2 is single state, skip checking rows for both
                if size(state1, 1) ==  1 || size(state2, 1) ==  1
                    nav.internal.validation.validateStateMatrix(state1, nan, 7, 'distance', 'state1');
                    nav.internal.validation.validateStateMatrix(state2, nan, 7, 'distance', 'state2');
                else %otherwise both states have to have the same number of rows
                    nav.internal.validation.validateStateMatrix(state1, nan, 7, 'distance', 'state1');
                    nav.internal.validation.validateStateMatrix(state2, size(state1,1), 7, 'distance', 'state2');
                end
            end
            
            % Call the builtins function directly. Only retrieve the
            % optimal Dubins connection for distance calculation.
            distRow = matlabshared.planning.internal.DubinsBuiltins.autonomousDubinsSegments(...
                double(state1), double(state2), obj.MinTurningRadius, 'optimal', {});
            dist = distRow.';
        end
        
        function interpState = interpolate(obj, state1, state2, ratios)
            %INTERPOLATE Interpolate between two states
            %   INTERPSTATE = INTERPOLATE(SPACE, STATE1, STATE2, RATIOS) computes
            %   interpolated states between two states at points given by RATIOS,
            %   which is specified as a vector of values between [0,1].
            %   The ratio values represent the distance along the Dubins path segment
            %   that connects STATE1 and STATE2.
            %
            %   Example:
            %      space = stateSpaceDubins
            %
            %      % Interpolate half-way between 2 states
            %      state = INTERPOLATE(space, [2 10 -pi], [0 -2.5 -pi/4], 0.5)
            %
            %      % Interpolate multiple points with a fixed interval
            %      states = INTERPOLATE(space, [2 10 -pi], [0 -2.5 -pi/4], [0:0.02:1])
            
            narginchk(4,4);
            
            % Validate and make sure that state vectors are rows, since the builtins
            % require that input format.
            if ~obj.SkipStateValidation
                [state1, state2, ratios] = obj.validateInterpolateInput(state1, state2, ratios);
            end
            
            % Use the builtins directly for maximum performance
            [pathLength,segmentLengths,segmentTypes] = matlabshared.planning.internal.DubinsBuiltins.autonomousDubinsSegments(...
                state1, state2, obj.MinTurningRadius, 'optimal', {});
            
            % 'optimal' argument returns segmentLengths and segmentTypes as
            % vectors, and pathLength is a scalar
            interpState = matlabshared.planning.internal.DubinsBuiltins.autonomousDubinsInterpolateSegments(...
                state1, state2, double(ratios) * pathLength(1), obj.MinTurningRadius, ...
                segmentLengths(:)', uint32(segmentTypes(:)'));
            
            % Make sure all angles are wrapped to [-pi,pi] interval
            %interpState(:,3) = robotics.internal.wrapToPi(interpState(:,3));
        end
        
        function copyObj = copy(obj)
            %COPY Create deep copy of state space object
            %   COPYSPACE = COPY(SPACE) creates a deep copy of the state space
            %   object SPACE and return the new object in COPYSPACE. All
            %   data of SPACE is also present in COPYSPACE.
            %
            %   Example:
            %      % Create Dubins object and set custom MinTurningRadius
            %      space = stateSpaceDubins;
            %      space.MinTurningRadius = 0.5
            %
            %      % Make a deep copy
            %      space2 = COPY(space)
            %
            %      % Verify that property values are the same
            %      isequal(space.MinTurningRadius, space2.MinTurningRadius)
            
            copyObj = stateSpaceDubins3D(obj.StateBounds);
            obj.copyProperties(copyObj);
        end
    end
    
    methods
        function set.MinTurningRadius(obj, radius)
            %set.MinTurningRadius Setter for MinTurningRadius property
            
            validateattributes(radius, {'double'}, ...
                {'nonempty', 'scalar', 'real', 'nonnan', 'finite', 'positive'}, 'stateSpaceDubins', 'MinTurningRadius');
            
            obj.MinTurningRadius = double(radius);
        end
    end
    
    methods (Access = protected)
        function propgrp = getPropertyGroups(obj)
            %getPropertyGroups Custom property group display
            %   This function is overrides the function in the
            %   CustomDisplay base class.
            
            propListS3 = struct(...
                "Name", obj.Name,...
                "StateBounds", obj.StateBounds,...
                "NumStateVariables", obj.NumStateVariables);
            
            propgrpSE3 = matlab.mixin.util.PropertyGroup(propListS3, ...
                message("nav:navalgs:statespacese3:SE3Properties").getString);
            
            propListDubins = struct("MinTurningRadius", obj.MinTurningRadius);
            propgrpDubins = matlab.mixin.util.PropertyGroup(propListDubins, ...
                message("nav:navalgs:statespacedubins:DubinsProperties").getString);
            
            propgrp = [propgrpSE3, propgrpDubins];
        end
        
        function copyProperties(obj, copyObj)
            %copyProperties Copy property data from this object to new object
            
            % Copy all properties of SE2 base class
            copyProperties@stateSpaceSE2(copyObj);
            
            % Copy Dubins properties
            copyObj.MinTurningRadius = obj.MinTurningRadius;
        end
    end
end

[mukund shah]

I am getting

Error using nav.StateSpace (line 60)

Not enough input arguments.

anyone if they cab help please tell

[mukund shah]

propgrpSE3 = matlab.mixin.util.PropertyGroup(propListS3, ...

message("nav:navalgs:statespacese2:SE2Properties").getString);

if i make this line from SE2 to SE3 then its not able to find the function,but if tis SE2 then its working so will there be an issue if i let this line be SE2 ?

[Walter Roberson]

The purpose of the expression

 message("nav:navalgs:statespacese2:SE2Properties").getString

is to fetch a character vector using the language that the user has set the interface for. For example if the user is using the Japanese interface it would fetch the Japanese version of the character vector.

The character vector is then stored in the PropertyGroup, and is used as a title when the property group is displayed. It has no other effect on the operations.

You could change the message<etc> expression to a hard-coded title such as 'SE3 Properties'

[Walter Roberson]

            % Copy all properties of SE2 base class
            copyProperties@stateSpaceSE2(copyObj);

But you are not deriving from stateSpaceSE2

[mukund shah]

yea i changed it

classdef stateSpaceDubins3D < stateSpaceSE3
    %matlabshared.tracking.internal.CustomDisplay & ...
    %matlabshared.planning.internal.EnforceScalarHandle & ...
    %nav.algs.internal.InternalAccess
    %STATESPACEDUBINS State space for Dubins vehicles
    %   The Dubins state space is based on the SE(2) space with three
    %   state variables: x, y, and theta.
    %
    %   SPACE = stateSpaceDubins creates a Dubins state space object, SPACE,
    %   with default bounds for x, y, and theta. The Dubins state space has
    %   limits on the turning radius, MinTurningRadius, for navigating between
    %   states and uses the shortest feasible curve to connect states.
    %
    %   SPACE = stateSpaceDubins(BOUNDS) creates a state space object
    %   with [min max] BOUNDS for x, y, and theta, specified as a
    %   3-by-2 matrix.
    %
    %   stateSpaceDubins properties:
    %      Name              - Name of state space
    %      NumStateVariables - Number of state variables in space
    %      StateBounds       - Bounds of state variables
    %      MinTurningRadius  - Minimum turning radius
    %
    %   stateSpaceDubins methods:
    %      copy               - Create deep copy of object
    %      distance           - Distance between two states
    %      enforceStateBounds - Ensure state lies within state bounds
    %      interpolate        - Interpolate between two states
    %      sampleUniform      - Sample state using uniform distribution
    %      sampleGaussian     - Sample state using Gaussian distribution
    %
    %   See also stateSpaceReedsShepp, stateSpaceSE2.
    
    %   Copyright 2019-2021 The MathWorks, Inc.
    
    %#codegen
    properties
        %MinTurningRadius Minimum turning radius (in meters)
        %
        %   Default: 1
        MinTurningRadius = 1
    end
    
    methods
        function obj = stateSpaceDubins3D(varargin)
            %STATESPACEDUBINS Construct Dubins state space object
            
            obj@stateSpaceSE3(varargin{:});
            obj.Name = 'SE3 Dubins';
        end
    end
    
    methods
        function dist = distance(obj, state1, state2)
            %DISTANCE Distance between two states
            %   DIST = DISTANCE(SPACE, STATE1, STATE2) computes the distance
            %   along the optimal Dubins curve from STATE1 to STATE2.
            %   These states are M-by-3 matrices, where each row is a different
            %   state. The function calculates the distance between each row in
            %   the two matrices and returns a vector of M distances.
            %
            %   Example:
            %      space = stateSpaceDubins
            %
            %      % Calculate Dubins distance between 2 states
            %      dist = DISTANCE(space, [2 10 -pi], [0 -2.5 -pi/4])
            %
            %      % Decrease the minimum turning radius and calculate again
            %      space.MinTurningRadius = 0.25;
            %      dists = DISTANCE(space, [2 10 -pi; 2 10 0], [0 -2.5 -pi/4; 0 -2.5 -pi/2])
            
            narginchk(3,3);
            
            if ~obj.SkipStateValidation
                % If one of state1 and state2 is single state, skip checking rows for both
                if size(state1, 1) ==  1 || size(state2, 1) ==  1
                    nav.internal.validation.validateStateMatrix(state1, nan, 7, 'distance', 'state1');
                    nav.internal.validation.validateStateMatrix(state2, nan, 7, 'distance', 'state2');
                else %otherwise both states have to have the same number of rows
                    nav.internal.validation.validateStateMatrix(state1, nan, 7, 'distance', 'state1');
                    nav.internal.validation.validateStateMatrix(state2, size(state1,1), 7, 'distance', 'state2');
                end
            end
            
            % Call the builtins function directly. Only retrieve the
            % optimal Dubins connection for distance calculation.
            distRow = matlabshared.planning.internal.DubinsBuiltins.autonomousDubinsSegments(...
                double(state1), double(state2), obj.MinTurningRadius, 'optimal', {});
            dist = distRow.';
        end
        
        function interpState = interpolate(obj, state1, state2, ratios)
            %INTERPOLATE Interpolate between two states
            %   INTERPSTATE = INTERPOLATE(SPACE, STATE1, STATE2, RATIOS) computes
            %   interpolated states between two states at points given by RATIOS,
            %   which is specified as a vector of values between [0,1].
            %   The ratio values represent the distance along the Dubins path segment
            %   that connects STATE1 and STATE2.
            %
            %   Example:
            %      space = stateSpaceDubins
            %
            %      % Interpolate half-way between 2 states
            %      state = INTERPOLATE(space, [2 10 -pi], [0 -2.5 -pi/4], 0.5)
            %
            %      % Interpolate multiple points with a fixed interval
            %      states = INTERPOLATE(space, [2 10 -pi], [0 -2.5 -pi/4], [0:0.02:1])
            
            narginchk(4,4);
            
            % Validate and make sure that state vectors are rows, since the builtins
            % require that input format.
            if ~obj.SkipStateValidation
                [state1, state2, ratios] = obj.validateInterpolateInput(state1, state2, ratios);
            end
            
            % Use the builtins directly for maximum performance
            [pathLength,segmentLengths,segmentTypes] = matlabshared.planning.internal.DubinsBuiltins.autonomousDubinsSegments(...
                state1, state2, obj.MinTurningRadius, 'optimal', {});
            
            % 'optimal' argument returns segmentLengths and segmentTypes as
            % vectors, and pathLength is a scalar
            interpState = matlabshared.planning.internal.DubinsBuiltins.autonomousDubinsInterpolateSegments(...
                state1, state2, double(ratios) * pathLength(1), obj.MinTurningRadius, ...
                segmentLengths(:)', uint32(segmentTypes(:)'));
            
            % Make sure all angles are wrapped to [-pi,pi] interval
            %interpState(:,3) = robotics.internal.wrapToPi(interpState(:,3));
        end
        
        function copyObj = copy(obj)
            %COPY Create deep copy of state space object
            %   COPYSPACE = COPY(SPACE) creates a deep copy of the state space
            %   object SPACE and return the new object in COPYSPACE. All
            %   data of SPACE is also present in COPYSPACE.
            %
            %   Example:
            %      % Create Dubins object and set custom MinTurningRadius
            %      space = stateSpaceDubins;
            %      space.MinTurningRadius = 0.5
            %
            %      % Make a deep copy
            %      space2 = COPY(space)
            %
            %      % Verify that property values are the same
            %      isequal(space.MinTurningRadius, space2.MinTurningRadius)
            
            copyObj = stateSpaceDubins3D(obj.StateBounds);
            obj.copyProperties(copyObj);
        end
    end
    
    methods
        function set.MinTurningRadius(obj, radius)
            %set.MinTurningRadius Setter for MinTurningRadius property
            
            validateattributes(radius, {'double'}, ...
                {'nonempty', 'scalar', 'real', 'nonnan', 'finite', 'positive'}, 'stateSpaceDubins', 'MinTurningRadius');
            
            obj.MinTurningRadius = double(radius);
        end
    end
    
    methods (Access = protected)
        function propgrp = getPropertyGroups(obj)
            %getPropertyGroups Custom property group display
            %   This function is overrides the function in the
            %   CustomDisplay base class.
            
            propListS3 = struct(...
                "Name", obj.Name,...
                "StateBounds", obj.StateBounds,...
                "NumStateVariables", obj.NumStateVariables);
            
            propgrpSE3 = matlab.mixin.util.PropertyGroup(propListS3, ...
                message("nav:navalgs:statespacese2:SE2Properties").getString);
            
            propListDubins = struct("MinTurningRadius", obj.MinTurningRadius);
            propgrpDubins = matlab.mixin.util.PropertyGroup(propListDubins, ...
                message("nav:navalgs:statespacedubins:DubinsProperties").getString);
            
            propgrp = [propgrpSE3, propgrpDubins];
        end
        
        function copyProperties(obj, copyObj)
            %copyProperties Copy property data from this object to new object
            
            % Copy all properties of SE2 base class
            copyProperties@stateSpaceSE3(copyObj);
            
            % Copy Dubins properties
            copyObj.MinTurningRadius = obj.MinTurningRadius;
        end
    end
end

This the new one its working

[mukund shah]

but i am facing another issue

newspace = stateSpaceDubins3D([1 256;
    1 256;
    1 256;
    inf inf;
    inf inf;
    inf inf;
    inf inf]);

omap = map3D;
statevalidator2 = validatorOccupancyMap3D(space);
statevalidator2.Map = omap;
statevalidator2.ValidationDistance = 1;


planner = plannerRRT(newspace,statevalidator2);
planner.MaxConnectionDistance = 5;
planner.MaxIterations = 1000;

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

start = [40 180 25 0.7 0.2 0 0.1];
goal = [96 140 144 0.3 0 0.1 0.6];
[pthObj,solnInfo] = plan(planner,start,goal);
pthObj.States
isValid = isStateValid(statevalidator2,pthObj.States)

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

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

After creating statespace i am using rrt planner on my statespace but its giving error

Error using plannerRRT/validateConstructorInput (line 549)

StateSpace property of stateValidator must point to the same state space object as stateSpace.

Error in plannerRRT (line 183)

obj.validateConstructorInput(stateSpace, stateValidator);

how to resolve this error,kindly help

@Walter Roberson

[Walter Roberson]

statevalidator2 = validatorOccupancyMap3D(space);

Notice that statevalidator2 is derived from space not from newspace

[mukund shah]

thanks for that

but now this error is showing up

Error using nav.algs.internal.builtin.SearchTree/nearestNeighbor

The user specified distance function has encountered an unknown error.

Error in nav.algs.internal.SearchTree/nearestNeighbor (line 34)

idx = obj.BuiltinTree.nearestNeighbor(randState);

Error in plannerRRT/extend (line 351)

idx = treeInternal.nearestNeighbor(randState);

Error in plannerRRT/plan (line 289)

[newNodeId, statusCode, treeInternal] = extend(obj, randState, treeInternal);

Error in dubspace3d (line 24)

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

so how do we change this ?

@Walter Roberson @Jianxin Sun

[Walter Roberson]

Sorry, I do not know. You would probably need to

dbstop if caught error

and debug the code.

Answer 1

[Remo Pillat]

If you still see this error message, please reach out to MathWorks tech support (https://www.mathworks.com/support/contact_us.html), so they can help you debug your code.