RPY_Euler_Quaternion_AngleAxis角度转化：Matlab、Python、ROS、Halcon版本

UR协作机器人和Franka机器人导出的位姿为angleVector，三个量表示，在Matlab中angleVector是四个量表示。如果是三个量的表示推荐使用Python的scipy库做转换。

在线欧拉角-四元数-齐次矩阵转换

3D Rotation Converter

Rotation vector介绍

https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector

一、RPY_Euler_Quaternion_AngleAxis角度转化：Matlab机器人工具箱

1.1 Quaternion转Matrix （带位置和姿态）

% 位置及四元数xyzw转齐次矩阵
robotHtool =[0.10345922, -0.48407779,  0.29668114, -0.03533355,  0.09830182, -0.86382214,  0.49284846];
% w x y z
robotHtool_qua = Quaternion([robotHtool(7), robotHtool(4), robotHtool(5) , robotHtool(6)])   
robotHtool_matrix = transl(robotHtool(1), robotHtool(2), robotHtool(3)) *  robotHtool_qua.T 

% 位置及欧拉角xyz转齐次矩阵（默认单位：弧度）
robotHtool =[0.400422, -0.0262847, 0.365594, 3.1342, 0.0193, -1.6122];
robotHtool_rpy = rpy2tr([robotHtool(4), robotHtool(5) , robotHtool(6)]);  % 'deg' or 'zyx'  
T61 = transl(robotHtool(1), robotHtool(2), robotHtool(3)) * robotHtool_rpy;

二、Python的scipy 

2.1 UR机器人rotvec转换为RPY_rxryrz （RPY固定角左乘等价于EulerXYZ，相对角右乘）

UR机械臂通过30003端口发送过来的是rotation vector

Universal Robots - Explanation on robot orientation

https://www.universal-robots.com/articles/ur/programming/rpy-tofrom-rotation-vector/

而且示教器上点击笛卡尔坐标显示后台窗口的RPY值就是Euler_XYZ的角度或者弧度值（绕X坐标系转一个角，再绕新的Y转另一个角度） 

安装或升级scipy（≥1.2.0）：

Python3：pip3 install scipy==1.2.0

python2：pip install scipy==1.2.0

from scipy.spatial.transform import Rotation as R
r = R.from_rotvec([-0.001220983, 3.1162765, 0.038891915])
Euler_xyz = r.as_euler('xyz', degrees=False)

scipy spatial transform官方帮助：

scipy.spatial.transform.Rotation — SciPy v1.7.1 Manual

r1 = R.from_euler('zyx', [0, 0, 180], degrees=True)

r2 = R.from_quat(0, 0, 0, 1) (scalar-last format即x y z w模式)

r3 = R.from_matrix([[1, 0, 0], [0, -1, 0], [0, 0, -1]])

r1.as_matrix()

r2.as_rotvec()

r3.as_euler('xyz', degrees=True)

r_array = np.asarray(r)

  scipy中的四元数有时候会遇到正负号计算出来跟别的（matlab等）刚好相反的问题，按官方说法其实都是同一种姿态。（看具体平台，但是有时候确实会让结果出错，Just for your information.）

 2.2 ROS tf坐标转换

http://docs.ros.org/en/kinetic/api/tf/html/python/transformations.html

import tf.transformations

R = tf.transformations.euler_matrix(3.14, 0, 0,  axes='rxyz')

R[0][3]  = 1.0

R[1][3]  = 2.0

R[2][3]  = 3.0

scale, shear, angles, trans, persp = tf.transformations.decompose_matrix(R)
T0 = [1, 2, 3]
R1 = tf.transformations.compose_matrix(scale, shear, angles, T0, persp)

R3 = quaternion_matrix((1, 0, 0, 0))

tf.transformations.quaternion_from_matrix(R)

tf.transformations.inverse_matrix(R)

tf.transformations.euler_from_matrix(R, axes='rzyx')

=================== C++ =================== 

依赖项：tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>

四元数Quaternion —> R3x3 —> RPY

tf2::Quaternion quat(x,y,z,w);
double roll_rx, pitch_ry, yaw_rz;

tf2::Matrix3x3 m(q_pose);
m.getRPY(roll_rx, pitch_ry, yaw_rz);

--------------------------------------------------------------------------------
RPY->四元数 RPY ——> Quaternion
tf2::Quaternion q;
q.setRPY(0, 0, 0);
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();

RPY->旋转矩阵R3x3 ( RPY ——> R )
tf2::Matrix3x3 R;
R.setRPY();
R.setEulerZYX();

--------------------------------------------------------------------------------
旋转矩阵R3x3转四元数 ( R ——> quaternion)

#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>

tf2::Matrix3x3 R(1, 0, 0, 0, -1, 0, 0, 0, -1); // 行排列
tf2::Quaternion myQuaternion;
R.getRotation(myQuaternion);

