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_state_free.m 2.68 KB
Copy Edit Web IDE Raw Blame History
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
% - quad_dim contains the size of the quadcopter bounding-box
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
if collides(obstacles, radius, s)
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
if collides(obstacles, radius, s)
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
%
% if collides(obstacles, radius, s)
% 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

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