Add INERTIAL_FIXED_REPRESENTATION to velocity representations

src/adam/core/constants.py

@@ -4,3 +4,4 @@
 class Representations(IntEnum):
     BODY_FIXED_REPRESENTATION = auto()
     MIXED_REPRESENTATION = auto()
+    INERTIAL_FIXED_REPRESENTATION = auto()

src/adam/core/rbd_algorithms.py

@@ -233,6 +233,29 @@ def block_index(node_idx: int) -> int | None:
             )
             return M_mixed, Jcm_mixed
 
+        if (
+            self.frame_velocity_representation
+            == Representations.INERTIAL_FIXED_REPRESENTATION
+        ):
+            Xi = math.adjoint_inverse(base_transform)
+            In = math.factory.eye(batch_shape + (n,))
+            Z6n = math.factory.zeros(batch_shape + (6, n))
+            Zn6 = math.factory.zeros(batch_shape + (n, 6))
+
+            top = math.concatenate([Xi, Z6n], axis=-1)
+            bot = math.concatenate([Zn6, In], axis=-1)
+            X_to_inertial = math.concatenate([top, bot], axis=-2)
+
+            M_inertial = math.mtimes(
+                math.swapaxes(X_to_inertial, -2, -1), math.mtimes(M, X_to_inertial)
+            )
+            Xm = math.adjoint_mixed_inverse(base_transform)  # rotation-only, for cmm
+            wrench_transform = math.swapaxes(Xm, -2, -1)
+            Jcm_inertial = math.mtimes(
+                wrench_transform, math.mtimes(Jcm, X_to_inertial)
+            )
+            return M_inertial, Jcm_inertial
+
         raise ValueError(
             f"Unknown frame velocity representation: {self.frame_velocity_representation}"
         )
@@ -356,10 +379,33 @@ def jacobian(
 
             J_tot = LI_X_L @ J_tot @ X
             return J_tot
-        else:
-            raise NotImplementedError(
-                "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented"
+
+        if (
+            self.frame_velocity_representation
+            == Representations.INERTIAL_FIXED_REPRESENTATION
+        ):
+            w_H_L = w_H_B @ B_H_L
+            I_X_L = self.math.adjoint(w_H_L)
+
+            top_left = self.math.adjoint_inverse(base_transform)
+            top_right = self.math.factory.zeros(
+                joint_positions.shape[:-1] + (6, self.NDoF)
+            )
+            bottom_left = self.math.factory.zeros(
+                joint_positions.shape[:-1] + (self.NDoF, 6)
             )
+            bottom_right = self.math.factory.eye(
+                joint_positions.shape[:-1] + (self.NDoF,)
+            )
+            top = self.math.concatenate([top_left, top_right], axis=-1)
+            bottom = self.math.concatenate([bottom_left, bottom_right], axis=-1)
+            X = self.math.concatenate([top, bottom], axis=-2)
+
+            return I_X_L @ J_tot @ X
+
+        raise NotImplementedError(
+            "Only BODY_FIXED_REPRESENTATION, MIXED_REPRESENTATION and INERTIAL_FIXED_REPRESENTATION are implemented"
+        )
 
     def relative_jacobian(
         self, frame: str, joint_positions: npt.ArrayLike
@@ -384,10 +430,19 @@ def relative_jacobian(
             B_H_L = self.forward_kinematics(frame, eye, joint_positions)
             LI_X_L = self.math.adjoint_mixed(B_H_L)
             return LI_X_L @ self.joints_jacobian(frame, joint_positions)
-        else:
-            raise NotImplementedError(
-                "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented"
-            )
+
+        elif (
+            self.frame_velocity_representation
+            == Representations.INERTIAL_FIXED_REPRESENTATION
+        ):
+            eye = self.math.factory.eye(4)
+            B_H_L = self.forward_kinematics(frame, eye, joint_positions)
+            I_X_L = self.math.adjoint(B_H_L)
+            return I_X_L @ self.joints_jacobian(frame, joint_positions)
+
+        raise NotImplementedError(
+            "Only BODY_FIXED_REPRESENTATION, MIXED_REPRESENTATION and INERTIAL_FIXED_REPRESENTATION are implemented"
+        )
 
     def jacobian_dot(
         self,
@@ -425,21 +480,28 @@ def jacobian_dot(
         L_H_B = self.math.homogeneous_inverse(B_H_L)
 
         if self.frame_velocity_representation == Representations.MIXED_REPRESENTATION:
-            B_v_IB = self.math.mxv(
+            B_v_I = self.math.mxv(
                 self.math.adjoint_mixed_inverse(base_transform), base_velocity
             )
+        elif (
+            self.frame_velocity_representation
+            == Representations.INERTIAL_FIXED_REPRESENTATION
+        ):
+            B_v_I = self.math.mxv(
+                self.math.adjoint_inverse(base_transform), base_velocity
+            )
         elif (
             self.frame_velocity_representation
             == Representations.BODY_FIXED_REPRESENTATION
         ):
-            B_v_IB = base_velocity
+            B_v_I = base_velocity
         else:
             raise NotImplementedError(
                 "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented"
             )
 
-        v = self.math.mxv(self.math.adjoint(L_H_B), B_v_IB)
-        a = self.math.mxv(self.math.adjoint_derivative(L_H_B, v), B_v_IB)
+        v = self.math.mxv(self.math.adjoint(L_H_B), B_v_I)
+        a = self.math.mxv(self.math.adjoint_derivative(L_H_B, v), B_v_I)
 
         J_base_full = self.math.adjoint_inverse(B_H_L)
         J_base_cols = [J_base_full[..., :, i : i + 1] for i in range(6)]
@@ -487,30 +549,45 @@ def jacobian_dot(
         ):
             return J_dot
 
+        elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION:
+            adj = self.math.adjoint_mixed
+            adj_derivative = self.math.adjoint_mixed_derivative
+            adj_inverse = self.math.adjoint_mixed_inverse
+        elif (
+            self.frame_velocity_representation
+            == Representations.INERTIAL_FIXED_REPRESENTATION
+        ):
+            adj = self.math.adjoint
+            adj_derivative = self.math.adjoint_derivative
+            adj_inverse = self.math.adjoint_inverse
+        else:
+            raise NotImplementedError(
+                "Only BODY_FIXED_REPRESENTATION, MIXED_REPRESENTATION and INERTIAL_FIXED_REPRESENTATION are implemented"
+            )
         I_H_L = self.forward_kinematics(frame, base_transform, joint_positions)
-        LI_X_L = self.math.adjoint_mixed(I_H_L)
-        LI_v_L = self.math.mxv(LI_X_L, v)
-        LI_X_L_dot = self.math.adjoint_mixed_derivative(I_H_L, LI_v_L)
+        I_X_L = adj(I_H_L)
+        I_v_L = self.math.mxv(I_X_L, v)
+        I_X_L_dot = adj_derivative(I_H_L, I_v_L)
 
-        adj_mixed_inv = self.math.adjoint_mixed_inverse(base_transform)
+        adj_inv = adj_inverse(base_transform)
 
         Z_6xN = self.math.factory.zeros(batch_size + (6, self.NDoF))
         Z_Nx6 = self.math.factory.zeros(batch_size + (self.NDoF, 6))
         I_N = self.math.factory.eye(batch_size + (self.NDoF,))
 
-        top = self.math.concatenate([adj_mixed_inv, Z_6xN], axis=-1)
+        top = self.math.concatenate([adj_inv, Z_6xN], axis=-1)
         bottom = self.math.concatenate([Z_Nx6, I_N], axis=-1)
         X = self.math.concatenate([top, bottom], axis=-2)
 
         B_H_I = self.math.homogeneous_inverse(base_transform)
-        B_H_I_deriv = self.math.adjoint_mixed_derivative(B_H_I, -B_v_IB)
+        B_H_I_deriv = adj_derivative(B_H_I, -B_v_I)
 
         Z_NxN = self.math.factory.zeros(batch_size + (self.NDoF, self.NDoF))
         topd = self.math.concatenate([B_H_I_deriv, Z_6xN], axis=-1)
         bottomd = self.math.concatenate([Z_Nx6, Z_NxN], axis=-1)
         X_dot = self.math.concatenate([topd, bottomd], axis=-2)
 
-        return (LI_X_L_dot @ J @ X) + (LI_X_L @ J_dot @ X) + (LI_X_L @ J @ X_dot)
+        return (I_X_L_dot @ J @ X) + (I_X_L @ J_dot @ X) + (I_X_L @ J @ X_dot)
 
     def CoM_position(
         self, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike
@@ -559,19 +636,29 @@ def CoM_jacobian(
         ori_frame_velocity_representation = self.frame_velocity_representation
         self.frame_velocity_representation = Representations.MIXED_REPRESENTATION
         _, Jcm = self.crba(base_transform, joint_positions)
+        Xm = self.math.adjoint_mixed(base_transform)
+        In = self.math.factory.eye(batch_size + (self.NDoF,))
+        Z6n = self.math.factory.zeros(batch_size + (6, self.NDoF))
+        Zn6 = self.math.factory.zeros(batch_size + (self.NDoF, 6))
         if (
             ori_frame_velocity_representation
             == Representations.BODY_FIXED_REPRESENTATION
         ):
-            Xm = self.math.adjoint_mixed(base_transform)
-            In = self.math.factory.eye(batch_size + (self.NDoF,))
-            Z6n = self.math.factory.zeros(batch_size + (6, self.NDoF))
-            Zn6 = self.math.factory.zeros(batch_size + (self.NDoF, 6))
 
             top = self.math.concatenate([Xm, Z6n], axis=-1)
             bot = self.math.concatenate([Zn6, In], axis=-1)
             X = self.math.concatenate([top, bot], axis=-2)
             Jcm = Jcm @ X
+        elif (
+            ori_frame_velocity_representation
+            == Representations.INERTIAL_FIXED_REPRESENTATION
+        ):
+            Xi = self.math.adjoint_inverse(base_transform)
+            A = self.math.mtimes(Xm, Xi)
+            top = self.math.concatenate([A, Z6n], axis=-1)
+            bot = self.math.concatenate([Zn6, In], axis=-1)
+            X = self.math.concatenate([top, bot], axis=-2)
+            Jcm = Jcm @ X
         self.frame_velocity_representation = ori_frame_velocity_representation
         return Jcm[..., :3, :] / self._convert_to_arraylike(self.get_total_mass())
 
@@ -626,25 +713,37 @@ def rnea(
             tuple(base_transform.shape[:-2]) if base_transform.ndim > 2 else ()
         )
 
-        gravity_X = math.adjoint_mixed_inverse(base_transform)  # (...,6,6)
+        if (
+            self.frame_velocity_representation
+            == Representations.INERTIAL_FIXED_REPRESENTATION
+        ):
+            gravity_X = math.adjoint_inverse(base_transform)
+        else:
+            gravity_X = math.adjoint_mixed_inverse(base_transform)  # (...,6,6)
 
         if (
             self.frame_velocity_representation
             == Representations.BODY_FIXED_REPRESENTATION
         ):
-            B_X_BI = math.factory.eye(batch_shape + (6,))
+            B_X_I = math.factory.eye(batch_shape + (6,))
             transformed_acc = math.factory.zeros(batch_shape + (6,))
         elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION:
-            B_X_BI = math.adjoint_mixed_inverse(base_transform)
+            B_X_I = math.adjoint_mixed_inverse(base_transform)
             omega = base_velocity[..., 3:]
             vlin = base_velocity[..., :3]
             skew_omega_times_vlin = math.mxv(math.skew(omega), vlin)
-            top3 = -math.mxv(B_X_BI[..., :3, :3], skew_omega_times_vlin)
+            top3 = -math.mxv(B_X_I[..., :3, :3], skew_omega_times_vlin)
             bot3 = math.factory.zeros(batch_shape + (3,))
             transformed_acc = math.concatenate([top3, bot3], axis=-1)
+        elif (
+            self.frame_velocity_representation
+            == Representations.INERTIAL_FIXED_REPRESENTATION
+        ):
+            B_X_I = math.adjoint_inverse(base_transform)
+            transformed_acc = math.factory.zeros(batch_shape + (6,))
         else:
             raise NotImplementedError(
-                "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented"
+                "Only BODY_FIXED_REPRESENTATION, MIXED_REPRESENTATION and INERTIAL_FIXED_REPRESENTATION are implemented"
             )
 
         a0 = -(math.mxv(gravity_X, g)) + transformed_acc
@@ -667,7 +766,7 @@ def rnea(
 
             if idx == root_idx:
                 Xup[idx] = self._root_spatial_transform
-                v[idx] = math.mxv(B_X_BI, base_velocity)
+                v[idx] = math.mxv(B_X_I, base_velocity)
                 a[idx] = math.mxv(Xup[idx], a0)
                 continue
 
@@ -717,7 +816,7 @@ def rnea(
                         math.swapaxes(Xup[idx], -2, -1), Fi
                     )
 
-        tau_base = math.mxv(math.swapaxes(B_X_BI, -2, -1), tau_base)
+        tau_base = math.mxv(math.swapaxes(B_X_I, -2, -1), tau_base)
 
         if n > 0:
             zero_tau = math.factory.zeros(batch_shape + (1,))
@@ -806,19 +905,28 @@ def aba(
         joint_torques_eff = joint_torques + joint_ext
 
         if self.frame_velocity_representation == Representations.MIXED_REPRESENTATION:
-            B_X_BI = math.adjoint_mixed_inverse(base_transform)
+            B_X_I = math.adjoint_mixed_inverse(base_transform)
+            I_X_B = math.adjoint_mixed(base_transform)
+        elif (
+            self.frame_velocity_representation
+            == Representations.INERTIAL_FIXED_REPRESENTATION
+        ):
+            B_X_I = math.adjoint_inverse(base_transform)
+            I_X_B = math.adjoint(base_transform)
         elif (
             self.frame_velocity_representation
             == Representations.BODY_FIXED_REPRESENTATION
         ):
-            B_X_BI = math.factory.eye(batch_shape + (6,))
+            B_X_I = I_X_B = math.factory.eye(batch_shape + (6,))
         else:
             raise NotImplementedError(
-                "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented"
+                "Only BODY_FIXED_REPRESENTATION, MIXED_REPRESENTATION and INERTIAL_FIXED_REPRESENTATION are implemented"
             )
 
-        base_velocity_body = math.mxv(B_X_BI, base_velocity)
-        base_ext_body = math.mxv(B_X_BI, base_ext)
+        base_velocity_body = math.mxv(B_X_I, base_velocity)
+
+        B_star_BI = math.swapaxes(I_X_B, -2, -1)
+        base_ext_body = math.mxv(B_star_BI, base_ext)
 
         a0_input = math.mxv(math.adjoint_mixed_inverse(base_transform), g)
 
@@ -979,6 +1087,14 @@ def tile_batch(arr):
             base_vel_mixed = math.mxv(Xm, base_velocity_body)
             Xm_dot = math.adjoint_mixed_derivative(base_transform, base_vel_mixed)
             base_acc = math.mxv(Xm, a_base) + math.mxv(Xm_dot, base_velocity_body)
+        elif (
+            self.frame_velocity_representation
+            == Representations.INERTIAL_FIXED_REPRESENTATION
+        ):
+            X = math.adjoint(base_transform)
+            base_vel_inertial = math.mxv(X, base_velocity_body)
+            X_dot = math.adjoint_derivative(base_transform, base_vel_inertial)
+            base_acc = math.mxv(X, a_base) + math.mxv(X_dot, base_velocity_body)
         else:
             base_acc = a_base
 

tests/conftest.py

@@ -54,6 +54,7 @@ class RobotCfg:
 VELOCITY_REPRESENTATIONS = [
     Representations.MIXED_REPRESENTATION,
     Representations.BODY_FIXED_REPRESENTATION,
+    Representations.INERTIAL_FIXED_REPRESENTATION,
 ]
 
 ROBOTS = [
@@ -126,6 +127,8 @@ def tests_setup(request) -> RobotCfg | State:
         idyn_representation = idyntree.BODY_FIXED_REPRESENTATION
     elif velocity_representation == Representations.MIXED_REPRESENTATION:
         idyn_representation = idyntree.MIXED_REPRESENTATION
+    elif velocity_representation == Representations.INERTIAL_FIXED_REPRESENTATION:
+        idyn_representation = idyntree.INERTIAL_FIXED_REPRESENTATION
     else:
         raise ValueError(f"Unknown velocity representation: {velocity_representation}")
     kin_dyn.setFrameVelocityRepresentation(idyn_representation)