C++ vs2015 + Qt 5.9.8 + Eigen3 实现机器人位姿数据转换与旋转

一、环境说明

        开发环境：vs2015、Qt5.9.8、Eigen3

        开发语言：C++

二、理论知识

        关于机器人位姿数据的基本概念，以及各类位姿数据的转换公式参考前文，不再赘述。

机器人位姿数据形式转换与旋转矩阵总结（欧拉角、RPY、NOAP）

三、代码实现

        1.项目主函数 main.cpp

#include "calcu.h"
#include <QtWidgets/QApplication>

int main(int argc, char *argv[])
{
    QApplication a(argc, argv);
    calcu w;
    w.show();
    return a.exec();
}

        2.主界面 calcu.cpp

#include "calcu.h"
#include <iostream>
#include <istream>
#include <fstream>
#include <sstream>
#include "eigen3\Eigen\Dense"
#include "RpyTrans.h"
#include <cmath>
#include <iomanip>
#include <QDialog>
#include <QFileDialog>

using namespace std;
using namespace Eigen;

calcu::calcu(QWidget *parent)
    : QMainWindow(parent)
{
    ui.setupUi(this);
	calcu::init();
}

string dataPath;
RpyTrans rt;

void calcu::init()
{
	// 连接“PLC断开”按钮
	connect(ui.RpyTransBtn, &QPushButton::clicked, this, &calcu::test_RpyTrans);
	// 连接“传感器连接”按钮
	connect(ui.NOAPTransBtn, &QPushButton::clicked, this, &calcu::test_NOAPTrans);
	// 连接“选择文件”按钮
	connect(ui.chooseBtn, &QPushButton::clicked, this, &calcu::chooseFile);
	// 连接“坐标转化”按钮
	connect(ui.convBtn, &QPushButton::clicked, this, &calcu::KukaTo);
}

vector <double> TransRes;

int calcu::test_RpyTrans()
{
	RpyTrans rt;	
	double rx, ry, rz;
	rx = ui.RpyTrans1Text->text().toFloat();
	ry = ui.RpyTrans2Text->text().toFloat();
	rz = ui.RpyTrans3Text->text().toFloat();
	cout << "Rx_ori = " << (float)rx << " ; " << "Ry_ori = " << (float)ry << " ; " << "Rz_ori = " << (float)rz << " ; " << endl;

	vector <double> res;
	res.clear();
	res = rt.Rpy_to_NOAP(rx, ry, rz);
	cout << std::fixed;
	cout << (float)res[0] << " ; " << (float)res[1] << " ; " << (float)res[2] << " ; " << endl;
	cout << (float)res[3] << " ; " << (float)res[4] << " ; " << (float)res[5] << " ; " << endl;
	cout << (float)res[6] << " ; " << (float)res[7] << " ; " << (float)res[8] << " ; " << endl;
	TransRes.clear();
	TransRes = res;
	return 0;
}

int calcu::test_NOAPTrans()
{
	RpyTrans rt;
	vector <double> Rpy;
	vector <double> res;
	Rpy.clear();
	Rpy.resize(9);
	Rpy[0] = ui.NOAPTrans1Text->text().toFloat();
	Rpy[1] = ui.NOAPTrans2Text->text().toFloat();
	Rpy[2] = ui.NOAPTrans3Text->text().toFloat();
	Rpy[3] = ui.NOAPTrans4Text->text().toFloat();
	Rpy[4] = ui.NOAPTrans5Text->text().toFloat();
	Rpy[5] = ui.NOAPTrans6Text->text().toFloat();
	Rpy[6] = ui.NOAPTrans7Text->text().toFloat();
	Rpy[7] = ui.NOAPTrans8Text->text().toFloat();
	Rpy[8] = ui.NOAPTrans9Text->text().toFloat();
	res = rt.NOAP_to_Rpy(Rpy);
	cout << "Rx = " << (float)res[0] << " ; " << "Ry = " << (float)res[1] << " ; " << "Rz = " << (float)res[2] << " ; " << endl;
	return 0;
}

        calcu.h

#pragma once

#include <QtWidgets/QMainWindow>
#include "ui_calcu.h"

class calcu : public QMainWindow
{
    Q_OBJECT

public:
    calcu(QWidget *parent = Q_NULLPTR);

	void calcu::init();
	int calcu::test_RpyTrans();
	int calcu::test_NOAPTrans();
	void calcu::chooseFile();
	void calcu::KukaToJob();

private:
    Ui::calcuClass ui;
};

        3.功能实现模块 RpyTrans.cpp

#include "RpyTrans.h"
#include <vector>
#include <cmath>
#include <iostream>
using namespace std;

RpyTrans::RpyTrans()
{
}

RpyTrans::~RpyTrans()
{
}

