使用好机器人工具的使用可以提高学习机器人知识的兴趣，并且利用工具掌握方法去解决问题。

本周整理下机器人Matlab工具箱及相关SO(2)， SE(2)， SO(3)， SE(3)描述的数学工具。

使用资料整理，不足之处，后续再进行完善。

机器人工具箱：

为了进行系统的学习，首先分享一下机器人工具箱：

工具箱包含函数和类，以矩阵、四元数、扭转、三重角和矩阵指数的形式表示二维和3D中的方向和姿势(SO(2)， SE(2)， SO(3)， SE(3))。工具箱还提供了在数据类型之间进行操作和转换的函数，如向量、齐次变换和表示三维位置和方向所必需的单位四元数。

工具箱使用一种非常通用的方法来表示串联机器人的运动学和动力学作为MATLAB®对象-机器人对象可以由用户为任何串行链接机械手创建，并提供了来自Kinova、Universal Robotics、Rethink以及经典机器人如Puma 560和Stanford臂的许多例子。该工具箱还支持具有机器人运动模型(单轮车，自行车)、路径规划算法(bug，距离变换，D*， PRM)、运动学规划(网格，RRT)、定位(EKF，粒子滤波)、地图构建(EKF)和同步定位和映射(EKF)以及非完整车辆的Simulink模型的移动机器人。工具箱还包括一个四旋翼飞行机器人的详细Simulink模型。

对于工具箱的获取，我们可以通过以下命令可获取工具箱及相关数学工具的使用。

git clone https://github.com/petercorke/robotics-toolbox-matlab rtb

git clone https://github.com/petercorke/spatial-math smtb

git clone https://github.com/petercorke/toolbox-common-matlab common

指令如下：

addpath rtb common smtb

参考的书目：

除此之外，Robotics System Toolbox也可以实现机器人建模、运动学、动力学与控制的仿真计算。

对移动机器人和操作臂的运动学和动力学进行建模。使用常用机器人模型，或者导入 URDF 文件或 Simscape Multibody™ 模型来创建自定义机器人模型。可视化和仿真机器人运动以验证自己设计的算法。

使用loadrobot功能可实现快速访问常见的机器人模型，该功能可加载商用机器人模型，如Universal Robots™UR10合作机器人、Boston Dynamics™Atlas人形机器人和KINOVA™Gen 3机械手。

接下来探索如何生成关节信息并与机器人模型交互。

要导入自己的通用机器人描述格式(URDF)，可以使用importrobot函数。

ur10 = loadrobot("universalUR10");
atlas = loadrobot("atlas");
gen3 = loadrobot("kinovaGen3","DataFormat","column");
%在Matlab中输入上面的几行代码，就可以加载出自己所需的模型了。
%loadrobot函数返回一个rigidBodyTree对象，该对象表示每个机器人模型的运动学和动力学。
disp(gen3);

运行后的展示结果：

rigidBodyTree with properties:

     NumBodies: 8
        Bodies: {1x8 cell}
          Base: [1x1 rigidBody]
     BodyNames: {1x8 cell}
      BaseName: 'base_link'
       Gravity: [0 0 0]
    DataFormat: 'column'

show(atlas);

UR机器人也可以以同样的方式进行调出来！

调用的函数其具体代码如下：

function [robot,robotData] = loadrobot(robotName,varargin)

    narginchk(1,7);

    % Double validation of robotName for clear error message
    validateattributes(robotName,{'char','string'},{'nonempty','scalartext'},'loadrobot','robotName');

    if isempty(coder.target)
        robotName = validatestring(robotName, robotics.manip.internal.RobotList.allRobots, 'loadrobot', 'robotName');

        parser = inputParser;
        addParameter(parser, 'Gravity', [0 0 0], @(x)validateattributes(x,{'numeric'}, {'real', 'nonsparse',...
                                                                                        'finite','nonempty','numel',3,'vector'}, 'loadrobot', 'Gravity'));
        addParameter(parser, 'DataFormat', 'struct');
        addParameter(parser, 'Version', 1);

        parse(parser, varargin{:});
        userInput = parser.Results;

        % Double validation of DataFormat for clear error message
        validateattributes(userInput.DataFormat,{'char','string'},{'nonempty','scalartext'},'loadrobot','DataFormat');
        userInput.DataFormat = validatestring(userInput.DataFormat, {'struct', 'row', 'column'}, 'loadrobot', 'DataFormat');

        % Load the pre-generated RBTs
        % Fetch the number of versions and the corresponding MAT file of
        % the robot
        [~, matFileName] = robotics.manip.internal.RobotList.getNumVersionsAndMATFileName(robotName, userInput.Version);

        % Load the MAT file corresponding to the version
        fileToLoad = matFileName;
        S=load(strcat(fileToLoad,'.mat'), 'robot', 'robotData', 'inertialFlag');

        robot = S.robot;
        robotData = S.robotData;

        % Check for Zero inertial components
        inertialFlag = S.inertialFlag;
        if inertialFlag
            robotics.manip.internal.warning('urdfimporter:IntertialComponentsMissing');
        end
        % Set the DataFormat parameter
        robot.DataFormat = userInput.DataFormat;

        if (robotData.Manipulator)&&(~inertialFlag)
            robotics.manip.internal.JointSpaceMotionModelGravityAndDataFormatUpdator.update(...
                robotData.ManipulatorMotionModel,userInput.Gravity,userInput.DataFormat);
        end
        robotData.FilePath = which(strcat(fileToLoad,'.urdf'));
        robotData = rmfield(robotData,'Mobile');
        robotData = rmfield(robotData,'Manipulator');

        % Set the gravity parameter
        robot.Gravity = userInput.Gravity;
    else
        % Throw an error if the user expects output of robotdata
        coder.internal.assert(nargout <= 1, 'robotics:robotmanip:rigidbodytree:CodegenRobotData');

        % Parse inputs
        params = struct('Gravity', uint32([0, 0, 0]), ...
                        'DataFormat', char(0), ...
                        'Version', uint32(1));

        pstruct = coder.internal.parseParameterInputs(params, [], varargin{:});
        Gravity = coder.internal.getParameterValue(pstruct.Gravity, [0, 0, 0], varargin{:});

        DataFormat = coder.internal.getParameterValue(pstruct.DataFormat, 'struct', varargin{:});

        Version = coder.internal.getParameterValue(pstruct.Version, 1, varargin{:});

        % For coder, make an extrinsic call to internal extractStructFromRBT that returns a
        % codegen compatible RBT struct
        rbt = coder.const(@feval, 'robotics.manip.internal.populateRBTStructFromFunction', ...
                          'loadrobot', robotName, 'DataFormat', DataFormat, 'Version', Version);

        % Convert RBT struct back to rigidBodyTree object
        robot = rigidBodyTree(robotics.manip.internal.RigidBodyTree(rbt.NumBodies, rbt));

        % Set the gravity value
        robot.Gravity = Gravity;

        % Output empty robotdata
        robotData=[];
    end
end

补充：

SE3:齐次变换，一个4x4矩阵，属于在SE(3)中的术语

SO3:旋转矩阵，标准正交3x3矩阵，属于在SO(3)中的术语

其他数学相关工具箱如下链接获取：

https://github.com/petercorke/robotics-toolbox-matlab