四元数转欧拉角中beta角值域问题

Question

求求大神帮助：四元数转欧拉角的一个问题，这里beta角的值域为 -pi/2 ~ pi/2 如果想变成-pi ~ pi的话应该怎么搞呢？

Answer 1

import PyKDL
import rospy
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from math import *
import threading
import os
import subprocess
import yaml
 
def quat_to_angle(quat):
  rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
  return rot.GetRPY()[]
def normalize_angle(angle):
  res = angle
  while res > pi:
    res -= 2.0*pi
  while res < -pi:
    res += 2.0*pi
  return res
 
class CalibrateRobot:
  def __init__(self):
    self.lock = threading.Lock()
    self.sub_imu = rospy.Subscriber('imu', Imu, self.imu_cb)
    self.last_imu_angle =
    self.imu_angle =
    while not rospy.is_shutdown():
      rospy.sleep(0.3)
      rospy.loginfo("imu_angle:"+str((self.imu_angle)*/3.1415926))
 
  def imu_cb(self, msg):
    with self.lock:
      angle = quat_to_angle(msg.orientation)
      self.imu_angle = angle
      self.imu_time = msg.header.stamp
 
def main():
  rospy.init_node('scan_to_angle')
  CalibrateRobot()
 
if __name__ == '__main__':
  main()

参考一下