## PengLu / 使用kinodynamic RRTStar算法规划轨迹 .gitee-modal { width: 500px !important; }

Explore and code with more than 6 million developers，Free private repositories ！：）
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
is_state_free.m 2.68 KB
function [ ok ] = is_state_free( state, state_limits, obstacles, radius, time_range)
% IS_STATE_FREE returns true if the given state is valid
% - state is the 6 dimensional state vector
% - state_limits limits for the state variables
% - obstacles is an n by 6 matrix where each row contains one corner and
%   the distance to the other

ok = true;
max_dist = 5;

if isa(state,'sym') % isa()确定输入state是否为指定类sym的对象

%dt = time_range(2)-time_range(1);
r = time_range(1):max_dist:time_range(2);

s = eval(subs(state,r)); % subs()变量替换；eval()变量的值计算结果

for ii=1:size(state_limits, 1)
if sum(s(ii,:)<state_limits(ii, 1)) > 0 || sum(s(ii,:)>state_limits(ii, 2)) > 0
%if ~isAlways(state(ii) >= state_limits(ii, 1)) || ~isAlways(state(ii) <= state_limits(ii, 2))
ok = false;
return;
end
ok = false;
return;
end
end
elseif isa(state, 'function_handle')

%dt = time_range(2)-time_range(1);
r = time_range(1):max_dist:time_range(2);

for jj=1:length(r)
s = state(r(jj));
for ii = 1:size(state_limits, 1)
if s(ii) < state_limits(ii, 1) || s(ii) > state_limits(ii, 2)
ok = false;
return;
end
end

ok = false;
return;
end
end

% elseif size(state, 2) > 1  % 当state是不同时刻输入量组成的矩阵(每列是一个时刻的state)时，执行以下程序
%
%     %dt = time_range(2)-time_range(1);
%     r = time_range(1):max_dist:time_range(2);
%
%     for jj=1:length(r)
%         s = state(:, floor(r(jj) / h) + 1);
%         for ii=1:size(state_limits, 1)
%             if s(ii) < state_limits(ii, 1) || s(ii) > state_limits(ii, 2)
%                 ok = false;
%                 return;
%             end
%         end
%
%             ok = false;
%             return;
%         end
%     end
else
for ii=1:size(state_limits, 1)
if state(ii) < state_limits(ii, 1) || state(ii) > state_limits(ii, 2)
ok = false;
return;
end
end
end

end

function [coll] = collides(obs, ~, s)

coll = false;

% 判断采样点是否位于Free Space边界外
if s(1) <= obs(1,1) || s(1) >= obs(3,1)
coll = true;
elseif s(2) <= obs(1,2) || s(2) >= obs(5,2)
coll = true;
elseif s(3) <= obs(2,3) || s(3) >= obs(1,3)
coll = true;
elseif s(1) >= -18000 && s(1) <= -16000 && s(3) >= 8500
coll = true;
end

end