SO3Hermite trajectory bug

[rtkg]

Hi, thanks for your work on this project.

I discovered some strange behavior when using SO3Hermite trajectory in that the angular velocities yielded by the .deriv() method don't correspond to the milestones I set when queried for the milestone times. Also, the curves look suspicious as shown in the minimal example below:

from klampt.model.trajectory import SO3HermiteTrajectory
from klampt.math import so3
import matplotlib.pyplot as plt
import numpy as np

times = np.arange(0, 5, 0.1).tolist()
milestones = []
velocities = []
milestones_plot = []
velocities_plot = []
for t in times:
    # rotation around x axis
    milestones.append(so3.from_rotation_vector([np.cos(t), 0, 0]))
    # corresponding angular velocity 
    velocities.append(so3.cross_product([-np.sin(t), 0, 0]))

    milestones_plot.append(np.cos(t))
    velocities_plot.append(-np.sin(t))

# Create the SO3Trajectory
traj = SO3HermiteTrajectory(times, milestones, velocities)

# re-sample the trajectory at a smaller rate
t = np.arange(0, times[-1], 0.01)
X_ = np.zeros(len(t))
V_ = np.zeros(len(t))

for i in range(len(t)):
    X_[i] = so3.rotation_vector(traj.eval(t[i]))[0]
    V_[i] = so3.deskew(traj.deriv(t[i]))[0]

plt.plot(times, milestones_plot, "*", label="X")
plt.plot(times, velocities_plot, "*", label="V")

plt.plot(t, X_, label="X_")
plt.plot(t, V_, label="V_")

plt.legend()
plt.show()

Maybe I'm just misunderstanding the meaning of 'outgoing velocities' in the argument list?

[krishauser]

Two things are happening: 1) a bug in the velocity interpolation that failed to scale by the inverse of the time step, and 2) a difference between derivative representations. There are three choices, intrinsic angular velocity, extrinsic angular velocity, If $w$ is the angular velocity at a rotation $R$ expressed in local coordinates, then the a) intrinsic representation of the derivative would be $[w]$, b) the extrinsic (world space angular velocity) representation would be $[Rw]$, and c) the matrix derivative representation would be $[Rw]R$. At some point I chose c) because it matches more closely with the naive derivative. However, on second thought I think it would be better to use an extrinsic representation since it's more intuitive to set up.

I just pushed a change to main to make these changes. You can use patch_a_pip_install.py to get the fixes. Let me know if this works.

[rtkg]

That looks much better, thanks for the fix. I also think that using the angular velocity vector expressed in the inertial frame makes sense.