python 使用PyKDL 四元數轉歐拉角

 

安裝：

sudo apt-get install ros-indigo-kdl-parser-py

使用：

import PyKDL
import math

def quat_to_angle(quat):
  rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
  return rot.GetRPY()[2]

def normalize_angle(angle):
    res = angle
    while res > math.pi:
        res -= 2.0*math.pi
    while res < -math.pi:
        res += 2.0*math.pi
    return res



https://github.com/gt-ros-pkg/hrl-kdl
https://www.programcreek.com/python/example/99836/sensor_msgs.msg.Imu