79 lines
2.4 KiB
Matlab
79 lines
2.4 KiB
Matlab
function buildPRMnew()
|
|
% Parameters
|
|
L1 = 2; % Length of the first link in mm
|
|
L2 = 1; % Length of the second link in mm
|
|
nbPoints = 10; % Number of valid points in the roadmap
|
|
|
|
q1q2_valid = [];
|
|
positions = [];
|
|
connectionMap = zeros(nbPoints, nbPoints);
|
|
|
|
% Plotting setup
|
|
figure;
|
|
|
|
% Generating points and checking for obstacles
|
|
while size(q1q2_valid, 2) < nbPoints
|
|
q1 = rand() * 360.0;
|
|
q2 = rand() * 360.0;
|
|
|
|
% DH parameters
|
|
theta = [q1; q2] * pi / 180; % Convert to radians
|
|
d = [0; 0];
|
|
a = [L1; L2];
|
|
alpha = [0; 0];
|
|
|
|
% Forward Kinematics
|
|
wTee = dh2ForwardKinematics(theta, d, a, alpha);
|
|
|
|
% Position of the end-effector
|
|
position_ee = wTee(1:2, 4);
|
|
|
|
if ~isHittingObstacle(position_ee, L1, L2)
|
|
q1q2_valid = [q1q2_valid [q1; q2]];
|
|
positions = [positions position_ee];
|
|
|
|
% Plot in joint space
|
|
subplot(1, 2, 1);
|
|
plot(q1, q2, '+g'); hold on;
|
|
xlabel('q1(deg)');
|
|
ylabel('q2(deg)');
|
|
title('Joint space');
|
|
xlim([0 360]);
|
|
ylim([0 360]);
|
|
|
|
% Plot in cartesian space
|
|
subplot(1, 2, 2);
|
|
plot(position_ee(1), position_ee(2), '+g'); hold on;
|
|
xlabel('x(mm)');
|
|
ylabel('y(mm)');
|
|
title('Cartesian space');
|
|
end
|
|
end
|
|
|
|
% Connect valid points and plot their connections
|
|
for i = 1:size(q1q2_valid, 2)
|
|
for j = 1:size(q1q2_valid, 2)
|
|
if i ~= j && ~isLineIntersectingObstacle(positions(:, i), positions(:, j), L1, L2)
|
|
connectionMap(i, j) = 1;
|
|
subplot(1, 2, 2);
|
|
plot([positions(1, i), positions(1, j)], [positions(2, i), positions(2, j)], 'b-'); hold on;
|
|
end
|
|
end
|
|
end
|
|
|
|
% Save data to a .mat file
|
|
save('robot_arm_data.mat', 'q1q2_valid', 'connectionMap');
|
|
end
|
|
|
|
% Function to compute the forward kinematics using D-H parameters
|
|
function T = dh2ForwardKinematics(theta, d, a, alpha)
|
|
T = eye(4);
|
|
for i = 1:length(theta)
|
|
T_i = [cos(theta(i)), -sin(theta(i))*cos(alpha(i)), sin(theta(i))*sin(alpha(i)), a(i)*cos(theta(i));
|
|
sin(theta(i)), cos(theta(i))*cos(alpha(i)), -cos(theta(i))*sin(alpha(i)), a(i)*sin(theta(i));
|
|
0, sin(alpha(i)), cos(alpha(i)), d(i);
|
|
0, 0, 0, 1];
|
|
T = T * T_i;
|
|
end
|
|
end
|