机械臂运动学正解验证

clc

d1 = 0.0985
d2 = 0.1215
a2 = 0.408
a3 = 0.376
d5 = 0.1025
d6 = 0.094


L(1)=Link([0,d1,0,0,0,pi],       'modified')
L(2)=Link([0,d2,0,-pi/2,0,-pi/2],  'modified')
L(3)=Link([0,0,a2,pi],           'modified')
L(4)=Link([0,0,a3,pi,0,-pi/2],     'modified')
L(5)=Link([0,d5,0,-pi/2],        'modified')
L(6)=Link([0,d6,0,pi/2],         'modified')


robot =SerialLink(L,'name','aubo_i5')
title("T")

T = robot.fkine([0 0 0 0 0 0])
robot.plot([0 0 0 0 0 0])

const double ARC_TO_DEG = 57.29577951308238;
const double DEG_TO_ARC = 0.0174532925199433;

bool isRotationMatirx(Eigen::Matrix3d R)
{
    double err=1e-6;
    Eigen::Matrix3d shouldIdenity;
    shouldIdenity=R*R.transpose();
    Eigen::Matrix3d I=Eigen::Matrix3d::Identity();
    return (shouldIdenity - I).norm() < err;
}


Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R)
{
    assert(isRotationMatirx(R));
    double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));
    bool singular = sy < 1e-6;
    double x, y, z;
    if (!singular)
    {
        x = atan2( R(2,1), R(2,2));
        y = atan2(-R(2,0), sy);
        z = atan2( R(1,0), R(0,0));
    }
    else
    {
        x = atan2(-R(1,2), R(1,1));
        y = atan2(-R(2,0), sy);
        z = 0;
    }
    return {x, y, z};
}

int main(int argc, char *argv[])
{
    // 设定欧拉角(角度)，绕固定轴
    double roll_deg = 90.0f;    // 绕X轴
    double pitch_deg = 0.0f;     // 绕Y轴
    double yaw_deg = 0.0f;     // 绕Z轴

    //        double roll_deg = 178.340073;      // 绕X轴
    //        double pitch_deg = 1.419545;     // 绕Y轴
    //        double yaw_deg = 133.484192;     // 绕Z轴

    // 转化为弧度
    double roll_arc = roll_deg * DEG_TO_ARC;    // 绕X轴
    double pitch_arc = pitch_deg * DEG_TO_ARC;  // 绕Y轴
    double yaw_arc = yaw_deg * DEG_TO_ARC;      // 绕Z轴

    cout << endl;
    cout << "roll_arc = " << roll_arc << endl;
    cout << "pitch_arc = " << pitch_arc << endl;
    cout << "yaw_arc = " << yaw_arc << endl;

    // 初始化欧拉角（rpy）,对应绕x轴，绕y轴，绕z轴的旋转角度
    Eigen::Vector3d euler_angle(roll_arc, pitch_arc, yaw_arc);

    // 使用Eigen库将欧拉角转换为旋转矩阵
    Eigen::Matrix3d rotation_matrix1;

    rotation_matrix1 = Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitX());


    cout << "\nrotation matrix1 =\n" << rotation_matrix1 << endl << endl;


    // 使用自定义函数将旋转矩阵转换为欧拉角
    Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(rotation_matrix1); // roll,pitch,yaw
    cout << "roll_2 pitch_2 yaw_2 = " << eulerAngle2[0] << " " << eulerAngle2[1]
         << " " << eulerAngle2[2] << endl << endl;
}