%ABB
close?all;
clear;
clc;
%建立機器人模型
%?%?alpha:連桿扭角；a:連桿長度；?theta:關(guān)節(jié)轉(zhuǎn)角；?d:關(guān)節(jié)距離；?offset:偏移
%?%???????theta????d????????a????????alpha?????offset
%?L1=link（[0??????200?????200????????pi/2????????0?????]）;?%定義連桿的D-H參數(shù)
%?L2=link（[pi/2???0???????550?????????0??????????0?????]）;
%?L3=link（[0??????0???????160????????pi/2????????0?????]）;
%?L4=link（[0??????600??????0?????????pi/2????????0?????]）;
%?L5=link（[pi/2???0????????0?????????-pi/2???????0?????]）;
%?L6=link（[0??????0????????0?????????0???????????0?????]）;
%?robot=Seriallink（[L1?L2?L3?L4?L5?L6]‘name‘‘ABB‘）;?%連接連桿，機器人取名manman

%???????theta????d????????a????????alpha?????offset
L1=link（[0???????486.5????150??????-pi/2??????0?????]）;?%定義連桿的D-H參數(shù)
L2=link（[0???????0????????700??????0?????????0?????]）;
L3=link（[0???????0????????0????????-pi/2??????0?????]）;
L4=link（[0???????600??????0????????pi/2??????0?????]）;
L5=link（[0???????0????????0????????-pi/2??????0?????]）;
L6=link（[0???????0????????0????????0?????????0?????]）;
robot=Seriallink（[L1?L2?L3?L4?L5?L6]‘name‘‘ABB‘）;?%連接連桿，機器人取名ABB
%?robot.plot（[000000]）;
%輸出機器人模型，后面的六個角為輸出時的theta姿態(tài)
%?robot.display（）;
%?teach（robot）;

%?xy=[638?1008;
%?????566?1127;
%?????549?1210;
%?????839?1214;
%?????8231130;
%?????7551004;
%?????638?1008;];
%?figure;
%?plot（xy（:1）xy（:2））;

T1=transl（1006000）;%根據(jù)給定起始點，得到起始點位姿
T2=transl（10000500）;%根據(jù)給定終止點，得到終止點位姿
qz=robot.ikine（T1）;%根據(jù)起始點位姿，得到起始點關(guān)節(jié)角
qr=robot.ikine（T2）;


[q?qd?qdd]=jtraj（qzqr50）;?%五次多項式軌跡，得到關(guān)節(jié)角度，角速度，角加速度，50為采樣點個數(shù)
grid?on
T=robot.fkine（q）;%根據(jù)插值，得到末端執(zhí)行器位姿

X=T（1:）.T;
x=squeeze（X（14:））;
y=squeeze（X（24:））;
z=squeeze（X（34:））;
plot3（xyz‘k‘‘LineWidth‘2）;%輸出末端軌跡
hold?on
axis（[-1000?2000?-1000?1000?-1000?1000]）;
robot.plot（q?）;


%?xy=[638?1008;
%?????566?1127;
%?????549?1210;
%?????839?1214;
%?????8231130;
%?????7551004;
%?????638?1008;];
%?figure;
%?plot（xy（:1）xy（:2））;

%動畫演示



%?t=[0:0.02:1.5];
%?T=ctraj（T1T2length（t））;
%?q=robot.ikine（T）;%根據(jù)插值，得到末端執(zhí)行器位姿


%?theta=[000000];?????%指定的關(guān)節(jié)角
%?p=robot.fkine（theta）?????%fkine正解函數(shù)，根據(jù)我們給定的關(guān)節(jié)角theta，求解出末端位姿p
%?q=robot.ikine（p）?????????%ikine逆解函數(shù)，根據(jù)我們給定的末端位姿p，求解出關(guān)節(jié)角q

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

?????文件???????2447??2018-11-21?10:47??機械臂軌跡規(guī)劃\jiandan.m

?????文件???17526839??2018-11-03?09:17??機械臂軌跡規(guī)劃\Robotics?Toolbox?for?MATLAB.mltbx

?????目錄??????????0??2018-11-21?10:45??機械臂軌跡規(guī)劃

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

?????????????17529286????????????????????3