trinterp() returns invalid rotations in transform matrix

Offen

#165 erstellt am 19.2.2025 2 Kommentare 0 Reaktionen 1 zugewiesene Person Auf GitHub ansehen

@tweng-bdai arbeitet bereits daran.

Seit 19.2.2025.

#166 von @tweng-bdai — gemerged

Bewertung

Dieses Issue wurde noch nicht bewertet.

Beschreibung

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.

Vorherrschende Sprache: Python
Sterne: 641
Forks: 102
Ø Merge: 1 T. 18 Std.
Gemergte PRs (30 T.): 4

Beitragsleitfaden

Für dieses Repository ist kein Beitragsleitfaden indexiert

Erste Schritte

Lesen Sie das ganze Issue und danach den Beitragsleitfaden des Projekts.
Schreiben Sie ins Issue, dass Sie es übernehmen — das erspart doppelte Arbeit.
Forken Sie das Repository und arbeiten Sie in einem Branch.
Öffnen Sie einen Pull Request, der die Issue-Nummer nennt.

Mehr aus rai-opensource/spatialmath-python

Documentation outdated for `printline()` Offen

enhancement

Schwierigkeit 2/5 1-3 Stunden Anfängerfreundlichkeit 65/100

rai-opensource/spatialmath-python#32 · 4 Kommentare · Erstellt am 27.10.2021
AngleAxis and getunit error with Numpy 2.4 Offen

Schwierigkeit 2/5 1-3 Stunden Anfängerfreundlichkeit 50/100

rai-opensource/spatialmath-python#177 · Erstellt am 04.2.2026
animated frames do not match the same frames when plotted Offen

Schwierigkeit 3/5 1-2 Tage Anfängerfreundlichkeit 42/100

rai-opensource/spatialmath-python#171 · Erstellt am 24.3.2025
Type-check spatialmath Offen

rai-opensource/spatialmath-python#143 · 8 Kommentare · 1 zugewiesene Person · Erstellt am 28.10.2024
spatialmath import error on Ubuntu 22.04 Offen

under investigation

Schwierigkeit 3/5 1-2 Tage Anfängerfreundlichkeit 25/100

rai-opensource/spatialmath-python#78 · 2 Kommentare · Erstellt am 28.5.2023

Alle Issues in rai-opensource/spatialmath-python