欧拉角和四元数之间的python转换代码

1、欧拉角转换到四元数,转换顺序是ZYX

python 复制代码
def rpy2quaternion(roll, pitch, yaw):
    x=sin(roll/2)*cos(pitch/2)*cos(yaw/2)-cos(roll/2)*sin(pitch/2)*sin(yaw/2)
    y=cos(roll/2)*sin(pitch/2)*cos(yaw/2)+sin(roll/2)*cos(pitch/2)*sin(yaw/2)
    z=cos(roll/2)*cos(pitch/2)*sin(yaw/2)-sin(roll/2)*sin(pitch/2)*cos(yaw/2)
    w=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(pitch/2)*sin(yaw/2)
    return np.array([w, x, y, z ])

2、四元数转换到欧拉角,转换顺序是ZYX

python 复制代码
def quart_to_rpy(w,x, y, z):
    roll = atan2(2 * (w * x + y * z), 1 - 2 * (x ** 2 + y ** 2))
    pitch = asin(2 * (w * y - z * x))
    yaw = atan2(2 * (w * z + x * y), 1 - 2 * (y ** 2 + z ** 2))

    return np.array([roll, pitch, yaw])

实验的时候发现,欧拉角转到四元数,再从四元数转回来,有些角度存在问题。

相关推荐
Java展博6 分钟前
[Go 后端转场] 从 try-catch 到 if err != nil:写惯 Java 的人第一次上手最别扭的 4 件事
java·开发语言·golang
SEO_juper13 分钟前
Node.js 全站 SEO 体检报告生成器:一次查完标题、描述、canonical、H1、图片
开发语言·前端·node.js·html·bash
典学长编程25 分钟前
LangGraph 快速入门:从 StateGraph 到 ReAct Agent
python·有向图·无向图·langgraph·react agent·stategraph
风落青芜1 小时前
数据结构:堆的概念与实现
c语言·开发语言·数据结构·算法
用户019027581613 小时前
AlphaFeed 查不到代码返回空?非法/缺后缀 symbol 的行为与处理
python
isfox3 小时前
Python 单元测试:别再用 print 调试了,写个测试一劳永逸
python
用户298698530143 小时前
在 Python 中把 Word 文档转换为 JSON 的三种实现方式
python·json·api
用户0332126663674 小时前
使用 Python 删除 Excel 中的空白行或空白列
python·excel
databook4 小时前
时间序列特征,哪些当时就能算出来
python·数据分析
kevin_org4 小时前
LangChain入门学习(四)
python·学习·langchain·fastapi