get_pose_euler() doesn't return the correct angle if it surpasses 90/-90 degree

[kazoo-kmt]

When the controller's angle exceeds 90 degree, get_pose_euler() method doesn't return the correct angle value. For example, when it surpasses 1 degree, it returns 89 degree instead of 91 degree.

[kazoo-kmt]

For example, you can implement like this:

cosine_for_pitch = math.sqrt(pose_mat[0][0] ** 2 + pose_mat[1][0] ** 2)
is_singular = cosine_for_pitch < 10**-6
if not is_singular:
    yaw = math.atan2(pose_mat[1][0], pose_mat[0][0])
    pitch = math.atan2(-pose_mat[2][0], cosine_for_pitch)
    roll = math.atan2(pose_mat[2][1], pose_mat[2][2])
else:
    yaw = math.atan2(-pose_mat[1][2], pose_mat[1][1])
    pitch = math.atan2(-pose_mat[2][0], cosine_for_pitch)
    roll = 0

Also, it might be easier to use transforms3d package.

[pgambardella]

hello @kazoo-kmt. Thank you for your comment, it was really useful for an issue I had with Vive Tracker. Do you have tried this on vive tracker? I don't get why rotation axis change, it is different from controllers.

[lgbeno]

Regarding changes to the angle calculations, pull requests are welcome!

[pgambardella]

you got one! :)

[pgambardella]

nope, sorry, that didn't work.

[kazoo-kmt]

@paologambardella I only tried with Vive Controller.

[lvgeng]

It appears that this problem is still not solved yet. Using the pure openvr might be a reliable solution.