Orientation_error function in Franka ik

Would some body please elaborate on the formula that the following function is based on? are the outputs roll, pitch and yaw? is this formula (so the function as well) always valid?

def orientation_error(desired, current):
    cc = quat_conjugate(current)
    q_r = quat_mul(desired, cc)
    return q_r[:, 0:3] * torch.sign(q_r[:, 3]).unsqueeze(-1)

Edit: thanks for replays, but none of them answers me. To be more specific, this function takes in two quaternions and outputs a vector that is not quaternion (cause just returns the first three elements [0:3] and is not [0:4]). We know that the error of two quaternions is a quaternion as well. so why do the results have 3 elements instead of 4? Does it assume the inputs are normalized (I,j,k,w=1) and so the results are normalized (di, dj , dk, w=1) so just drop the w=1".

is this formula valid for small errors (when desired and current are close together) or always is valid?

Based on quaternions.

“current” is the result of the rotation at that point (guessed from the name).

This outputs “cc” is conjugate of “current” quaternion.(guessed from the function name).
“desired”, “current” should be normalized.

“desired” is the target rotation (guessed from the name).
“quat_mul (desired, cc)” is the product of the following quaternions.
quat_mul(desired, cc) = current・desired・cc
‘q_r’ is the amount of rotation.
‘r’ in ‘q_r’ is ‘r’ in ‘rotation’.(guessed)

In short, if there is no difference between “desired” and “current”, there is no rotation, and if there is a difference, rotation occurs.Here, that difference is an error.

do not understand.

Here, roll, pitch and yaw have nothing to do with.

I think it depends on the subject.

It’s based on quaternion representation in 3d space that you can rotate vector q_current to align with q_desired by using some quaternion vector. In this case is q_error

q_desired = q_error * q_current

By post multiply both side with conjugate of q_current. you will get q_error

q_error = q_desired * q_current_conj

The quaternion vector is equivalent with angle-axis representation as.

[qx, qy, qz, qw] = [ ax * sin(th/2), ay * sin(th/2), az * sin(th/2), cos(th/2) ]

where ax, ay, az are axis of rotation and “th” is angle to be rotate. If “orentation_error” is “th” we could just use this equation to solve it.

th = 2*atan2(sqrt(qx^2 + qy^2 + qz^2), qw)

However from this code

return q_r[:, 0:3] * torch.sign(q_r[:, 3]).unsqueeze(-1)

I think it defined “orentation_error” to be sum of absolute instead ( |qx| + |qy| + |qz| )