ros Python 四元数转欧拉角

时间:2020-04-29 01:15:05   收藏:0   阅读:253
Python 版本
def to_euler_angles(w, x, y, z):
    """w、x、y、z to euler angles"""
    angles = {‘pitch‘: 0.0, ‘roll‘: 0.0, ‘yaw‘: 0.0}
    r = math.atan2(2*(w*x+y*z),1-2*(x*x+y*y))
    p = math.asin(2*(w*y-z*z))
    y = math.atan2(2*(w*z+x*y),1-2*(z*z+y*y))

    angles[‘roll‘] = r*180/math.pi
    angles[‘pitch‘] = p*180/math.pi
    angles[‘yaw‘] = y*180/math.pi

    return angles

C++ 版本

ROS中quaternion四元数和RPY欧拉角转换


参考:
ros python 四元数 转 欧拉角

原文:https://blog.51cto.com/laok8/2491298

评论(0
© 2014 bubuko.com 版权所有 - 联系我们:wmxa8@hotmail.com
打开技术之扣,分享程序人生!