build added

buildPRM.m

@@ -1,4 +1,4 @@
-function retval = buildPRM (L1, L2, nbVP)
+function buildPRM(L1, L2, nbPoints)
 %%%%%%%%%%%%%%%%%%%
 %function buildPRM(L1, L2, nbV)
 %ex. buildPRM(2000, 1000, 10)
@@ -15,40 +15,121 @@ function retval = buildPRM (L1, L2, nbVP)
 %%%%%%%%%%%%%%%%%%%
 
 q1q2_valid = [];
+counter = 0;
 
-while (size(q1q2_valid) < nbVP)
-  % Sample randomly the joint Space
-  q1 = rand() * 360;
-  q2 = rand() * 360;
+% creates a figure
+figure;
 
-  % Creats the DH table
-  theta = [q1, q2];
-  d = [0 0];
-  a = [L1 L2];
-  alpha = [0 0];
+while (size(q1q2_valid,2) < nbPoints)
+	% samples randomly the joint space
+	q1 = rand()*360.0;
+	q2 = rand()*360.0;
 
-  % Computes the FK
-  wTee = dh2ForwardKinematics(theta, d, a, alpha, 1);
+	% creates the DH table
+	theta = [q1; q2];
+	d = [0; 0];
+	a = [L1; L2];
+	alpha = [0; 0];
 
-  % Find Position of End Effector
-  position_ee = wTee(1:2, end);
+	% computes the FK
+	wTee = dh2ForwardKinematics(theta, d, a, alpha, 1);
 
-  % Check if the end-effector is not hitting an obstacle
-  eeHittingObst = 0;
-  if(postionn_ee(2) >= L1 && postionn_ee(2) <= -L1)
-    eeHittingObst = 1;
-  endif
-  if(postionn_ee(1) >= -L2 && position_ee(1) <= L2 && position_ee(2) >= -L2 && position_ee(2) <= L2)
-    eeHittingObst = 1;
-  endif
+	% determines the position of the end-effector
+	position_ee = wTee(1:2,end);
 
-  % If not interference, store the value
-  if (eeHittingObst == 0 )
-    q1q2_valid = [q1q2_valid, theta];
-  endif
+	% 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
 
-  counter += 1;
+	% stores these random values
+	if (eeHittingObstacle == 0)
+		q1q2_valid = [q1q2_valid theta];
 
-endwhile
+		% 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
 
-endfunction
+	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

clear.m

@@ -0,0 +1,4 @@
+clear
+clear all
+clc
+

run.m

@@ -0,0 +1 @@
+buildPRM(2000, 1000, 10);