Plotting quaternions

[imranlivt]

The orientation in most of ros messages is of type quaternions. It doesnot make sense to plot x,y,z,w values. Is there any way I can convert the quaternion to euler angles, roll, pitch, yaw and plot them in foxglove. It seems to be qn important feature , yet i found no information regarding that. Am i missing something

[rgov]

You could probably use a Node Playground script to implement a transformation to roll, pitch, yaw.

According to this StackOverflow answer the calculation should be like this:

yaw = Math.atan2(2.0*(qy*qz + qw*qx), qw*qw - qx*qx - qy*qy + qz*qz);
pitch = Math.asin(-2.0*(qx*qz - qw*qy));
roll = Math.atan2(2.0*(qx*qy + qw*qz), qw*qw + qx*qx - qy*qy - qz*qz);

In the past I've used this Python function (the origin is lost to me) which could be translated to JavaScript fairly literally:

def euler_from_quaternion(q):
    """
    Convert a quaternion into euler angles (roll, pitch, yaw)
    roll is rotation around x in radians (counterclockwise)
    pitch is rotation around y in radians (counterclockwise)
    yaw is rotation around z in radians (counterclockwise)
    """
    t0 = +2.0 * (q.w * q.x + q.y * q.z)
    t1 = +1.0 - 2.0 * (q.x * q.x + q.y * q.y)
    roll_x = math.atan2(t0, t1)
    
    t2 = +2.0 * (q.w * q.y - q.z * q.x)
    t2 = max(-1.0, min(+1.0, t2))
    pitch_y = math.asin(t2)
    
    t3 = +2.0 * (q.w * q.z + q.x * q.y)
    t4 = +1.0 - 2.0 * (q.y * q.y + q.z * q.z)
    yaw_z = math.atan2(t3, t4)
    
    return Vector3(x=roll_x, y=pitch_y, z=yaw_z)

If you come up with a script that works, please share it, since this has come up on Slack in the past.

[defunctzombie]

We're happy to add a function to the node playground built-in utilities or examples if someone wants to take the initiative and write one up.

https://github.com/foxglove/studio/tree/main/packages/studio-base/src/players/UserNodePlayer/nodeTransformerWorker/typescript/userUtils