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

Question

mukund shah 2021-12-11

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1608630-i-want-to-create-3d-dubins-state-space-but-i-am-getting-an-error

编辑： Remo Pillat 2023-3-31

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

9 个评论
显示 7更早的评论隐藏 7更早的评论

mukund shah 2021-12-11

在 MATLAB Online 中打开

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 2021-12-11

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 2021-12-11

在 MATLAB Online 中打开

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

dbstop if caught error

and debug the code.

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Remo Pillat 2023-3-31

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1608630-i-want-to-create-3d-dubins-state-space-but-i-am-getting-an-error#answer_1205964

编辑：Remo Pillat 2023-3-31

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.

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

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

9 个评论
显示 7更早的评论隐藏 7更早的评论

回答（1 个）

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

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

9 个评论 显示 7更早的评论隐藏 7更早的评论

回答（1 个）

0 个评论 显示 -2更早的评论隐藏 -2更早的评论

另请参阅

类别

标签

产品

版本

Community Treasure Hunt

9 个评论
显示 7更早的评论隐藏 7更早的评论

0 个评论
显示 -2更早的评论隐藏 -2更早的评论