A problem about the conversion from Euler angle to quaternion

edit

Hi, all. we are trying to construct a 3D scanning system to implement 3D mapping algorithm. Its hardware system is showed in following figure : .

A Hokuyo laser(UTM-30LX) is mounted on a plate which is rotated by a Dynamixel(MX-28R). The Dynamixel works in back_and_ forth way. Its rotation angles' range is between -1.57 and 1.57, i.e. [-90° 90°] .

The tf tree is showed in following figure : .

The frame_id "Laser" is the local scanning coordinate of Hokuyo laser. The frame_id "HN07_N101" is the local rotating(in back_and_forth way) coordinate of MX-28R. The frame_id "BaseLink" is the base_link coordinate of the system. The joint between "Laser" and "HN07_N101" is fixed. But there is a revolute joint between "HN07_N101" and "BaseLink".

The frame “HN07_N101” is rotated around its own "z" axis which is the blue axis in above figure. In the coordinate of "BaseLink", the origin of "HN07_N101" is (0.22375, 0, 0.1367), the initial orientation is (0.5, 0.5, 0.5, 0.5), as the left part of above figure shows.

As the Dynamixel rotating the Hokuyo laser, it is necessary to compute the coordinate transform between parent frame "BaseLink" and child frame "HN07_N101". The dynamixel motor sends JointState message which contain the current rotating angle around "Z" axis in radians.

However, when I run my project, the transform between pararent "BaseLink" and "HN07_N101" is *

not smooth.

*. Although there is no jump in the value of Dynamixel's msg, both the frame "HN07_N101"(including its child frame "Laser") and the 3D models of Hokuyo and Dynamixel alway flicking. The so called "flicking" is, while the frame "HN07_N101" is rotating, the green "y" axis and the red "x" axis suddenly switch positions with each other and recover.

I think the flicking 3D models displayed in rviz means that there is problem in the transfrom between "BaseLink" and "HN07_N101". But I don't know where the problem is.

Is the problem at the conversion from Euler angle (the current_pos in Dynamixel's msg) to the quaternion ( the variable "q" in above code) ?

Looking forwards any valuable advices. Thank you very much!