Skip to content

Add INERTIAL_FIXED_REPRESENTATION to velocity representations #144

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Giulero merged 9 commits into from
Nov 26, 2025
Merged

Add INERTIAL_FIXED_REPRESENTATION to velocity representations #144

Show file tree
Hide file tree
Changes from 8 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
9e51252
Add INERTIAL_FIXED_REPRESENTATION to Representations enum
Giulero Nov 21, 2025
e3d338a
Implement INERTIAL_FIXED_REPRESENTATION support in RBDAlgorithms
Giulero Nov 21, 2025
0b63081
Fix tranformation bug
Giulero Nov 21, 2025
faab0ad
Set transformed_acc to zero for inertial repr
Giulero Nov 21, 2025
bd5f094
Add INERTIAL_FIXED_REPRESENTATION in tests
Giulero Nov 21, 2025
3573bd0
Some refactor.
Giulero Nov 21, 2025
69b4968
Apply black
Giulero Nov 21, 2025
2429155
Update src/adam/core/rbd_algorithms.py
Giulero Nov 21, 2025
aac1bcb
Specify a generic frame C and describe it for every vel representation
Giulero Nov 24, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/adam/core/constants.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,3 +4,4 @@
class Representations(IntEnum):
BODY_FIXED_REPRESENTATION = auto()
MIXED_REPRESENTATION = auto()
INERTIAL_FIXED_REPRESENTATION = auto()
186 changes: 151 additions & 35 deletions src/adam/core/rbd_algorithms.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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}"
)
Expand Down Expand Up @@ -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
Expand All @@ -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,
Expand Down Expand Up @@ -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"
"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_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)]
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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())

Expand Down Expand Up @@ -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)
Comment on lines 635 to 742
Copy link
Contributor

@traversaro traversaro Nov 22, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are these changes in the non-inertial fixed code path intentional?

Copy link
Collaborator Author

@Giulero Giulero Nov 24, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yup! Before everything was written in mixed (e.g. B_X_BI), also for body fixed and I wanted to make it more generic.
But now, thinking twice, I could define a generic frame C and specify in a comment to which frame I'm referring to (e.g. C = B[I]).

Copy link
Collaborator Author

@Giulero Giulero Nov 24, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done in aac1bcb!

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
Expand All @@ -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

Expand Down Expand Up @@ -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,))
Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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

Expand Down
3 changes: 3 additions & 0 deletions tests/conftest.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ class RobotCfg:
VELOCITY_REPRESENTATIONS = [
Representations.MIXED_REPRESENTATION,
Representations.BODY_FIXED_REPRESENTATION,
Representations.INERTIAL_FIXED_REPRESENTATION,
]

ROBOTS = [
Expand Down Expand Up @@ -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)
Expand Down
Loading