diff --git a/src/adam/core/constants.py b/src/adam/core/constants.py index 44ef2d35..449848e5 100644 --- a/src/adam/core/constants.py +++ b/src/adam/core/constants.py @@ -4,3 +4,4 @@ class Representations(IntEnum): BODY_FIXED_REPRESENTATION = auto() MIXED_REPRESENTATION = auto() + INERTIAL_FIXED_REPRESENTATION = auto() diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 97cca9b6..2a622018 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/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,31 @@ 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( + # frame C = B[I] for mixed representation + B_v_C = self.math.mxv( self.math.adjoint_mixed_inverse(base_transform), base_velocity ) + elif ( + self.frame_velocity_representation + == Representations.INERTIAL_FIXED_REPRESENTATION + ): + # frame C = I for inertial-fixed representation + B_v_C = 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 + # frame C = B for body-fixed representation + B_v_C = base_velocity else: raise NotImplementedError( - "Only BODY_FIXED_REPRESENTATION and MIXED_REPRESENTATION are implemented" + "Only BODY_FIXED_REPRESENTATION, MIXED_REPRESENTATION, and INERTIAL_FIXED_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_C) + a = self.math.mxv(self.math.adjoint_derivative(L_H_B, v), B_v_C) 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 +552,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_C) 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 +639,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 +716,40 @@ 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,)) + # frame C = B for body-fixed representation + B_X_C = 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) + # frame C = B[I] for mixed representation + B_X_C = 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_C[..., :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 + ): + # frame C = I for inertial-fixed representation + B_X_C = 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 +772,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_C, base_velocity) a[idx] = math.mxv(Xup[idx], a0) continue @@ -717,7 +822,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_C, -2, -1), tau_base) if n > 0: zero_tau = math.factory.zeros(batch_shape + (1,)) @@ -806,19 +911,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 +1093,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 diff --git a/tests/conftest.py b/tests/conftest.py index f1f50c65..f56e0a48 100644 --- a/tests/conftest.py +++ b/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)