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
plot_field.m 3.50 KB
Copy Edit Web IDE Raw Blame History
function [ scratch ] = plot_field(scratch, obj, tree, parents, goal_state, goal_cost, goal_parent)
% PLOT_FIELD Summary of this function goes here
% Detailed explanation goes here
clf; % clf清除当前窗口
hold on;
if true
for ii=2:size(tree,2) %root node has no parents -> start with 2
src = tree(:,parents(ii));
dst = tree(:,ii);
draw_trajectory(obj, src, dst, 'magenta', 0.5);
end
end
%% 绘制433Eros
% h = trisurf(obj.facet_, obj.vertex_(:,1), obj.vertex_(:,2), obj.vertex_(:,3)); % 绘制三角形面组成的多面体
% h.EdgeColor = 'none'; %去掉棱的颜色
% h.FaceColor = [0.8,0.8,0.8];
% h.FaceLighting = 'gouraud'; %使表面光滑
% light; %添加光照效果
% axis equal
%% 绘制障碍物
grid on
view(-11,30)
% obstacles = [-21000,-1000,11000; %Free Space的边界点坐标(每行为一个点的坐标)
% -21000,-1000, 5189;
% -7014, -1000, 5189;
% -7014, -1000,11000;
% -21000, 1000,11000;
% -21000, 1000, 5189;
% -7014, 1000, 5189;
% -7014, 1000,11000];
X1 = [-21000,-21000,-7014,-7014];
Y1 = [-1000,-1000,-1000,-1000];
Z1 = [11000,5189,5189,11000];
p1 = patch(X1,Y1,Z1,[0.5 0.5 0.5]);
p1.FaceAlpha = 0.1;
p1.EdgeColor = [0.5 0.5 0.5];
X2 = [-21000,-21000,-7014,-7014];
Y2 = [ 1000, 1000, 1000, 1000];
Z2 = [11000,5189,5189,11000];
p2 = patch(X2,Y2,Z2,[0.5 0.5 0.5]);
p2.FaceAlpha = 0.1;
p2.EdgeColor = [0.5 0.5 0.5];
X3 = [-21000 -21000 -21000 -21000];
Y3 = [-1000 -1000 1000 1000];
Z3 = [11000 5189 5189 11000];
p3 = patch(X3,Y3,Z3,[0.5 0.5 0.5]);
p3.FaceAlpha = 0.1;
p3.EdgeColor = [0.5 0.5 0.5];
X4 = [-7014 -7014 -7014 -7014];
Y4 = [-1000 -1000 1000 1000];
Z4 = [11000 5189 5189 11000];
p4 = patch(X4,Y4,Z4,[0.5 0.5 0.5]);
p4.FaceAlpha = 0.1;
p4.EdgeColor = [0.5 0.5 0.5];
X5 = [-21000 -7014 -7014 -21000];
Y5 = [-1000 -1000 1000 1000];
Z5 = [5189 5189 5189 5189];
p5 = patch(X5,Y5,Z5,[0.5 0.5 0.5]);
p5.FaceAlpha = 0.1;
p5.EdgeColor = [0.5 0.5 0.5];
X6 = [-18000,-18000,-18000,-18000];
Y6 = [-1000,-1000,1000,1000];
Z6 = [11000,8500,8500,11000];
p6 = patch(X6,Y6,Z6,[0.5 0.5 0.5]);
p6.FaceAlpha = 0.1;
p6.EdgeColor = [0.5 0.5 0.5];
X7 = [-18000,-16000,-16000,-18000];
Y7 = [-1000,-1000,1000,1000];
Z7 = [8500,8500,8500,8500];
p7 = patch(X7,Y7,Z7,[0.5 0.5 0.5]);
p7.FaceAlpha = 0.1;
p7.EdgeColor = [0.5 0.5 0.5];
X8 = [-16000,-16000,-16000,-16000];
Y8 = [-1000,-1000,1000,1000];
Z8 = [11000,8500,8500,11000];
p8 = patch(X8,Y8,Z8,[0.5 0.5 0.5]);
p8.FaceAlpha = 0.1;
p8.EdgeColor = [0.5 0.5 0.5];
% X9 = [];
% Y9 = [];
% Z9 = [];
% p9 = patch(X9,Y9,Z9,[0.5 0.5 0.5]);
% p9.FaceAlpha = 0.1;
% p9.EdgeColor = [0.5 0.5 0.5];
xlabel('x')
ylabel('y')
zlabel('z')
axis equal
%%
if goal_cost < inf
p = goal_parent;
h = draw_trajectory(obj, tree(:,p), goal_state, 'black', 2);
uistack(h, 'top')
c = p;
p = parents(c);
while p > 0
h = draw_trajectory(obj, tree(:,p), tree(:,c), 'black', 2);
uistack(h, 'top')
c = p;
p = parents(c);
end
end
hold off;
title(['cost: ', num2str(goal_cost)]);
drawnow
end
function [h] = draw_trajectory(obj,x0,x1,color, thickness)
t = obj.evaluate_arrival_time(x0,x1,obj.h_); % 使用obj.就不要添加参数obj
[states, ~] = obj.evaluate_states_and_inputs(x0,x1,obj.h_,obj.B_,obj.R_);
X = [];
Y = [];
Z = [];
for jj=[0:obj.h_*5:t,t]
p = states(jj);
X = [X,p(1)];
Y = [Y,p(2)];
Z = [Z,p(3)];
end
h = line(X, Y, Z, 'Color', color, 'LineWidth', thickness);
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