3.2 写一个UR机器人运动学库

本博文属于工程机械臂末端柔顺控制（Ros+Gazebo仿真实现）

注：本文参考文献忘了，参考的是一篇中国学者发表的一篇关于和UR构型一致的6自由度机械臂求逆解规避掉第六个关节可能由于奇异构型无法求解的问题。

0 引言

末端柔顺控制必然牵扯到机械臂逆运动学求解。

注：由于本文涉及到矩阵运算，需要事先下载好Eigen矩阵运算库，可自行搜索如何安装。

1 工程目录

RoboticsLib
    bin(存放可执行文件)
    include(存放头文件)
        Commom.h
        DH_config.h
        FrameTransform.h
        Kinematics.h
    src(存放源代码)
        FrameTransform.cpp
        Kinematics.cpp
        test.cpp
    lib(存放生成的库文件)
    build(编译生成的中间文件)
    CMakeLists.txt

2 头文件

（1）Commom.h

#pragma once
#include <Eigen/Core>

typedef Eigen::Matrix<double, 6, 1> JointsState;
typedef Eigen::Matrix<double, 3, 1> Points;
typedef Eigen::Matrix<double, 4, 4> TransMat;
typedef Eigen::Matrix<double, 3, 3> RotMat;

主要给后续需要用到的一些矩阵取别名。

JointState: 表示关节状态，一共6个关节，为6行1列矩阵；

Points：表示一个点，3行1列矩阵；

TransMat: 坐标系变换齐次矩阵，4行4列矩阵；

RotMat: 坐标系变换的旋转矩阵，3行3列。

上述定义的矩阵后续不一定全部被使用。

（2）DH_config.h

#pragma once
#include <vector>
#include <cmath>

namespace UR5_DH
{
    std::vector<double> a = {0, -0.425, -0.39225, 0, 0, 0};
    std::vector<double> alpha = {M_PI / 2, 0, 0, M_PI / 2, -1 * M_PI / 2, 0};
    std::vector<double> d = {0.089459, 0, 0, 0.10915, 0.09465, 0.0823};
}

本文档定义了UR5机器人的DH参数，后续如有需要，与UR系列相同构型的机械臂的DH参数也可以在此定义，后续解算运动学的时候可直接传入DH参数。

（3）FrameTransform.h

/* The API of Frame transformation */
#pragma once
#include <Eigen/Core>
#include "Common.h"

namespace FT
{
    TransMat rotx(double angle);
    TransMat roty(double angle);
    TransMat rotz(double angle);
    TransMat trans(double x, double y, double z);
}

声明了几个函数，主要是求解沿着不同坐标轴旋转或平移时的齐次变换矩阵。

（4）Kinematics.h

#pragma once
#include <vector>
#include "Common.h"

class Kinematics
{
public:
    /* a, alpha and d are the DH parameters */
    Kinematics(std::vector<double> a, std::vector<double> alpha, s