trinterp() returns invalid rotations in transform matrix

Aperta
#165 2 commenti 0 reazioni 1 assegnatario Vedi su GitHub

@tweng-bdai ci sta già lavorando.

Dal 19/2/2025.

  • #166 di @tweng-bdai — integrata

Valutazione

Questa issue non è ancora stata valutata.

Descrizione

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.

Lingua principale
Python
Stelle
641
Fork
102
Merge medio
1g 18h
PR unite (30g)
4

Guida per i contributori

Nessuna guida per i contributori indicizzata per questo repository

Come iniziare

  1. Leggi tutta la issue e poi la guida ai contributi del progetto.
  2. Commenta sulla issue per dire che te ne occupi tu — evita che due persone facciano lo stesso lavoro.
  3. Fai un fork del repository e lavora su un branch.
  4. Apri una pull request che faccia riferimento al numero della issue.

Altre issue di rai-opensource/spatialmath-python

Tutte le issue di rai-opensource/spatialmath-python

Issue simili

Altre issue su Python

Ricevi le nuove issue nella tua casella

Un breve riepilogo di issue GitHub adatte ai principianti.