机械臂 tcp 坐标 转旋转矩阵 （旋转向量转旋转矩阵）

 C++  Eigen版本

#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <vector>
Eigen::MatrixXd  ur_tcp_pose2matrix4d(double* ur_tcp_pose)
{
    auto p = Eigen::Map<Eigen::VectorXd>(ur_tcp_pose, 6);// [x, y, z, rx, ry, rz]
    auto position = Eigen::Vector3d(p.block<3, 1>(0, 0));// [x, y, z]
    auto rotation = Eigen::Vector3d(p.block<3, 1>(3, 0));// [rx, ry, rz]
    //rotation.norm() == sqrt(rx * rx + ry * ry + rz * rz)
    //rotation.normalized() ==[rx, ry, rz] / rotation.norm()
    auto rotation_vector = Eigen::AngleAxisd(rotation.norm(), rotation.normalized());
    Eigen::Matrix4d matrix4d = Eigen::Matrix4d::Identity();
    matrix4d.block<3, 3>(0, 0) = rotation_vector.toRotationMatrix();//旋转矩阵
    matrix4d.block<3, 1>(0, 3) = position;//位移

    std::cout<<"===========ur_tcp_pose2matrix4d================"<<std::endl;
    std::cout<<matrix4d<<std::endl<<std::endl;
    return matrix4d;
}

int main()
{
    double tcp[6] = {-0.117025, -0.435893, 0.151821, -0.012413, 3.111990, -0.018048};
    auto m1 = ur_tcp_pose2matrix4d(tcp);
    return 1;
}

输出： 

===========ur_tcp_pose2matrix4d================
  -0.999532 -0.00780419   0.0295668   -0.117025
 -0.0081466    0.999901  -0.0114782   -0.435893
 -0.0294743  -0.0117137   -0.999497    0.151821
          0           0           0           1

OpenCV  罗德里格斯（Rodrigues）变换 进行验证

#include <opencv2/opencv.hpp>
int main()
{
    //机械臂末端位姿,x,y,z,rx,ry,rz
    double tcp[6] = {-0.117025, -0.435893, 0.151821, -0.012413, 3.111990, -0.018048};

    // 旋转向量 rx,ry,rz 转旋转矩阵3*3
    cv::Mat rvec = (cv::Mat_<double>(3,1) << -0.012413, 3.111990, -0.018048);
    cv::Mat rotm;
    cv::Rodrigues(rvec, rotm);
    std::cout<<rotm<<std::endl;

    // 旋转矩阵3*3 转 旋转向量 rx,ry,rz
    cv::Matx31d dst;
    cv::Rodrigues(rotm, dst);
    std::cout<<"===================================="<<std::endl;
    std::cout<<dst.t()<<std::endl;
}

输出3*3 变换矩阵 和 Eigen的 3*3变换矩阵一致，逆推结果一致

[-0.9995323402147245, -0.007804193909980208, 0.0295667959422684;
 -0.008146602948917855, 0.9999009374881466, -0.01147815624209654;
 -0.02947428922415253, -0.01171365731702634, -0.9994969016995452]
====================================
[-0.01241300000000004, 3.111989999999997, -0.01804799999999993]

机械臂正解

void kinematics(double* theta_input)
{
    //ur5数据
//    double d[6]     = {0.089159,  0.00000,  0.00000, 0.109150,  0.094650, 0.082300 };
//    double a[6]     = {0.000000, -0.42500, -0.39225, 0.000000,  0.000000, 0.000000 };
//    double alpha[6] = {1.570796,  0.00000,  0.00000, 1.570796, -1.570796, 0.000000 };
    //ur5e数据
    double d[6]     = {0.162500,  0.00000,  0.00000, 0.133300,  0.099700, 0.099600};
    double a[6]     = {0.000000, -0.4250,  -0.39220, 0.000000,  0.000000, 0.000000};
    double alpha[6] = {1.570796,  0.00000,  0.00000, 1.570796, -1.570796, 0.000000};
    Eigen::Matrix4d T[6];//为了和theta对应，0不用
    for (int i = 0; i <6; ++i)
    {
        //theta_input[i] = theta_input[i] * 3.1415926/180;
        //cout << "   alpha=" << alpha[i-1] << "     a0=" << a[1] << endl;
        T[i](0, 0) = cos(theta_input[i]);
        T[i](0, 1) = -sin(theta_input[i]) * cos(alpha[i]);
        T[i](0, 2) = sin(theta_input[i]) * sin(alpha[i]);
        T[i](0, 3) = a[i] * cos(theta_input[i]);
        T[i](1, 0) = sin(theta_input[i]);
        T[i](1, 1) = cos(theta_input[i]) * cos(alpha[i]);
        T[i](1, 2) = -cos(theta_input[i]) * sin(alpha[i]);
        T[i](1, 3) = a[i] * sin(theta_input[i]);
        T[i](2, 0) = 0;
        T[i](2, 1) = sin(alpha[i]);
        T[i](2, 2) = cos(alpha[i]);
        T[i](2, 3) = d[i];
        T[i](3, 0) = 0;
        T[i](3, 1) = 0;
        T[i](3, 2) = 0;
        T[i](3, 3) = 1;
    }
    Eigen::Matrix4d T06 = T[0] * T[1] * T[2] * T[3] * T[4] * T[5];
    std::cout<<"===============kinematics======================" << std::endl;
    std::cout<< T06 << std::endl<< std::endl;
}

 输入的是机械臂的关节角

int main()
{
    //double tcp[6] = {-0.117025, -0.435893, 0.151821, -0.012413, 3.111990, -0.018048};
    //tcp 对应的关节角 {-88.235500, -104.851277, -130.634377,-33.804510, 91.673247, 1.317803};
    double joint_q[6] = {-88.235500, -104.851277, -130.634377,-33.804510, 91.673247, 1.317803};//关节角角度
    double joint_q1[6] = {-1.540, -1.830, -2.280, -0.590, 1.600, 0.023};//关节角弧度
    double tcp[6] = {-0.117025, -0.435893, 0.151821, -0.012413, 3.111990, -0.018048};
    //auto m1 = ur_tcp_pose2matrix4d(tcp);
    kinematics(joint_q1);
    return 1;
}

输出结果和上面的结果一致：

===============kinematics======================
  -0.999532 -0.00780361   0.0295667   -0.117025
-0.00814604    0.999901  -0.0114788   -0.435893
 -0.0294742  -0.0117142   -0.999497    0.151821
          0           0           0           1