将相机坐标系下获取的四元数转移至机械臂末端坐标系下

最新推荐文章于 2024-07-20 17:12:48 发布

秋冬无暖阳123

最新推荐文章于 2024-07-20 17:12:48 发布

阅读量288

点赞数 1

分类专栏：深度相机 Python相关文章标签： python

本文链接：https://blog.csdn.net/ZNC1998/article/details/133959031

版权

Python相关同时被 2 个专栏收录

14 篇文章 3 订阅

订阅专栏

深度相机

11 篇文章 0 订阅

订阅专栏

# 将表示姿态法向量的四元数转移到机械臂坐标系下
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)