clc
clear?all
close?all
deg=pi/180;
warning?off;
%%?讀取數(shù)據(jù)_轉(zhuǎn)換旋轉(zhuǎn)矩陣
data?=?xlsread（‘handian.xlsx‘）;
%第一個點構建位姿矩陣
E1=deg2rad（data（15:7））;
p1=data（12:4）‘/1000;
R1=eul2rotm（E1?‘ZYX‘）;
R11=R1;
R11（:4）=p1;
R11（4:）=[0?0?0?1];%旋轉(zhuǎn)矩陣加位置，構建點1位姿矩陣
%第二個點構建位姿矩陣
E2=deg2rad（data（25:7））;
p2=data（22:4）‘/1000;
R2=eul2rotm（E2‘ZYX‘）;
R22=R2;
R22（:4）=p2;
R22（4:）=[0?0?0?1];%旋轉(zhuǎn)矩陣加位置，構建點1位姿矩陣

%%?建立機器人_計算正逆運動學
L1=?link（‘d‘?0?‘a(chǎn)‘?0?‘a(chǎn)lpha‘?0‘modified‘‘qlim‘?[-180?180].*deg）;
L2?=?link（‘d‘?0?‘a(chǎn)‘?0.325?‘a(chǎn)lpha‘?-pi/2‘modified‘‘qlim‘?[-150?-14].*deg）;
L3?=?link（‘d‘?0?‘a(chǎn)‘?1.150?‘a(chǎn)lpha‘?0‘modified‘‘qlim‘?[-147?90].*deg）;
L4?=?link（‘d‘?1.225?‘a(chǎn)‘?0.300?‘a(chǎn)lpha‘?-pi/2?‘modified‘‘qlim‘?[-210?210].*deg）;
L5?=?link（‘d‘?0?‘a(chǎn)‘?0?‘a(chǎn)lpha‘?pi/2‘‘modified‘?‘qlim‘?[-130?130].*deg）;
L6?=?link（‘d‘?0.225?‘a(chǎn)‘?0?‘a(chǎn)lpha‘?-pi/2‘modified‘‘qlim‘?[-210?210].*deg）;
robot=Seriallink（[L1L2L3L4L5L6]‘name‘‘安川ms165‘）;??%Seriallink類函數(shù)
robot.display（）;%link類函數(shù)，顯示建立機器人DH參數(shù)
robot.plot（[0?0?0?0?0?0]）;%初始位置
q1=mymodikine（R11）;
q2=mymodikine（R22）;
q11=q1（2:）;
q22=q2（2:）;
%q1=robot.ikine（R11）;%計算機器人逆運動學，關節(jié)坐標向量
%q2=robot.ikine（R22）;
t=?linspace（0211）;%步長11，等分10段
g=jtraj（q11q22t）;%五次多項式規(guī)劃軌跡
T=robot.fkine（g）;%正運動學計算軌跡每個點的位姿
robot.plot（g）%顯示機器人軌跡
teach（robot）;

%%?計算軌跡長度
long=0;
i=1;
for?i?=?1:11
????x=T（14i）;
????y=T（24i）;
????z=T（34i）;
????long=long+sqrt（x^2?+?y^2?+?z^2）;
????i=i+1;
end

%%?旋轉(zhuǎn)矩陣轉(zhuǎn)歐拉角
for?j=?1:11
????Eul=rotm2eul（T（1:31:3j）?‘ZYX‘）;
????Eo=Eul.*（180/pi）;
????a=T（1:34j）‘;
????save（j:）=[a?Eo?];
????j=j+1;
end

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

?????文件???????1775??2019-09-25?16:20??jtraj\guiji.m

?????文件??????11202??2019-09-24?18:15??jtraj\handian.xlsx

?????文件???????2769??2019-09-25?16:18??jtraj\myfkine.m

?????文件???????6534??2019-09-25?16:30??jtraj\mymodikine.m

????..A..H.???????165??2019-09-23?10:14??jtraj\~$handian.xlsx

?????目錄??????????0??2019-09-25?18:10??jtraj

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

????????????????22445????????????????????6