naren200
diff --git a/‎roboticstoolbox/robot/Robot.py
Lines changed: 142 additions & 1 deletion b/‎roboticstoolbox/robot/Robot.py
Lines changed: 142 additions & 1 deletion
@@ -37,7 +37,13 @@
 from swift import Swift
 from spatialgeometry import Shape, CollisionShape, Cylinder, SceneNode
 
-from spatialmath import SE3
+from spatialmath import (
+    SE3,
+    SpatialAcceleration,
+    SpatialVelocity,
+    SpatialInertia,
+    SpatialForce,
+)
 import spatialmath.base as smb
 from spatialmath.base.argcheck import (
     isvector,
@@ -4404,3 +4410,138 @@ def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray):
                         bin = np.concatenate((bin, l_bin))
 
         return Ain, bin
+
+    # --------------------------------------------------------------------- #
+    # --------- Dynamics Methods ------------------------------------------ #
+    # --------------------------------------------------------------------- #
+
+    def rne(
+        self,
+        q: NDArray,
+        qd: NDArray,
+        qdd: NDArray,
+        symbolic: bool = False,
+        gravity: Union[ArrayLike, None] = None,
+    ):
+        """
+        Compute inverse dynamics via recursive Newton-Euler formulation
+
+        ``rne_dh(q, qd, qdd)`` where the arguments have shape (n,) where n is
+        the number of robot joints.  The result has shape (n,).
+
+        ``rne_dh(q, qd, qdd)`` where the arguments have shape (m,n) where n
+        is the number of robot joints and where m is the number of steps in
+        the joint trajectory.  The result has shape (m,n).
+
+        ``rne_dh(p)`` where the input is a 1D array ``p`` = [q, qd, qdd] with
+        shape (3n,), and the result has shape (n,).
+
+        ``rne_dh(p)`` where the input is a 2D array ``p`` = [q, qd, qdd] with
+        shape (m,3n) and the result has shape (m,n).
+
+        Parameters
+        ----------
+        q
+            Joint coordinates
+        qd
+            Joint velocity
+        qdd
+            Joint acceleration
+        symbolic
+            If True, supports symbolic expressions
+        gravity
+            Gravitational acceleration, defaults to attribute
+            of self
+
+        Returns
+        -------
+        tau
+            Joint force/torques
+
+        Notes
+        -----
+        - This version supports symbolic model parameters
+        - Verified against MATLAB code
+
+        """
+
+        n = self.n
+
+        # allocate intermediate variables
+        Xup = SE3.Alloc(n)
+        Xtree = SE3.Alloc(n)
+
+        v = SpatialVelocity.Alloc(n)
+        a = SpatialAcceleration.Alloc(n)
+        f = SpatialForce.Alloc(n)
+        I = SpatialInertia.Alloc(n)  # noqa
+        s = []  # joint motion subspace
+
+        if symbolic:
+            Q = np.empty((n,), dtype="O")  # joint torque/force
+        else:
+            Q = np.empty((n,))  # joint torque/force
+
+        # TODO Should the dynamic parameters of static links preceding joint be
+        # somehow merged with the joint?
+
+        # A temp variable to handle static joints
+        Ts = SE3()
+
+        # A counter through joints
+        j = 0
+
+        # initialize intermediate variables
+        for link in self.links:
+            if link.isjoint:
+                I[j] = SpatialInertia(m=link.m, r=link.r)
+                if symbolic and link.Ts is None:
+                    Xtree[j] = SE3(np.eye(4, dtype="O"), check=False)
+                else:
+                    Xtree[j] = Ts * SE3(link.Ts, check=False)
+
+                if link.v is not None:
+                    s.append(link.v.s)
+
+                # Increment the joint counter
+                j += 1
+
+                # Reset the Ts tracker
+                Ts = SE3()
+            else:
+                # TODO Keep track of inertia and transform???
+                Ts *= SE3(link.Ts, check=False)
+
+        if gravity is None:
+            a_grav = -SpatialAcceleration(self.gravity)
+        else:
+            a_grav = -SpatialAcceleration(gravity)
+
+        # forward recursion
+        for j in range(0, n):
+            vJ = SpatialVelocity(s[j] * qd[j])
+
+            # transform from parent(j) to j
+            Xup[j] = SE3(self.links[j].A(q[j])).inv()
+
+            if self.links[j].parent is None:
+                v[j] = vJ
+                a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qdd[j])
+            else:
+                jp = self.links[j].parent.jindex  # type: ignore
+                v[j] = Xup[j] * v[jp] + vJ
+                a[j] = Xup[j] * a[jp] + SpatialAcceleration(s[j] * qdd[j]) + v[j] @ vJ
+
+            f[j] = I[j] * a[j] + v[j] @ (I[j] * v[j])
+
+        # backward recursion
+        for j in reversed(range(0, n)):
+
+            # next line could be dot(), but fails for symbolic arguments
+            Q[j] = sum(f[j].A * s[j])
+
+            if self.links[j].parent is not None:
+                jp = self.links[j].parent.jindex  # type: ignore
+                f[jp] = f[jp] + Xup[j] * f[j]
+
+        return Q