Calculating the Workspace of a Robot

Question

Sara Jones 2019-7-27

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/473721-calculating-the-workspace-of-a-robot

回答： Star Strider 2019-7-27

I am trying to calculate the workspace of a 7DOF Da Vinci Robot, as shown in the code below, where

‘a’ – represents the movement along the X axis relative to the current frame,
‘alpha' represents the rotation about the X axis relative to the current frame,
‘d’ – represents the movement along the Z axis relative to the current frame and
‘theta' represents the rotation about the Z axis relative to the current frame

However, currently I am getting the error that q1 to q7 are currently undefined - I am aware of why I recieve this error, however I am not sure how to rectify this problem. Each value of theta has a motion limit between min and max for each respective value of q, however how do I incorporate this into the code?

a1 = 0; 
a2 = 0;
a3 = 0;
a4 = 0;
a5 = 0;
a6 = 0.0091;
a7 = 0;
alph1 = pi/2;
alph2 = -pi/2;
alph3 = pi/2;
alph4 = 0;
alph5 = -pi/2;
alph6 = -pi/2;
alph7 = -pi/2;
alpha1_min = -1.605;
alpha1_max = 1.5994;
alpha2_min = -0.93556;
alpha2_max = 0.94249;
alpha3_min = -0.002444;
alpha3_max = 0.24001;
alpha4_min = -3.0456;
alpha4_max = 3.0485;
alpha5_min = -3.0414;
alpha5_max = 3.0528;
alpha6_min = -3.0481;
alpha6_max = 3.0376;
alpha7_min = -3.0498;
alpha7_max = 3.0399;
theta1 = q1 + pi/2;
theta2 = q2 - pi/2;
theta3 = 0;
theta4 = q4;
theta5 = q5 - pi/2;
theta6 = q6 - pi/2;
theta7 = 0;
d1 = 0;
d2 = 0;
d3 = q3 - 0.4318;
d4 = 0.4162;
d5 = 0;
d6 = 0;
d7 = 0.0102;
N = 20000;
t1 = alpha1_max + (alpha1_max - alpha1_min)*rand(N,1);
t2 = alpha2_max + (alpha2_max - alpha2_min)*rand(N,1);
t3 = alpha3_max + (alpha3_max - alpha3_min)*rand(N,1);
t4 = alpha4_max + (alpha4_max - alpha4_min)*rand(N,1);
t5 = alpha5_max + (alpha5_max - alpha5_min)*rand(N,1);
t6 = alpha6_max + (alpha6_max - alpha6_min)*rand(N,1);
 
for i = 1:N
    A1 = TransMat(t1(i),a1, d1, alph1);
    A2 = TransMat(t2(i),a2, d2, alph2);
    A3 = TransMat(t3(i),a3, d3, alph3);
    A4 = TransMat(t4(i),a4, d4, alph4);
    A5 = TransMat(t5(i),a5, d5, alph5);
    A6 = TransMat(t6(i),a6, d6, alph6);
    T = A1 * A2 * A3 * A4 * A5 * A6;
    X=T(1,4);
    X_min = min(X);
    X_max = max(X);
    Y=T(2,4);
    Y_min = min(Y);
    Y_max = max(Y);
    Z=T(3,4);
    Z_min = min(Z);
    Z_max = max(Z);
    plot3(X,Y,Z,'.')
    hold on;
end
view(3);
title('Isometric view');
xlabel('x (mm)');
ylabel('y (mm)');
zlabel('z (mm) ');
% view(2); % top view
% title(' Top view');
% xlabel('x (mm)');
% ylabel('y (mm)');
% view([0 1 0]); % y-z plane
% title('Side view, Y-Z');
% ylabel('y (m)');
% zlabel('z (m)');
 
 
function [ T ] = TransMat(thet, a, d, alph)
T = [ cos(thet), -sin(thet)*cos(alph), sin(thet)*sin(alph), a*cos(thet);
      sin(thet), cos(thet)*cos(alph), -cos(thet)*sin(alph), a*sin(thet);
      0, sin(alph), cos(alph), d;
      0, 0, 0, 1];
end