creating random points with buildPRM.m

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
+
+
+

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
+