vector <double> RpyTrans::Rpy_to_NOAP(double rx_, double ry_, double rz_)
{
	const double Pi = acos(-1);
	double rx = rx_ / 180.0 * Pi, ry = ry_ / 180.0 * Pi, rz = rz_ / 180.0 * Pi;
	vector <double> res;
	res.clear();
	res.resize(9);

	double Nx = cos(rz) * cos(ry);
	double Ny = sin(rz) * cos(ry);
	double Nz = -sin(ry);

	double Ox = (cos(rz) * sin(ry) * sin(rx)) - (sin(rz) * cos(rx));
	double Oy = (sin(rz) * sin(ry) * sin(rx)) + (cos(rz) * cos(rx));
	double Oz = cos(ry) * sin(rx);

	double Ax = (cos(rz) * sin(ry) * cos(rx)) + (sin(rz) * sin(rx));
	double Ay = (sin(rz) * sin(ry) * cos(rx)) - (cos(rz) * sin(rx));
	double Az = cos(ry) * cos(rx);

	res[0] = Nx; res[1] = Ny; res[2] = Nz;
	res[3] = Ox; res[4] = Oy; res[5] = Oz;
	res[6] = Ax; res[7] = Ay; res[8] = Az;

	return res;
}

vector <double> RpyTrans::NOAP_to_Rpy(vector <double> Rpy)
{
	double rx, ry, rz;
	Rpy.resize(9);
	const double Pi = acos(-1);

	ry = atan2(-Rpy[2], sqrt(pow(Rpy[0], 2) + pow(Rpy[1], 2)));
	rz = atan2(Rpy[1] / cos(ry), Rpy[0] / cos(ry));
	rx = atan2(Rpy[5] / cos(ry), Rpy[8] / cos(ry));

	double rx_, ry_, rz_;
	rx != 0 ? rx_ = rx / Pi * 180.0 : rx_ = 0;
	ry != 0 ? ry_ = ry / Pi * 180.0 : ry_ = 0;
	rz != 0 ? rz_ = rz / Pi * 180.0 : rz_ = 0;

	abs(rx_) < 0.00000001 ? rx_ = 0.0 : rx_ = rx_;
	abs(ry_) < 0.00000001 ? ry_ = 0.0 : ry_ = ry_;
	abs(rz_) < 0.00000001 ? rz_ = 0.0 : rz_ = rz_;

	vector <double> res;
	res.push_back(rx_);
	res.push_back(ry_);
	res.push_back(rz_);
	return res;
}

        RpyTrans.cpp.h

#pragma once
#include "RpyTrans.h"
#include <vector>
#include <cmath>

using namespace std;

class RpyTrans
{
public:
	RpyTrans();
	~RpyTrans();

	vector <double> Rpy_to_NOAP(double rx_, double ry_, double rz_);
	vector <double> RpyTrans::NOAP_to_Rpy(vector <double> Rpy);
};

        4.calcu.cpp的其他部分，用于计算原始KUKA机器人路径位姿数据，在转换到新的坐标系后的位姿数据。

void calcu::chooseFile()
{
	QString fullPath = QFileDialog::getOpenFileName();
	std::string filename, filePath;
	QFileInfo fileInfo = QFileInfo(fullPath);
	//文件名
	filename = fileInfo.fileName().toStdString();
	//绝对路径
	filePath = fileInfo.absolutePath().toStdString();
	ui.lineEditPath->setText(fileInfo.absolutePath() + "/" + fileInfo.fileName());
	dataPath.clear();
	dataPath = filePath + "/" + filename;
	std::cout << "file = " << dataPath << endl;
}

