1 Star 2 Fork 3

PengLu / 使用kinodynamic RRTStar算法规划轨迹

Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
This repository doesn't specify license. Without author's permission, this code is only for learning and cannot be used for other purposes.
Clone or download
is_input_free.m 1.67 KB
Copy Edit Web IDE Raw Blame History
function [ ok ] = is_input_free( input, input_limits, time_range)
% IS_INPUT_FREE Summary of this function goes here
% Detailed explanation goes here
ok = true;
resolution = 50;
if isa(input,'sym')
dt = time_range(2)-time_range(1);
r = time_range(1):dt/resolution:time_range(2);
s = eval(subs(input,r));
for ii=1:size(input_limits, 1)
if sum(s(ii,:)<input_limits(ii, 1)) > 1 || sum(s(ii,:)>input_limits(ii, 2)) > 1
%if ~isAlways(state(ii) >= state_limits(ii, 1)) || ~isAlways(state(ii) <= state_limits(ii, 2))
ok = false;
return;
end
end
elseif isa(input, 'function_handle')
dt = time_range(2)-time_range(1);
r = time_range(1):dt/resolution:time_range(2);
for jj=1:length(r)
s = input(r(jj));
for ii=1:size(input_limits, 1)
if s(ii) < input_limits(ii, 1) || s(ii) > input_limits(ii, 2)
ok = false;
return;
end
end
end
% elseif size(input, 2) > 1 % 当input是不同时刻输入量组成的矩阵(每列是一个时刻的input)时,执行以下程序
%
% dt = time_range(2)-time_range(1);
% r = time_range(1):dt/resolution:time_range(2);
%
% for jj=1:length(r)
% s = input(:, floor(r(jj) / h) + 1);
% for ii=1:size(input_limits, 1)
% if s(ii) < input_limits(ii, 1) || s(ii) > input_limits(ii, 2)
% ok = false;
% return;
% end
% end
% end
else
for ii=1:size(input_limits, 1)
if input(ii) < input_limits(ii, 1) || input(ii) > input_limits(ii, 2)
ok = false;
return;
end
end
end
end

Comment ( 0 )

Sign in for post a comment

Matlab
1
https://gitee.com/olupengo/kinodynamic-RRTStar-for-landing-on-small-body.git
git@gitee.com:olupengo/kinodynamic-RRTStar-for-landing-on-small-body.git
olupengo
kinodynamic-RRTStar-for-landing-on-small-body
使用kinodynamic RRTStar算法规划轨迹
master

Search