运动学移植的问题

[Zz-er]

码中一个SDH表定义的构型似乎有问题，但是对后面的计算并没有影响，后面的计算中并没有访问dh表中的d和a参数
如下图所示6dof_kinematics.cpp文件中tmp_DH_matrix定义的SDH构型

将上面的构型用matlab绘制出来如下图所示
`
% 清除环境
clear all; close all; clc;

% 注意：Link的参数顺序为 [theta d a alpha]
L1 = Link([0 0.550 0.180 -pi/2]);
L2 = Link([-pi/2 0 0.680 0 ]);
L3 = Link([pi/2 0.165 0 pi/2]);
% L3 = Link([pi/2 0 -0.165 pi/2]); % L3的DH参数应该定义成这样
L4 = Link([0 0.842 0 -pi/2]);
L5 = Link([0 0 0 pi/2]);
L6 = Link([0 0.12 0 0 ]);
% 创建机器人模型，将所有链节按顺序组合
SDH_robot = SerialLink([L1 L2 L3 L4 L5 L6], 'name', 'SDHRobot');
view(3)

% SDH_robot.plot([0 0 0 0 0 0]);

% 设置机器人绘图参数（可选）
SDH_robot.display

SDH_robot.teach([0 -pi/2 pi/2 0 0 0]);`

D_ELBOW参数和示意图中描述的不同

启用上面matlab代码中的注释，将L3定义为 L3 = Link([pi/2 0 -0.165 pi/2]);

虽然对后面的计算没有影响，应该定义为

    float tmp_DH_matrix[6][4] = {
        // theta            d                   a                   alpha
        {0.0f,            armConfig.L_BASE,    armConfig.D_BASE, -(float)M_PI_2},
        {-(float)M_PI_2, 0.0f,                armConfig.L_ARM,  0.0f},
        {(float)M_PI_2,  0.0f,                 -armConfig.D_ELBOW, (float)M_PI_2},
        {0.0f,            armConfig.L_FOREARM, 0.0f,             -(float)M_PI_2},
        {0.0f,            0.0f,                0.0f,             (float)M_PI_2},
        {0.0f,            armConfig.L_WRIST, 0.0f, 0.0f}
    };