DynamicsMixin.accel_x references undefined variables (NameError at runtime)

[petercorke]

Migrated from tech-debt.md (deleted, see repo history via git log -- tech-debt.md).

DynamicsMixin.accel_x (Dynamics.py ~L1294) computes operational-space forward dynamics but references variables T and J that are never defined in scope -- would raise NameError: name 'T' is not defined if called.

From the surrounding comments:

# Ja = T J       (T maps geometric -> analytical Jacobian, J is self.jacob0(qk))
# Jad = Td J + T Jd
# assume Td = 0  -> Jad = T Jd

The broken line:

xdd[k, :] = T @ (Jd @ qdk + J @ qdd)   # T and J undefined

Likely correct form (given the Td=0 assumption):

J0 = self.jacob0(qk)
T  = Ja @ np.linalg.pinv(J0)            # analytical-to-geometric transform
xdd[k, :] = T @ Jd @ qdk + Ja @ qdd     # = T Jd qd + Ja qdd

Action: fix the implementation and add a test that exercises accel_x against a known robot (e.g. Puma560).

[tahazarif10]

I don't think T needs to be reconstructed here

jacob0_dot already supports the analytical representation directly, so this could just be

Jad = self.jacob0_dot(qk, qdk, representation=representation)
xdd[k] = Jad @ qdk + Ja @ qdd

That also avoids the Td = 0 assumption

[tahazarif10]

There's another non-6DOF issue here

xdd is allocated with shape (m, self.n) but operational acceleration is always 6D

Since this function explicitly enables pinv when n != 6, I think this should be (m, 6)