From f1c4fa6b2b29efd80062fc8919f29780a7d857bd Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Thu, 30 Nov 2023 10:12:48 +0100 Subject: [PATCH 01/30] Add first version of ABA implementation --- src/adam/core/rbd_algorithms.py | 74 ++++++++++++++++++++++++++++++++- 1 file changed, 72 insertions(+), 2 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index ba68391f..8503a1e2 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -457,5 +457,75 @@ def rnea( tau[:6] = B_X_BI.T @ tau[:6] return tau - def aba(self): - raise NotImplementedError + def aba( + self, + base_transform: npt.ArrayLike, + joint_positions: npt.ArrayLike, + joint_velocities: npt.ArrayLike, + joint_accelerations: npt.ArrayLike, + tau: npt.ArrayLike, + g: npt.ArrayLike, + ) -> npt.ArrayLike: + """Implementation of Articulated Body Algorithm + + Args: + base_transform (T): The homogenous transform from base to world frame + joint_positions (T): The joints position + joint_velocities (T): The joints velocity + joint_accelerations (T): The joints acceleration + tau (T): The generalized force variables + g (T): The 6D gravity acceleration + + Returns: + a (T): The base acceleration + qdd (T): The joints acceleration + """ + # Pass 1 + for i in range(self.model.N): + ii = i - 1 + + # Parent-child transform + i_X_pi[i] = self.model.tree[i].joint.homogeneous(joint_positions[i]) + v_J = self.model.tree[i].joint.motion_subspace() * joint_velocities[i] + + v[i] = i_X_pi[i] @ v[pi] + v_J + c[i] = i_X_pi[i] @ c[pi] + self.math.spatial_skew(v[i]) @ v_J + + IA = self.model.tree[i].link.spatial_inertia() + + pA[i] = IA @ c[i] + self.math.spatial_skew_star(v[i]) @ IA @ v[i] + + # Pass 2 + for i in reversed(range(self.model.N)): + ii = i - 1 + + pi = self.model.tree[i].parent.idx + + U[i] = IA[i] @ self.model.tree[i].joint.motion_subspace() + D[i] = self.model.tree[i].joint.motion_subspace().T @ U[i] + u[i] = tau[i] - self.model.tree[i].joint.motion_subspace().T @ pA[i] + + Ia = IA[i] - U[i] / D[i] @ U[i].T + pa = pA[i] + Ia @ c[i] + U[i] * u[i] / D[i] + + # If model is floating base and i is the root link + if pi != 0: + IA[pi] += i_X_pi[i].T @ Ia @ i_X_pi[i] + pA[pi] += i_X_pi[i].T @ pa + + # Pass 3 + for i in range(self.model.N): + ii = i - 1 + + pi = self.model.tree[i].parent.idx + + a[i] = ( + i_X_pi[i] @ a[pi] + + self.model.tree[i].joint.motion_subspace() * joint_accelerations[i] + ) + f[i] = IA[i] @ a[i] + self.math.spatial_skew_star(v[i]) @ IA[i] @ v[i] + + if pi != 0: + f[i] += i_X_pi[i].T @ f[pi] + + return a, qdd From e24ddb7e0273daef37c1d38ae0c2a7b99b3de37f Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Fri, 1 Dec 2023 10:05:46 +0100 Subject: [PATCH 02/30] Update tests --- tests/body_fixed/test_CasADi_body_fixed.py | 9 +++++++++ tests/body_fixed/test_Jax_body_fixed.py | 9 +++++++++ tests/body_fixed/test_NumPy_body_fixed.py | 9 +++++++++ tests/body_fixed/test_pytorch_body_fixed.py | 9 +++++++++ tests/mixed/test_CasADi_mixed.py | 9 +++++++++ tests/mixed/test_Jax_mixed.py | 9 +++++++++ tests/mixed/test_NumPy_mixed.py | 9 +++++++++ tests/mixed/test_pytorch_mixed.py | 9 +++++++++ 8 files changed, 72 insertions(+) diff --git a/tests/body_fixed/test_CasADi_body_fixed.py b/tests/body_fixed/test_CasADi_body_fixed.py index d9686912..9f72f791 100644 --- a/tests/body_fixed/test_CasADi_body_fixed.py +++ b/tests/body_fixed/test_CasADi_body_fixed.py @@ -239,3 +239,12 @@ def test_relative_jacobian(): J_test2 = cs.DM(comp.relative_jacobian("l_sole", joints_val)) assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) assert iDynNumpyRelativeJ - J_test2 == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + aWb_idyntree = kinDyn.getFloatingBaseAcceleration() + base_acc = comp.forward_dynamics( + H_b, joints_val, base_vel, joints_vel, joint_torques + ) + assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_Jax_body_fixed.py b/tests/body_fixed/test_Jax_body_fixed.py index 10330947..695231ea 100644 --- a/tests/body_fixed/test_Jax_body_fixed.py +++ b/tests/body_fixed/test_Jax_body_fixed.py @@ -192,3 +192,12 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + aWb_idyntree = kinDyn.getFloatingBaseAcceleration() + base_acc = comp.forward_dynamics( + H_b, joints_val, base_vel, joints_vel, joint_torques + ) + assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_NumPy_body_fixed.py b/tests/body_fixed/test_NumPy_body_fixed.py index 5704c87d..04b67476 100644 --- a/tests/body_fixed/test_NumPy_body_fixed.py +++ b/tests/body_fixed/test_NumPy_body_fixed.py @@ -189,3 +189,12 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + aWb_idyntree = kinDyn.getFloatingBaseAcceleration() + base_acc = comp.forward_dynamics( + H_b, joints_val, base_vel, joints_vel, joint_torques + ) + assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_pytorch_body_fixed.py b/tests/body_fixed/test_pytorch_body_fixed.py index b0df67bb..48991c33 100644 --- a/tests/body_fixed/test_pytorch_body_fixed.py +++ b/tests/body_fixed/test_pytorch_body_fixed.py @@ -191,3 +191,12 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - np.asarray(G_test) == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + aWb_idyntree = kinDyn.getFloatingBaseAcceleration() + base_acc = comp.forward_dynamics( + H_b, joints_val, base_vel, joints_vel, joint_torques + ) + assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_CasADi_mixed.py b/tests/mixed/test_CasADi_mixed.py index 60c1c102..eee6126a 100644 --- a/tests/mixed/test_CasADi_mixed.py +++ b/tests/mixed/test_CasADi_mixed.py @@ -239,3 +239,12 @@ def test_relative_jacobian(): J_test2 = cs.DM(comp.relative_jacobian("l_sole", joints_val)) assert iDynNumpyRelativeJ - J_test == pytest.approx(0.0, abs=1e-4) assert iDynNumpyRelativeJ - J_test2 == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + aWb_idyntree = kinDyn.getFloatingBaseAcceleration() + base_acc = comp.forward_dynamics( + H_b, joints_val, base_vel, joints_vel, joint_torques + ) + assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_Jax_mixed.py b/tests/mixed/test_Jax_mixed.py index 636a3391..76476542 100644 --- a/tests/mixed/test_Jax_mixed.py +++ b/tests/mixed/test_Jax_mixed.py @@ -192,3 +192,12 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + aWb_idyntree = kinDyn.getFloatingBaseAcceleration() + base_acc = comp.forward_dynamics( + H_b, joints_val, base_vel, joints_vel, joint_torques + ) + assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_NumPy_mixed.py b/tests/mixed/test_NumPy_mixed.py index d4710441..319998a6 100644 --- a/tests/mixed/test_NumPy_mixed.py +++ b/tests/mixed/test_NumPy_mixed.py @@ -188,3 +188,12 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + aWb_idyntree = kinDyn.getFloatingBaseAcceleration() + base_acc = comp.forward_dynamics( + H_b, joints_val, base_vel, joints_vel, joint_torques + ) + assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_pytorch_mixed.py b/tests/mixed/test_pytorch_mixed.py index de6a8229..df55ad5e 100644 --- a/tests/mixed/test_pytorch_mixed.py +++ b/tests/mixed/test_pytorch_mixed.py @@ -191,3 +191,12 @@ def test_gravity_term(): ) G_test = comp.gravity_term(H_b, joints_val) assert G_iDyn_np - np.asarray(G_test) == pytest.approx(0.0, abs=1e-4) + + +def test_fd(): + joint_torques = np.random.rand(n_dofs) + aWb_idyntree = kinDyn.getFloatingBaseAcceleration() + base_acc = comp.forward_dynamics( + H_b, joints_val, base_vel, joints_vel, joint_torques + ) + assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) From 69816673f4e543841cf27eb0b887e043708ae595 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Fri, 1 Dec 2023 10:08:10 +0100 Subject: [PATCH 03/30] Update variables name in ABA --- src/adam/core/rbd_algorithms.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 8503a1e2..0a0969eb 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -477,8 +477,8 @@ def aba( g (T): The 6D gravity acceleration Returns: - a (T): The base acceleration - qdd (T): The joints acceleration + base_acceleration (T): The base acceleration in mixed representation + joint_accelerations (T): The joints acceleration """ # Pass 1 for i in range(self.model.N): From 3e79a863491dc1274208dc5319c93a498ef7eb79 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Fri, 1 Dec 2023 10:08:17 +0100 Subject: [PATCH 04/30] Add `forward_dynamics` in computations --- src/adam/casadi/computations.py | 30 ++++++++++++++++++++++++++++++ src/adam/jax/computations.py | 30 ++++++++++++++++++++++++++++++ src/adam/numpy/computations.py | 31 +++++++++++++++++++++++++++++++ src/adam/pytorch/computations.py | 30 ++++++++++++++++++++++++++++++ 4 files changed, 121 insertions(+) diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index eab62718..211b3b70 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -411,3 +411,33 @@ def CoM_position( CoM (Union[cs.SX, cs.DM]): The CoM position """ return self.rbdalgos.CoM_position(base_transform, joint_positions).array + + def forward_dynamics( + self, + base_transform: Union[cs.SX, cs.DM], + joint_positions: Union[cs.SX, cs.DM], + base_velocity: Union[cs.SX, cs.DM], + joint_velocities: Union[cs.SX, cs.DM], + joint_torques: Union[cs.SX, cs.DM], + ) -> Union[cs.SX, cs.DM]: + """Returns the base acceleration and joint accelerations + + Args: + base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame + joint_positions (Union[cs.SX, cs.DM]): The joints position + base_velocity (Union[cs.SX, cs.DM]): The base velocity in mixed representation + joint_velocities (Union[cs.SX, cs.DM]): The joints velocity + joint_torques (Union[cs.SX, cs.DM]): The joints torque + + Returns: + base_acceleration (Union[cs.SX, cs.DM]): The base acceleration in mixed representation + joint_accelerations (Union[cs.SX, cs.DM]): The joints acceleration + """ + return self.rbdalgos.aba( + base_transform, + joint_positions, + base_velocity, + joint_velocities, + joint_torques, + self.g, + ) diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index 2f68d092..dfc7042b 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -239,3 +239,33 @@ def get_total_mass(self) -> float: mass: The total mass """ return self.rbdalgos.get_total_mass() + + def forward_dynamics( + self, + base_transform: jnp.array, + joint_positions: jnp.array, + base_velocity: jnp.array, + joint_velocities: jnp.array, + joint_torques: jnp.array, + ) -> jnp.array: + """Returns the forward dynamics equation + + Args: + base_transform (jnp.array): The homogenous transform from base to world frame + joint_positions (jnp.array): The joints position + base_velocity (jnp.array): The base velocity in mixed representation + joint_velocities (jnp.array): The joints velocity + joint_torques (jnp.array): The joints torques + + Returns: + base_acceleration (jnp.array): The base acceleration in mixed representation + joint_accelerations (jnp.array): The joints acceleration + """ + return self.rbdalgos.aba( + base_transform, + joint_positions, + base_velocity, + joint_velocities, + joint_torques, + self.g, + ).array.squeeze() diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index ad531f08..35d72817 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -244,3 +244,34 @@ def get_total_mass(self) -> float: mass: The total mass """ return self.rbdalgos.get_total_mass() + + def forward_dynamics( + self, + base_transform: np.ndarray, + joint_positions: np.ndarray, + base_velocity: np.ndarray, + joint_velocities: np.ndarray, + joint_torques: np.ndarray, + ) -> np.ndarray: + """Returns the forward dynamics of the floating-base dynamics equation, + using a reduced RNEA (no acceleration and external forces) + + Args: + base_transform (np.ndarray): The homogenous transform from base to world frame + joint_positions (np.ndarray): The joints position + base_velocity (np.ndarray): The base velocity in mixed representation + joint_velocities (np.ndarray): The joint velocities + joint_torques (np.ndarray): The joint torques + + Returns: + base_acceleration (np.ndarray): the base acceleration + joint_accelerations (np.ndarray): the joint accelerations + """ + return self.rbdalgos.aba( + base_transform, + joint_positions, + base_velocity.reshape(6, 1), + joint_velocities, + joint_torques, + self.g, + ).array.squeeze() diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index f614acf9..d83e4935 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -250,3 +250,33 @@ def get_total_mass(self) -> float: mass: The total mass """ return self.rbdalgos.get_total_mass() + + def forward_dynamic( + self, + base_transform: torch.Tensor, + joint_positions: torch.Tensor, + base_velocity: torch.Tensor, + joint_velocities: torch.Tensor, + joint_torques: torch.Tensor, + ) -> torch.Tensor: + """Returns the forward dynamics of the floating-base system + + Args: + base_transform (torch.tensor): The homogenous transform from base to world frame + joint_positions (torch.tensor): The joints positions + base_velocity (torch.tensor): The base velocity in mixed representation + joint_velocities (torch.tensor): The joints velocities + joint_torques (torch.tensor): The joints torques + + Returns: + base_acceleration (torch.tensor): The base acceleration in mixed representation + joint_accelerations (torch.tensor): The joints accelerations + """ + return self.rbdalgos.aba( + base_transform, + joint_positions, + base_velocity.reshape(6, 1), + joint_velocities, + joint_torques, + self.g, + ).array.squeeze() From 1259756bd52951889586cd37450c8de8e883ac2e Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Fri, 1 Dec 2023 17:04:19 +0100 Subject: [PATCH 05/30] Update tests --- tests/body_fixed/test_CasADi_body_fixed.py | 7 +------ tests/body_fixed/test_Jax_body_fixed.py | 15 +++++++++++---- tests/body_fixed/test_NumPy_body_fixed.py | 7 +------ tests/body_fixed/test_pytorch_body_fixed.py | 7 +------ tests/mixed/test_CasADi_mixed.py | 7 +------ tests/mixed/test_Jax_mixed.py | 7 +------ tests/mixed/test_NumPy_mixed.py | 7 +------ tests/mixed/test_pytorch_mixed.py | 7 +------ 8 files changed, 18 insertions(+), 46 deletions(-) diff --git a/tests/body_fixed/test_CasADi_body_fixed.py b/tests/body_fixed/test_CasADi_body_fixed.py index 9f72f791..aa1d8ed8 100644 --- a/tests/body_fixed/test_CasADi_body_fixed.py +++ b/tests/body_fixed/test_CasADi_body_fixed.py @@ -242,9 +242,4 @@ def test_relative_jacobian(): def test_fd(): - joint_torques = np.random.rand(n_dofs) - aWb_idyntree = kinDyn.getFloatingBaseAcceleration() - base_acc = comp.forward_dynamics( - H_b, joints_val, base_vel, joints_vel, joint_torques - ) - assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) + pass diff --git a/tests/body_fixed/test_Jax_body_fixed.py b/tests/body_fixed/test_Jax_body_fixed.py index 695231ea..b4a9c3ea 100644 --- a/tests/body_fixed/test_Jax_body_fixed.py +++ b/tests/body_fixed/test_Jax_body_fixed.py @@ -196,8 +196,15 @@ def test_gravity_term(): def test_fd(): joint_torques = np.random.rand(n_dofs) - aWb_idyntree = kinDyn.getFloatingBaseAcceleration() - base_acc = comp.forward_dynamics( - H_b, joints_val, base_vel, joints_vel, joint_torques + joints_vel = np.random.rand(n_dofs) + reference_acc = jnp.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + jnp.concatenate((jnp.zeros(6), joint_torques)) + - comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) + - comp.gravity_term(H_b, joints_val) ) - assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) + base_acc, joint_acc = comp.forward_dynamics( + H_b, joints_val, joints_vel, joint_torques + ) + + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_NumPy_body_fixed.py b/tests/body_fixed/test_NumPy_body_fixed.py index 04b67476..80cf6e1b 100644 --- a/tests/body_fixed/test_NumPy_body_fixed.py +++ b/tests/body_fixed/test_NumPy_body_fixed.py @@ -192,9 +192,4 @@ def test_gravity_term(): def test_fd(): - joint_torques = np.random.rand(n_dofs) - aWb_idyntree = kinDyn.getFloatingBaseAcceleration() - base_acc = comp.forward_dynamics( - H_b, joints_val, base_vel, joints_vel, joint_torques - ) - assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) + pass diff --git a/tests/body_fixed/test_pytorch_body_fixed.py b/tests/body_fixed/test_pytorch_body_fixed.py index 48991c33..13be362f 100644 --- a/tests/body_fixed/test_pytorch_body_fixed.py +++ b/tests/body_fixed/test_pytorch_body_fixed.py @@ -194,9 +194,4 @@ def test_gravity_term(): def test_fd(): - joint_torques = np.random.rand(n_dofs) - aWb_idyntree = kinDyn.getFloatingBaseAcceleration() - base_acc = comp.forward_dynamics( - H_b, joints_val, base_vel, joints_vel, joint_torques - ) - assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) + pass diff --git a/tests/mixed/test_CasADi_mixed.py b/tests/mixed/test_CasADi_mixed.py index eee6126a..f9288ed4 100644 --- a/tests/mixed/test_CasADi_mixed.py +++ b/tests/mixed/test_CasADi_mixed.py @@ -242,9 +242,4 @@ def test_relative_jacobian(): def test_fd(): - joint_torques = np.random.rand(n_dofs) - aWb_idyntree = kinDyn.getFloatingBaseAcceleration() - base_acc = comp.forward_dynamics( - H_b, joints_val, base_vel, joints_vel, joint_torques - ) - assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) + pass diff --git a/tests/mixed/test_Jax_mixed.py b/tests/mixed/test_Jax_mixed.py index 76476542..1123c3f0 100644 --- a/tests/mixed/test_Jax_mixed.py +++ b/tests/mixed/test_Jax_mixed.py @@ -195,9 +195,4 @@ def test_gravity_term(): def test_fd(): - joint_torques = np.random.rand(n_dofs) - aWb_idyntree = kinDyn.getFloatingBaseAcceleration() - base_acc = comp.forward_dynamics( - H_b, joints_val, base_vel, joints_vel, joint_torques - ) - assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) + pass diff --git a/tests/mixed/test_NumPy_mixed.py b/tests/mixed/test_NumPy_mixed.py index 319998a6..3ecc04fd 100644 --- a/tests/mixed/test_NumPy_mixed.py +++ b/tests/mixed/test_NumPy_mixed.py @@ -191,9 +191,4 @@ def test_gravity_term(): def test_fd(): - joint_torques = np.random.rand(n_dofs) - aWb_idyntree = kinDyn.getFloatingBaseAcceleration() - base_acc = comp.forward_dynamics( - H_b, joints_val, base_vel, joints_vel, joint_torques - ) - assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) + pass diff --git a/tests/mixed/test_pytorch_mixed.py b/tests/mixed/test_pytorch_mixed.py index df55ad5e..78d7f882 100644 --- a/tests/mixed/test_pytorch_mixed.py +++ b/tests/mixed/test_pytorch_mixed.py @@ -194,9 +194,4 @@ def test_gravity_term(): def test_fd(): - joint_torques = np.random.rand(n_dofs) - aWb_idyntree = kinDyn.getFloatingBaseAcceleration() - base_acc = comp.forward_dynamics( - H_b, joints_val, base_vel, joints_vel, joint_torques - ) - assert aWb_idyntree.toNumPy() - base_acc == pytest.approx(0.0, abs=1e-4) + pass From eb9e6f6c49c244993c870f859c1106a1d436e2df Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Fri, 1 Dec 2023 19:15:54 +0100 Subject: [PATCH 06/30] Fix `ABA` in core. Note: We should consider the possibility to use a reduced kinematic tree. --- src/adam/core/rbd_algorithms.py | 73 +++++++++++++++++++-------------- 1 file changed, 43 insertions(+), 30 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 0a0969eb..9b0ea67a 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -462,8 +462,7 @@ def aba( base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike, joint_velocities: npt.ArrayLike, - joint_accelerations: npt.ArrayLike, - tau: npt.ArrayLike, + joint_torques: npt.ArrayLike, g: npt.ArrayLike, ) -> npt.ArrayLike: """Implementation of Articulated Body Algorithm @@ -472,7 +471,6 @@ def aba( base_transform (T): The homogenous transform from base to world frame joint_positions (T): The joints position joint_velocities (T): The joints velocity - joint_accelerations (T): The joints acceleration tau (T): The generalized force variables g (T): The 6D gravity acceleration @@ -480,52 +478,67 @@ def aba( base_acceleration (T): The base acceleration in mixed representation joint_accelerations (T): The joints acceleration """ + i_X_pi = self.math.factory.zeros(self.model.N, 6, 6) + v = self.math.factory.zeros(self.model.N, 6, 1) + c = self.math.factory.zeros(self.model.N, 6, 1) + pA = self.math.factory.zeros(self.model.N, 6, 1) + IA = self.math.factory.zeros(self.model.N, 6, 6) + U = self.math.factory.zeros(self.model.N, 6, 6) + D = self.math.factory.zeros(self.model.N, 6, 6) + u = self.math.factory.zeros(self.model.N, 6, 1) + a = self.math.factory.zeros(self.model.N, 6, 1) + f = self.math.factory.zeros(self.model.N, 6, 1) + # Pass 1 - for i in range(self.model.N): - ii = i - 1 + for i, node in enumerate(self.model.tree): + link_i, joint_i, link_pi = node.get_elements() + + if link_i.name == self.root_link: + continue + + pi = self.model.tree.get_idx_from_name(link_pi.name) # Parent-child transform - i_X_pi[i] = self.model.tree[i].joint.homogeneous(joint_positions[i]) - v_J = self.model.tree[i].joint.motion_subspace() * joint_velocities[i] + i_X_pi[i] = joint_i.spatial_transform(joint_positions[i]) + v_J = joint_i.motion_subspace() * joint_velocities[i] v[i] = i_X_pi[i] @ v[pi] + v_J c[i] = i_X_pi[i] @ c[pi] + self.math.spatial_skew(v[i]) @ v_J - IA = self.model.tree[i].link.spatial_inertia() + IA[i] = link_i.spatial_inertia() - pA[i] = IA @ c[i] + self.math.spatial_skew_star(v[i]) @ IA @ v[i] + pA[i] = IA[i] @ c[i] + self.math.spatial_skew_star(v[i]) @ IA[i] @ v[i] # Pass 2 - for i in reversed(range(self.model.N)): - ii = i - 1 + for i, node in reversed(list(enumerate(self.model.tree))): + link_i, joint_i, link_pi = node.get_elements() - pi = self.model.tree[i].parent.idx + if link_i.name == self.root_link: + IA[pi] += i_X_pi[i].T @ Ia @ i_X_pi[i] + pA[pi] += i_X_pi[i].T @ pa + continue + + pi = self.model.tree.get_idx_from_name(link_pi.name) - U[i] = IA[i] @ self.model.tree[i].joint.motion_subspace() - D[i] = self.model.tree[i].joint.motion_subspace().T @ U[i] - u[i] = tau[i] - self.model.tree[i].joint.motion_subspace().T @ pA[i] + U[i] = IA[i] @ node.joint.motion_subspace() + D[i] = node.joint.motion_subspace().T @ U[i] + u[i] = tau[i] - node.joint.motion_subspace().T @ pA[i] Ia = IA[i] - U[i] / D[i] @ U[i].T pa = pA[i] + Ia @ c[i] + U[i] * u[i] / D[i] - # If model is floating base and i is the root link - if pi != 0: + # Pass 3 + for i, node in enumerate(self.model.tree): + link_i, joint_i, link_pi = node.get_elements() + + if link_i.name == self.root_link: IA[pi] += i_X_pi[i].T @ Ia @ i_X_pi[i] pA[pi] += i_X_pi[i].T @ pa - # Pass 3 - for i in range(self.model.N): - ii = i - 1 + pi = self.model.tree.get_idx_from_name(link_pi.name) - pi = self.model.tree[i].parent.idx - - a[i] = ( - i_X_pi[i] @ a[pi] - + self.model.tree[i].joint.motion_subspace() * joint_accelerations[i] - ) - f[i] = IA[i] @ a[i] + self.math.spatial_skew_star(v[i]) @ IA[i] @ v[i] + sdd = (u[i] - U[i].T @ a[i]) / D[i] - if pi != 0: - f[i] += i_X_pi[i].T @ f[pi] + a[i] = i_X_pi[i].T @ a[pi] + node.joint.motion_subspace() * sdd + c[i] - return a, qdd + return a, sdd From 111c83e7bee51a84af011d5c9acf5b03f5a28e2c Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Fri, 1 Dec 2023 19:17:19 +0100 Subject: [PATCH 07/30] Fix computations --- src/adam/casadi/computations.py | 5 +---- src/adam/jax/computations.py | 5 +---- src/adam/numpy/computations.py | 6 +----- src/adam/pytorch/computations.py | 5 +---- 4 files changed, 4 insertions(+), 17 deletions(-) diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 211b3b70..74be5807 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -416,16 +416,14 @@ def forward_dynamics( self, base_transform: Union[cs.SX, cs.DM], joint_positions: Union[cs.SX, cs.DM], - base_velocity: Union[cs.SX, cs.DM], joint_velocities: Union[cs.SX, cs.DM], joint_torques: Union[cs.SX, cs.DM], ) -> Union[cs.SX, cs.DM]: - """Returns the base acceleration and joint accelerations + """Returns base and joints accelerations of the floating-base dynamics equation Args: base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame joint_positions (Union[cs.SX, cs.DM]): The joints position - base_velocity (Union[cs.SX, cs.DM]): The base velocity in mixed representation joint_velocities (Union[cs.SX, cs.DM]): The joints velocity joint_torques (Union[cs.SX, cs.DM]): The joints torque @@ -436,7 +434,6 @@ def forward_dynamics( return self.rbdalgos.aba( base_transform, joint_positions, - base_velocity, joint_velocities, joint_torques, self.g, diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index dfc7042b..bd420ee2 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -244,16 +244,14 @@ def forward_dynamics( self, base_transform: jnp.array, joint_positions: jnp.array, - base_velocity: jnp.array, joint_velocities: jnp.array, joint_torques: jnp.array, ) -> jnp.array: - """Returns the forward dynamics equation + """Returns base and joints accelerations of the floating-base dynamics equation Args: base_transform (jnp.array): The homogenous transform from base to world frame joint_positions (jnp.array): The joints position - base_velocity (jnp.array): The base velocity in mixed representation joint_velocities (jnp.array): The joints velocity joint_torques (jnp.array): The joints torques @@ -264,7 +262,6 @@ def forward_dynamics( return self.rbdalgos.aba( base_transform, joint_positions, - base_velocity, joint_velocities, joint_torques, self.g, diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index 35d72817..f9b21303 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -249,17 +249,14 @@ def forward_dynamics( self, base_transform: np.ndarray, joint_positions: np.ndarray, - base_velocity: np.ndarray, joint_velocities: np.ndarray, joint_torques: np.ndarray, ) -> np.ndarray: - """Returns the forward dynamics of the floating-base dynamics equation, - using a reduced RNEA (no acceleration and external forces) + """Returns base and joints accelerations of the floating-base dynamics equation Args: base_transform (np.ndarray): The homogenous transform from base to world frame joint_positions (np.ndarray): The joints position - base_velocity (np.ndarray): The base velocity in mixed representation joint_velocities (np.ndarray): The joint velocities joint_torques (np.ndarray): The joint torques @@ -270,7 +267,6 @@ def forward_dynamics( return self.rbdalgos.aba( base_transform, joint_positions, - base_velocity.reshape(6, 1), joint_velocities, joint_torques, self.g, diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index d83e4935..d4390087 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -255,16 +255,14 @@ def forward_dynamic( self, base_transform: torch.Tensor, joint_positions: torch.Tensor, - base_velocity: torch.Tensor, joint_velocities: torch.Tensor, joint_torques: torch.Tensor, ) -> torch.Tensor: - """Returns the forward dynamics of the floating-base system + """Returns base and joints accelerations of the floating-base dynamics equation Args: base_transform (torch.tensor): The homogenous transform from base to world frame joint_positions (torch.tensor): The joints positions - base_velocity (torch.tensor): The base velocity in mixed representation joint_velocities (torch.tensor): The joints velocities joint_torques (torch.tensor): The joints torques @@ -275,7 +273,6 @@ def forward_dynamic( return self.rbdalgos.aba( base_transform, joint_positions, - base_velocity.reshape(6, 1), joint_velocities, joint_torques, self.g, From 3088164525d33496e471e45f9f83d233632a4d47 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Mon, 4 Dec 2023 09:29:12 +0100 Subject: [PATCH 08/30] Add abstractions for solving linear systems --- src/adam/core/spatial_math.py | 12 ++++++++++++ src/adam/jax/jax_like.py | 12 ++++++++++++ src/adam/numpy/numpy_like.py | 12 ++++++++++++ src/adam/pytorch/torch_like.py | 12 ++++++++++++ 4 files changed, 48 insertions(+) diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 3e1a0f8c..0bfdd089 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -158,6 +158,18 @@ def cos(x: npt.ArrayLike) -> npt.ArrayLike: def skew(x): pass + @abc.abstractmethod + def solve(A: npt.ArrayLike, b: npt.ArrayLike) -> npt.ArrayLike: + """ + Args: + A (npt.ArrayLike): matrix + b (npt.ArrayLike): vector + + Returns: + npt.ArrayLike: solution of the linear system Ax=b + """ + pass + def R_from_axis_angle(self, axis: npt.ArrayLike, q: npt.ArrayLike) -> npt.ArrayLike: """ Args: diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py index 23ddc2a1..5f898b7e 100644 --- a/src/adam/jax/jax_like.py +++ b/src/adam/jax/jax_like.py @@ -211,3 +211,15 @@ def horzcat(*x) -> "JaxLike": else: v = jnp.hstack([x[i] for i in range(len(x))]) return JaxLike(v) + + @staticmethod + def solve(A: "JaxLike", b: "JaxLike") -> "JaxLike": + """ + Args: + A (JaxLike): Matrix + b (JaxLike): Vector + + Returns: + JaxLike: Solution of Ax=b + """ + return JaxLike(jnp.linalg.solve(A.array, b.array)) diff --git a/src/adam/numpy/numpy_like.py b/src/adam/numpy/numpy_like.py index ddfac73a..107624f2 100644 --- a/src/adam/numpy/numpy_like.py +++ b/src/adam/numpy/numpy_like.py @@ -213,3 +213,15 @@ def skew(x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike": return -np.cross(np.array(x), np.eye(3), axisa=0, axisb=0) x = x.array return NumpyLike(-np.cross(np.array(x), np.eye(3), axisa=0, axisb=0)) + + @staticmethod + def solve(A: "NumpyLike", b: "NumpyLike") -> "NumpyLike": + """ + Args: + A (NumpyLike): matrix + b (NumpyLike): vector + + Returns: + NumpyLike: solution of Ax=b + """ + return NumpyLike(np.linalg.solve(A.array, b.array)) diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py index f24d91c5..3241d1ca 100644 --- a/src/adam/pytorch/torch_like.py +++ b/src/adam/pytorch/torch_like.py @@ -225,3 +225,15 @@ def horzcat(*x: ntp.ArrayLike) -> "TorchLike": else: v = torch.FloatTensor(x) return TorchLike(v) + + @staticmethod + def solve(A: "TorchLike", b: "TorchLike") -> "TorchLike": + """ + Args: + A (TorchLike): matrix + b (TorchLike): vector + + Returns: + TorchLike: solution of Ax = b + """ + return TorchLike(torch.linalg.solve(A.array, b.array)) From ab5023bc5ffb2638b8a04a918e04095212182dbf Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Tue, 5 Dec 2023 11:31:42 +0100 Subject: [PATCH 09/30] Add `floating_base` field --- src/adam/model/model.py | 4 ++++ src/adam/model/tree.py | 7 +++++++ 2 files changed, 11 insertions(+) diff --git a/src/adam/model/model.py b/src/adam/model/model.py index 0f51306f..3421cfdf 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -18,6 +18,7 @@ class Model: tree: Tree NDoF: int actuated_joints: List[str] + floating_base: bool = True def __post_init__(self): """set the "length of the model as the number of links""" @@ -61,6 +62,8 @@ def build(factory: ModelFactory, joints_name_list: List[str] = None) -> "Model": tree = Tree.build_tree(links=links, joints=joints) + floating_base = tree.is_floating_base() + # generate some useful dict joints: Dict(str, Joint) = {joint.name: joint for joint in joints} links: Dict(str, Link) = {link.name: link for link in links} @@ -74,6 +77,7 @@ def build(factory: ModelFactory, joints_name_list: List[str] = None) -> "Model": tree=tree, NDoF=len(joints_name_list), actuated_joints=joints_name_list, + floating_base=floating_base, ) def get_joints_chain(self, root: str, target: str) -> List[Joint]: diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index dc96bfe5..8d99d328 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -136,6 +136,13 @@ def get_node_from_name(self, name: str) -> Node: """ return self.graph[name] + def is_floating_base(self) -> bool: + """ + Returns: + bool: True if the model is floating base + """ + return len(self.graph[self.root].children) > 1 + def __iter__(self) -> Node: """This method allows to iterate on the model Returns: From e148da1df5bdd302a4bed98add0a8b2d63403320 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Tue, 5 Dec 2023 11:32:15 +0100 Subject: [PATCH 10/30] Overwrite `__eq__` dunder --- src/adam/casadi/casadi_like.py | 6 ++++++ src/adam/jax/jax_like.py | 6 ++++++ src/adam/numpy/numpy_like.py | 7 +++++++ src/adam/pytorch/torch_like.py | 7 +++++++ 4 files changed, 26 insertions(+) diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py index 9ee0ce02..8949e381 100644 --- a/src/adam/casadi/casadi_like.py +++ b/src/adam/casadi/casadi_like.py @@ -93,6 +93,12 @@ def __getitem__(self, idx) -> "CasadiLike": """Overrides get item operator""" return CasadiLike(self.array[idx]) + def __eq__(self, other: Union["CasadiLike", npt.ArrayLike]) -> bool: + """Overrides == operator""" + if type(self) is not type(other): + return self.array == other + return self.array == other.array + @property def T(self) -> "CasadiLike": """ diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py index 5f898b7e..06aff4bb 100644 --- a/src/adam/jax/jax_like.py +++ b/src/adam/jax/jax_like.py @@ -107,6 +107,12 @@ def __neg__(self) -> "JaxLike": """Overrides - operator""" return JaxLike(-self.array) + def __eq__(self, other: Union["JaxLike", npt.ArrayLike]) -> bool: + """Overrides == operator""" + if type(self) is not type(other): + return self.array.squeeze() == other.squeeze() + return self.array.squeeze() == other.array.squeeze() + class JaxLikeFactory(ArrayLikeFactory): @staticmethod diff --git a/src/adam/numpy/numpy_like.py b/src/adam/numpy/numpy_like.py index 107624f2..c0f26a31 100644 --- a/src/adam/numpy/numpy_like.py +++ b/src/adam/numpy/numpy_like.py @@ -106,6 +106,13 @@ def __neg__(self): """Overrides - operator""" return NumpyLike(-self.array) + def __eq__(self, other: Union["NumpyLike", npt.ArrayLike]) -> bool: + """Overrides == operator""" + if type(self) is type(other): + return self.array == other.array + else: + return self.array == other + class NumpyLikeFactory(ArrayLikeFactory): @staticmethod diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py index 3241d1ca..4753d45d 100644 --- a/src/adam/pytorch/torch_like.py +++ b/src/adam/pytorch/torch_like.py @@ -112,6 +112,13 @@ def __neg__(self) -> "TorchLike": """Overrides - operator""" return TorchLike(-self.array) + def __eq__(self, other: Union["TorchLike", ntp.ArrayLike]) -> bool: + """Overrides == operator""" + if type(self) is type(other): + return self.array == other.array + else: + return self.array == other + class TorchLikeFactory(ArrayLikeFactory): @staticmethod From 3c2de136412e5e9955f4fb251addc6920fd34a3d Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Tue, 5 Dec 2023 11:33:16 +0100 Subject: [PATCH 11/30] Fix typos and finalize ABA --- src/adam/casadi/casadi_like.py | 12 ++++++++ src/adam/core/rbd_algorithms.py | 54 ++++++++++++++++++++++++--------- src/adam/jax/computations.py | 6 ++-- 3 files changed, 55 insertions(+), 17 deletions(-) diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py index 8949e381..94a9bd34 100644 --- a/src/adam/casadi/casadi_like.py +++ b/src/adam/casadi/casadi_like.py @@ -212,6 +212,18 @@ def horzcat(*x) -> "CasadiLike": y = [xi.array if isinstance(xi, CasadiLike) else xi for xi in x] return CasadiLike(cs.horzcat(*y)) + @staticmethod + def solve(A: "CasadiLike", b: "CasadiLike") -> "CasadiLike": + """ + Args: + A (CasadiLike): matrix + b (CasadiLike): vector + + Returns: + CasadiLike: solution of A*x=b + """ + return CasadiLike(cs.solve(A.array, b.array)) + if __name__ == "__main__": math = SpatialMath() diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 9b0ea67a..24a202d3 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -462,7 +462,7 @@ def aba( base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike, joint_velocities: npt.ArrayLike, - joint_torques: npt.ArrayLike, + tau: npt.ArrayLike, g: npt.ArrayLike, ) -> npt.ArrayLike: """Implementation of Articulated Body Algorithm @@ -483,11 +483,12 @@ def aba( c = self.math.factory.zeros(self.model.N, 6, 1) pA = self.math.factory.zeros(self.model.N, 6, 1) IA = self.math.factory.zeros(self.model.N, 6, 6) - U = self.math.factory.zeros(self.model.N, 6, 6) - D = self.math.factory.zeros(self.model.N, 6, 6) - u = self.math.factory.zeros(self.model.N, 6, 1) + U = self.math.factory.zeros(self.model.N, 6, 1) + D = self.math.factory.zeros(self.model.N, 1, 1) + u = self.math.factory.zeros(self.model.N, 1, 1) a = self.math.factory.zeros(self.model.N, 6, 1) - f = self.math.factory.zeros(self.model.N, 6, 1) + sdd = self.math.factory.zeros(self.model.N, 1, 1) + B_X_W = self.math.adjoint_mixed_inverse(base_transform) # Pass 1 for i, node in enumerate(self.model.tree): @@ -495,12 +496,14 @@ def aba( if link_i.name == self.root_link: continue + q = joint_positions[joint_i.idx] if joint_i.idx is not None else 0.0 + q_dot = joint_velocities[joint_i.idx] if joint_i.idx is not None else 0.0 pi = self.model.tree.get_idx_from_name(link_pi.name) # Parent-child transform - i_X_pi[i] = joint_i.spatial_transform(joint_positions[i]) - v_J = joint_i.motion_subspace() * joint_velocities[i] + i_X_pi[i] = joint_i.spatial_transform(q) + v_J = joint_i.motion_subspace() * q_dot v[i] = i_X_pi[i] @ v[pi] + v_J c[i] = i_X_pi[i] @ c[pi] + self.math.spatial_skew(v[i]) @ v_J @@ -519,26 +522,47 @@ def aba( continue pi = self.model.tree.get_idx_from_name(link_pi.name) + tau_i = tau[joint_i.idx] if joint_i.idx is not None else 0.0 - U[i] = IA[i] @ node.joint.motion_subspace() - D[i] = node.joint.motion_subspace().T @ U[i] - u[i] = tau[i] - node.joint.motion_subspace().T @ pA[i] + U[i] = IA[i] @ joint_i.motion_subspace() + D[i] = joint_i.motion_subspace().T @ U[i] + u[i] = self.math.vertcat(tau_i) - joint_i.motion_subspace().T @ pA[i] Ia = IA[i] - U[i] / D[i] @ U[i].T pa = pA[i] + Ia @ c[i] + U[i] * u[i] / D[i] + a[0] = B_X_W @ g if self.model.floating_base else self.math.solve(-IA[0], pA[0]) + # Pass 3 for i, node in enumerate(self.model.tree): link_i, joint_i, link_pi = node.get_elements() if link_i.name == self.root_link: - IA[pi] += i_X_pi[i].T @ Ia @ i_X_pi[i] - pA[pi] += i_X_pi[i].T @ pa + continue pi = self.model.tree.get_idx_from_name(link_pi.name) - sdd = (u[i] - U[i].T @ a[i]) / D[i] + sdd[i - 1] = (u[i] - U[i].T @ a[i]) / D[i] + + a[i] += i_X_pi[i].T @ a[pi] + joint_i.motion_subspace() * sdd[i - 1] + c[i] + + # Filter sdd to remove NaNs generate with lumped joints + s_ddot = self.math.vertcat( + *[sdd[i] for i in range(self.model.N) if sdd[i] == sdd[i]] + ) - a[i] = i_X_pi[i].T @ a[pi] + node.joint.motion_subspace() * sdd + c[i] + if ( + self.frame_velocity_representation + == Representations.BODY_FIXED_REPRESENTATION + ): + return a[0], s_ddot - return a, sdd + elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: + return ( + self.math.vertcat( + self.math.solve(B_X_W, a[0]) + g + if self.model.floating_base + else self.math.zeros(6, 1), + ), + s_ddot, + ) diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index bd420ee2..183b47ee 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -259,10 +259,12 @@ def forward_dynamics( base_acceleration (jnp.array): The base acceleration in mixed representation joint_accelerations (jnp.array): The joints acceleration """ - return self.rbdalgos.aba( + base_acceleration, joint_accelerations = self.rbdalgos.aba( base_transform, joint_positions, joint_velocities, joint_torques, self.g, - ).array.squeeze() + ) + + return base_acceleration.array.squeeze(), joint_accelerations.array.squeeze() From 80c77574b39d8a67c64ae37efc89cd74c0332968 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Thu, 4 Jan 2024 13:13:01 +0100 Subject: [PATCH 12/30] Add base velocity as input --- src/adam/casadi/computations.py | 3 +++ src/adam/core/rbd_algorithms.py | 2 ++ src/adam/jax/computations.py | 3 +++ src/adam/numpy/computations.py | 3 +++ src/adam/pytorch/computations.py | 3 +++ 5 files changed, 14 insertions(+) diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 74be5807..64a45e69 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -415,6 +415,7 @@ def CoM_position( def forward_dynamics( self, base_transform: Union[cs.SX, cs.DM], + base_velocity: Union[cs.SX, cs.DM], joint_positions: Union[cs.SX, cs.DM], joint_velocities: Union[cs.SX, cs.DM], joint_torques: Union[cs.SX, cs.DM], @@ -423,6 +424,7 @@ def forward_dynamics( Args: base_transform (Union[cs.SX, cs.DM]): The homogenous transform from base to world frame + base_velocity (Union[cs.SX, cs.DM]): The base velocity in mixed representation joint_positions (Union[cs.SX, cs.DM]): The joints position joint_velocities (Union[cs.SX, cs.DM]): The joints velocity joint_torques (Union[cs.SX, cs.DM]): The joints torque @@ -433,6 +435,7 @@ def forward_dynamics( """ return self.rbdalgos.aba( base_transform, + base_velocity, joint_positions, joint_velocities, joint_torques, diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 24a202d3..55ba5c0e 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -460,6 +460,7 @@ def rnea( def aba( self, base_transform: npt.ArrayLike, + base_velocity: npt.ArrayLike, joint_positions: npt.ArrayLike, joint_velocities: npt.ArrayLike, tau: npt.ArrayLike, @@ -469,6 +470,7 @@ def aba( Args: base_transform (T): The homogenous transform from base to world frame + base_velocity (T): The base velocity in mixed representation joint_positions (T): The joints position joint_velocities (T): The joints velocity tau (T): The generalized force variables diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index 183b47ee..e99a2848 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -243,6 +243,7 @@ def get_total_mass(self) -> float: def forward_dynamics( self, base_transform: jnp.array, + base_velocity: jnp.array, joint_positions: jnp.array, joint_velocities: jnp.array, joint_torques: jnp.array, @@ -251,6 +252,7 @@ def forward_dynamics( Args: base_transform (jnp.array): The homogenous transform from base to world frame + base_velocity (jnp.array): The base velocity in mixed representation joint_positions (jnp.array): The joints position joint_velocities (jnp.array): The joints velocity joint_torques (jnp.array): The joints torques @@ -261,6 +263,7 @@ def forward_dynamics( """ base_acceleration, joint_accelerations = self.rbdalgos.aba( base_transform, + base_velocity, joint_positions, joint_velocities, joint_torques, diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index f9b21303..badb1bf4 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -248,6 +248,7 @@ def get_total_mass(self) -> float: def forward_dynamics( self, base_transform: np.ndarray, + base_velocity: np.ndarray, joint_positions: np.ndarray, joint_velocities: np.ndarray, joint_torques: np.ndarray, @@ -256,6 +257,7 @@ def forward_dynamics( Args: base_transform (np.ndarray): The homogenous transform from base to world frame + base_velocity (np.ndarray): The base velocity in mixed representation joint_positions (np.ndarray): The joints position joint_velocities (np.ndarray): The joint velocities joint_torques (np.ndarray): The joint torques @@ -266,6 +268,7 @@ def forward_dynamics( """ return self.rbdalgos.aba( base_transform, + base_velocity, joint_positions, joint_velocities, joint_torques, diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index d4390087..c33dbc82 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -254,6 +254,7 @@ def get_total_mass(self) -> float: def forward_dynamic( self, base_transform: torch.Tensor, + base_velocity: torch.Tensor, joint_positions: torch.Tensor, joint_velocities: torch.Tensor, joint_torques: torch.Tensor, @@ -262,6 +263,7 @@ def forward_dynamic( Args: base_transform (torch.tensor): The homogenous transform from base to world frame + base_velocity (torch.tensor): The base velocity in mixed representation joint_positions (torch.tensor): The joints positions joint_velocities (torch.tensor): The joints velocities joint_torques (torch.tensor): The joints torques @@ -272,6 +274,7 @@ def forward_dynamic( """ return self.rbdalgos.aba( base_transform, + base_velocity, joint_positions, joint_velocities, joint_torques, From 8568e1719994211cc5eff7e78fcdf7e9d897461c Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Fri, 5 Jan 2024 18:23:39 +0100 Subject: [PATCH 13/30] Reduce model tree --- src/adam/model/model.py | 45 +++++++++++++++++++++++ src/adam/model/std_factories/std_link.py | 23 ++++++++++++ src/adam/model/tree.py | 47 ++++++++++++++++++++++++ 3 files changed, 115 insertions(+) diff --git a/src/adam/model/model.py b/src/adam/model/model.py index 3421cfdf..cad482d0 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -80,6 +80,51 @@ def build(factory: ModelFactory, joints_name_list: List[str] = None) -> "Model": floating_base=floating_base, ) + def reduce(self, joints_name_list: List[str]) -> "Model": + """reduce the model to a subset of joints + + Args: + joints_name_list (List[str]): the list of the joints to keep + + Returns: + Model: the reduced model + """ + + # check if the joints in the list are in the model + for joint_str in joints_name_list: + if joint_str not in self.joints.keys(): + raise ValueError( + f"{joint_str} is not in the robot model. Check the joints_name_list" + ) + + tree = self.tree.reduce(joints_name_list) + joints_list = list( + filter( + lambda joint: joint.name in self.actuated_joints, + self.joints.values(), + ) + ) + joints_list.sort(key=lambda joint: joint.idx) + # update nodes dict + links = {node.name: node.link for node in tree.graph.values()} + joints = {joint.name: joint for joint in joints_list} + frames = { + node.name: node.link + for node in tree.graph.values() + if node.link.inertial is None + } + + return Model( + name=self.name, + links=links, + frames=frames, + joints=joints, + tree=tree, + NDoF=len(joints_name_list), + actuated_joints=joints_name_list, + floating_base=self.floating_base, + ) + def get_joints_chain(self, root: str, target: str) -> List[Joint]: """generate the joints chains from a link to a link diff --git a/src/adam/model/std_factories/std_link.py b/src/adam/model/std_factories/std_link.py index cefac67c..6ecefac2 100644 --- a/src/adam/model/std_factories/std_link.py +++ b/src/adam/model/std_factories/std_link.py @@ -44,3 +44,26 @@ def homogeneous(self) -> npt.ArrayLike: self.inertial.origin.xyz, self.inertial.origin.rpy, ) + + def lump(self, other: "StdLink", relative_transform: npt.ArrayLike) -> "StdLink": + """lump two links together + + Args: + other (StdLink): the other link + relative_transform (npt.ArrayLike): the transform between the two links + + Returns: + StdLink: the lumped link + """ + other_inertia = ( + relative_transform.T @ other.spatial_inertia() @ relative_transform + ) + + # lump the inertial properties + lumped_mass = self.inertial.mass + other.inertial.mass + lumped_inertia = self.spatial_inertia() + other_inertia + + self.mass = lumped_mass + self.inertia = lumped_inertia + + return self diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index 8d99d328..e8447871 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -71,6 +71,53 @@ def build_tree(links: List[Link], joints: List[Joint]) -> "Tree": raise ValueError("The model has more than one root link") return Tree(nodes, root_link[0]) + def reduce(self, considered_joint_names: List[str]) -> "Tree": + """reduces the tree to the considered joints + + Args: + considered_joint_names (List[str]): the list of the considered joints + + Returns: + Tree: the reduced tree + """ + # find the nodes between two fixed joints + nodes_to_lump = list( + { + joint.child + for node in self.graph.values() + for joint in node.arcs + if joint.name not in considered_joint_names + } + ) + # TODO: neck_2, l_wrist_1, r_wrist_1 don't get lumped + while nodes_to_lump != []: + node = self.graph[nodes_to_lump.pop()] + parent_node = self.graph[node.parent.name] + + # lump the inertial properties + parent_node.link = node.parent.lump( # r_hip_1 + other=node.link, # r_hip_2 + relative_transform=node.parent_arc.spatial_transform(0), + ) + + # update the parent + node.parent = parent_node.link + + # update the children + if node.name in parent_node.children: + parent_node.children.remove(node.name) + parent_node.children.append(node.children) + + # update the arcs + if node.parent_arc.name not in considered_joint_names: + parent_node.arcs.remove(node.parent_arc) + parent_node.arcs.append(node.arcs) + + # remove the node + self.graph.pop(node.name) + + return Tree(self.graph, self.root) + def print(self, root) -> str: """prints the tree From 241750f1659d8d5ada0ccfadae3fa3427572a630 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Fri, 12 Jan 2024 16:15:53 +0100 Subject: [PATCH 14/30] Use reduce model in ABA --- src/adam/core/rbd_algorithms.py | 110 +++++++++++++++++++------------- 1 file changed, 65 insertions(+), 45 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 55ba5c0e..2876e7a7 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -480,78 +480,98 @@ def aba( base_acceleration (T): The base acceleration in mixed representation joint_accelerations (T): The joints acceleration """ - i_X_pi = self.math.factory.zeros(self.model.N, 6, 6) - v = self.math.factory.zeros(self.model.N, 6, 1) - c = self.math.factory.zeros(self.model.N, 6, 1) - pA = self.math.factory.zeros(self.model.N, 6, 1) - IA = self.math.factory.zeros(self.model.N, 6, 6) - U = self.math.factory.zeros(self.model.N, 6, 1) - D = self.math.factory.zeros(self.model.N, 1, 1) - u = self.math.factory.zeros(self.model.N, 1, 1) - a = self.math.factory.zeros(self.model.N, 6, 1) - sdd = self.math.factory.zeros(self.model.N, 1, 1) - B_X_W = self.math.adjoint_mixed_inverse(base_transform) + model = self.model.reduce(self.model.actuated_joints) - # Pass 1 - for i, node in enumerate(self.model.tree): - link_i, joint_i, link_pi = node.get_elements() + joints = list( + filter( + lambda joint: joint.name in self.model.actuated_joints, + self.model.joints.values(), + ) + ) - if link_i.name == self.root_link: - continue - q = joint_positions[joint_i.idx] if joint_i.idx is not None else 0.0 - q_dot = joint_velocities[joint_i.idx] if joint_i.idx is not None else 0.0 + joints.sort(key=lambda joint: joint.idx) + + NB = model.N + + i_X_pi = self.math.factory.zeros(NB, 6, 6) + v = self.math.factory.zeros(NB, 6, 1) + c = self.math.factory.zeros(NB, 6, 1) + pA = self.math.factory.zeros(NB, 6, 1) + IA = self.math.factory.zeros(NB, 6, 6) + U = self.math.factory.zeros(NB, 6, 1) + D = self.math.factory.zeros(NB, 1, 1) + u = self.math.factory.zeros(NB, 1, 1) + + a = self.math.factory.zeros(NB, 6, 1) + sdd = self.math.factory.zeros(NB, 1, 1) + B_X_W = self.math.adjoint_mixed(base_transform) + + if self.model.floating_base: + IA[0] = self.model.tree.get_node_from_name( + self.root_link + ).link.spatial_inertia() + v[0] = B_X_W @ base_velocity + pA[0] = ( + self.math.spatial_skew_star(v[0]) @ IA[0] @ v[0] + ) # - self.math.adjoint_inverse(B_X_W).T @ f_ext[0] + tree_transform = self.model.tree - pi = self.model.tree.get_idx_from_name(link_pi.name) + # Pass 1 + for i, joint in enumerate(joints[1:], start=1): + q = joint_positions[i] + q_dot = joint_velocities[i] + + pi = model.tree.get_idx_from_name(joint.parent) # Parent-child transform - i_X_pi[i] = joint_i.spatial_transform(q) - v_J = joint_i.motion_subspace() * q_dot + i_X_pi[i] = joint.spatial_transform(q) + v_J = joint.motion_subspace() * q_dot v[i] = i_X_pi[i] @ v[pi] + v_J c[i] = i_X_pi[i] @ c[pi] + self.math.spatial_skew(v[i]) @ v_J - IA[i] = link_i.spatial_inertia() + IA[i] = model.tree.get_node_from_name(joint.parent).link.spatial_inertia() pA[i] = IA[i] @ c[i] + self.math.spatial_skew_star(v[i]) @ IA[i] @ v[i] # Pass 2 - for i, node in reversed(list(enumerate(self.model.tree))): - link_i, joint_i, link_pi = node.get_elements() - - if link_i.name == self.root_link: - IA[pi] += i_X_pi[i].T @ Ia @ i_X_pi[i] - pA[pi] += i_X_pi[i].T @ pa - continue - - pi = self.model.tree.get_idx_from_name(link_pi.name) - tau_i = tau[joint_i.idx] if joint_i.idx is not None else 0.0 + for i, joint in reversed( + list( + enumerate( + joints[1:], + start=1, + ) + ) + ): + pi = self.model.tree.get_idx_from_name(joint.parent) - U[i] = IA[i] @ joint_i.motion_subspace() - D[i] = joint_i.motion_subspace().T @ U[i] - u[i] = self.math.vertcat(tau_i) - joint_i.motion_subspace().T @ pA[i] + U[i] = IA[i] @ joint.motion_subspace() + D[i] = joint.motion_subspace().T @ U[i] + u[i] = self.math.vertcat(tau[joint.idx]) - joint.motion_subspace().T @ pA[i] Ia = IA[i] - U[i] / D[i] @ U[i].T pa = pA[i] + Ia @ c[i] + U[i] * u[i] / D[i] + if joint.parent != self.root_link or not self.model.floating_base: + IA[pi] += i_X_pi[i].T @ Ia @ i_X_pi[i] + pA[pi] += i_X_pi[i].T @ pa + continue + a[0] = B_X_W @ g if self.model.floating_base else self.math.solve(-IA[0], pA[0]) # Pass 3 - for i, node in enumerate(self.model.tree): - link_i, joint_i, link_pi = node.get_elements() - - if link_i.name == self.root_link: + for i, joint in enumerate(joints[1:], start=1): + if joint.parent == self.root_link: continue - pi = self.model.tree.get_idx_from_name(link_pi.name) + pi = self.model.tree.get_idx_from_name(joint.parent) sdd[i - 1] = (u[i] - U[i].T @ a[i]) / D[i] - a[i] += i_X_pi[i].T @ a[pi] + joint_i.motion_subspace() * sdd[i - 1] + c[i] + a[i] += i_X_pi[i].T @ a[pi] + joint.motion_subspace() * sdd[i - 1] + c[i] - # Filter sdd to remove NaNs generate with lumped joints - s_ddot = self.math.vertcat( - *[sdd[i] for i in range(self.model.N) if sdd[i] == sdd[i]] - ) + # Squeeze sdd + s_ddot = self.math.vertcat(*[sdd[i] for i in range(sdd.shape[0])]) if ( self.frame_velocity_representation From eda6cd3c2cc922d0fb1d49fe2bf892525f889b43 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Wed, 17 Jan 2024 17:12:51 +0100 Subject: [PATCH 15/30] Add inverse function in spatial math --- src/adam/casadi/casadi_like.py | 18 ++++++++++++++++++ src/adam/core/spatial_math.py | 4 ++++ src/adam/jax/jax_like.py | 11 +++++++++++ src/adam/numpy/numpy_like.py | 14 ++++++++++++++ src/adam/pytorch/torch_like.py | 14 ++++++++++++++ 5 files changed, 61 insertions(+) diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py index 94a9bd34..be56d7dd 100644 --- a/src/adam/casadi/casadi_like.py +++ b/src/adam/casadi/casadi_like.py @@ -155,6 +155,24 @@ def skew(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": else: return CasadiLike(cs.skew(x)) + @staticmethod + def inv(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": + """ + Args: + x (Union[CasadiLike, npt.ArrayLike]): matrix + + Returns: + CasadiLike: inverse of x + """ + if isinstance(x, CasadiLike): + return CasadiLike(cs.inv(x.array)) + else: + return ( + CasadiLike(cs.inv(x)) + if x.size1() <= 5 + else CasadiLike(self.solve(x, self.eye(x.size1()))) + ) + @staticmethod def sin(x: npt.ArrayLike) -> "CasadiLike": """ diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index 0bfdd089..e2cb800d 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -158,6 +158,10 @@ def cos(x: npt.ArrayLike) -> npt.ArrayLike: def skew(x): pass + @abc.abstractmethod + def inv(x): + pass + @abc.abstractmethod def solve(A: npt.ArrayLike, b: npt.ArrayLike) -> npt.ArrayLike: """ diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py index 06aff4bb..bfa347a9 100644 --- a/src/adam/jax/jax_like.py +++ b/src/adam/jax/jax_like.py @@ -194,6 +194,17 @@ def skew(x: Union["JaxLike", npt.ArrayLike]) -> "JaxLike": x = x.array return JaxLike(-jnp.cross(jnp.array(x), jnp.eye(3), axisa=0, axisb=0)) + @staticmethod + def inv(x: "JaxLike") -> "JaxLike": + """ + Args: + x (JaxLike): Matrix + + Returns: + JaxLike: Inverse of x + """ + return JaxLike(jnp.linalg.inv(x.array)) + @staticmethod def vertcat(*x) -> "JaxLike": """ diff --git a/src/adam/numpy/numpy_like.py b/src/adam/numpy/numpy_like.py index c0f26a31..4e5700f1 100644 --- a/src/adam/numpy/numpy_like.py +++ b/src/adam/numpy/numpy_like.py @@ -221,6 +221,20 @@ def skew(x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike": x = x.array return NumpyLike(-np.cross(np.array(x), np.eye(3), axisa=0, axisb=0)) + @staticmethod + def inv(x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike": + """ + Args: + x (Union["NumpyLike", npt.ArrayLike]): matrix + + Returns: + NumpyLike: inverse of x + """ + if isinstance(x, NumpyLike): + return NumpyLike(np.linalg.inv(x.array)) + else: + return NumpyLike(np.linalg.inv(x)) + @staticmethod def solve(A: "NumpyLike", b: "NumpyLike") -> "NumpyLike": """ diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py index 4753d45d..8ffe0c14 100644 --- a/src/adam/pytorch/torch_like.py +++ b/src/adam/pytorch/torch_like.py @@ -209,6 +209,20 @@ def skew(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": torch.FloatTensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) ) + @staticmethod + def inv(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": + """ + Args: + x (Union["TorchLike", ntp.ArrayLike]): matrix + + Returns: + TorchLike: inverse of x + """ + if isinstance(x, TorchLike): + return TorchLike(torch.inverse(x.array)) + else: + return TorchLike(torch.inverse(torch.tensor(x))) + @staticmethod def vertcat(*x: ntp.ArrayLike) -> "TorchLike": """ From de550d9aa4a8a0a6e87d75241daa81a8f876200f Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Wed, 17 Jan 2024 17:16:56 +0100 Subject: [PATCH 16/30] Update tree transform and parent array calculation --- src/adam/core/rbd_algorithms.py | 56 +++++++++++++++++++++++++-------- 1 file changed, 43 insertions(+), 13 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 2876e7a7..06daf68b 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -514,21 +514,55 @@ def aba( pA[0] = ( self.math.spatial_skew_star(v[0]) @ IA[0] @ v[0] ) # - self.math.adjoint_inverse(B_X_W).T @ f_ext[0] - tree_transform = self.model.tree + + def get_tree_transform(self, joints) -> "Array": + """returns the tree transform + + Returns: + Array: the tree transform + """ + relative_transform = lambda j: self.math.inv( + self.model.tree.graph[j.parent].parent_arc.spatial_transform(0) + ) @ j.spatial_transform(0) + + return self.math.vertcat( + [self.math.factory.eye(6).array] + + list( + map( + lambda j: relative_transform(j).array + if j.parent != self.root_link + else self.math.factory.eye(6).array, + joints, + ) + ) + ) + + tree_transform = get_tree_transform(self, joints) + + find_parent = ( + lambda j: find_parent(model.tree.get_node_from_name(j.parent).parent_arc) + if model.tree.get_node_from_name(j.parent).parent_arc.idx is None + else model.tree.get_node_from_name(j.parent).parent_arc.idx + ) + + p = [-1] + [ + model.tree.get_idx_from_name(i.parent) + if model.tree.get_idx_from_name(i.parent) < NB + else find_parent(i) + for i in joints + ] # Pass 1 for i, joint in enumerate(joints[1:], start=1): q = joint_positions[i] q_dot = joint_velocities[i] - pi = model.tree.get_idx_from_name(joint.parent) - # Parent-child transform - i_X_pi[i] = joint.spatial_transform(q) + i_X_pi[i] = joint.spatial_transform(q) @ tree_transform[i] v_J = joint.motion_subspace() * q_dot - v[i] = i_X_pi[i] @ v[pi] + v_J - c[i] = i_X_pi[i] @ c[pi] + self.math.spatial_skew(v[i]) @ v_J + v[i] = i_X_pi[i] @ v[p[i]] + v_J + c[i] = i_X_pi[i] @ c[p[i]] + self.math.spatial_skew(v[i]) @ v_J IA[i] = model.tree.get_node_from_name(joint.parent).link.spatial_inertia() @@ -543,8 +577,6 @@ def aba( ) ) ): - pi = self.model.tree.get_idx_from_name(joint.parent) - U[i] = IA[i] @ joint.motion_subspace() D[i] = joint.motion_subspace().T @ U[i] u[i] = self.math.vertcat(tau[joint.idx]) - joint.motion_subspace().T @ pA[i] @@ -553,8 +585,8 @@ def aba( pa = pA[i] + Ia @ c[i] + U[i] * u[i] / D[i] if joint.parent != self.root_link or not self.model.floating_base: - IA[pi] += i_X_pi[i].T @ Ia @ i_X_pi[i] - pA[pi] += i_X_pi[i].T @ pa + IA[p[i]] += i_X_pi[i].T @ Ia @ i_X_pi[i] + pA[p[i]] += i_X_pi[i].T @ pa continue a[0] = B_X_W @ g if self.model.floating_base else self.math.solve(-IA[0], pA[0]) @@ -564,11 +596,9 @@ def aba( if joint.parent == self.root_link: continue - pi = self.model.tree.get_idx_from_name(joint.parent) - sdd[i - 1] = (u[i] - U[i].T @ a[i]) / D[i] - a[i] += i_X_pi[i].T @ a[pi] + joint.motion_subspace() * sdd[i - 1] + c[i] + a[i] += i_X_pi[i].T @ a[p[i]] + joint.motion_subspace() * sdd[i - 1] + c[i] # Squeeze sdd s_ddot = self.math.vertcat(*[sdd[i] for i in range(sdd.shape[0])]) From e4a2c4aa3b5ac9d733e15b9dd35c98af17296fb4 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Wed, 17 Jan 2024 17:17:22 +0100 Subject: [PATCH 17/30] Return SpatialMath object in ABA --- src/adam/core/rbd_algorithms.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 06daf68b..bfafd028 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -607,10 +607,10 @@ def get_tree_transform(self, joints) -> "Array": self.frame_velocity_representation == Representations.BODY_FIXED_REPRESENTATION ): - return a[0], s_ddot + return self.math.horzcat(a[0], s_ddot) elif self.frame_velocity_representation == Representations.MIXED_REPRESENTATION: - return ( + return self.math.horzcat( self.math.vertcat( self.math.solve(B_X_W, a[0]) + g if self.model.floating_base From a9d1b16b40ad10116dfa960ceccacf8ebcb5d10b Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Wed, 17 Jan 2024 17:26:24 +0100 Subject: [PATCH 18/30] Update tests --- tests/body_fixed/test_CasADi_body_fixed.py | 12 +++++++++++- tests/body_fixed/test_Jax_body_fixed.py | 11 ++++------- tests/body_fixed/test_NumPy_body_fixed.py | 12 +++++++++++- tests/body_fixed/test_pytorch_body_fixed.py | 12 +++++++++++- tests/mixed/test_CasADi_mixed.py | 12 +++++++++++- tests/mixed/test_Jax_mixed.py | 13 +++++++++++-- tests/mixed/test_NumPy_mixed.py | 12 +++++++++++- tests/mixed/test_pytorch_mixed.py | 12 +++++++++++- 8 files changed, 81 insertions(+), 15 deletions(-) diff --git a/tests/body_fixed/test_CasADi_body_fixed.py b/tests/body_fixed/test_CasADi_body_fixed.py index aa1d8ed8..7ee75b47 100644 --- a/tests/body_fixed/test_CasADi_body_fixed.py +++ b/tests/body_fixed/test_CasADi_body_fixed.py @@ -242,4 +242,14 @@ def test_relative_jacobian(): def test_fd(): - pass + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_Jax_body_fixed.py b/tests/body_fixed/test_Jax_body_fixed.py index b4a9c3ea..72d33537 100644 --- a/tests/body_fixed/test_Jax_body_fixed.py +++ b/tests/body_fixed/test_Jax_body_fixed.py @@ -6,7 +6,6 @@ import icub_models import idyntree.swig as idyntree -import jax.numpy as jnp import numpy as np import pytest from jax import config @@ -197,14 +196,12 @@ def test_gravity_term(): def test_fd(): joint_torques = np.random.rand(n_dofs) joints_vel = np.random.rand(n_dofs) - reference_acc = jnp.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( - jnp.concatenate((jnp.zeros(6), joint_torques)) - - comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) - - comp.gravity_term(H_b, joints_val) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) ) base_acc, joint_acc = comp.forward_dynamics( - H_b, joints_val, joints_vel, joint_torques + H_b, base_vel, joints_vel, joints_vel, joint_torques ) - assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_NumPy_body_fixed.py b/tests/body_fixed/test_NumPy_body_fixed.py index 80cf6e1b..47f783ef 100644 --- a/tests/body_fixed/test_NumPy_body_fixed.py +++ b/tests/body_fixed/test_NumPy_body_fixed.py @@ -192,4 +192,14 @@ def test_gravity_term(): def test_fd(): - pass + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/body_fixed/test_pytorch_body_fixed.py b/tests/body_fixed/test_pytorch_body_fixed.py index 13be362f..6d7fc041 100644 --- a/tests/body_fixed/test_pytorch_body_fixed.py +++ b/tests/body_fixed/test_pytorch_body_fixed.py @@ -194,4 +194,14 @@ def test_gravity_term(): def test_fd(): - pass + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_CasADi_mixed.py b/tests/mixed/test_CasADi_mixed.py index f9288ed4..3779f995 100644 --- a/tests/mixed/test_CasADi_mixed.py +++ b/tests/mixed/test_CasADi_mixed.py @@ -242,4 +242,14 @@ def test_relative_jacobian(): def test_fd(): - pass + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_Jax_mixed.py b/tests/mixed/test_Jax_mixed.py index 1123c3f0..93eb750e 100644 --- a/tests/mixed/test_Jax_mixed.py +++ b/tests/mixed/test_Jax_mixed.py @@ -6,7 +6,6 @@ import icub_models import idyntree.swig as idyntree -import jax.numpy as jnp import numpy as np import pytest from jax import config @@ -195,4 +194,14 @@ def test_gravity_term(): def test_fd(): - pass + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_NumPy_mixed.py b/tests/mixed/test_NumPy_mixed.py index 3ecc04fd..2e5f6516 100644 --- a/tests/mixed/test_NumPy_mixed.py +++ b/tests/mixed/test_NumPy_mixed.py @@ -191,4 +191,14 @@ def test_gravity_term(): def test_fd(): - pass + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_pytorch_mixed.py b/tests/mixed/test_pytorch_mixed.py index 78d7f882..b97bc719 100644 --- a/tests/mixed/test_pytorch_mixed.py +++ b/tests/mixed/test_pytorch_mixed.py @@ -194,4 +194,14 @@ def test_gravity_term(): def test_fd(): - pass + joint_torques = np.random.rand(n_dofs) + joints_vel = np.random.rand(n_dofs) + reference_acc = np.linalg.inv(comp.mass_matrix(H_b, joints_val)) @ ( + np.concatenate((np.zeros(6), joint_torques)) + - comp.bias_force(H_b, joints_val, base_vel, joints_vel) + ) + base_acc, joint_acc = comp.forward_dynamics( + H_b, base_vel, joints_vel, joints_vel, joint_torques + ) + assert base_acc - reference_acc[:6] == pytest.approx(0.0, abs=1e-4) + assert joint_acc - reference_acc[6:] == pytest.approx(0.0, abs=1e-4) From 874c16c92fe4689e7edd97276749bee896c65b3b Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Thu, 18 Jan 2024 19:11:41 +0100 Subject: [PATCH 19/30] Update tree reduction --- src/adam/model/tree.py | 83 ++++++++++++++++++++++++++++++------------ 1 file changed, 59 insertions(+), 24 deletions(-) diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index e8447871..0b72dd4e 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -1,4 +1,6 @@ import dataclasses +import logging + from typing import Dict, Iterable, List, Tuple, Union from adam.model.abc_factories import Joint, Link @@ -89,32 +91,65 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree": if joint.name not in considered_joint_names } ) - # TODO: neck_2, l_wrist_1, r_wrist_1 don't get lumped - while nodes_to_lump != []: - node = self.graph[nodes_to_lump.pop()] - parent_node = self.graph[node.parent.name] - - # lump the inertial properties - parent_node.link = node.parent.lump( # r_hip_1 - other=node.link, # r_hip_2 - relative_transform=node.parent_arc.spatial_transform(0), - ) - - # update the parent - node.parent = parent_node.link - # update the children - if node.name in parent_node.children: - parent_node.children.remove(node.name) - parent_node.children.append(node.children) - - # update the arcs - if node.parent_arc.name not in considered_joint_names: - parent_node.arcs.remove(node.parent_arc) - parent_node.arcs.append(node.arcs) + relative_transform = ( + lambda node: node.link.math.inv( + self.graph[node.parent.name].parent_arc.spatial_transform(0) + ) + @ node.parent_arc.spatial_transform(0) + if node.parent.name != self.root + else node.parent_arc.spatial_transform(0) + ) - # remove the node - self.graph.pop(node.name) + last = [] + leaves = [node for node in self.graph.values() if node.children == last] + + while all(leaf.name != self.root for leaf in leaves): + for leaf in leaves: + if leaf is self.graph[self.root]: + continue + + if leaf.parent_arc.name not in considered_joint_names: + # create the new node + new_node = Node( + name=leaf.parent.name, + link=None, + arcs=[], + children=None, + parent=None, + parent_arc=None, + ) + + # update the link + new_node.link = leaf.parent.lump( + other=leaf.link, + relative_transform=relative_transform(leaf), + ) + + # update the parents + new_node.parent = self.graph[leaf.parent.name].parent + new_node.parent_arc = self.graph[new_node.name].parent_arc + new_node.parent_arc.parent = ( + leaf.children[0].parent_arc.name if leaf.children != [] else [] + ) + + # update the children + new_node.children = leaf.children + + # update the arcs + if leaf.arcs != []: + for arc in leaf.arcs: + if arc.name in considered_joint_names: + new_node.arcs.append(arc) + + logging.debug(f"Removing {leaf.name}") + self.graph.pop(leaf.name) + self.graph[new_node.name] = new_node + leaves = [ + self.get_node_from_name((leaf.parent.name)) + for leaf in leaves + if leaf.name != self.root + ] return Tree(self.graph, self.root) From 01533b626e60bf3104faf92d9a89fb25c7320ce8 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Thu, 18 Jan 2024 19:12:17 +0100 Subject: [PATCH 20/30] Update model reduction logic --- src/adam/core/rbd_algorithms.py | 54 +++++++++++---------------------- src/adam/model/model.py | 12 +++----- src/adam/model/tree.py | 44 ++++++++++++--------------- 3 files changed, 42 insertions(+), 68 deletions(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index bfafd028..8b94cbc1 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -481,15 +481,7 @@ def aba( joint_accelerations (T): The joints acceleration """ model = self.model.reduce(self.model.actuated_joints) - - joints = list( - filter( - lambda joint: joint.name in self.model.actuated_joints, - self.model.joints.values(), - ) - ) - - joints.sort(key=lambda joint: joint.idx) + joints = list(model.joints.values()) NB = model.N @@ -506,10 +498,8 @@ def aba( sdd = self.math.factory.zeros(NB, 1, 1) B_X_W = self.math.adjoint_mixed(base_transform) - if self.model.floating_base: - IA[0] = self.model.tree.get_node_from_name( - self.root_link - ).link.spatial_inertia() + if model.floating_base: + IA[0] = model.tree.get_node_from_name(self.root_link).link.spatial_inertia() v[0] = B_X_W @ base_velocity pA[0] = ( self.math.spatial_skew_star(v[0]) @ IA[0] @ v[0] @@ -522,7 +512,7 @@ def get_tree_transform(self, joints) -> "Array": Array: the tree transform """ relative_transform = lambda j: self.math.inv( - self.model.tree.graph[j.parent].parent_arc.spatial_transform(0) + model.tree.graph[j.child].parent_arc.spatial_transform(0) ) @ j.spatial_transform(0) return self.math.vertcat( @@ -538,19 +528,7 @@ def get_tree_transform(self, joints) -> "Array": ) tree_transform = get_tree_transform(self, joints) - - find_parent = ( - lambda j: find_parent(model.tree.get_node_from_name(j.parent).parent_arc) - if model.tree.get_node_from_name(j.parent).parent_arc.idx is None - else model.tree.get_node_from_name(j.parent).parent_arc.idx - ) - - p = [-1] + [ - model.tree.get_idx_from_name(i.parent) - if model.tree.get_idx_from_name(i.parent) < NB - else find_parent(i) - for i in joints - ] + p = lambda i: list(model.tree.graph).index(joints[i].parent) # Pass 1 for i, joint in enumerate(joints[1:], start=1): @@ -561,8 +539,8 @@ def get_tree_transform(self, joints) -> "Array": i_X_pi[i] = joint.spatial_transform(q) @ tree_transform[i] v_J = joint.motion_subspace() * q_dot - v[i] = i_X_pi[i] @ v[p[i]] + v_J - c[i] = i_X_pi[i] @ c[p[i]] + self.math.spatial_skew(v[i]) @ v_J + v[i] = i_X_pi[i] @ v[p(i)] + v_J + c[i] = i_X_pi[i] @ c[p(i)] + self.math.spatial_skew(v[i]) @ v_J IA[i] = model.tree.get_node_from_name(joint.parent).link.spatial_inertia() @@ -579,17 +557,21 @@ def get_tree_transform(self, joints) -> "Array": ): U[i] = IA[i] @ joint.motion_subspace() D[i] = joint.motion_subspace().T @ U[i] - u[i] = self.math.vertcat(tau[joint.idx]) - joint.motion_subspace().T @ pA[i] + u[i] = ( + self.math.vertcat(tau[joint.idx]) - joint.motion_subspace().T @ pA[i] + if joint.idx is not None + else 0.0 + ) Ia = IA[i] - U[i] / D[i] @ U[i].T pa = pA[i] + Ia @ c[i] + U[i] * u[i] / D[i] - if joint.parent != self.root_link or not self.model.floating_base: - IA[p[i]] += i_X_pi[i].T @ Ia @ i_X_pi[i] - pA[p[i]] += i_X_pi[i].T @ pa + if joint.parent != self.root_link or not model.floating_base: + IA[p(i)] += i_X_pi[i].T @ Ia @ i_X_pi[i] + pA[p(i)] += i_X_pi[i].T @ pa continue - a[0] = B_X_W @ g if self.model.floating_base else self.math.solve(-IA[0], pA[0]) + a[0] = B_X_W @ g if model.floating_base else self.math.solve(-IA[0], pA[0]) # Pass 3 for i, joint in enumerate(joints[1:], start=1): @@ -598,7 +580,7 @@ def get_tree_transform(self, joints) -> "Array": sdd[i - 1] = (u[i] - U[i].T @ a[i]) / D[i] - a[i] += i_X_pi[i].T @ a[p[i]] + joint.motion_subspace() * sdd[i - 1] + c[i] + a[i] += i_X_pi[i].T @ a[p(i)] + joint.motion_subspace() * sdd[i - 1] + c[i] # Squeeze sdd s_ddot = self.math.vertcat(*[sdd[i] for i in range(sdd.shape[0])]) @@ -613,7 +595,7 @@ def get_tree_transform(self, joints) -> "Array": return self.math.horzcat( self.math.vertcat( self.math.solve(B_X_W, a[0]) + g - if self.model.floating_base + if model.floating_base else self.math.zeros(6, 1), ), s_ddot, diff --git a/src/adam/model/model.py b/src/adam/model/model.py index cad482d0..1c9903bc 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -98,14 +98,12 @@ def reduce(self, joints_name_list: List[str]) -> "Model": ) tree = self.tree.reduce(joints_name_list) - joints_list = list( - filter( - lambda joint: joint.name in self.actuated_joints, - self.joints.values(), - ) - ) + + joints_list = [ + node.parent_arc for node in tree.graph.values() if node.name != tree.root + ] joints_list.sort(key=lambda joint: joint.idx) - # update nodes dict + links = {node.name: node.link for node in tree.graph.values()} joints = {joint.name: joint for joint in joints_list} frames = { diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index 0b72dd4e..ef6d2af0 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -82,15 +82,6 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree": Returns: Tree: the reduced tree """ - # find the nodes between two fixed joints - nodes_to_lump = list( - { - joint.child - for node in self.graph.values() - for joint in node.arcs - if joint.name not in considered_joint_names - } - ) relative_transform = ( lambda node: node.link.math.inv( @@ -101,21 +92,17 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree": else node.parent_arc.spatial_transform(0) ) - last = [] - leaves = [node for node in self.graph.values() if node.children == last] + # find the tree leaves and proceed until the root + leaves = [node for node in self.graph.values() if node.children == []] while all(leaf.name != self.root for leaf in leaves): for leaf in leaves: - if leaf is self.graph[self.root]: - continue - - if leaf.parent_arc.name not in considered_joint_names: - # create the new node + if leaf.parent_arc.name not in considered_joint_names + [self.root]: new_node = Node( name=leaf.parent.name, link=None, arcs=[], - children=None, + children=[], parent=None, parent_arc=None, ) @@ -129,22 +116,29 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree": # update the parents new_node.parent = self.graph[leaf.parent.name].parent new_node.parent_arc = self.graph[new_node.name].parent_arc - new_node.parent_arc.parent = ( - leaf.children[0].parent_arc.name if leaf.children != [] else [] - ) # update the children - new_node.children = leaf.children + new_node.children = [ + child for child in leaf.children if child.name in self.graph + ] + + for child in new_node.children: + child.parent = new_node.link + child.parent_arc = new_node.parent_arc # update the arcs - if leaf.arcs != []: - for arc in leaf.arcs: - if arc.name in considered_joint_names: - new_node.arcs.append(arc) + new_node.arcs = ( + [arc for arc in leaf.arcs if arc.name in considered_joint_names] + if leaf.arcs != [] + else [] + ) + for j in new_node.arcs: + j.parent = new_node.link.name logging.debug(f"Removing {leaf.name}") self.graph.pop(leaf.name) self.graph[new_node.name] = new_node + self.ordered_nodes_list.remove(leaf.name) leaves = [ self.get_node_from_name((leaf.parent.name)) for leaf in leaves From e053633d743491c286c35ab0b8c311c48112504a Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Fri, 19 Jan 2024 15:37:43 +0100 Subject: [PATCH 21/30] Add `vee` operator --- src/adam/casadi/casadi_like.py | 15 +++++++++++++++ src/adam/core/spatial_math.py | 4 ++++ src/adam/jax/jax_like.py | 14 ++++++++++++++ src/adam/numpy/numpy_like.py | 13 +++++++++++++ src/adam/pytorch/torch_like.py | 16 ++++++++++++++++ 5 files changed, 62 insertions(+) diff --git a/src/adam/casadi/casadi_like.py b/src/adam/casadi/casadi_like.py index be56d7dd..fbdee660 100644 --- a/src/adam/casadi/casadi_like.py +++ b/src/adam/casadi/casadi_like.py @@ -155,6 +155,21 @@ def skew(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": else: return CasadiLike(cs.skew(x)) + @staticmethod + def vee(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": + """ + Args: + x (Union[CasadiLike, npt.ArrayLike]): 3x3 matrix + + Returns: + CasadiLike: the vector from the skew symmetric matrix x + """ + vee = lambda x: cs.vertcat(x[2, 1], x[0, 2], x[1, 0]) + if isinstance(x, CasadiLike): + return CasadiLike(vee(x.array)) + else: + return CasadiLike(vee(x)) + @staticmethod def inv(x: Union["CasadiLike", npt.ArrayLike]) -> "CasadiLike": """ diff --git a/src/adam/core/spatial_math.py b/src/adam/core/spatial_math.py index e2cb800d..778257ee 100644 --- a/src/adam/core/spatial_math.py +++ b/src/adam/core/spatial_math.py @@ -158,6 +158,10 @@ def cos(x: npt.ArrayLike) -> npt.ArrayLike: def skew(x): pass + @abc.abstractmethod + def vee(x): + pass + @abc.abstractmethod def inv(x): pass diff --git a/src/adam/jax/jax_like.py b/src/adam/jax/jax_like.py index bfa347a9..89b95d79 100644 --- a/src/adam/jax/jax_like.py +++ b/src/adam/jax/jax_like.py @@ -194,6 +194,20 @@ def skew(x: Union["JaxLike", npt.ArrayLike]) -> "JaxLike": x = x.array return JaxLike(-jnp.cross(jnp.array(x), jnp.eye(3), axisa=0, axisb=0)) + @staticmethod + def vee(x: Union["JaxLike", npt.ArrayLike]) -> "JaxLike": + """ + Args: + x (Union[JaxLike, npt.ArrayLike]): matrix + + Returns: + JaxLike: the vee operator from x + """ + if not isinstance(x, JaxLike): + return JaxLike(jnp.array([x[2, 1], x[0, 2], x[1, 0]])) + x = x.array + return JaxLike(jnp.array([x[2, 1], x[0, 2], x[1, 0]])) + @staticmethod def inv(x: "JaxLike") -> "JaxLike": """ diff --git a/src/adam/numpy/numpy_like.py b/src/adam/numpy/numpy_like.py index 4e5700f1..4a761f2c 100644 --- a/src/adam/numpy/numpy_like.py +++ b/src/adam/numpy/numpy_like.py @@ -221,6 +221,19 @@ def skew(x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike": x = x.array return NumpyLike(-np.cross(np.array(x), np.eye(3), axisa=0, axisb=0)) + @staticmethod + def vee(x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike": + """ + Args: + x (Union[NumpyLike, npt.ArrayLike]): skew symmetric matrix + + Returns: + NumpyLike: vector from the skew symmetric matrix + """ + if not isinstance(x, NumpyLike): + return np.array([x[2, 1], x[0, 2], x[1, 0]]) + return np.array([x.array[2, 1], x.array[0, 2], x.array[1, 0]]) + @staticmethod def inv(x: Union["NumpyLike", npt.ArrayLike]) -> "NumpyLike": """ diff --git a/src/adam/pytorch/torch_like.py b/src/adam/pytorch/torch_like.py index 32708dd0..c9d7091a 100644 --- a/src/adam/pytorch/torch_like.py +++ b/src/adam/pytorch/torch_like.py @@ -220,6 +220,22 @@ def skew(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": torch.tensor([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) ) + @staticmethod + def vee(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": + """ + Args: + x (Union["TorchLike", ntp.ArrayLike]): matrix + + Returns: + TorchLike: vector from skew matrix x + """ + if isinstance(x, TorchLike): + return TorchLike( + torch.tensor([x.array[2, 1], x.array[0, 2], x.array[1, 0]]) + ) + else: + return TorchLike(torch.tensor([x[2, 1], x[0, 2], x[1, 0]])) + @staticmethod def inv(x: Union["TorchLike", ntp.ArrayLike]) -> "TorchLike": """ From e7d2f3ed82205fc819d09cf589561b71e6ee8627 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Tue, 20 Feb 2024 18:02:54 +0100 Subject: [PATCH 22/30] Make StdJoint and StdLink hashable --- src/adam/model/std_factories/std_joint.py | 3 +++ src/adam/model/std_factories/std_link.py | 3 +++ 2 files changed, 6 insertions(+) diff --git a/src/adam/model/std_factories/std_joint.py b/src/adam/model/std_factories/std_joint.py index a72787d8..3043b795 100644 --- a/src/adam/model/std_factories/std_joint.py +++ b/src/adam/model/std_factories/std_joint.py @@ -104,3 +104,6 @@ def motion_subspace(self) -> npt.ArrayLike: 0, 0, ) + + def __hash__(self) -> int: + return hash(self.name) diff --git a/src/adam/model/std_factories/std_link.py b/src/adam/model/std_factories/std_link.py index 6ecefac2..d8c4f2e5 100644 --- a/src/adam/model/std_factories/std_link.py +++ b/src/adam/model/std_factories/std_link.py @@ -67,3 +67,6 @@ def lump(self, other: "StdLink", relative_transform: npt.ArrayLike) -> "StdLink" self.inertia = lumped_inertia return self + + def __hash__(self): + return hash(self.name) From 71995acaaf80df2e620bdf5074ceaa4079512550 Mon Sep 17 00:00:00 2001 From: Filippo Luca Ferretti Date: Tue, 20 Feb 2024 18:03:24 +0100 Subject: [PATCH 23/30] Add method to get the joint list --- src/adam/model/tree.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index ef6d2af0..f01e1505 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -1,7 +1,7 @@ import dataclasses import logging -from typing import Dict, Iterable, List, Tuple, Union +from typing import Dict, Iterable, List, Tuple, Union, Set from adam.model.abc_factories import Joint, Link @@ -219,6 +219,13 @@ def is_floating_base(self) -> bool: """ return len(self.graph[self.root].children) > 1 + def get_joint_list(self) -> Set[Joint]: + """ + Returns: + Set[Joint]: the set of the joints + """ + return {arc for node in self.graph.values() for arc in node.arcs} + def __iter__(self) -> Node: """This method allows to iterate on the model Returns: From 431687e01f60d4a66d3ff762a32707a1c78f0846 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 6 Mar 2024 18:58:55 +0100 Subject: [PATCH 24/30] Tree merging logic and update type hints --- src/adam/model/tree.py | 107 +++++++++++++++++++---------------------- 1 file changed, 49 insertions(+), 58 deletions(-) diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index f01e1505..90015e72 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -1,7 +1,7 @@ import dataclasses import logging -from typing import Dict, Iterable, List, Tuple, Union, Set +from typing import Dict, Iterable, List, Tuple, Union, Set, Iterator from adam.model.abc_factories import Joint, Link @@ -50,7 +50,7 @@ def build_tree(links: List[Link], joints: List[Joint]) -> "Tree": Returns: Tree: the directed tree """ - nodes: Dict(str, Node) = { + nodes: Dict[str, Node] = { l.name: Node( name=l.name, link=l, arcs=[], children=[], parent=None, parent_arc=None ) @@ -83,8 +83,8 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree": Tree: the reduced tree """ - relative_transform = ( - lambda node: node.link.math.inv( + relative_transform = lambda node: ( + node.link.math.inv( self.graph[node.parent.name].parent_arc.spatial_transform(0) ) @ node.parent_arc.spatial_transform(0) @@ -92,59 +92,50 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree": else node.parent_arc.spatial_transform(0) ) - # find the tree leaves and proceed until the root - leaves = [node for node in self.graph.values() if node.children == []] - - while all(leaf.name != self.root for leaf in leaves): - for leaf in leaves: - if leaf.parent_arc.name not in considered_joint_names + [self.root]: - new_node = Node( - name=leaf.parent.name, - link=None, - arcs=[], - children=[], - parent=None, - parent_arc=None, - ) - - # update the link - new_node.link = leaf.parent.lump( - other=leaf.link, - relative_transform=relative_transform(leaf), - ) - - # update the parents - new_node.parent = self.graph[leaf.parent.name].parent - new_node.parent_arc = self.graph[new_node.name].parent_arc - - # update the children - new_node.children = [ - child for child in leaf.children if child.name in self.graph - ] - - for child in new_node.children: - child.parent = new_node.link - child.parent_arc = new_node.parent_arc - - # update the arcs - new_node.arcs = ( - [arc for arc in leaf.arcs if arc.name in considered_joint_names] - if leaf.arcs != [] - else [] - ) - for j in new_node.arcs: - j.parent = new_node.link.name - - logging.debug(f"Removing {leaf.name}") - self.graph.pop(leaf.name) - self.graph[new_node.name] = new_node - self.ordered_nodes_list.remove(leaf.name) - leaves = [ - self.get_node_from_name((leaf.parent.name)) - for leaf in leaves - if leaf.name != self.root - ] + # find the fixed joints using the considered_joint_names + fixed_joints = [ + joint + for joint in self.get_joint_list() + if joint.name not in considered_joint_names + ] + for f_joint in fixed_joints: + + parent_node = self.graph[f_joint.parent] + child_node = self.graph[f_joint.child] + + merged_node = parent_node + merged_neighbors = child_node + + merged_node.children.remove(merged_neighbors) + merged_node.children.extend(merged_neighbors.children) + # update the arcs + merged_node.arcs.remove(f_joint) + merged_node.arcs.extend(merged_neighbors.arcs) + + # we need to updated the parents and child on the joints in fixed_joints + for joint in fixed_joints: + if joint.parent == child_node.name: + joint.parent = merged_node.name + if joint.child == child_node.name: + joint.child = merged_node.name + + for child in merged_node.children: + + child.parent = merged_node.link + child.parent_arc = merged_node.parent_arc + + self.graph.pop(merged_neighbors.name) + self.graph[merged_node.name] = merged_node + + if {joint.name for joint in self.get_joint_list()} != set( + considered_joint_names + ): + raise ValueError( + "The joints remaining in the graph are not equal to the considered joints" + ) + tree = Tree(self.graph, self.root) + tree.print(self.root) return Tree(self.graph, self.root) def print(self, root) -> str: @@ -226,7 +217,7 @@ def get_joint_list(self) -> Set[Joint]: """ return {arc for node in self.graph.values() for arc in node.arcs} - def __iter__(self) -> Node: + def __iter__(self) -> Iterator[Node]: """This method allows to iterate on the model Returns: Node: the node istance @@ -236,7 +227,7 @@ def __iter__(self) -> Node: """ yield from [self.graph[name] for name in self.ordered_nodes_list] - def __reversed__(self) -> Node: + def __reversed__(self) -> Iterator[Node]: """ Returns: Node From 6a983c6874bd2b14c0bd1b119d8e6d5f13aaa28d Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 6 Mar 2024 19:46:30 +0100 Subject: [PATCH 25/30] Refactor model.py to use type hints and improve code readability --- src/adam/model/model.py | 32 +++++++++++--------------------- 1 file changed, 11 insertions(+), 21 deletions(-) diff --git a/src/adam/model/model.py b/src/adam/model/model.py index 1c9903bc..4c1f9299 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -65,9 +65,9 @@ def build(factory: ModelFactory, joints_name_list: List[str] = None) -> "Model": floating_base = tree.is_floating_base() # generate some useful dict - joints: Dict(str, Joint) = {joint.name: joint for joint in joints} - links: Dict(str, Link) = {link.name: link for link in links} - frames: Dict(str, Link) = {frame.name: frame for frame in frames} + joints: Dict[str, Joint] = {joint.name: joint for joint in joints} + links: Dict[str, Link] = {link.name: link for link in links} + frames: Dict[str, Link] = {frame.name: frame for frame in frames} return Model( name=factory.name, @@ -90,32 +90,22 @@ def reduce(self, joints_name_list: List[str]) -> "Model": Model: the reduced model """ - # check if the joints in the list are in the model - for joint_str in joints_name_list: - if joint_str not in self.joints.keys(): - raise ValueError( - f"{joint_str} is not in the robot model. Check the joints_name_list" - ) - tree = self.tree.reduce(joints_name_list) - joints_list = [ - node.parent_arc for node in tree.graph.values() if node.name != tree.root - ] - joints_list.sort(key=lambda joint: joint.idx) - links = {node.name: node.link for node in tree.graph.values()} - joints = {joint.name: joint for joint in joints_list} - frames = { - node.name: node.link - for node in tree.graph.values() - if node.link.inertial is None + joints = { + joint.name: joint + for joint in self.joints.values() + if joint.name in joints_name_list } + for link in self.links: + if link not in tree.graph: + self.frames[link] = link return Model( name=self.name, links=links, - frames=frames, + frames=self.frames, joints=joints, tree=tree, NDoF=len(joints_name_list), From a1f4ba20eb3653585b527b6b544fc8005ca5c9d9 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 7 Mar 2024 13:20:32 +0100 Subject: [PATCH 26/30] Fixing joint update --- src/adam/model/tree.py | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index 90015e72..100c7901 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -33,7 +33,7 @@ def get_elements(self) -> Tuple[Link, Joint, Link]: class Tree(Iterable): """The directed tree class""" - graph: Dict + graph: Dict[str, Node] root: str def __post_init__(self): @@ -98,14 +98,13 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree": for joint in self.get_joint_list() if joint.name not in considered_joint_names ] + # set fixed joints to fixed + for joint in fixed_joints: + joint.type = "fixed" for f_joint in fixed_joints: - - parent_node = self.graph[f_joint.parent] - child_node = self.graph[f_joint.child] - - merged_node = parent_node - merged_neighbors = child_node + merged_node = self.graph[f_joint.parent] + merged_neighbors = self.graph[f_joint.child] merged_node.children.remove(merged_neighbors) merged_node.children.extend(merged_neighbors.children) @@ -113,11 +112,20 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree": merged_node.arcs.remove(f_joint) merged_node.arcs.extend(merged_neighbors.arcs) + merged_node.link = merged_node.link.lump( + other=merged_neighbors.link, joint=f_joint + ) + + merged_joint = merged_node.parent_arc + removed_joint = merged_neighbors.parent_arc + # update the parent arc of the merged node + merged_node.parent_arc = merged_node.parent_arc.lump(removed_joint) + # we need to updated the parents and child on the joints in fixed_joints - for joint in fixed_joints: - if joint.parent == child_node.name: + for joint in self.get_joint_list(): + if joint.parent == merged_neighbors.name: joint.parent = merged_node.name - if joint.child == child_node.name: + if joint.child == merged_neighbors.name: joint.child = merged_node.name for child in merged_node.children: From 6191da9f91257fc2f1d95f34e1fd5836465c1594 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 7 Mar 2024 14:27:38 +0100 Subject: [PATCH 27/30] Refactor chain calculation in Model class --- src/adam/model/model.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/adam/model/model.py b/src/adam/model/model.py index 4c1f9299..6a58b50a 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -130,18 +130,18 @@ def get_joints_chain(self, root: str, target: str) -> List[Joint]: if target == root: return [] chain = [] - current_node = [ + current_joint = [ joint for joint in self.joints.values() if joint.child == target ][0] - chain.insert(0, current_node) - while current_node.parent != root: - current_node = [ + chain.insert(0, current_joint) + while current_joint.parent != root: + current_joint = [ joint for joint in self.joints.values() - if joint.child == current_node.parent + if joint.child == current_joint.parent ][0] - chain.insert(0, current_node) + chain.insert(0, current_joint) return chain def get_total_mass(self) -> float: From 924da1f308883638286eb49141fbc9696a327f6a Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 7 Mar 2024 14:28:14 +0100 Subject: [PATCH 28/30] Cleanup commits --- src/adam/model/model.py | 2 +- src/adam/model/std_factories/std_model.py | 10 ++---- src/adam/model/tree.py | 43 +++++++++++------------ 3 files changed, 25 insertions(+), 30 deletions(-) diff --git a/src/adam/model/model.py b/src/adam/model/model.py index 6a58b50a..5d34a62f 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -124,7 +124,7 @@ def get_joints_chain(self, root: str, target: str) -> List[Joint]: List[Joint]: the list of the joints """ - if target not in list(self.links) and target not in list(self.frames): + if target not in list(self.links) and target not in list(self.tree.graph): raise ValueError(f"{target} is not not in the robot model.") if target == root: diff --git a/src/adam/model/std_factories/std_model.py b/src/adam/model/std_factories/std_model.py index ffeff713..bf3c3e75 100644 --- a/src/adam/model/std_factories/std_model.py +++ b/src/adam/model/std_factories/std_model.py @@ -17,10 +17,7 @@ def urdf_remove_sensors_tags(xml_string): for sensors_tag in root.findall("sensor"): root.remove(sensors_tag) - # Convert the modified XML back to a string - modified_xml_string = ET.tostring(root) - - return modified_xml_string + return ET.tostring(root) class URDFModelFactory(ModelFactory): @@ -44,9 +41,8 @@ def __init__(self, path: str, math: SpatialMath): # to have a useless and noisy warning, let's remove before hands all the sensor elements, # that anyhow are not parser by urdf_parser_py or adam # See https://github.com/ami-iit/ADAM/issues/59 - xml_file = open(path, "r") - xml_string = xml_file.read() - xml_file.close() + with open(path, "r") as xml_file: + xml_string = xml_file.read() xml_string_without_sensors_tags = urdf_remove_sensors_tags(xml_string) self.urdf_desc = urdf_parser_py.urdf.URDF.from_xml_string( xml_string_without_sensors_tags diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index 100c7901..78dd115e 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -102,39 +102,38 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree": for joint in fixed_joints: joint.type = "fixed" - for f_joint in fixed_joints: - merged_node = self.graph[f_joint.parent] - merged_neighbors = self.graph[f_joint.child] + for fixed_j in fixed_joints: + saved_node = self.graph[fixed_j.parent] + removing_node = self.graph[fixed_j.child] - merged_node.children.remove(merged_neighbors) - merged_node.children.extend(merged_neighbors.children) + saved_node.children.remove(removing_node) + saved_node.children.extend(removing_node.children) # update the arcs - merged_node.arcs.remove(f_joint) - merged_node.arcs.extend(merged_neighbors.arcs) + saved_node.arcs.remove(fixed_j) + saved_node.arcs.extend(removing_node.arcs) - merged_node.link = merged_node.link.lump( - other=merged_neighbors.link, joint=f_joint + saved_node.link = saved_node.link.lump( + other=removing_node.link, joint=fixed_j ) - merged_joint = merged_node.parent_arc - removed_joint = merged_neighbors.parent_arc + merged_joint = saved_node.parent_arc + removed_joint = removing_node.parent_arc # update the parent arc of the merged node - merged_node.parent_arc = merged_node.parent_arc.lump(removed_joint) + # saved_node.parent_arc = saved_node.parent_arc.lump(removed_joint) # we need to updated the parents and child on the joints in fixed_joints for joint in self.get_joint_list(): - if joint.parent == merged_neighbors.name: - joint.parent = merged_node.name - if joint.child == merged_neighbors.name: - joint.child = merged_node.name + if joint.parent == removing_node.name: + joint.parent = saved_node.name + if joint.child == removing_node.name: + joint.child = saved_node.name - for child in merged_node.children: + for child in saved_node.children: + child.parent = saved_node.link + child.parent_arc = saved_node.parent_arc - child.parent = merged_node.link - child.parent_arc = merged_node.parent_arc - - self.graph.pop(merged_neighbors.name) - self.graph[merged_node.name] = merged_node + self.graph.pop(removing_node.name) + self.graph[saved_node.name] = saved_node if {joint.name for joint in self.get_joint_list()} != set( considered_joint_names From 6c1b74d13d0f884687418973547fcb3d97e0172f Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 8 Mar 2024 14:43:21 +0100 Subject: [PATCH 29/30] Commented out code for lumping links --- src/adam/model/tree.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/adam/model/tree.py b/src/adam/model/tree.py index 78dd115e..9347116d 100644 --- a/src/adam/model/tree.py +++ b/src/adam/model/tree.py @@ -112,12 +112,12 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree": saved_node.arcs.remove(fixed_j) saved_node.arcs.extend(removing_node.arcs) - saved_node.link = saved_node.link.lump( - other=removing_node.link, joint=fixed_j - ) + # saved_node.link = saved_node.link.lump( + # other=removing_node.link, joint=fixed_j + # ) - merged_joint = saved_node.parent_arc - removed_joint = removing_node.parent_arc + # merged_joint = saved_node.parent_arc + # removed_joint = removing_node.parent_arc # update the parent arc of the merged node # saved_node.parent_arc = saved_node.parent_arc.lump(removed_joint) From 72a24b1d25f9499c80cf8635ea6882a776445f97 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 8 Mar 2024 17:11:15 +0100 Subject: [PATCH 30/30] Fix target check in Model class --- src/adam/model/model.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/adam/model/model.py b/src/adam/model/model.py index 5d34a62f..b2224f7b 100644 --- a/src/adam/model/model.py +++ b/src/adam/model/model.py @@ -124,7 +124,7 @@ def get_joints_chain(self, root: str, target: str) -> List[Joint]: List[Joint]: the list of the joints """ - if target not in list(self.links) and target not in list(self.tree.graph): + if target not in list(self.frames) and target not in list(self.tree.graph): raise ValueError(f"{target} is not not in the robot model.") if target == root: