Closed S-Dafarra closed 9 months ago
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
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,
)
)
Makes sense! Let's go for the solution you proposed.
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.
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