# 将表示姿态法向量的四元数转移到机械臂坐标系下
import numpy as np
from scipy.spatial.transform import Rotation
def transform_camera_quaternion_to_arm(camera_quaternion, hand_eye_matrix):
# Convert camera quaternion to rotation matrix
camera_rotation = Rotation.from_quat(camera_quaternion)
camera_rotation_matrix = camera_rotation.as_matrix()
# Transform camera rotation matrix to arm rotation matrix
arm_rotation_matrix = np.dot(hand_eye_matrix[:3, :3], camera_rotation_matrix)
# Convert arm rotation matrix to quaternion
arm_rotation = Rotation.from_matrix(arm_rotation_matrix)
arm_quaternion = arm_rotation.as_quat()
return arm_quaternion
# Example hand-eye matrix (replace with your own values)
# hand_eye_matrix = np.array([[ 7.03178404e-01, -7.10991045e-01, -5.64506277e-03, 2.17625306e-02],
# [ 7.11002245e-01, 7.03189737e-01, -3.22089035e-05, -6.90680181e-02],
# [ 3.99245045e-03, -3.99100370e-03, 9.99984066e-01, 4.01377097e-02],
# [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
hand_eye_matrix = np.array([[0.665037, -0.746393, 0.0249527, 0.442627],
[-0.746141, -0.662656, 0.0645062, -0.242624],
[-0.0316119, -0.0615173, -0.997605, 0.731147],
[0, 0, 0, 1]])
# Example camera quaternion (replace with your own values)
camera_quaternion = np.array([ 0.70347928, 0.66650123, -0.20174794, -0.14209425])
# Convert camera quaternion to arm quaternion
arm_quaternion = transform_camera_quaternion_to_arm(camera_quaternion, hand_eye_matrix)
print("Arm Quaternion:", arm_quaternion)
将相机坐标系下获取的四元数转移至机械臂末端坐标系下
最新推荐文章于 2024-01-24 18:47:49 发布