fix: compute accel_x with analytical Jacobian derivative

Author: tandede

src/roboticstoolbox/robot/Dynamics.py

@@ -1256,21 +1256,23 @@ def accel_x(
         :returns: Operational space accelerations of the end-effector
         :rtype: ndarray(6,)
 
-        ``xdd = accel_x(q, qd, wrench)`` is the operational space acceleration
+        ``xdd = accel_x(q, xd, wrench)`` is the operational space acceleration
         due to ``wrench`` applied to the end-effector of a robot in joint
-        configuration ``q`` and joint velocity ``qd``.
+        configuration ``q`` and operational space velocity ``xd``.
 
         .. math::
 
-            \ddot{x} = \mathbf{J}(q) \mathbf{M}(q)^{-1} \left(
-                \mathbf{J}(q)^T w - \mathbf{C}(q)\dot{q} - \mathbf{g}(q)
+            \ddot{x} = \dot{\mathbf{J}}_a(q, \dot{q})\dot{q}
+                + \mathbf{J}_a(q) \mathbf{M}(q)^{-1} \left(
+                    \mathbf{J}_a(q)^T w - \mathbf{C}(q)\dot{q}
+                    - \mathbf{g}(q)
                 \right)
 
         **Trajectory operation**
 
-        If `q`, `qd`, torque are matrices (m,n) then ``qdd`` is a matrix (m,n)
-        where each row is the acceleration corresponding to the equivalent rows
-        of q, qd, wrench.
+        If ``q`` is a matrix (m,n), and ``xd`` and ``wrench`` are matrices
+        (m,6), then ``xdd`` is a matrix (m,6) where each row is the acceleration
+        corresponding to the equivalent rows of ``q``, ``xd``, and ``wrench``.
 
         .. rubric:: Notes
 
@@ -1294,7 +1296,7 @@ def accel_x(
         if q.shape[1] != 6:
             pinv = True
 
-        xdd = np.zeros((q.shape[0], self.n))
+        xdd = np.zeros((q.shape[0], 6))
 
         for k, (qk, xdk, wk) in enumerate(zip(q, xd, w)):
             Ja = self.jacob0_analytical(qk, representation=representation)
@@ -1322,16 +1324,10 @@ def accel_x(
 
             # xd = Ja qd
             # xdd = Jad qd + Ja qdd
-            #
-            # Ja = T J
-            # Jad = Td J + T Jd
-            # assume Td = 0, not sure how valid that is
-
-            # need Jacobian dot
             qdk = Ji @ xdk
-            Jd = self.jacob0_dot(qk, qdk, J0=Ja)
+            Jad = self.jacob0_dot(qk, qdk, representation=representation)
 
-            xdd[k, :] = T @ (Jd @ qdk + J @ qdd)
+            xdd[k, :] = Jad @ qdk + Ja @ qdd
 
         if q.shape[0] == 1:
             return xdd[0, :]

tests/test_DHRobot.py

@@ -1138,6 +1138,35 @@ def test_accel(self):
         nt.assert_array_almost_equal(qdd1[0, :], res, decimal=4)
         nt.assert_array_almost_equal(qdd1[1, :], res, decimal=4)
 
+    def test_accel_x(self):
+        puma = rp.models.DH.Puma560()
+        q = np.array([0.2, -0.7, 0.4, 0.3, 0.5, -0.2])
+        xd = np.array([0.1, -0.2, 0.15, 0.05, -0.1, 0.2])
+        wrench = np.array([1.0, -0.5, 0.25, 0.1, -0.2, 0.3])
+
+        xdd = puma.accel_x(q, xd, wrench)
+        xdd_traj = puma.accel_x(
+            np.vstack((q, q)),
+            np.vstack((xd, xd)),
+            np.vstack((wrench, wrench)),
+        )
+
+        nt.assert_allclose(
+            xdd,
+            [-1.645157, 4.489468, -4.854711, 3.818259, 6.472284, -2.428306],
+            rtol=1e-5,
+            atol=1e-6,
+        )
+        nt.assert_allclose(xdd_traj, np.vstack((xdd, xdd)))
+
+    def test_accel_x_redundant_robot_returns_cartesian_acceleration(self):
+        panda = rp.models.DH.Panda()
+
+        xdd = panda.accel_x(panda.qr, np.zeros(6), np.zeros(6))
+
+        self.assertEqual(xdd.shape, (6,))
+        self.assertTrue(np.isfinite(xdd).all())
+
     def test_inertia(self):
         puma = rp.models.DH.Puma560()
         puma.q = puma.qn