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

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

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

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

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

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

?????目錄??????????0??2009-01-28?22:30??六自由度機器人源程序

-----------?---------??----------?-----??----

???????????????181753????????????????????5