trinterp() returns invalid rotations in transform matrix

Open
#165 2 comments 0 reactions 1 assignee View on GitHub

@tweng-bdai is already working on this.

Since Feb 19, 2025.

  • #166 by @tweng-bdai — merged

Assessment

This issue has not been assessed yet.

Description

bug

the trinterp() method does not make sure quaternions are valid before converting them to transforms. It calls qslerp which can sometimes generate invalid quaternions.

Repro:

from spatialmath import SE3

se3_1 = SE3()
se3_1.t = np.array([0.5705748101710814, 0.29623210833184527, 0.10764106509086407])
se3_1.R = np.array([[ 0.2852875203191073  ,  0.9581330588259315  ,
    -0.024332536551692617],
   [ 0.9582072394229962  , -0.28568756930438033 ,
    -0.014882844564011068],
   [-0.021211248608609852, -0.019069722856395098,
    -0.9995931315303468  ]])
assert SE3.isvalid(se3_1.A)

se3_2 = SE3()
se3_2.t = np.array([0.5150284150005691 , 0.25796537207802533, 0.1558725490743694])
se3_2.R = np.array([[ 0.42058255728234184  ,  0.9064420651629983   ,
    -0.038380919906699236 ],
   [ 0.9070822373513454   , -0.4209501599465646   ,
    -0.0016665901233428627],
   [-0.01766712176680449  , -0.0341137119645545   ,
    -0.9992617912561634   ]])
assert SE3.isvalid(se3_2.A)

path_se3 = se3_1.interp(end=se3_2, s=15, shortest=False)
print(path_se3[2])
->   1         0         0         0         
     0         1         0         0         
     0         0         1         0         
     0         0         0         1    
print(path_se3[3])
->   0.3149    0.9487   -0.0275    0.5587    
     0.9489   -0.3153   -0.01222   0.288     
    -0.02027  -0.02225  -0.9995    0.118     
     0         0         0         1

The interp() method returns an SE3 object that holds the SE3 transformation matrices created from the interpolation: https://github.com/bdaiinstitute/spatialmath-python/blob/4c68fa923bc90047a0d79a2eab5c5a84b6cee7b7/spatialmath/baseposematrix.py#L449-L455.

However, there is a validity check in the SE3 object that will turn any invalid transforms into identity matrices.

A possible solution is to modify the trinterp() method to turn all quaternions into unit quaternions before converting them into rotation matrices: https://github.com/bdaiinstitute/spatialmath-python/blob/4c68fa923bc90047a0d79a2eab5c5a84b6cee7b7/spatialmath/base/transforms3d.py#L1697-L1700.

I am not sure if this is the only location in the spatialmath codebase that would benefit from this change.

Dominant language
Python
Stars
641
Forks
102
Avg merge
1d 18h
Merged PRs (30d)
4

Contributor guide

No contributing guide indexed for this repository

First steps

  1. Read the whole issue, then the project's contributing guide.
  2. Comment on the issue to say you are picking it up — it saves two people doing the same work.
  3. Fork the repository and make your change on a branch.
  4. Open a pull request that references the issue number.

More from rai-opensource/spatialmath-python

All issues in rai-opensource/spatialmath-python

Similar issues

More Python issues

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.