3.3 柔顺控制代码实现

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

0 前言

        之前写了一个UR5的运动学库，参考3.2 写一个UR机器人运动学库，发现该运动学库需要补充一部分内容，主要是关于雅各比矩阵的，后面能不能用到雅各比不知道，但是运动学库里面怎么能不包含雅各比矩阵呢？所以先写上去再说。

        于是乎添加了一个获取当前雅各比矩阵的函数，代码如下：

bool Kinematics::GetJacobian(const JointsDisplacement &angles, JacobianMat &mat)
{
    try
    {
        TransMat T12 = FT::rotz(angles[0]) * FT::trans(0, 0, this->d[0]) * FT::rotx(this->alpha[0]) 
                        * FT::trans(this->a[0], 0, 0);
        TransMat T23 = FT::rotz(angles[1]) * FT::trans(0, 0, this->d[1]) * FT::rotx(this->alpha[1]) 
                        * FT::trans(this->a[1], 0, 0);
        TransMat T34 = FT::rotz(angles[2]) * FT::trans(0, 0, this->d[2]) * FT::rotx(this->alpha[2]) 
                        * FT::trans(this->a[2], 0, 0);
        TransMat T45 = FT::rotz(angles[3]) * FT::trans(0, 0, this->d[3]) * FT::rotx(this->alpha[3]) 
                        * FT::trans(this->a[3], 0, 0);
        TransMat T56 = FT::rotz(angles[4]) * FT::trans(0, 0, this->d[4]) * FT::rotx(this->alpha[4]) 
                        * FT::trans(this->a[4], 0, 0);
        TransMat T67 = FT::rotz(angles[5]) * FT::trans(0, 0, this->d[5]) * FT::rotx(this->alpha[5]) 
                        * FT::trans(this->a[5], 0, 0);

        TransMat T13 = T12 * T23;
        TransMat T14 = T13 * T34;
        TransMat T15 = T14 * T45;
        TransMat T16 = T15 * T56;
        TransMat T17 = T16 * T67;

        Direction o1, o2, o3, o4, o5, o6, o7;
        o1 << 0.0, 0.0, 0.0;
        o2 << T12(0, 3), T12(1, 3), T12(2, 3);
        o3 << T13(0, 3), T13(1, 3), T13(2, 3);
        o4 << T14(0, 3), T14(1, 3), T14(2, 3);
        o5 << T15(0, 3), T15(1, 3), T15(2, 3);
        o6 << T16(0, 3), T16(1, 3), T16(2, 3);
        o7 << T17(0, 3), T17(1, 3), T17(2, 3);

        Direction z1, z2, z3, z4, z5, z6, z7;
        z1 << 0.0, 0.0, 1.0;
        z2 << T12(0, 2), T12(1, 2), T12(2, 2);
        z3 << T13(0, 2), T13(1, 2), T13(2, 2);
        z4 << T14(0, 2), T14(1, 2), T14(2, 2);
        z5 << T15(0, 2), T15(1, 2), T15(2, 2);
        z6 << T16(0, 2), T16(1, 2), T16(2, 2);
        z7 << T17(0, 2), T17(1, 2), T17(2, 2);

        auto Jv1 = z1.cross(o7 - o1);
        auto Jv2 = z2.cross(o7 - o2);
        auto Jv3 = z3.cross(o7 - o3);
        auto Jv4 = z4.cross(o7 - o4);
        auto Jv5 = z5.cross(o7 - o5);
        auto Jv6 = z6.cross(o7 - o6);

        mat << Jv1(0), Jv2(0), Jv3(0), Jv4(0), Jv5(0), Jv6(0),
            Jv1(1), Jv2(1), Jv3(1), Jv4(1), Jv5(1), Jv6(1),
            Jv1(2), Jv2(2), Jv3(2), Jv4(2), Jv5(2), Jv6(2),
            z1(0), z2(0), z3(0), z4(0), z5(0), z6(0),
            z1(1), z