%PUMA560正解

function?cartesianVector?=?foward_Kinematics（joint_variables）

KTable?=?[?0??????0??????-pi/2????joint_variables（11）;
???????????0??????0.432???0???????joint_variables（21）;
???????????0.149??0.02???pi/2????joint_variables（31）;
???????????0.433??0???????-pi/2???joint_variables（41）;
???????????0??????0???????pi/2????joint_variables（51）;
???????????0??????0???????0???????joint_variables（61）;];
???????
A?=?zeros（446）;?

for?i?=?1:1:6
????A（::i）?=?[cos（KTable（i4））???-sin（KTable（i4））*cos（KTable（i3））????sin（KTable（i4））*sin（KTable（i3））??KTable（i2）*cos（KTable（i4））;
????????????????sin（KTable（i4））????cos（KTable（i4））*cos（KTable（i3））???-cos（KTable（i4））*sin（KTable（i3））??KTable（i2）*sin（KTable（i4））;
????????????????0???????????????????sin（KTable（i3））?????????????????????cos（KTable（i3））???????????????????KTable（i1）;
????????????????0???????????????????0????????????????????????????????????0??????????????????????????????????1;];
????????????
????for?j?=?1:1:i
????????if?j?==?1
????????????T（::i）?=?A（::1）;????????????
????????end
????????
????????if?j>1
????????????T（::i）?=?T（::i）*A（::j）;
????????end
????end
end


T6?=?zeros（44）;
T6?=?T（::6）;????%?T6?是?D-H?模型所求的結果

x?=?T6（14）;
y?=?T6（24）;
z?=?T6（34）;



%?EulerAngle?=?zeros（13）;
%?if?T6（abs（T6（13））?==?0?&&?abs（T6（23）?==?0））
%?????if?T6（33）>0
%?????????EulerAngle（12）?=?0;?%?theta?=?0
%?????else
%?????????EulerAngle（12）?=?180;?%?theta?=?180
%?????end
%?????EulerAngle（13）?=?0;
%?????if?（（T6（21）==0）?&&?（T6（11）==0））
%?????????EulerAngle（11）?=?0;
%?????else
%?????????EulerAngle（11）?=?atan2（T6（21）T6（11））;
%?????end
%?else
%?????????EulerAngle（11）?=?atan2（T6（23）T6（13））;?
%?????
%?????????if?（cos（EulerAngle（11））*T6（13）+sin（EulerAngle（11））*T6（23））*T6（33）?==?0
%?????????????EulerAngle（12）?=?0;?????
%?????????else
%?????????????EulerAngle（12）?=?atan2（（cos（EulerAngle（11））*T6（13）+sin（EulerAngle（11））*T6（23））T6（33））;
%?????????end
%?????
%?????????if?（-sin（EulerAngle（11））*T6（11）+cos（EulerAngle（11））*T6（21））*（?-sin（EulerAngle（11）*T6（12））+cos（EulerAngle（11））*T6（22）?）?==?0
%?????????????EulerAngle（13）?=?0;
%?
%?????????else
%?????????????EulerAngle（13）?=?atan2（??（-sin（EulerAngle（11））*T6（11）+cos（EulerAngle（11））*T6（21））??（?-sin（EulerAngle（11）*T6（12））+cos（EulerAngle（11））*T6（22）?）??）;
%?????????
%?????????end
%?????
%?end
%?
%?phy?=?EulerAngle（11）;
%?theta?=?EulerAngle（12）;
%?psi?=?EulerAngle（13）;

cartesianVector?=?[x?y?z];
end

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2020-04-27?01:34??PUMA560機械臂的正逆解\
?????文件????????2575??2020-04-10?13:07??PUMA560機械臂的正逆解\foward_Kinematics.m
?????文件????????9770??2020-04-11?17:38??PUMA560機械臂的正逆解\inverse_Kinematics.m
?????文件?????????696??2020-04-27?01:32??PUMA560機械臂的正逆解\PUMA560.m
?????文件?????????122??2020-04-27?01:38??PUMA560機械臂的正逆解\說明.txt