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];