clc;
clear;
format short
successNum = 0;
num = 10000;
tic
for k=1:num
result = [];
for m=1:7
q(m) = rand()*2*pi;
end
% 生成臂角参数
theta_ram = rand()*2*pi;
% 泰科7轴机械臂 SRS构型无偏置 M-DH
alp = [0,90,-90,90,-90,90,-90]/180*pi;
a = [0,0,0,0,0,0,0];
L1 = 208.6;
L2 = 390;
L3 = 289.2;
L4 = 180.8;
L = [L1,0,L2,0,L3,0,L4];
T07 = eye(4);
for i=1:7
Tii = connectingRodTransfer([alp(i), a(i), L(i), q(i)],0);
T07 = T07 * Tii;
end
SE = L2;
EW = L3;
nx=T07(1,1);ox=T07(1,2);ax=T07(1,3);
ny=T07(2,1);oy=T07(2,2);ay=T07(2,3);
nz=T07(3,1);oz=T07(3,2);az=T07(3,3);
B = [0,0,0]';
W = T07(:,4)-T07(:,3)*L4;
W = W(1:3);
S = [0,0,L1]';
SW = norm(W-S);
P = (SE+EW+SW)/2;
SP = sqrt(P*(P-SE)*(P-EW)*(P-SW));
h = 2*SP/SW;
hfs = sqrt(SE^2-h^2);
F = S+(W-S)/norm((W-S))*hfs;
q4_1 = pi-acos((SE^2+EW^2-SW^2)/(2*SE*EW));
q4_2 = -q4_1;
if(abs(q4_1)<1e-5)
disp("奇异情况");
continue;
end
normSW = (W-S)/norm((W-S));
normBS = (S-B)/norm((S-B));
n = cross(normSW,normBS);
AAA=normSW(1)^2+normSW(2)^2;
% FE = [(normSW(1)*normSW(3))/AAA,(normSW(2)*normSW(3))/AAA,-1]';
FE = [(-normSW(1)*normSW(3))/AAA,(-normSW(2)*normSW(3))/AAA,1]';
normEF = FE/norm(FE);
E = normEF*h+F;
SE = E-S;
normSE = SE/norm(SE);
Rz = normSE;
% Ry = cross(normSE,normSW);
% Rx = cross(cross(normSE,normSW),normSE);
Ry = -cross(normSE,normSW);
Rx = cross(-cross(normSE,normSW),normSE);
R03_max = [Rx/norm(Rx),Ry/norm(Ry),normSE];
u =[0,-normSW(3), normSW(2);
normSW(3),0, -normSW(1);
-normSW(2),normSW(1),0];
R03 = (eye(3)+u*sin(theta_ram)+u*u*(1-cos(theta_ram)))*R03_max;
q2_1 = atan2(-sqrt(R03(3,1)^2+R03(3,2)^2),R03(3,3));
q2_2 = atan2(sqrt(R03(3,1)^2+R03(3,2)^2),R03(3,3));
q3_1 = atan2(R03(3,2)/-sin(q2_1),R03(3,1)/sin(q2_1));
q3_2 = atan2(R03(3,2)/-sin(q2_2),R03(3,1)/sin(q2_2));
q1_1 = atan2(R03(2,3)/-sin(q2_1),R03(1,3)/-sin(q2_1));
q1_2 = atan2(R03(2,3)/-sin(q2_2),R03(1,3)/-sin(q2_2));
r21_1 = (sin(q1_1)*sin(q3_1)*sin(q4_1) - cos(q1_1)*cos(q4_1)*sin(q2_1) - cos(q1_1)*cos(q2_1)*cos(q3_1)*sin(q4_1))*nx+(- cos(q4_1)*sin(q1_1)*sin(q2_1) - cos(q1_1)*sin(q3_1)*sin(q4_1) - cos(q2_1)*cos(q3_1)*sin(q1_1)*sin(q4_1))*ny+(-cos(q3_1)*sin(q4_1)*sin(q2_1)+cos(q2_1)*cos(q4_1))*nz;
r22_1 = (sin(q1_1)*sin(q3_1)*sin(q4_1) - cos(q1_1)*cos(q4_1)*sin(q2_1) - cos(q1_1)*cos(q2_1)*cos(q3_1)*sin(q4_1))*ox+(- cos(q4_1)*sin(q1_1)*sin(q2_1) - cos(q1_1)*sin(q3_1)*sin(q4_1) - cos(q2_1)*cos(q3_1)*sin(q1_1)*sin(q4_1))*oy+(-cos(q3_1)*sin(q4_1)*sin(q2_1)+cos(q2_1)*cos(q4_1))*oz;
r23_1 = (sin(q1_1)*sin(q3_1)*sin(q4_1) - cos(q1_1)*cos(q4_1)*sin(q2_1) - cos(q1_1)*cos(q2_1)*cos(q3_1)*sin(q4_1))*ax+(- cos(q4_1)*sin(q1_1)*sin(q2_1) - cos(q1_1)*sin(q3_1)*sin(q4_1) - cos(q2_1)*cos(q3_1)*sin(q1_1)*sin(q4_1))*ay+(-cos(q3_1)*sin(q4_1)*sin(q2_1)+cos(q2_1)*cos(q4_1))*az;
r13_1 = (cos(q1_1)*cos(q2_1)*cos(q3_1)*cos(q4_1)-cos(q1_1)*sin(q2_1)*sin(q4_1)-sin(q3_1)*sin(q1_1)*cos(q4_1))*ax+(sin(q1_1)*cos(q2_1)*cos(q3_1)*cos(q4_1)+cos(q1_1)*cos(q4_1)*sin(q3_1)-sin(q2_1)*sin(q1_1)*sin(q4_1))*ay+(cos(q3_1)*cos(q4_1)*sin(q2_1)+cos(q2_1)*sin(q4_1))*az;
r33_1 = (cos(q1_1)*cos(q2_1)*sin(q3_1)+sin(q1_1)*cos(q3_1))*ax+(sin(q1_1)*cos(q2_1)*sin(q3_1)-cos(q1_1)*cos(q3_1))*ay + sin(q2_1)*sin(q3_1)*az;
q6_1 = atan2(sqrt(r21_1^2+r22_1^2),r23_1);
q6_2 = atan2(-sqrt(r21_1^2+r22_1^2),r23_1);
q7_1 = atan2(r22_1/-sin(q6_1),r21_1/sin(q6_1));
q7_2 = atan2(r22_1/-sin(q6_2),r21_1/sin(q6_2));
q5_1 = atan2(r33_1/sin(q6_1),r13_1/-sin(q6_1));
q5_2 = atan2(r33_1/sin(q6_2),r13_1/-sin(q6_2));
theta_STD = [
q1_1,q2_1,q3_1,q4_1,q5_1,q6_1,q7_1;
q1_1,q2_1,q3_1,q4_1,q5_2,q6_2,q7_2;
q1_2,q2_2,q3_2,q4_1,q5_1,q6_1,q7_1;
q1_2,q2_2,q3_2,q4_1,q5_2,q6_2,q7_2;
q1_1,q2_1,q3_2,q4_2,q5_2,q6_1,q7_1;
q1_1,q2_1,q3_2,q4_2,q5_1,q6_2,q7_2;
q1_2,q2_2,q3_1,q4_2,q5_2,q6_1,q7_1;
q1_2,q2_2,q3_1,q4_2,q5_1,q6_2,q7_2;
]*180/pi;
q_result = theta_STD(1,:)/180*pi;
T_07 = eye(4);
for i=1:7
Tii = connectingRodTransfer([alp(i), a(i), L(i), q_result(i)],0);
T_07 = T_07 * Tii;
end
% T_07 - T07;
for k=1:4
for j=1:4
if(abs(T_07(k,j)-T07(k,j))>1e-8)
disp("false")
return
end
end
end
successNum = successNum+1;
end
toc
successNum
function TArr = connectingRodTransfer(dh,theta,inv)
if nargin == 2
inv=0;
end
ct = cos(dh(4)+theta);
st = sin(dh(4)+theta);
ca = cos(dh(1));
sa = sin(dh(1));
a = dh(2);
d = dh(3);
TArr = [ ct -st 0 a;
st*ca ct*ca -sa -sa*d;
st*sa ct*sa ca ca*d;
0 0 0 1 ];
if inv
TArr=[TArr(1:3,1:3)' -TArr(1:3,1:3)'*TArr(1:3,4);0 0 0 1];
end
end
