UR協(xié)作機(jī)器人和Franka機(jī)器人導(dǎo)出的位姿為angleVector，三個(gè)量表示，在Matlab中angleVector是四個(gè)量表示。如果是三個(gè)量的表示推薦使用Python的scipy庫做轉(zhuǎn)換。

一、RPY_Euler_Quaternion_AngleAxis角度轉(zhuǎn)化：Matlab機(jī)器人工具箱

1.1 Quaternion轉(zhuǎn)Matrix (帶位置和姿態(tài))

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

二、Python的scipy

2.1 UR機(jī)器人rotvec轉(zhuǎn)換為RPY_rxryrz

UR機(jī)械臂通過30003端口發(fā)送過來的是rotation vector, 示教器Polyscope界面上Move標(biāo)簽欄中顯示的是RPY_rxryrz

scipy spatial transform官方幫助：

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)

2.2 川崎機(jī)器人Euler_ZYZ轉(zhuǎn)四元數(shù)Quaternion

三、Halcon姿態(tài)變換

3.1 Halcon 的姿態(tài)、齊次變換和四元數(shù)

create_pose函數(shù)是包含位置和姿態(tài)的，姿態(tài)格式為RPY_rx_ry_rz，注意輸入為角度。pose_to_hom_mat3d是RPY_rx_ry_rz轉(zhuǎn)為齊次矩陣。四元數(shù)的順序是w, x, y, z

robot_V_cam := [0.418906862152, 0.471104634456, 0.729862740299, 2.851, -1.241, -0.008]

* 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)

********************* right-left old zyx **********************************

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)

總結(jié)

以上是生活随笔為你收集整理的matlab中quat2angle,RPY_Euler_Quaternion_AngleAxis角度转化：Matlab、Python、Halcon版本的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。