From 11da0a737e1086e55795c18dc64ab836f7a15169 Mon Sep 17 00:00:00 2001 From: Gabri6 Date: Sun, 3 Dec 2023 16:37:58 +0100 Subject: [PATCH] creating random points with buildPRM.m --- buildPRM.m | 139 +++++++++++++++++++++++++++++++++++++++++++++++++++++ main.m | 20 ++++++++ 2 files changed, 159 insertions(+) create mode 100644 buildPRM.m create mode 100644 main.m diff --git a/buildPRM.m b/buildPRM.m new file mode 100644 index 0000000..21f92e9 --- /dev/null +++ b/buildPRM.m @@ -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 + + + diff --git a/main.m b/main.m new file mode 100644 index 0000000..c1869ca --- /dev/null +++ b/main.m @@ -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 +