%%?函數說明
%******************************
%函數名稱：fkine_UR5
%函數功能：UR5機器人正解
%函數輸入：各個關節角度，單位是角度，為1×6向量
%函數輸出：末端姿態，為4×4矩陣
%修改時間：201800403
%******************************

%%?根據建立的DH參數得到機器人正解
function?T=fkine_UR5（theta）
DH_UR5=load（‘F:\matlab?file\20171206\UR5_CB3\DH_UR5.txt‘）;
?
IfAlphabet=0;???????????%IfAlpahabet為真（非0）表示結果用字母顯示出來，此時無論輸入任何角度將被屏蔽；為假（0）表示用實數顯示出來
?
if?IfAlphabet
????????syms?theta1?theta2?theta3?theta4?theta5?theta6;
????????syms?a2?a3;
????????syms?d1?d4?d5?d6;
????????afa0=DH_UR5（11）*pi/180;
????????afa1=DH_UR5（21）*pi/180;
????????afa2=DH_UR5（31）*pi/180;
????????afa3=DH_UR5（41）*pi/180;
????????afa4=DH_UR5（51）*pi/180;
????????afa5=DH_UR5（61）*pi/180;
????????a0=DH_UR5（12）;
????????a1=DH_UR5（22）;
????????a4=DH_UR5（52）;
????????a5=DH_UR5（62）;
????????d2=DH_UR5（23）;
????????d3=DH_UR5（33）;
else
????????theta1=theta（1）*pi/180;
????????theta2=theta（2）*pi/180;
????????theta3=theta（3）*pi/180;
????????theta4=theta（4）*pi/180;
????????theta5=theta（5）*pi/180;
????????theta6=theta（6）*pi/180;
????????afa0=DH_UR5（11）*pi/180;
????????afa1=DH_UR5（21）*pi/180;
????????afa2=DH_UR5（31）*pi/180;
????????afa3=DH_UR5（41）*pi/180;
????????afa4=DH_UR5（51）*pi/180;
????????afa5=DH_UR5（61）*pi/180;
????????a0=DH_UR5（12）;
????????a1=DH_UR5（22）;
????????a2=DH_UR5（32）;
????????a3=DH_UR5（42）;
????????a4=DH_UR5（52）;
????????a5=DH_UR5（62）;
????????d1=DH_UR5（13）;
????????d2=DH_UR5（23）;
????????d3=DH_UR5（33）;
????????d4=DH_UR5（43）;
????????d5=DH_UR5（53）;
????????d6=DH_UR5（63）;
end
T01=[cos（theta1）-sin（theta1）0a0;sin（theta1）*cos（afa0）cos（theta1）*cos（afa0）-sin（afa0）-sin（afa0）*d1;sin（theta1）*sin（afa0）cos（theta1）*sin（afa0）cos（afa0）cos（afa0）*d1;0001];
T12=[cos（theta2）-sin（theta2）0a1;sin（theta2）*cos（afa1）cos（theta2）*cos（afa1）-sin（afa1）-sin（afa1）*d2;sin（theta2）*sin（afa1）cos（theta2）*sin（afa1）cos（afa1）cos（afa1）*d2;0001];
T23=[cos（theta3）-sin（theta3）0a2;sin（theta3）*cos（afa2）cos（theta3）*cos（afa2）-sin（afa2）-sin（afa2）*d3;sin（theta3）*sin（afa2）cos（theta3）*sin（afa2）cos（afa2）cos（afa2）*d3;0001];
T34=[cos（theta4）-sin（theta4）0a3;sin（theta4）*cos（afa3）cos（theta4）*cos（afa3）-sin（afa3）-sin（afa3）*d4;sin（theta4）*sin（afa3）cos（theta4）*sin（afa3）cos（afa3）cos（afa3）*d4;0001];
T45=[cos（theta5）-sin（theta5）0a4;sin（theta5）*cos（afa4）cos（theta5）*cos（afa4）-sin（afa4）-sin（afa4）*d5;sin（theta5）*sin（afa4）cos（theta5）*sin（afa4）cos（afa4）cos（afa4）*d5;0001];
T56=[cos（theta6）-sin（theta6）0a5;sin（theta6）*cos（afa5）cos（theta6）*cos（afa5）-sin（afa5）-sin（afa5）*d6;sin（theta6）*sin（afa5）cos（theta6）*sin（afa5）cos（afa5）cos（afa5）*d6;0001];
T06=T01*T12*T23*T34*T45*T56;
T16=T12*T23*T34*T45*T56;
T=T06;

if?~IfAlphabet
????p=itransf（T）
end

?
if?IfAlphabet
????for?i=1:4
????????for?j=1:4
????????????fprintf（‘T（%d%d）=%s\n‘ijT（ij））;
????????end
????end
end
end

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件??????????80??2018-04-04?10:02??UR\DH_UR5.txt
?????文件?????????379??2018-04-08?11:10??UR\ThetaAndPosition.txt
?????文件??????345046??2018-04-09?10:43??UR\UR5運動學解.doc
?????文件????????3007??2018-04-08?10:53??UR\fkine_UR5.m
?????文件????????7627??2018-04-08?10:55??UR\ikine_UR5.m
?????目錄???????????0??2018-04-09?10:44??UR\