slerp_step returns NaN if the the angle between the two quaternions is zero

[S-Dafarra]

When dividing by cs.sin(angle) in https://github.com/ami-iit/liecasadi/blob/ea55d1f370be0fe95d2927e9824c7f3ddf90b5dc/src/liecasadi/quaternion.py#L126, there is a division by zero when the angle is zero.

One possibility could be to check if they are aligned before interpolating. Maybe using Casadi if_else https://github.com/casadi/casadi/wiki/L_113?

cc @Giulero

[Giulero]

Thanks @S-Dafarra for opening the issue!

Yeah, that's a solution! Something like

    dot = cs.dot(q1, q2)
    angle = cs.acos(dot)
    angle = cs.if_else(
        cs.fabs(angle) < 1e-6,
        1.0,
        angle,
    )
    return Quaternion(
        (cs.sin((1.0 - t) * angle) * q1 + cs.sin(t * angle) * q2) / cs.sin(angle)
    )

Another possible solution might be to add a small tolerance in this way

        dot = cs.dot(q1, q2)
        angle = cs.acos(dot)
        return Quaternion(
            (cs.sin((1.0 - t) * angle) * q1 + cs.sin(t * angle) * q2)
            / cs.sin(angle + cs.np.finfo(cs.np.float64).eps)
        )

What do you think can be the best solution? Maybe using the if_else operator may work in the case (although remote) that angle = cs.np.finfo(cs.np.float64).eps

[S-Dafarra]

For the first solution, I am not sure. I would expect that if the angle is zero, forcefully setting it to 1 may return an unexpected output.

Regarding the second, I think it is worth trying!

Anyhow, I suppose that in case the angle is zero, you don't really need to touch the output. What about

return Quaternion(
            cs.if_else(
                cs.fabs(angle) > 1e-6,
                (cs.sin((1.0 - t) * angle) * q1 + cs.sin(t * angle) * q2) / cs.sin(angle)
                q1,
            )
        )

[Giulero]

Makes sense! Let's go for the solution you proposed.

[traversaro]

Just FYI, it seems a common method to handle slerp for nearly identical quaternions is to fallback to linear interpolation (see https://github.com/mosra/magnum/commit/b2328f57cdf487d9500cbd27e4abcbb0e687afd4 or https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Quaternion.h?ref_type=heads#L800). However, I am not sure what are the advantage w.r.t. using the solution you used there.