1.introduction
使用matlab robotics工具箱，這個工具箱功能上類似ros，并且提供和ros互動的接口。
在做一個三軸機械臂的項目，使用manipulator algorithms的部分。
目标是：讓三軸機械臂tracking一條直線。
2.建立模型
我們的模型非常簡單，不使用urdf了，直接手動模組化。
2.1基礎概念
-
robot
object的集合
-
body
object,看成是機器人的部件。
- joint
joint object that defines how a rigid body moves relative to an attachment point. In a tree-structured robot, a joint always belongs to a specific rigid body, and each rigid body has one joint
2.2如何組裝
組裝有幾種方法，坐标系轉換法和DH矩陣法
-
坐标系轉換法
先确定joint的坐标系，然後再平移link
從坐标變換的角度來看，因為body的相對坐标系是建立在joint上，joint之間可以用歸一化的變換矩陣表示，或者是四元數。但是參數太大，表達起來很麻煩。DH将變換矩陣中的參數簡化成了4個。
matlab Robotics System Toolbox 1 ----- Manipulator Algorithms1.introduction2.建立模型References -
DH矩陣法
标準DH變換如下圖[1]
每個DH方程描述一個link，DH方程對應的坐标系建在link的末端， D H 方 程 需 要 能 刻 畫 l i n k 的 末 端 的 位 姿 {\color{red}DH方程需要能刻畫link的末端的位姿} DH方程需要能刻畫link的末端的位姿
标準DH方程，先在z軸上移動，然後在旋轉x軸，最後再移動x軸。
D H 方 程 是 r e v o l u t i o n 和 p r i s m a t i c 這 兩 種 特 殊 變 換 的 描 述 ， 如 果 關 節 是 萬 向 節 ， 四 元 數 等 工 具 去 描 述 j o i n t 之 間 的 坐 标 變 換 關 系 會 更 加 方 便 。 \color{red}DH方程是revolution和prismatic 這兩種特殊變換的描述，如果關節是萬向節，四元數等工具去描述joint之間的坐标變換關系會更加友善。 DH方程是revolution和prismatic這兩種特殊變換的描述，如果關節是萬向節，四元數等工具去描述joint之間的坐标變換關系會更加友善。
matlab Robotics System Toolbox 1 ----- Manipulator Algorithms1.introduction2.建立模型References
2.2.1使用坐标系轉換法
參考matlab: https://www.mathworks.com/help/robotics/ug/build-a-robot-step-by-step.html
body上添加joint以後确定了body的坐标系，body相對于parent body的坐标系轉換通過TF來定義。
body1 = robotics.RigidBody('body1'); % 建構實體
jnt1 = robotics.Joint('jnt1','revolute');
jnt1.HomePosition = pi/4; % 設定新坐标系相對于parent 角度
tform = trvec2tform([0.25, 0.25, 0]); % 坐标位移
setFixedTransform(jnt1,tform);
body1.Joint = jnt1; % 确定關節
addBody(robot,body1,'base'); %通過名稱索引parent object， base是預設的基座，不需要重新建立
總結：坐标系轉換法，設定TF中的位移和旋轉，确定關節相對于parent關節的轉換矩陣
2.2.2檢視安裝後的結果
showdetails(robot); % display body information
show(robot);
view(2)
2.2.2 使用DH方程描述下面的結構
第 一 列 描 述 的 l i n k 1 末 端 的 坐 标 系 \color{red}第一列描述的link1末端的坐标系 第一列描述的link1末端的坐标系
gstRobot.stSDHParams[0].twistQ = 1.5708f;
gstRobot.stSDHParams[0].length = 0.798f;
gstRobot.stSDHParams[0].offsetDistant = 0.516f;
gstRobot.stSDHParams[0].emJointType = EM_JOINT_REVOLUTION;
後面的關節同理。
2.3機械臂末端執行機構tracking一條直線
思路：移動機械臂到某一點，然後末端沿着那條直線運動。
2.3.1運動學正解計算該點的位置
matlab 通過configuration 擷取坐标系之間轉換的結果
config = randomConfiguration(robot)
tform = getTransform(robot,config,'endeffector','base')
you can generate robot configurations. With a given configuration, you can also get a transformation between two body frames using rigidBodyTree.getTransform. Get a transformation from the end effector to the base
如果根據homegenous transfom 獲得歐拉角
eul = rotm2eul(rotm)； %擷取歐拉角
2.3.2修改revolute joint的角度
參考這裡:
https://www.mathworks.com/help/robotics/ref/rigidbodytree.randomconfiguration.html
設定jointposition，因為這個案例中是通過joInt控制機器位置
config = homeConfiguration(puma1)
config(2).JointPosition = pi/2;
show(puma1,config);
transformEndEffector = getTransform(robot,config,'Endeffector'); % 擷取末端相對于基準坐标系的轉換矩陣
posEndEffector = transformEndEffector (1:3, 4); % 擷取末端XYZ坐标
2.4 末端走直線
2.4.1如何動畫顯示
讓arm2和arm3都均速轉動，計算此時arm1的軌迹（角度和角速度）
動畫顯示的思路是将用drawnow + waitfor 控制固定頻率更新。
framesPerSecond = 15;
r = rateControl(framesPerSecond);
for i = 1:count
show(robot,config,'PreservePlot',false);
drawnow
waitfor(r);
end
2.4.2算法上處理
transEndEffector = getTransform(robot, config, 'endEffector');
position(1,:) = transEndEffector(1:2, 4)';
h = position(2);
%% 控制小臂和鏟鬥以勻速行駛
qs = zeros(500, 3);
qs(1, :) = [alphaDist+alpha, betaDist+beta, gama+gamaDist];
figure(2)
ax = gca;
ax.Projection = 'orthographic';
timeStamp = 2:1:500; % 5s run time
framesPerSecond = 15;
r = robotics.Rate(framesPerSecond);
show(robot,config,'PreservePlot',false);
view(2)
hold on;
drawnow;
waitfor(r);
for i = timeStamp
betaW = 5 * 0.01 * i * pi/180; % 5°/s的運作速度
config(2) = beta + betaDist - betaW;
config(3) = gama + gamaDist;
% 擷取絕對角度，計算arm1的角度
q_arm1 = asin((h - length2 * sin(config(2)+config(1)) - length3 * sin(config(1)+config(2)+config(3)))/length1);
config(1) = q_arm1;
qs(i, :) = [config(1), config(2), config(3)];
% 擷取末端相對于base的位置
transEndEffector = getTransform(robot, config, 'endEffector');
position(i,:) = transEndEffector(1:2, 4)';
show(robot,config,'PreservePlot',false);
drawnow;
waitfor(r);
end
figure(3);
plot(position(:,2));
References
[1] Corke, Peter. Robotics, vision and control: fundamental algorithms in MATLAB® second, completely revised. Vol. 118. Springer, 2017.