trinterp() returns invalid rotations in transform matrix

Open

#165 opened on Feb 19, 2025 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

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

Similar issues

create_predictable_zip member order differs on Windows for directory input Open

bug

Difficulty 2/5 1-3 hours Newbie friendliness 90/100

learningequality/ricecooker#747 · Opened Sep 23, 2026
[Anon] Mechanicum: skitarii as mechanicum allies rule Open

Difficulty 2/5 1-3 hours Newbie friendliness 68/100

BSData/horus-heresy-3rd-edition#3171 · Opened Sep 23, 2026
CHANGELOG.md entries are too long to maintain, and their length is what makes every PR conflict expensive Open

enhancement

Difficulty 2/5 1-3 hours Newbie friendliness 72/100

lanl/PyBNF#809 · Opened Sep 23, 2026
[Bug]: PrevNextNodePostprocessor loses retrieved scores when neighbor windows overlap Open

Difficulty 2/5 1-3 hours Newbie friendliness 76/100

run-llama/llama_index#23199 · Opened Sep 23, 2026
Cannot export point cloud without radius attribute Open

Difficulty 2/5 1-3 hours Newbie friendliness 84/100

KhronosGroup/glTF-Blender-IO#2769 · Opened Sep 23, 2026

More Python issues