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

    function A = Centroid(theta)
%此程序 主要用于求解质心坐标

Initialization

CD_trans = cell(1,7);
CD_Ter2Ba = cell(1,7);
Coordinate_Ba = cell(1,7);



for i=1:7
    CD_trans{i} = CD_Trans(A(i),d(i),theta(i),Alpha(i));   
end

for i=1:7
    CD_Ter2Ba{i} = eye(4);   
    for j = 1:i
    CD_Ter2Ba{i} = CD_Ter2Ba{i}*CD_trans{j};    
    end
end

for i = 1:7
    Coordinate_Ba{i} = CD_Ter2Ba{i}*Coordinate_Ter{i};
    Coordinate_Ba{i} = Coordinate_Ba{i}(1:3);
end

for i=1:7
    Coordinate = [0 0 0]';
    Coordinate = m(i)*Coordinate_Ba{i}+Coordinate;
end
Coordinate = Coordinate/sum(m);

A =[eye(3),-Coordinate;
           0,0,0,1];