Robotics ToolBox機械臂仿真

選修實驗要求研究一下Robotics ToolBox，好不容易在網上找到一些參考資料，沒想到新的工具箱中許多函數都有變化了，只好硬着頭皮看Lecture Notes了，好不容易憋出了幾行程序

%第一個機器人模型
%6個旋轉關節
%xmfbit
clear L1 L2 L3 L4 L5 L6 R qz qr t q qdd qd
close all
clc


L1=Link([0,0,20,0,0]);
L2=Link([0,0,20,0,0]);
L3=Link([0,0,20,pi/2,0]);
L4=Link([0,0,20,0,0]);
L5=Link([0,0,20,0,0]);
L6=Link([0,0,20,0,0]);
disp('模型構建完成,參數如下：');
R=SerialLink([L1,L2,L3,L4,L5,L6])
qz=[0,0,0,0,0,0];           %初始位置關節點參數


%做機械臂初始狀態的圖
figure
R.plot(qz)                       
title('機械臂的初始位姿');




%軌跡求解
qr=[0,pi/2,0,pi/2,0,0];       %終了位置6個關節點的參數
t=[0:0.5:10]';
disp('轉動過程中的關節點參數：');
[q,qd,qdd]=jtraj(qz,qr,t);
disp('位移參數：');
q
disp('速度參數：');
qd
disp('加速度參數：');
qdd


%畫動態圖
figure
R.plot(q)
title('機械臂運動動態圖');


%以第四個關節點爲例畫位移，速度，加速度參數變化圖
figure
%提取矩陣第四列，即爲第四個關節點的位移，速度，加速度參數
q4=q(:,4);
qd4=qd(:,4);
qdd4=qdd(:,4);
subplot(2,2,1)
plot(t,q4)
title('位移變化');
subplot(2,2,2)
plot(t,qd4)
title('速度變化');
subplot(2,2,3)
plot(t,qdd4)
title('加速度變化');