六自由度机器人matlab源程序

六自由度机器人matlab源程序，六自由度机器人matlab源程序，六自由度机器人matlab源程序

function [x]=leglength（u）
%u的值就是上平面坐标系的原点在下平面坐标系中的位置
%因为是平移，所以这个值是有规律的
%详见simulink模块，当然也可以改变这个设置

%生成上平台原点在下平台中的位置矩阵
MoinB（11）=u（1）;
MoinB（21）=u（2）;
MoinB（31）=u（3）;

%上平台各点在上平台坐标系中的位置矩阵
a_posinM=[ -114.9067  114.9067  140.9539   26.0472  -26.0472 -140.9539;
   96.4181   96.4181   51.3030 -147.7212 -147.7212   51.3030;
  0  0  0  0  0  0];
%下平台坐标各点在下平台坐标系的位置矩阵
b_posinB=[  -52.0945   52.0945  281.9078  229.8133 -229.8133 -281.9078;
  295.4423  295.4423 -102.6060 -192.8363 -192.8363 -102.6060;
         0         0         0         0         0         0];

%上平面的位姿不发生变化，相对于下平面坐标系而言
rot=eye（3）;

for i=1:6
    a_posinB（:i）=rot*a_posinM（:i）+MoinB;
    %635.2606为初始长度
    x（i）=norm（a_posinB（:i）-b_posinB（:i））-635.2606;
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      53952  2009-01-28 22:15  六自由度机器人源程序\stewart.dwg

     文件     124946  2009-01-28 22:29  六自由度机器人源程序\MyStewart_mech.mdl

     文件        897  2009-01-28 20:38  六自由度机器人源程序\leglength.m

     文件       1958  2009-01-28 22:19  六自由度机器人源程序\Position.m

     目录          0  2009-01-28 22:30  六自由度机器人源程序

----------- ---------  ---------- -----  ----

               181753                    5