如何解决在 Matlab RRT-planner 中创建自定义转向方法
我正在尝试实现自定义转向方法（即定义自定义状态空间而不是例如 stateSpaceSE2 或 dubin，使用多项式作为转向方法。）我收到以下错误：
变量 state2 似乎是 NaN（不是数字），这似乎是由于插值路径似乎在障碍物内部而产生的错误。但是，我看不到 state2 在哪里变成 NaN 值。有没有人遇到过这个非常具体的问题？
代码示例：
% This class defines a template for creating a custom state space
deFinition
% that can be used by the sampling-based path planners like plannerRRT and
% plannerRRTStar. The state space allows sampling,interpolation,and
calculating
% the distance between states.
%
% To access documentation for how to define a state space,enter the following
% at the MATLAB command prompt:
%
% >> doc nav.StateSpace
%
% For a concrete implementation of the same interface,see the following
% nav.StateSpace class:
%
% >> edit stateSpaceSE2
%
%
% To use this custom state space for path planning,follow the steps
% outlined below and complete the class deFinition. Then,save this
% file somewhere on the MATLAB path. You can add a folder to the
% path using the ADdpath function.
%global goal; %added,change later
classdef polynomial_StateSpace < nav.StateSpace & ...
matlabshared.planning.internal.EnforceScalarHandle
%---------------------------------------------------------------------
% Step 1: Define properties to be used by your state space.
% All properties in this section are user-defined properties.
properties
%Uniformdistribution - Uniform distribution for sampling
Uniformdistribution
%normaldistribution - normal distribution for sampling
normaldistribution
%------------------------------------------------------------------
% Place your properties here or replace default properties.
%------------------------------------------------------------------
end
properties (Access = {?nav.algs.internal.InternalAccess})
%skipStateValidation Skip validation in certain member functions
% This switch is used by internal functions only
SkipStateValidation
end
%----------------------------------------------------------------------
% Step 2: Define functions used for managing your state space.
methods
% a) Use the constructor to set the name of the state space,the
% number of state variables,and to define its boundaries.
%
function obj = polynomial_StateSpace
spaceName = "polynomial_Optimization";
numStateVariables = 4;
v_max = 50/3.6; % keep here for Now
% sample pos_x,pos_y,angle,speed
% For each state variable define the lower and upper valid
% limit (one [min,max] limit per row)
stateBounds = [-100 100; -100 100; -pi pi; -v_max v_max];
% Call the constructor of the base class
obj@nav.StateSpace(spaceName,numStateVariables,stateBounds);
% Create the probability distributions for sampling.
obj.normaldistribution = matlabshared.tracking.internal.normaldistribution(numStateVariables);
obj.Uniformdistribution = matlabshared.tracking.internal.Uniformdistribution(numStateVariables);
%--------------------------------------------------------------
% Place your code here or replace default function behavior.
%--------------------------------------------------------------
end
% b) Define how the object is being copied (a new object is created
% from the current one). You have to customize this function
% if you define your own properties or special constructor.
%
% For more help,see
% >> doc nav.StateSpace/copy
%
function copyObj = copy(obj)
% Default behavior: Create a new object of the same type with no arguments.
copyObj = feval(class(obj));
copyObj.StateBounds = obj.StateBounds;
copyObj.Uniformdistribution = obj.Uniformdistribution.copy;
copyObj.normaldistribution = obj.normaldistribution.copy;
%--------------------------------------------------------------
% Place your code here or replace default function behavior.
%--------------------------------------------------------------
end
% c) Define how states are forced to lie within the state boundaries.
% You have to customize this function if you want to have
% special bounding behavior,for example for wrapped states like
% angles. The STATE input can be a single state (row) or
% multiple states (one state per row).
%
% For more help,see
% >> doc nav.StateSpace/enforceStateBounds
%
function boundedState = enforceStateBounds(obj,state)
% Default behavior: States are saturated to the [min,max] interval
nav.internal.validation.validateStateMatrix(state,nan,obj.NumStateVariables,"enforceStateBounds","state");
boundedState = state;
boundedState = min(max(boundedState,obj.StateBounds(:,1)'),...
obj.StateBounds(:,2)');
%--------------------------------------------------------------
% Place your code here or replace default function behavior.
%--------------------------------------------------------------
end
% d) Define how you can sample uniformly in your state space.
% You need to support the following calling Syntaxes:
% STATE = sampleUniform(OBJ)
% STATE = sampleUniform(OBJ,NUMSAMPLES)
% STATE = sampleUniform(OBJ,NEARSTATE,disT)
% STATE = sampleUniform(OBJ,disT,NUMSAMPLES)
% You have to customize this function if you deal with angular
% state variables.
%
% For more help,see
% >> doc nav.StateSpace/sampleUniform
%
function state = sampleUniform(obj,varargin)
narginchk(1,4);
[numSamples,stateBounds] = obj.validateSampleUniformInput(varargin{:});
% Default behavior: Sample uniformly in all state variables
% based on the user input.a
obj.Uniformdistribution.RandomVariableLimits = stateBounds;
state = obj.Uniformdistribution.sample(numSamples);
%--------------------------------------------------------------
% Place your code here or replace default function behavior.
%--------------------------------------------------------------
end
% e) Define how you can sample a Gaussian distribution in your
% state space. You need to support the following calling
% Syntaxes:
% STATE = sampleGaussian(OBJ,MEANSTATE,STDDEV)
% STATE = sampleGaussian(OBJ,STDDEV,see
% >> doc nav.StateSpace/sampleGaussian
%
function state = sampleGaussian(obj,meanState,stdDev,varargin)
narginchk(3,4);
% Default behavior: Sample from a multi-variate normal
% distribution based on the user input.
[meanState,numSamples] = obj.validateSampleGaussianInput(meanState,varargin{:});
% Configure normal distribution for sampling in all state variables
obj.normaldistribution.Mean = meanState;
obj.normaldistribution.Covariance = diag(stdDev.^2);
% Sample state(s)
state = obj.normaldistribution.sample(numSamples);
% Make sure all state samples are within state bounds. This
% saturation is not ideal,since it distorts the normal
% distribution on the state boundaries,but similar to what OMPL is doing.
state = obj.enforceStateBounds(state);
%--------------------------------------------------------------
% Place your code here or replace default function behavior.
%--------------------------------------------------------------
end
% f) Define how you can interpolate between two states in your
% state space. FRACTION is a scalar or a vector,where each number
% represents a fraction of the path segment length in the [0,1] interval.
%
% For more help,see
% >> doc nav.StateSpace/interpolate
%
function interpState = interpolate(obj,state1,state2,fraction)
narginchk(4,4);
% Default behavior: Calculate the linear interpolation between
% states.
state1 = double(state1)
state2 = double(state2)
% goal = [11.25,52,pi/2,6.9444]; % added for avoiding NaN error
% if any(isnan(state1),'all')
% state1 = 0.95*goal;
% end
% if any(isnan(state2),'all')
% state2 = goal;
% end
if ~obj.SkipStateValidation
[state1,fraction] = obj.validateInterpolateInput(state1,fraction);
end
%[state1,fraction);
%stateDiff = state2 - state1;
%interpState = state1 + fraction' * stateDiff;
%--------------------------------------------------------------
% Place your code here or replace default function behavior.
%--------------------------------------------------------------
% Algorithm to produce the interpState ...
% outputs matrix of interpolated states along polynomial
% segment
interpState = double([pos_x',pos_y',theta',vel']);
end
% g) Define how you can calculate the distance between two states in
% your state space. The STATE1 and STATE2 inputs can be a single
% state (row) or multiple states (one state per row).
%
% For more help,see
% >> doc nav.StateSpace/distance
%
function dist = distance(obj,state2)
narginchk(3,3);
state1 = double(state1);
state2 = double(state2);
% goal = [11.25,'all')
% state2 = goal;
% end
[state1,state2] = obj.validatedistanceInput(obj.NumStateVariables,state2);
% Default behavior: Calculate the Euclidean 2-norm between each pair
of
% state1 and state2 rows
stateDiff = bsxfun(@minus,state1);
dist = sqrt( sum( stateDiff.^2,2 ) );
%--------------------------------------------------------------
% Place your code here or replace default function behavior.
%--------------------------------------------------------------
end
end
end
版权声明：本文内容由互联网用户自发贡献，该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 dio@foxmail.com 举报，一经查实，本站将立刻删除。
