function?TH?=?niyundx_change（A1）

a2=431.8;a3=20.32;d2=149.09;d4=433.07;

nx=A1（11）;?ox=A1（12）;?ax=A1（13）;?px=A1（14）;
ny=A1（21）;?oy=A1（22）;?ay=A1（23）;?py=A1（24）;
nz=A1（31）;?oz=A1（32）;?az=A1（33）;?pz=A1（34）;

th1（1）?=?atan2（pypx）-atan2（d2（sqrt（abs（px^2+py^2-d2^2））））;
th1（2）?=?atan2（pypx）-atan2（d2-（sqrt（abs（px^2+py^2-d2^2））））;

k?=?（px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2）/（2*a2）;
th3（1）?=?atan2（a3d4）-atan2（k（sqrt（abs（a3^2+d4^2-k^2））））;
th3（2）?=?atan2（a3d4）-atan2（k-（sqrt（abs（a3^2+d4^2-k^2））））;

%th23（11）的兩列，第一列指的是第幾個th3第二列指的是第幾個th1；
th23（11）?=?atan2（（（-a3-a2*cos（th3（1）））*pz?+?（cos（th1（1））*px+sin（th1（1））*py）*（a2*sin（th3（1））-...
????d4））（（-d4+a2*sin（th3（1）））*pz-（cos（th1（1））*px+sin（th1（1））*py）*（-a2*cos（th3（1））-a3）））;
th23（12）?=?atan2（（（-a3-a2*cos（th3（1）））*pz?+?（cos（th1（2））*px+sin（th1（2））*py）*（a2*sin（th3（1））-...
????d4））（（-d4+a2*sin（th3（1）））*pz-（cos（th1（2））*px+sin（th1（2））*py）*（-a2*cos（th3（1））-a3）））;
th23（21）?=?atan2（（（-a3-a2*cos（th3（2）））*pz?+?（cos（th1（1））*px+sin（th1（1））*py）*（a2*sin（th3（2））-...
????d4））（（-d4+a2*sin（th3（2）））*pz-（cos（th1（1））*px+sin（th1（1））*py）*（-a2*cos（th3（2））-a3）））;
th23（22）?=?atan2（（（-a3-a2*cos（th3（2）））*pz?+?（cos（th1（2））*px+sin（th1（2））*py）*（a2*sin（th3（2））-...
????d4））（（-d4+a2*sin（th3（2）））*pz-（cos（th1（2））*px+sin（th1（2））*py）*（-a2*cos（th3（2））-a3）））;

th2（11）?=?th23（11）?-?th3（1）;
th2（12）?=?th23（12）?-?th3（1）;
th2（21）?=?th23（21）?-?th3（2）;
th2（22）?=?th23（22）?-?th3（2）;

th4（11）?=?atan2（（-ax*sin（th1（1））+ay*cos（th1（1）））（-ax*cos（th1（1））*cos（th23（11））-...
????ay*sin（th1（1））*cos（th23（11））+az*sin（th23（11））））;
th4（12）?=?atan2（（-ax*sin（th1（2））+ay*cos（th1（2）））（-ax*cos（th1（2））*cos（th23（12））-...
????ay*sin（th1（2））*cos（th23（12））+az*sin（th23（12））））;
th4（21）?=?atan2（（-ax*sin（th1（1））+ay*cos（th1（1）））（-ax*cos（th1（1））*cos（th23（21））-...
????ay*sin（th1（1））*cos（th23（21））+az*sin（th23（21））））;
th4（22）?=?atan2（（-ax*sin（th1（2））+ay*cos（th1（2）））（-ax*cos（th1（2））*cos（th23（22））-...
????ay*sin（th1（2））*cos（th23（22））+az*sin（th23（22））））;

s5（11）?=?-（ax*（cos（th1（1））*cos（th23（11））*cos（th4（11））+sin（th1（1））*sin（th4（11）））+...
????ay*（sin（th1（1））*cos（th23（11））*cos（th4（11））-cos（th1（1））*sin（th4（11）））-...
????az*（sin（th23（11））*cos（th4（11））））;
c5（11）?=?ax*（-cos（th1（1））*sin（th23（11）））+ay*（-sin（th1（1））*sin（th23（11）））+...
????az*（-cos（th23（11）））;
s5（12）?=?-（ax*（cos（th1（2））*cos（th23（12））*cos（th4（12））+sin（th1（2））*sin（th4（12）））+...
????ay*（sin（th1（2））*cos（th23（12））*cos（th4（12））-cos（th1（2））*sin（th4（12）））-...
????az*（sin（th23（12））*cos（th4（12））））;
c5（12）?=?ax*（-cos（th1（2））*sin（th23（12）））+ay*（-sin（th1（2））*sin（th23（12）））+...
????az*（-cos（th23（12）））;
s5（21）?=?-（ax*（cos（th1（1））*cos（th23（21））*cos（th4（21））+sin（th1（1））*sin（th4（21）））+...
????ay*（sin（th1（1））*cos（th23（21））*cos（th4（21））-cos（th1（1））*sin（th4（21）））-...
????az*（sin（th23（21））*cos（th4（21））））;
c5（21）?=?ax*（-cos（th1（1））*sin（th23（21）））+ay*（-sin（th1（1））*sin（th23（21）））+...
????az*（-cos（th23（21）））;
s5（22）?=?-（

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2020-07-09?16:50??逆運動學\
?????文件????????7002??2020-07-09?16:34??逆運動學\niyundx_change.m