旋转矩阵R3x3到RPY ( R ——> RPY)
double roll_rx, pitch_ry, yaw_rz;
R.getRPY(roll_rx, pitch_ry, yaw_rz);

 2.3 川崎机器人Euler_ZYZ转四元数Quaternion

import sys
import numpy as np

from scipy.spatial.transform import Rotation as R

f_result=open('/home/yake/Python_ws/kawasaki_convert_qua.txt','a')

with open(r'/home/yake/Python_ws/kawasaki.txt','r') as f:
      while True:
          line = f.readline()
          if line:
              list = line.split (",")
              li = []
              for i in list:
                li.append(float(i))
              
              result = []
              result.append( li[0]/1000.0)
              result.append( li[1]/1000.0)
              result.append( li[2]/1000.0)
              r = R.from_euler('zyz', [li[3], li[4], li[5]], degrees=True)
              euler_xyz = r.as_quat()
              result.append( euler_xyz[0] )
              result.append( euler_xyz[1])
              result.append( euler_xyz[2])
              result.append( euler_xyz[3])
              np.savetxt( f_result, result, fmt='%0.8f', delimiter=',',newline=',')
              f_result.write("\n")
           

          else:
              break  

f_result.close()

 三、Halcon姿态变换

3.1 Halcon 的姿态、齐次变换和四元数

create_pose函数是包含位置和姿态的，姿态格式为RPY_rx_ry_rz，注意输入为角度。pose_to_hom_mat3d是RPY_rx_ry_rz转为齐次矩阵。四元数的顺序是w, x, y, z

hom_mat3d_rotate_local 右乘

hom_mat3d_rotate 左乘

* Euler_XYZ
robot_V_cam := [0.7742, 0.01278, 0.820693648761, 3.1304,   -0.1557,    1.6486]

* Create pose use degrees.
create_pose (robot_V_cam[0], robot_V_cam[1], robot_V_cam[2], deg(robot_V_cam[3]), deg(robot_V_cam[4]), deg(robot_V_cam[5]), 'Rp+T', 'gba', 'point', Pose)
pose_to_hom_mat3d(Pose, robot_H_cam)
pose_to_quat(Pose, robot_Q_cam)
hom_mat3d_to_pose(robot_H_cam, pose_test1)

************************* xyz *********************************************
hom_mat3d_identity (HomMat3DIdentity)
hom_mat3d_translate (HomMat3DIdentity, robot_V_cam[0], robot_V_cam[1], robot_V_cam[2], HomMat3DTranslate)
hom_mat3d_rotate_local (HomMat3DTranslate, robot_V_cam[3], 'x', HomMat3DT_Rl)
hom_mat3d_rotate_local (HomMat3DT_Rl, robot_V_cam[4], 'y', HomMat3DT_Rl_Rm)
hom_mat3d_rotate_local (HomMat3DT_Rl_Rm, robot_V_cam[5], 'z', HomMat3D)
hom_mat3d_to_pose (HomMat3D, pose_test2)


********************* fixed frame zyx and then get xyz**********************************
hom_mat3d_identity (HomMat3DIdent)

hom_mat3d_rotate (HomMat3DIdent, robot_V_cam[5], 'z', 0, 0, 0, HomMat3DRotZ)
hom_mat3d_rotate (HomMat3DRotZ, robot_V_cam[4], 'y', 0, 0, 0, HomMat3DRotYZ)
hom_mat3d_rotate (HomMat3DRotYZ, robot_V_cam[3], 'x', 0, 0, 0, HomMat3DXYZ3)
hom_mat3d_translate(HomMat3DXYZ3, robot_V_cam[0], robot_V_cam[1], robot_V_cam[2], HomMat3Dzyx)
hom_mat3d_to_pose(HomMat3Dzyx, pose_test3)

********************* Euler_zyx to Matrix **********************************
robot_V_cam := [ 0.7742, 0.01278, 0.820693648761, -1.647717770053, -0.023198144506, 2.987195177743]
hom_mat3d_identity (HomMat3DIdent)
 
hom_mat3d_rotate (HomMat3DIdent, robot_V_cam[5], 'x', 0, 0, 0, HomMat3DRotZ)
hom_mat3d_rotate (HomMat3DRotZ, robot_V_cam[4], 'y', 0, 0, 0, HomMat3DRotYZ)
hom_mat3d_rotate (HomMat3DRotYZ, robot_V_cam[3], 'z', 0, 0, 0, HomMat3DXYZ3)
hom_mat3d_translate(HomMat3DXYZ3, robot_V_cam[0], robot_V_cam[1], robot_V_cam[2], HomMat3Dzyx)
************************ Euler_zyx to Matrix2 ************
create_pose (robot_V_cam[0], robot_V_cam[1], robot_V_cam[2], deg(robot_V_cam[5]), deg(robot_V_cam[4]), deg(robot_V_cam[3]), 'Rp+T', 'abg', 'point', robot_V_camPose)
hom_mat3d_to_pose(robot_V_camPose, pose_test4)