matlab Robotics System Toolbox 1 ----- Manipulator Algorithms

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个。
• DH矩阵法
  标准DH变换如下图[1]
  每个DH方程描述一个link，DH方程对应的坐标系建在link的末端，$DH方程需要能刻画link的末端的位姿$
  标准DH方程，先在z轴上移动，然后在旋转x轴，最后再移动x轴。
  $DH方程是revolution和prismatic这两种特殊变换的描述，如果关节是万向节，四元数等工具去描述joint之间的坐标变换关系会更加方便。$
  

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方程描述下面的结构

$第一列描述的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 获取坐标系之间转换的结果

robot = importrobot('robot.urdf');
show(robot)
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控制机器位置

robot = importrobot('puma.urdf');
show(robot)
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.