【C++】RPY角度转为四元数程序

1.前言

在使用ROS moveit进行自定义轨迹规划时，末端位姿的角度需要使用四元数表达，四元数具有更高的计算效率，而我觉得RPY角度能够更加直观的反应出末端角度信息，故编写了这个小程序，同时也巩固了我对C++类的编程。

转换所用算法：https://blog.csdn.net/fantasysolo/article/details/80758319

目前测试都是正确的

2.代码

#include <iostream>
#include <cmath>
 
using namespace std;

#define PI 3.14

class RPY2qua
{
   public:
	double roll_deg;
	double pitch_deg;
	double yaw_deg;
 
      // 成员函数声明
    double getQua_x(void);
    double getQua_y(void);
    double getQua_z(void);
    double getQua_w(void);
};
 
// 成员函数定义
double RPY2qua::getQua_x(void)
{
	return sin(roll_deg*PI/180.0/2)*cos(yaw_deg*PI/180.0/2)*cos(pitch_deg*PI/180.0/2)-cos(roll_deg*PI/180.0/2)*sin(yaw_deg*PI/180.0/2)*sin(pitch_deg*PI/180.0/2);
}

double RPY2qua::getQua_y(void)
{
	return cos(roll_deg*PI/180.0/2)*sin(yaw_deg*PI/180.0/2)