creating random points with buildPRM.m
This commit is contained in:
Gabri6
parent
ff1367f30d
commit
11da0a737e
|
|
@ -0,0 +1,139 @@
|
||||||
|
function buildPRM(L1, L2, nbPoints)
|
||||||
|
%%%%%%%%%%%%%%%%%%
|
||||||
|
%function buildPRM(L1, L2, nbPoints)
|
||||||
|
% ex. buildPRM(2000, 1000, 10)
|
||||||
|
%
|
||||||
|
% Inputs:
|
||||||
|
% -L1: lenght of the first link (in mm)
|
||||||
|
% -L2: length of the second link (in mm)
|
||||||
|
% -nbPoints: the number of valid points in the roadmap
|
||||||
|
%
|
||||||
|
% Outputs: None
|
||||||
|
%
|
||||||
|
% author: Guillaume Gibert, guillaume.gibert@ecam.fr
|
||||||
|
% date: 22/11/2023
|
||||||
|
%%%%%%%%%%%%%%%%%%
|
||||||
|
|
||||||
|
q1q2_valid = [];
|
||||||
|
counter = 0;
|
||||||
|
|
||||||
|
% creates a figure
|
||||||
|
figure;
|
||||||
|
|
||||||
|
while (size(q1q2_valid,2) < nbPoints)
|
||||||
|
% samples randomly the joint space
|
||||||
|
q1 = rand()*360.0;
|
||||||
|
q2 = rand()*360.0;
|
||||||
|
|
||||||
|
% creates the DH table
|
||||||
|
theta = [q1; q2];
|
||||||
|
d = [0; 0];
|
||||||
|
a = [L1; L2];
|
||||||
|
alpha = [0; 0];
|
||||||
|
|
||||||
|
% computes the FK
|
||||||
|
wTee = dh2ForwardKinematics(theta, d, a, alpha, 1);
|
||||||
|
|
||||||
|
% determines the position of the end-effector
|
||||||
|
position_ee = wTee(1:2,end);
|
||||||
|
|
||||||
|
% checks if the end-effector is not hitting any obstacle
|
||||||
|
eeHittingObstacle = 0;
|
||||||
|
if (position_ee(2) >= L1)
|
||||||
|
eeHittingObstacle = 1;
|
||||||
|
end
|
||||||
|
if (position_ee(2) <= -L1)
|
||||||
|
eeHittingObstacle = 1;
|
||||||
|
end
|
||||||
|
if (position_ee(1) >= -L2 && position_ee(1) <= L2 && position_ee(2) >= -L2 && position_ee(2) <= L2)
|
||||||
|
eeHittingObstacle = 1;
|
||||||
|
end
|
||||||
|
|
||||||
|
% stores these random values
|
||||||
|
if (eeHittingObstacle == 0)
|
||||||
|
q1q2_valid = [q1q2_valid theta];
|
||||||
|
|
||||||
|
% plots the new valid sample
|
||||||
|
%subplot(1,2,1);
|
||||||
|
%plot(q1,q2, '+g'); hold on;
|
||||||
|
%subplot(1,2,2);
|
||||||
|
plot(position_ee(1),position_ee(2), '+g'); hold on;
|
||||||
|
else
|
||||||
|
% plots the new invalid sample
|
||||||
|
%subplot(1,2,1);
|
||||||
|
%plot(q1,q2, '*r'); hold on;
|
||||||
|
%subplot(1,2,2);
|
||||||
|
plot(position_ee(1),position_ee(2), '*r'); hold on;
|
||||||
|
end
|
||||||
|
|
||||||
|
counter = counter + 1;
|
||||||
|
end
|
||||||
|
|
||||||
|
% displays stats
|
||||||
|
fprintf("%d points were sorted to achieve %d valid points\n", counter, nbPoints)
|
||||||
|
|
||||||
|
% add limits, lables and obstacles to the map
|
||||||
|
%subplot(1,2,1);
|
||||||
|
% xlim([0 360]);
|
||||||
|
% ylim([0 360]);
|
||||||
|
|
||||||
|
% xlabel('q1(deg)');
|
||||||
|
% ylabel('q2(deg)');
|
||||||
|
% title('Joint space');
|
||||||
|
|
||||||
|
%subplot(1,2,2);
|
||||||
|
|
||||||
|
plot([-L2; L2], [L2; L2], 'r'); hold on;
|
||||||
|
plot([L2; L2], [L2; -L2], 'r'); hold on;
|
||||||
|
plot([L2; -L2], [-L2; -L2], 'r'); hold on;
|
||||||
|
plot([-L2; -L2], [-L2; L2], 'r'); hold on;
|
||||||
|
|
||||||
|
plot([-(L1+L2); L1+L2], [L1 ; L1], 'r'); hold on;
|
||||||
|
plot([-(L1+L2); L1+L2], [-L1 ; -L1], 'r'); hold on;
|
||||||
|
|
||||||
|
drawCircle(0,0, L1+L2);
|
||||||
|
drawCircle(0,0, L1-L2);
|
||||||
|
|
||||||
|
xlim([-(L1+L2) (L1+L2)]);
|
||||||
|
ylim([-(L1+L2) (L1+L2)]);
|
||||||
|
|
||||||
|
xlabel('x(mm)');
|
||||||
|
ylabel('y(mm)');
|
||||||
|
title('Cartesian space');
|
||||||
|
|
||||||
|
% creates a connection map
|
||||||
|
connectionMap = zeros(nbPoints, nbPoints);
|
||||||
|
|
||||||
|
for l=1:size(q1q2_valid,2)
|
||||||
|
for m=1:size(q1q2_valid,2)
|
||||||
|
if (l ~=m)
|
||||||
|
% creates the DH table
|
||||||
|
theta = q1q2_valid(:,l);
|
||||||
|
d = [0; 0];
|
||||||
|
a = [L1; L2];
|
||||||
|
alpha = [0; 0];
|
||||||
|
% computes the FK
|
||||||
|
wTee = dh2ForwardKinematics(theta, d, a, alpha, 1);
|
||||||
|
A = wTee(1:2,end);
|
||||||
|
|
||||||
|
% creates the DH table
|
||||||
|
theta = q1q2_valid(:,m);
|
||||||
|
d = [0; 0];
|
||||||
|
a = [L1; L2];
|
||||||
|
alpha = [0; 0];
|
||||||
|
% computes the FK
|
||||||
|
wTee = dh2ForwardKinematics(theta, d, a, alpha, 1);
|
||||||
|
B = wTee(1:2,end);
|
||||||
|
|
||||||
|
% computes y =ax + b
|
||||||
|
|
||||||
|
% if no collision
|
||||||
|
%connectionMap(l,m) = 1;
|
||||||
|
% else
|
||||||
|
%connectionMap(l,m) = 0;
|
||||||
|
end
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,20 @@
|
||||||
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||||
|
% Main function for Probabilistic RoadMap
|
||||||
|
%
|
||||||
|
%
|
||||||
|
% author: Gabriel Lucas, gabriel.lucas@ecam.fr
|
||||||
|
% date: 22/11/2023
|
||||||
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||||
|
L1 = 1000;
|
||||||
|
L2 = 2000;
|
||||||
|
|
||||||
|
|
||||||
|
x = [0, 4000, 4000, 0];
|
||||||
|
y = [1500, 1500, 2000, 2000];
|
||||||
|
|
||||||
|
fill(x, y,'r'); %make polygon
|
||||||
|
hold on;
|
||||||
|
plot(x(1:2),[1, 1.5],'b'); %make line
|
||||||
|
hold on;
|
||||||
|
plot(2.5,2.5,'g'); %make dot
|
||||||
|
|
||||||
Loading…
Reference in New Issue