www.gusucode.com > matlab编程求解机器人的正逆解 采用遗传算法对机器人进行路径规划源码程序 > code/基本代码/CoorTransform.m

    
%此程序主要用来求质心坐标及建立惯性系

%求坐标变换矩阵

%CD_Trans( a,d,theta,alpha )
Initialization

CD0_trans = cell(1,7);
CDf_trans = cell(1,7);
CD0_Ter2Ba = cell(1,7);
CDf_Ter2Ba = cell(1,7);
Coordinate0_Ba = cell(1,7);
Coordinatef_Ba = cell(1,7);
Coordinate_Ter = cell(1,7);
Coordinate_0 = cell(1,7);             %惯性系下各质心坐标
SAT_Ba = [0 0 0 1]';

theta_i0 = [90 90 90 0 90 -90 0];
theta_id = [90 80 70 25 90 -75 0];

for i=1:7
    CD0_trans{i} = CD_Transd(A(i),d(i),theta_i0(i),Alpha(i));
    CDf_trans{i} = CD_Transd(A(i),d(i),theta_id(i),Alpha(i));
end

%求到基坐标系的变换矩阵

for i=1:7
    CD0_Ter2Ba{i} = eye(4);
    CDf_Ter2Ba{i} = eye(4);
    for j = 1:i
    CD0_Ter2Ba{i} = CD0_Ter2Ba{i}*CD0_trans{j};
    CDf_Ter2Ba{i} = CDf_Ter2Ba{i}*CDf_trans{j};
    end
end

for i = 1:7
    Coordinate_Ter{i} = [0,0,0,1]';
end

Coordinate_Ter{3} = [-1.25,0,0,1]';
Coordinate_Ter{4} = [-1.25,0,0,1]';

for i = 1:7
    Coordinate0_Ba{i} = CD0_Ter2Ba{i}*Coordinate_Ter{i};
    Coordinate0_Ba{i} = Coordinate0_Ba{i}(1:3);
    
    Coordinatef_Ba{i} = CDf_Ter2Ba{i}*Coordinate_Ter{i};
    Coordinatef_Ba{i} = Coordinatef_Ba{i}(1:3);
end

for i=1:7
    Coordinate = [0 0 0]';
    Coordinate_f = [0 0 0]';
    Coordinate = m(i)*Coordinate0_Ba{i}+Coordinate;
    Coordinate_f = m(i)*Coordinatef_Ba{i}+Coordinate_f;
end
Coordinate = Coordinate/sum(m);            %质心坐标
Coordinate_f = Coordinate_f/sum(m);

%建立惯性坐标系原点为系统质心，求得惯性坐标系和基坐标系之间的变换矩阵

CD0_in2Ba =[eye(3),-Coordinate;
           0,0,0,1];
CDf_in2Ba = [eye(3),-Coordinate_f;
           0 0 0 1];
       
CD0_e = CD0_Ter2Ba{7}\CD0_in2Ba;
CDf_e = CDf_Ter2Ba{7}\CDf_in2Ba;

Q0 = RPY2Q(eye(3));
Qe = RPY2Q(CD0_e(1:3,1:3));
Q_d0 = RPY2Q(eye(3));
Q_ed = RPY2Q(CDf_e(1:3,1:3));
Pe = CD0_e(1:3,4);
Ped = CDf_e(1:3,4);
q0 = Q0(2:4);yita0 = Q0(1);
qe = Qe(2:4);yitae = Qe(1);
q_ed = Q_ed(2:4);yita_ed = Qe(1);