Rand_Tree/buildPRM.m

function buildPRM(L1, L2, nbPoints)
%%%%%%%%%%%%%%%%%%%
%function buildPRM(L1, L2, nbV)
%ex. buildPRM(2000, 1000, 10)
%
%Inputs:
%     -L1 : length of link 1(in mm)
%     -L2 : length of link 2(in mm)
%     -nbVP : Number of Valid Points
%
%Outputs:
%      - .mat file with the connectionMap
%
%Author : Thomas Périn, thomas.perin@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);
xy_valid = [];

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
      a = (A(2)-B(2))/(A(1)-B(1));
      b = A(2) - a * A(1);

      no_collision = true;
      % crosses x =-1000 ???
      if (min(A(1), B(1)) <= -1000 && max(A(1), B(1)) >= -1000)
        if ((a * (-1000) + b ) < -1000) || ((a * (-1000) + b ) > 1000)
          no_collision = true;
        else
          no_collision = false;
        end
      end
      % crosses x =1000 ???
      if (min(A(1), B(1)) <= 1000 && max(A(1), B(1)) >= 1000 && no_collision)
        if ((a * (1000) + b ) < -1000) || ((a * (1000) + b ) > 1000)
          no_collision = true;
        else
          no_collision = false;
        end
      end
      % crosses y =-1000 ???
      if (min(A(2), B(2)) <= -1000 && max(A(2), B(2)) >= -1000 && no_collision)
        if (((-1000-b)/a ) < -1000) || (((-1000-b)/a ) > 1000)
          no_collision = true;
        else
          no_collision = false;
        end
      end

			if no_collision
			  connectionMap(l,m) = 1;
        subplot(1,2,1);
		    plot([q1q2_valid(1,l) q1q2_valid(1,m)],[q1q2_valid(2,l) q1q2_valid(2,m)], '-g'); hold on;
        subplot(1,2,2);
		    plot([A(1) B(1)],[A(2) B(2)], '-g'); hold on;
			else
			  connectionMap(l,m) = 0;
      end
		end
	end
  xy_valid = [xy_valid [A(1); A(2)]];
end
connectionMap
xy_valid
% Save the matrix to a .mat file
save('dataFile.mat', 'connectionMap', 'xy_valid');