void calcu::KukaTo()
{
	const double Pi = acos(-1);
	double convert_Rx, convert_Ry, convert_Rz;
	double convert_Px, convert_Py, convert_Pz;
	convert_Px = ui.ConvertPxText->text().toDouble();
	convert_Py = ui.ConvertPyText->text().toDouble();
	convert_Pz = ui.ConvertPzText->text().toDouble();
	convert_Rx = ui.ConvertRxText->text().toDouble();
	convert_Ry = ui.ConvertRyText->text().toDouble();
	convert_Rz = ui.ConvertRzText->text().toDouble();
	vector<double> convertNOA = rt.Rpy_to_NOAP(convert_Rx, convert_Ry, convert_Rz);

	Eigen::MatrixXd m1(4, 4);
	m1 << convertNOA[0], convertNOA[1], convertNOA[2], 0,
		convertNOA[3], convertNOA[4], convertNOA[5], 0,
		convertNOA[6], convertNOA[7], convertNOA[8], 0,
		convert_Px, convert_Py, convert_Pz, 1;

	ifstream inFile1(dataPath);
	if (!inFile1)//判断是否打开成功
		cout << "open failed" << endl;

	//初始化vector对象
	string line, word;
	std::istringstream sin;

	ofstream out;
	out.open("RES_cache.txt", ios::trunc);

	int i = 10;
	while (getline(inFile1, line))//逐行读
	{			
		if (line.substr(0, 3) == "LIN")
		{
			sin.clear();
			vector <double> input;
			input.resize(6);
			sin.str(line.substr(4,line.size() - 11));
			while (std::getline(sin, word, ','))
			{
				if (word.substr(1, 1) == "X")
				{
					//cout << "X = " << word.substr(3, line.size() - 3) << endl;
					input[0] = stod(word.substr(3, line.size() - 3));
				}
				if (word.substr(1, 1) == "Y")
				{
					//cout << "Y = " << word.substr(3, line.size() - 3) << endl;
					input[1] = stod(word.substr(3, line.size() - 3));
				}
				if (word.substr(1, 1) == "Z")
				{
					//cout << "Z = " << word.substr(3, line.size() - 3) << endl;
					input[2] = stod(word.substr(3, line.size() - 3));
				}
				if (word.substr(1, 1) == "A")
				{
					//cout << "A = " << word.substr(3, line.size() - 3) << endl;
					input[3] = stod(word.substr(3, line.size() - 3));
				}
				if (word.substr(1, 1) == "B")
				{
					//cout << "B = " << word.substr(3, line.size() - 3) << endl;
					input[4] = stod(word.substr(3, line.size() - 3));
				}
				if (word.substr(1, 1) == "C")
				{
					//cout << "C = " << word.substr(3, line.size() - 3) << endl;
					input[5] = stod(word.substr(3, line.size() - 3));
				}
			}

			vector<double> inputNOA = rt.Rpy_to_NOAP(input[5], input[4], input[3]);
			Eigen::MatrixXd m2(4, 4);
			m2 << inputNOA[0], inputNOA[1], inputNOA[2], 0,
				inputNOA[3], inputNOA[4], inputNOA[5], 0,
				inputNOA[6], inputNOA[7], inputNOA[8], 0,
				input[0], input[1], input[2], 1;

			Eigen::MatrixXd mres1(4, 4);
			mres1 = m2 * m1;
			std::cout << "m3 = " << m3 << endl;

			Eigen::MatrixXd mc(4, 4);
			mc << 1, 0, 0, 0,
				0, 1, 0, 0,
				0, 0, 1, 0,
				0, 0, 0, 1;
			Eigen::MatrixXd mres2(4, 4);
			mres2 = mres1 * mc;

			vector<double> outputNOA;
			outputNOA.clear();
			outputNOA.push_back(mres2(0, 0));
			outputNOA.push_back(mres2(0, 1));
			outputNOA.push_back(mres2(0, 2));
			outputNOA.push_back(mres2(1, 0));
			outputNOA.push_back(mres2(1, 1));
			outputNOA.push_back(mres2(1, 2));
			outputNOA.push_back(mres2(2, 0));
			outputNOA.push_back(mres2(2, 1));
			outputNOA.push_back(mres2(2, 2));

			vector<double>outputRPY;
			outputRPY.clear();
			outputRPY.resize(3);
			outputRPY = rt.NOAP_to_Rpy(outputNOA);

			std::string resultline = "     " + to_string(i) + ": " + to_string(i - 9) + "  "
				+ "MOVL VL=300,OX=" + to_string(mres2(1, 0)).substr(0, to_string(mres2(1, 0)).length() )
				+ ",OY=" + to_string(mres2(1, 1)).substr(0, to_string(mres2(1, 1)).length() )
				+ ",OZ=" + to_string(mres2(1, 2)).substr(0, to_string(mres2(1, 2)).length() )
				+ ",AX=" + to_string(mres2(2, 0)).substr(0, to_string(mres2(2, 0)).length() )
				+ ",AY=" + to_string(mres2(2, 1)).substr(0, to_string(mres2(2, 1)).length() )
				+ ",AZ=" + to_string(mres2(2, 2)).substr(0, to_string(mres2(2, 2)).length() )
				+ ",PX=" + to_string(mres2(3, 0)).substr(0, to_string(mres2(3, 0)).length() )
				+ ",PY=" + to_string(mres2(3, 1)).substr(0, to_string(mres2(3, 1)).length() )
				+ ",PZ=" + to_string(mres2(3, 2)).substr(0, to_string(mres2(3, 2)).length() )
				+ ",E1=90.000000,MWORD=521,E2=0.0,E3=0.0,E4=0.0,E5=0.0,E6=0.0";

			std::cout << resultline << endl;
			out << resultline << endl;
			i++;
		}

	}

	inFile1.clear();
	inFile1.close();//关闭文件
	out.close();
}

        四、实现效果