将机器人六轴坐标转为4*4矩阵(Opencv/C++)

已知机器人六轴坐标x,y,z,rx,ry,rz,其中xyz表示空间位置坐标,rx,ry,rz是欧拉角;

需要将这六个值转为4*4的矩阵以便后续其它处理运算。

代码如下:

复制代码
#include <opencv2/core.hpp>
#include <iostream>
#include <cmath>

using namespace cv;
using namespace std;

// 将 xyz 和欧拉角转换为 4x4 矩阵
Mat xyzEulerToMatrix(double x, double y, double z, double rx, double ry, double rz) {
  // 转换为弧度
  rx = rx * CV_PI / 180.0;
  ry = ry * CV_PI / 180.0;
  rz = rz * CV_PI / 180.0;

  // 构建旋转矩阵
  Mat rotX = (Mat_<double>(3, 3) <<
    1.0, 0.0, 0.0,
    0.0, cos(rx), -sin(rx),
    0.0, sin(rx), cos(rx)
  );

  Mat rotY = (Mat_<double>(3, 3) <<
    cos(ry), 0.0, sin(ry),
    0.0, 1.0, 0.0,
    -sin(ry), 0.0, cos(ry)
  );

  Mat rotZ = (Mat_<double>(3, 3) <<
    cos(rz), -sin(rz), 0.0,
    sin(rz), cos(rz), 0.0,
    0.0, 0.0, 1.0
  );

  // 构建平移矩阵
  Mat translation = (Mat_<double>(4, 4) <<
    1.0, 0.0, 0.0, x,
    0.0, 1.0, 0.0, y,
    0.0, 0.0, 1.0, z,
    0.0, 0.0, 0.0, 1.0
  );

  // 旋转矩阵相乘
  Mat rotation = rotZ * rotY * rotX;

  // 合并旋转和平移
  Mat matrix = (Mat_<double>(4, 4) <<
    rotation.at<double>(0, 0), rotation.at<double>(0, 1), rotation.at<double>(0, 2), x,
    rotation.at<double>(1, 0), rotation.at<double>(1, 1), rotation.at<double>(1, 2), y,
    rotation.at<double>(2, 0), rotation.at<double>(2, 1), rotation.at<double>(2, 2), z,
    0.0, 0.0, 0.0, 1.0
  );

  return matrix;
}

int main() {
  // 定义参数
  double x = 111.0;
  double y = 222.0;
  double z = 333.0;
  double rx = 60.0;
  double ry = 30.0;
  double rz = -90.0;

  // 计算 4x4 矩阵
  Mat matrix = xyzEulerToMatrix(x, y, z, rx, ry, rz);

  // 打印结果
  cout << "4x4 矩阵:" << endl << matrix << endl;

  return 0;
}
相关推荐
民乐团扒谱机6 小时前
Rakazo 智能清洁机器人深度评测报告
机器人
feasibility.7 小时前
英伟达把物理AI的3D模型“应用商店”开放了:2575个SimReady仿真资产,总计约472GB,可让agent装配机器人训练场景
人工智能·3d·机器人·仿真·建模·具身智能·英伟达
eggwyw8 小时前
C++ union 的一种妙用法
开发语言·c++
ShineWinsu10 小时前
对于Qt:QHBoxLayout常见属性以及方法的解析
c++·qt·面试·gui·客户端·布局管理器·水平布局器
小七在进步11 小时前
string的实现(1)
c++·笔记·visual studio
wangxiaoxuan21711 小时前
机器人软件如何分层:驱动、算法、能力、动作与业务
机器人·科沃斯八界
kyle~13 小时前
模型预测控制(MPC)
算法·机器人·控制·传感器
敲上瘾13 小时前
Qt核心机制:信号与槽详解
开发语言·c++·qt·ui·qt6.3
飞Link14 小时前
灵巧手是什么?从机械结构、驱动方式到控制原理,认识机器人的精细操作能力
人工智能·python·机器人
牢姐与蒯14 小时前
cpp算法之哈希
c++·算法·哈希算法