3_机械臂位姿变换计算过程代码

auto cur_pose = rpc_cli->getRobotInterface(robot_name)
            ->getRobotState()
            ->getTcpPose();

std::vectortarget_pose;
target_pose = {0, 0, 0.0, 0, 0.0, 1.0};
        

auto ready_pose_ = rpc_cli->getMath()
                   ->poseTrans(cur_pose, target_pose);

  // 初始化欧拉角（rpy）,对应绕x轴，绕y轴，绕z轴的旋转角度
    Eigen::Vector3d euler_angle(pose_from.at(3) * DEG_TO_ARC,
                                pose_from.at(4) * DEG_TO_ARC,
                                pose_from.at(5) * DEG_TO_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());

    Eigen::Matrix     <double,            </double,3, 4> m3x4_to;
    Eigen::Matrix     <double,            </double,4, 4> m4x4_to;
    Eigen::Matrix     <double,            </double,4, 4> m4x4_ret;

    m3x4_to << rotation_matrix1_to, pos_to;
    cout << "m3x4_to is :\n" << m3x4_to << std::endl;// np.concatenate((a,b))

    m4x4_to << m3x4_to, homogeneous;
    m4x4_ret = m4x4*m4x4_to;

    cout << "m4x4_to is: \n" << m4x4_ret << std::endl;

    Eigen::Matrix   <double,  </double,3, 3> m3x3_ret = m4x4_ret.block(0, 0, 3, 3);
    cout << "m3x3_to ret is: \n" << m3x3_ret << std::endl;    

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};
}

/**

 * @brief myPoseTrans

     * @param pose_from 起始位姿（空间向量)

     * @param pose_from_to 相对于起始位姿的姿态变化（空间向量）

     * @return 结果位姿 (空间向量)

 */

std::vectormyPoseTrans(const std::vector&pose_from,
                                const std::vector&pose_from_to)
{
    std::vector pose;
    // 初始化欧拉角（rpy）,对应绕x轴，绕y轴，绕z轴的旋转角度
    Eigen::Vector3d euler_angle(pose_from.at(3) * DEG_TO_ARC,
                                pose_from.at(4) * DEG_TO_ARC,
                                pose_from.at(5) * DEG_TO_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());    
     

    Eigen::MatrixXd pos(3,1);
    pos(0,0)= pose_from.at(0);
    pos(1,0)= pose_from.at(1);
    pos(2,0)= pose_from.at(2);

    Eigen::MatrixXd homogeneous(1,4);
    homogeneous(0,0)=0;
    homogeneous(0,1)=0;
    homogeneous(0,2)=0;
    homogeneous(0,3)=1;

    Eigen::Matrix     <double,            </double,3, 4> m3x4;
    Eigen::Matrix     <double,            </double,4, 4> m4x4;      

    m3x4 << rotation_matrix1, pos;
    cout << "m3x4 is :\n" << m3x4 << std::endl;// np.concatenate((a,b))

    m4x4 << m3x4, homogeneous;
    cout << "m4x4 is: \n" <

    Eigen::MatrixXd pos_to(3,1);
    pos_to(0,0)= pose_from_to.at(0);
    pos_to(1,0)= pose_from_to.at(1);
    pos_to(2,0)= pose_from_to.at(2);
    
    // 转化为弧度
    Eigen::Vector3d euler_angle_to(pose_from_to.at(3) * DEG_TO_ARC,
                                   pose_from_to.at(4) * DEG_TO_ARC,
                                   pose_from_to.at(5) * DEG_TO_ARC);    

    // 使用Eigen库将欧拉角转换为旋转矩阵
    Eigen::Matrix3d rotation_matrix1_to;
    rotation_matrix1_to = Eigen::AngleAxisd(euler_angle_to[2], Eigen::Vector3d::UnitZ()) *
                          Eigen::AngleAxisd(euler_angle_to[1], Eigen::Vector3d::UnitY()) *    
                          Eigen::AngleAxisd(euler_angle_to[0], Eigen::Vector3d::UnitX());
          
    Eigen::Matrix     <double,            </double,3, 4> m3x4_to;
    Eigen::Matrix     <double,            </double,4, 4> m4x4_to;
    Eigen::Matrix     <double,            </double,4, 4> m4x4_ret;
      

    m3x4_to << rotation_matrix1_to, pos_to;
    cout << "m3x4_to is :\n" << m3x4_to << std::endl;// np.concatenate((a,b))

    m4x4_to << m3x4_to, homogeneous;
    m4x4_ret = m4x4*m4x4_to;

    cout << "m4x4_to is: \n" << m4x4_ret << std::endl;     

    Eigen::Matrix     <double,            </double,3, 3> m3x3_ret = m4x4_ret.block(0, 0, 3, 3);
    cout << "m3x3_to ret is: \n" << m3x3_ret << std::endl;
    // 使用自定义函数将旋转矩阵转换为欧拉角
    Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(m3x3_ret); // roll,pitch,yaw
    cout << "roll_2 pitch_2 yaw_2 = " << eulerAngle2[0] << " " << eulerAngle2[1]
         << " " << eulerAngle2[2] << endl << endl;


    pose.push_back(m4x4_ret(0,3));
    pose.push_back(m4x4_ret(1,3));
    pose.push_back(m4x4_ret(2,3));
    pose.push_back(eulerAngle2[0]*ARC_TO_DEG);
    pose.push_back(eulerAngle2[1]*ARC_TO_DEG);
    pose.push_back(eulerAngle2[2]*ARC_TO_DEG);

    return pose;
}