Closed tdamsma closed 4 months ago
Thanks for the detailed problem statement. The discrepancy is in the rotational part of the transforms, and that is ultimately performed by base/quaternions/qslerp()
. For a rotation about an axis there are always two ways to rotate and typically one is longer than the other. By default qslerp()
doesn't choose the shortest, but it looks like whether it is shortest or longest depends on the initial rotation. If I hack qslerp()
to have shortest=True
by default, then your code runs fine. The implication is that the SciPy SLERP function does this by default, and it has no options to control its behaviour in this regard.
The issue is how to resolve this. The shortest
option is not passed down from the .interp()
method you are calling but perhaps it should be.
@jcao-bdai we can change the default, would cause change in behaviour (but not wrong behaviour) for existing users or pass the option from pose3d/interp()
through base/transforms3d/trinterp()
to base/quaternions/qslerp()
. While we are it, I'd like to add an option to base/quaternions/r2q()
to enforce a quaternion with a non-negative scalar part.
@tdamsma @petercorke the second option seems more user-friendly, along that line, i can put together a quick PR which
shortest
argument to pose3d.interp(..., shortest=True)
through base.transform3d.trinterp(..., shortest=True)
to base.quaternion.qslerp(..., shortest=False)
(not changing default in the last one);base.transform3d.trrinterp2(..., shortest=True)
which a shortest angle logic will be added, for 2D case N==2 in pose3d.interp()
;shortest
argument to base.quaternions.r2q()
which ensures returning q with non-negative scalar part.please note that my response may be slow till next week (05/13) as i'm taking the upcoming week off.
~a draft PR is created at https://github.com/bdaiinstitute/spatialmath-python/pull/123~ ~i will also add some unit tests accordingly.~
please review https://github.com/bdaiinstitute/spatialmath-python/pull/123 (tests have been added)
updates have been merged. thank you all for looking into this issue!
I found using the interp function of the spatialmath toolbox that the interpolation results depend on the initial rotation frames. On further investigation I also found difference between the SE3.interp function and an implementation using scipy.spatial.transform.Slerp.
Steps to Reproduce
Demo code
The code below provides both examples where SE3.interp is not working as expected, and an alternative implementation based using scipy.
Output / Reported inconsistencies