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