将机械臂四元数模长修改成等于1

we-ai

于 2022-10-14 14:15:10 发布

阅读量533

点赞数

分类专栏： ros 文章标签： python

本文链接：https://blog.csdn.net/weixin_42314494/article/details/127319619

版权

ros 专栏收录该内容

8 篇文章 2 订阅

订阅专栏

经常通过rviz读取UR机械臂的四元数，输入到控制程序当中时，经常会模长与1的偏差太大而导致无法控制，以下函数可以修改四元数数值，将其优化到模长等于1

#!/usr/bin/env python
# -*- coding: utf-8 -*-


import numpy as np
import math



def calcu_ori():


    #pose_list = np.array([0.77325, -0.03945, -0.01062, 0.63279])
    #pose_list = np.array([0.55804, -0.1374, 0.04429, 0.81716])
    pose_list = np.array([0.91286, 0.023583, -0.40331, 0.058987])

    print("original_orientation: %.2f", pose_list)

    get_list = pose_list / np.linalg.norm(pose_list)
    # get_list = self.cal_orientation(pose_list)
    print("changed_orientation: %.2f", get_list)
    #sum_pose = get_list[0]*get_list[0] + get_list[1]*get_list[1] + get_list[2]*get_list[2] + get_list[3]*get_list[3]
    sum_pose = np.linalg.norm(get_list)
    print("mol:",sum_pose)



if __name__ == "__main__":
    calcu_ori()