前言:
初步了解位置与位姿:
参考链接:机器人学——1.0-位置与姿态概述_「已注销」的博客-CSDN博客_位置和姿态
二维空间位姿表示与变换矩阵推导:
参考链接:机器人学——1.1-二维空间位姿描述_「已注销」的博客-CSDN博客
深蓝学院参考:
C++简单实现相对位姿转换
#include <iostream>
#include<Eigen/Dense>
using namespace std;
using namespace Eigen;
int main(int argc, char** argv)
{
// 机器人B在坐标系O中的坐标:
Vector3d B(3, 4, M_PI);
// 坐标系B到坐标O的转换矩阵:
Matrix3d TOB;
TOB << cos(B(2)), -sin(B(2)), B(0),
sin(B(2)), cos(B(2)), B(1),
0, 0, 1;
// 坐标系O到坐标B的转换矩阵:
Matrix3d TBO = TOB.inverse();
// 机器人A在坐标系O中的坐标:
Vector3d A(1, 3, -M_PI / 2);
// 求机器人A在机器人B中的坐标:
Vector3d BA;
Matrix3d TOA;
TOA << cos(A(2)), -sin(A(2)), A(0),
sin(A(2)), cos(A(2)), A(1),
0, 0, 1;
Matrix3d TBA = TBO * TOA;
cout << TBA << endl;
BA << TBA(0, 2),
TBA(1, 2),
atan2(TBA(1, 0), TBA(0, 0));
cout << "Your answer is BA: " << BA.transpose() << endl;
return 0;
}
运行结果:
[Running] cd "/home/lxy/project/Eigen/" && g++ demo01.cpp -o demo01 && "/home/lxy/project/Eigen/"demo01
-1.83697e-16 -1 2
1 -1.83697e-16 1
0 0 1
Your answer is BA: 2 1 1.5708
[Done] exited with code=0 in 7.399 seconds
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)