94 lines
2.4 KiB
Matlab
94 lines
2.4 KiB
Matlab
## Copyright (C) 2023 borie
|
|
##
|
|
## This program is free software: you can redistribute it and/or modify it
|
|
## under the terms of the GNU General Public License as published by
|
|
## the Free Software Foundation, either version 3 of the License, or
|
|
## (at your option) any later version.
|
|
##
|
|
## This program is distributed in the hope that it will be useful, but
|
|
## WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
## GNU General Public License for more details.
|
|
##
|
|
## You should have received a copy of the GNU General Public License
|
|
## along with this program. If not, see
|
|
## <https://www.gnu.org/licenses/>.
|
|
|
|
## -*- texinfo -*-
|
|
## @deftypefn {} {@var{retval} =} buildRRT (@var{input1}, @var{input2})
|
|
##
|
|
## @seealso{}
|
|
## @end deftypefn
|
|
|
|
## Author: borie <borie@LAPTOP-D62TNEVS>
|
|
## Created: 2023-01-20
|
|
|
|
function retval = buildRRT (input1, input2)
|
|
|
|
addpath("C:/motion_planning");
|
|
clc
|
|
close all
|
|
|
|
randV = 360;
|
|
L1 = 2;
|
|
L2 = 1;
|
|
points = []; #Creation of empty matrices for storing
|
|
#points' coordinate and the one of onstacle
|
|
S = [2;0];
|
|
G = [-2;0];
|
|
gap = 0.2; #Interval between sampled pointns on the line
|
|
|
|
axis ([-4 4 -4 4]);
|
|
h = rectangle('Position', [-L2, -L2, 2*L2, 2*L2]); #Square
|
|
b = rectangle('Position', [-500*L2, -4*L2, 1000*L2, 2*L2]); ;#Lower boundary
|
|
c = rectangle('Position', [500*L2, 4*L2, -1000*L2, -2*L2]); #Upper boundary
|
|
|
|
set (h, "FaceColor", [1, 1, 1]);
|
|
set (b, "FaceColor", [0, 1, 1]);
|
|
set (c, "FaceColor", [0, 1, 1]);
|
|
hold on
|
|
|
|
text(S(1,1),S(2,1), " S",'Fontsize',15);
|
|
plot(S(1,1),S(2,1),'o-r');
|
|
|
|
text(G(1,1),G(2,1), " G",'Fontsize',15);
|
|
plot(G(1,1),G(2,1),'o-r');
|
|
|
|
|
|
|
|
|
|
%DH Parameters
|
|
d = [0; 0];
|
|
a = [L1; L2];
|
|
alpha = [0; 0];
|
|
jointNumber = 1;
|
|
%End effector position
|
|
Bmatrix = [0; 0; 0; 1];
|
|
|
|
n=1;
|
|
nPts = 10; #Number of points we want
|
|
while n<=nPts
|
|
|
|
q1 = randi(randV);
|
|
q2 = randi(randV);
|
|
|
|
theta = [q1; q2];
|
|
jTee = dh2ForwardKinematics(theta, d, a, alpha, jointNumber);
|
|
b_P_ee = jTee*Bmatrix;
|
|
%Is the end effector colliding with connectionMatrix
|
|
x=b_P_ee(1);
|
|
y=b_P_ee(2);
|
|
|
|
if(y >= L1 || y <= -L1 || (-L2<=x && x<=L2 && -L2<=y && y<=L2))
|
|
disp('You are in a prohibited area');
|
|
else
|
|
n++;
|
|
points = [points [q1;q2;x;y]];
|
|
[connectionMatrixC] = interCartesian (n, L2, gap, points, S)
|
|
|
|
endif
|
|
|
|
endwhile
|
|
|
|
endfunction
|