From ecd30d385408f3e75c4b331ddb928f29392b2c93 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 10:43:20 +0100 Subject: [PATCH 01/17] Add CoM Jacobian method to RBDAlgorithms class --- src/adam/core/rbd_algorithms.py | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 4c67d98..68c528e 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -347,6 +347,23 @@ def CoM_position( com_pos /= self.get_total_mass() return com_pos + def CoM_jacobian( + self, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike + ) -> npt.ArrayLike: + """Returns the center of mass (CoM) Jacobian using the centroidal momentum matrix. + + Args: + base_transform (T): The homogenous transform from base to world frame + joint_positions (T): The joints position + + Returns: + J_com (T): The CoM Jacobian + """ + # Retrieve the total mass and the centroidal momentum matrix + _, Jcm = self.crba(base_transform, joint_positions) + # Compute the CoM Jacobian + return Jcm[:3, :] / self.get_total_mass() + def get_total_mass(self): """Returns the total mass of the robot From cd7ad5b18f98f4fcbd0670c99bf2c1b2f52a1714 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:46:43 +0100 Subject: [PATCH 02/17] Fix the conversion from mixed to body fixed representation --- src/adam/core/rbd_algorithms.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index 68c528e..b8ce7be 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -359,8 +359,24 @@ def CoM_jacobian( Returns: J_com (T): The CoM Jacobian """ - # Retrieve the total mass and the centroidal momentum matrix + # The com velocity can be computed as dot_x = J_cm_mixed * nu_mixed = J_cm_body * nu_body + # For this reason we compute the centroidal momentum matrix in mixed representation and then we convert it to body fixed if needed + # Save the original frame velocity representation + ori_frame_velocity_representation = self.frame_velocity_representation + # Set the frame velocity representation to mixed and compute the centroidal momentum matrix + self.frame_velocity_representation = Representations.MIXED_REPRESENTATION _, Jcm = self.crba(base_transform, joint_positions) + if ( + ori_frame_velocity_representation + == Representations.BODY_FIXED_REPRESENTATION + ): + # if the frame velocity representation is body fixed we need to convert the centroidal momentum matrix to body fixed + # dot_x = J_cm_mixed * mixed_to_body * nu_body + X = self.math.factory.eye(6 + self.NDoF) + X[:6, :6] = self.math.adjoint_mixed(base_transform) + Jcm = Jcm @ X + # Reset the frame velocity representation + self.frame_velocity_representation = ori_frame_velocity_representation # Compute the CoM Jacobian return Jcm[:3, :] / self.get_total_mass() From bf8e1c602f030f3227d42f412447bb9abfc9e3c0 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:47:57 +0100 Subject: [PATCH 03/17] Add CoM Jacobian methods to KinDynComputations class --- src/adam/casadi/computations.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index ff67fda..64632a6 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -155,6 +155,19 @@ def CoM_position_fun(self) -> cs.Function: "CoM_pos", [base_transform, joint_positions], [com_pos.array], self.f_opts ) + def CoM_jacobian_fun(self) -> cs.Function: + """Returns the CoM Jacobian + + Returns: + J_com (casADi function): The CoM Jacobian + """ + joint_positions = cs.SX.sym("s", self.NDoF) + base_transform = cs.SX.sym("H", 4, 4) + J_com = self.rbdalgos.CoM_jacobian(base_transform, joint_positions) + return cs.Function( + "J_com", [base_transform, joint_positions], [J_com.array], self.f_opts + ) + def bias_force_fun(self) -> cs.Function: """Returns the bias force of the floating-base dynamics equation, using a reduced RNEA (no acceleration and external forces) @@ -466,3 +479,20 @@ def CoM_position(self, base_transform: cs.SX, joint_positions: cs.SX) -> cs.SX: ) return self.rbdalgos.CoM_position(base_transform, joint_positions).array + + def CoM_jacobian(self, base_transform: cs.SX, joint_positions: cs.SX) -> cs.SX: + """Returns the CoM Jacobian + + Args: + base_transform (cs.SX): The homogenous transform from base to world frame + joint_positions (cs.SX): The joints position + + Returns: + J_com (cs.SX): The CoM Jacobian + """ + if isinstance(base_transform, cs.MX) and isinstance(joint_positions, cs.MX): + raise ValueError( + "You are using casadi MX. Please use the function KinDynComputations.CoM_jacobian_fun()" + ) + + return self.rbdalgos.CoM_jacobian(base_transform, joint_positions).array From 521bbd398df08412db732dab5d9c935f70ce460e Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:48:59 +0100 Subject: [PATCH 04/17] Add tests for CoM Jacobian in test_casadi.py --- tests/test_casadi.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/tests/test_casadi.py b/tests/test_casadi.py index 4f213a4..b8836b9 100644 --- a/tests/test_casadi.py +++ b/tests/test_casadi.py @@ -43,6 +43,19 @@ def test_CoM_pos(setup_test): assert adam_com - idyn_com == pytest.approx(0.0, abs=1e-5) +def test_CoM_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_com_jacobian = robot_cfg.idyn_function_values.CoM_jacobian + adam_com_jacobian = cs.DM( + adam_kin_dyn.CoM_jacobian_fun()(state.H, state.joints_pos) + ) + assert adam_com_jacobian - idyn_com_jacobian == pytest.approx(0.0, abs=1e-5) + adam_com_jacobian = cs.DM( + adam_kin_dyn.CoM_jacobian(state.H, state.joints_pos) + ) + assert adam_com_jacobian - idyn_com_jacobian == pytest.approx(0.0, abs=1e-5) + + def test_total_mass(setup_test): adam_kin_dyn, robot_cfg, state = setup_test idyn_total_mass = robot_cfg.idyn_function_values.total_mass From 82bc6f6825ed390b730844d26dbbe4aefce50903 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:49:16 +0100 Subject: [PATCH 05/17] Add CoM Jacobian method to KinDynComputations class --- src/adam/numpy/computations.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index 9ddd2d2..1a68f05 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -162,6 +162,22 @@ def CoM_position( base_transform, joint_positions ).array.squeeze() + def CoM_jacobian( + self, base_transform: np.ndarray, joint_positions: np.ndarray + ) -> np.ndarray: + """Returns the CoM Jacobian + + Args: + base_transform (np.ndarray): The homogenous transform from base to world frame + joint_positions (np.ndarray): The joints position + + Returns: + Jcom (np.ndarray): The CoM Jacobian + """ + return self.rbdalgos.CoM_jacobian( + base_transform, joint_positions + ).array.squeeze() + def bias_force( self, base_transform: np.ndarray, From 9b82ad49d6796ab9f0cf04f1cfa80a3d544a1f5f Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:49:28 +0100 Subject: [PATCH 06/17] Add test for CoM Jacobian in test_numpy.py --- tests/test_numpy.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/tests/test_numpy.py b/tests/test_numpy.py index ba3e608..bb3f8de 100644 --- a/tests/test_numpy.py +++ b/tests/test_numpy.py @@ -34,6 +34,12 @@ def test_CoM_pos(setup_test): assert adam_com - idyn_com == pytest.approx(0.0, abs=1e-5) +def test_CoM_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_com_jacobian = robot_cfg.idyn_function_values.CoM_jacobian + adam_com_jacobian = adam_kin_dyn.CoM_jacobian(state.H, state.joints_pos) + assert adam_com_jacobian - idyn_com_jacobian == pytest.approx(0.0, abs=1e-5) + def test_total_mass(setup_test): adam_kin_dyn, robot_cfg, state = setup_test idyn_total_mass = robot_cfg.idyn_function_values.total_mass From 28e17d5008255140578e9a928b228ce100b04902 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:57:07 +0100 Subject: [PATCH 07/17] Add CoM Jacobian method to jax KinDynComputations class --- src/adam/jax/computations.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index 82d3611..42a38e6 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -229,6 +229,22 @@ def CoM_position( base_transform, joint_positions ).array.squeeze() + def CoM_jacobian( + self, base_transform: jnp.array, joint_positions: jnp.array + ) -> jnp.array: + """Returns the CoM Jacobian + + Args: + base_transform (jnp.array): The homogenous transform from base to world frame + joint_positions (jnp.array): The joints position + + Returns: + Jcom (jnp.array): The CoM Jacobian + """ + return self.rbdalgos.CoM_jacobian( + base_transform, joint_positions + ).array.squeeze() + def get_total_mass(self) -> float: """Returns the total mass of the robot From 1c7a6506c7937587de83e521c2c255f76415b7ee Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:57:14 +0100 Subject: [PATCH 08/17] Add test for CoM Jacobian in test_jax.py --- tests/test_jax.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/tests/test_jax.py b/tests/test_jax.py index a74449e..6387527 100644 --- a/tests/test_jax.py +++ b/tests/test_jax.py @@ -36,6 +36,12 @@ def test_CoM_pos(setup_test): adam_com = adam_kin_dyn.CoM_position(state.H, state.joints_pos) assert adam_com - idyn_com == pytest.approx(0.0, abs=1e-5) +def test_CoM_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_com_jacobian = robot_cfg.idyn_function_values.CoM_jacobian + adam_com_jacobian = adam_kin_dyn.CoM_jacobian(state.H, state.joints_pos) + assert adam_com_jacobian - idyn_com_jacobian == pytest.approx(0.0, abs=1e-5) + def test_total_mass(setup_test): adam_kin_dyn, robot_cfg, state = setup_test From 4cb6fc4ede397bd9517c32204e18326b06c1a053 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:57:25 +0100 Subject: [PATCH 09/17] Add CoM Jacobian to IDynFunctionValues and compute_idyntree_values --- tests/conftest.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/tests/conftest.py b/tests/conftest.py index 31c582b..a49b765 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -27,6 +27,7 @@ class IDynFunctionValues: mass_matrix: np.ndarray centroidal_momentum_matrix: np.ndarray CoM_position: np.ndarray + CoM_jacobian: np.ndarray total_mass: float jacobian: np.ndarray jacobian_non_actuated: np.ndarray @@ -184,6 +185,11 @@ def compute_idyntree_values( # CoM position idyn_com = kin_dyn.getCenterOfMassPosition().toNumPy() + # Com jacobian + idyn_com_jacobian = idyntree.MatrixDynSize(3, kin_dyn.model().getNrOfDOFs() + 6) + kin_dyn.getCenterOfMassJacobian(idyn_com_jacobian) + idyn_com_jacobian = idyn_com_jacobian.toNumPy() + # total mass total_mass = kin_dyn.model().getTotalMass() @@ -277,6 +283,7 @@ def compute_idyntree_values( mass_matrix=idyn_mass_matrix, centroidal_momentum_matrix=idyn_cmm, CoM_position=idyn_com, + CoM_jacobian=idyn_com_jacobian, total_mass=total_mass, jacobian=idyn_jacobian, jacobian_non_actuated=idyn_jacobian_non_actuated, From 664c529ee1eb3e345226c797ab6561faf9fe39cc Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:57:51 +0100 Subject: [PATCH 10/17] Add CoM Jacobian method to pytorch KinDynComputations class --- src/adam/pytorch/computations.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/src/adam/pytorch/computations.py b/src/adam/pytorch/computations.py index 3fc7080..7198104 100644 --- a/src/adam/pytorch/computations.py +++ b/src/adam/pytorch/computations.py @@ -157,7 +157,7 @@ def jacobian_dot( def CoM_position( self, base_transform: torch.Tensor, joint_positions: torch.Tensor ) -> torch.Tensor: - """Returns the CoM positon + """Returns the CoM position Args: base_transform (torch.tensor): The homogenous transform from base to world frame @@ -170,6 +170,20 @@ def CoM_position( base_transform, joint_positions ).array.squeeze() + def CoM_jacobian( + self, base_transform: torch.Tensor, joint_positions: torch.Tensor + ) -> torch.Tensor: + """Returns the CoM Jacobian + + Args: + base_transform (torch.tensor): The homogenous transform from base to world frame + joint_positions (torch.tensor): The joints position + + Returns: + Jcom (torch.tensor): The CoM Jacobian + """ + return self.rbdalgos.CoM_jacobian(base_transform, joint_positions).array + def bias_force( self, base_transform: torch.Tensor, From 392652db24c1b473f2ffc1a89562cb3276452513 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:58:03 +0100 Subject: [PATCH 11/17] Add test for CoM Jacobian in test_pytorch.py --- tests/test_pytorch.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/tests/test_pytorch.py b/tests/test_pytorch.py index 2af3bbc..4976cd0 100644 --- a/tests/test_pytorch.py +++ b/tests/test_pytorch.py @@ -44,6 +44,13 @@ def test_CoM_pos(setup_test): assert adam_com.numpy() - idyn_com == pytest.approx(0.0, abs=1e-5) +def test_CoM_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_com_jacobian = robot_cfg.idyn_function_values.CoM_jacobian + adam_com_jacobian = adam_kin_dyn.CoM_jacobian(state.H, state.joints_pos) + assert adam_com_jacobian.numpy() - idyn_com_jacobian == pytest.approx(0.0, abs=1e-5) + + def test_total_mass(setup_test): adam_kin_dyn, robot_cfg, state = setup_test idyn_total_mass = robot_cfg.idyn_function_values.total_mass From 246b3fdc18b80f5656c3372b567f2412a05874a2 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:58:13 +0100 Subject: [PATCH 12/17] Add CoM Jacobian method and function to pytorch KinDynComputationsBatch class --- src/adam/pytorch/computation_batch.py | 36 +++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/src/adam/pytorch/computation_batch.py b/src/adam/pytorch/computation_batch.py index b5e96a1..6964e6f 100644 --- a/src/adam/pytorch/computation_batch.py +++ b/src/adam/pytorch/computation_batch.py @@ -406,7 +406,7 @@ def fun(base_transform, joint_positions): def CoM_position( self, base_transform: torch.Tensor, joint_positions: torch.Tensor ) -> torch.Tensor: - """Returns the CoM positon + """Returns the CoM position Args: base_transform (torch.Tensor): The homogenous transform from base to world frame @@ -418,7 +418,7 @@ def CoM_position( return self.CoM_position_fun()(base_transform, joint_positions) def CoM_position_fun(self): - """Returns the CoM positon as a pytorch function + """Returns the CoM position as a pytorch function Returns: CoM (pytorch function): The CoM position @@ -435,6 +435,38 @@ def fun(base_transform, joint_positions): self.funcs["CoM_position"] = jax2torch(jit_vmapped_fun) return self.funcs["CoM_position"] + def CoM_jacobian( + self, base_transform: torch.Tensor, joint_positions: torch.Tensor + ) -> torch.Tensor: + """Returns the CoM Jacobian + + Args: + base_transform (torch.Tensor): The homogenous transform from base to world frame + joint_positions (torch.Tensor): The joints position + + Returns: + Jcom (torch.Tensor): The CoM Jacobian + """ + return self.CoM_jacobian_fun()(base_transform, joint_positions) + + def CoM_jacobian_fun(self): + """Returns the CoM Jacobian as a pytorch function + + Returns: + Jcom (pytorch function): The CoM Jacobian + """ + if self.funcs.get("CoM_jacobian") is not None: + return self.funcs["CoM_jacobian"] + print("[INFO] Compiling CoM Jacobian function") + + def fun(base_transform, joint_positions): + return self.rbdalgos.CoM_jacobian(base_transform, joint_positions).array + + vmapped_fun = jax.vmap(fun, in_axes=(0, 0)) + jit_vmapped_fun = jax.jit(vmapped_fun) + self.funcs["CoM_jacobian"] = jax2torch(jit_vmapped_fun) + return self.funcs["CoM_jacobian"] + def get_total_mass(self) -> float: """Returns the total mass of the robot From b3b1a3d17d5de4c52f3575f137fcdbee2b4ca1b2 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 14:58:30 +0100 Subject: [PATCH 13/17] Add test for CoM Jacobian in test_pytorch_batch.py --- tests/test_pytorch_batch.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/tests/test_pytorch_batch.py b/tests/test_pytorch_batch.py index 4839b20..e850996 100644 --- a/tests/test_pytorch_batch.py +++ b/tests/test_pytorch_batch.py @@ -73,6 +73,20 @@ def test_CoM_pos(setup_test): assert adam_com.shape == (n_samples, 3) +def test_CoM_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_com_jacobian = robot_cfg.idyn_function_values.CoM_jacobian + adam_com_jacobian = adam_kin_dyn.CoM_jacobian(state.H, state.joints_pos) + try: + adam_com_jacobian.sum().backward() + except: + raise ValueError(adam_com_jacobian) + assert adam_com_jacobian[0].detach().numpy() - idyn_com_jacobian == pytest.approx( + 0.0, abs=1e-4 + ) + assert adam_com_jacobian.shape == (n_samples, 3, robot_cfg.n_dof + 6) + + def test_jacobian(setup_test): adam_kin_dyn, robot_cfg, state, n_samples = setup_test idyn_jacobian = robot_cfg.idyn_function_values.jacobian From ed11080f178a132d0947f100d641f41cd294510b Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 15:00:15 +0100 Subject: [PATCH 14/17] Fix typo in CoM position docstrings across multiple computation classes and format with black --- src/adam/casadi/computations.py | 4 ++-- src/adam/core/rbd_algorithms.py | 2 +- src/adam/jax/computations.py | 2 +- src/adam/numpy/computations.py | 2 +- src/adam/parametric/casadi/computations_parametric.py | 2 +- src/adam/parametric/jax/computations_parametric.py | 2 +- src/adam/parametric/numpy/computations_parametric.py | 2 +- src/adam/parametric/pytorch/computations_parametric.py | 2 +- tests/test_casadi.py | 4 +--- tests/test_jax.py | 1 + tests/test_numpy.py | 1 + 11 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/adam/casadi/computations.py b/src/adam/casadi/computations.py index 64632a6..1d9191c 100644 --- a/src/adam/casadi/computations.py +++ b/src/adam/casadi/computations.py @@ -143,7 +143,7 @@ def jacobian_dot_fun(self, frame: str) -> cs.Function: ) def CoM_position_fun(self) -> cs.Function: - """Returns the CoM positon + """Returns the CoM position Returns: CoM (casADi function): The CoM position @@ -464,7 +464,7 @@ def gravity_term(self, base_transform: cs.SX, joint_positions: cs.SX) -> cs.SX: ).array def CoM_position(self, base_transform: cs.SX, joint_positions: cs.SX) -> cs.SX: - """Returns the CoM positon + """Returns the CoM position Args: base_transform (cs.SX): The homogenous transform from base to world frame diff --git a/src/adam/core/rbd_algorithms.py b/src/adam/core/rbd_algorithms.py index b8ce7be..8aa5a65 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -327,7 +327,7 @@ def jacobian_dot( def CoM_position( self, base_transform: npt.ArrayLike, joint_positions: npt.ArrayLike ) -> npt.ArrayLike: - """Returns the CoM positon + """Returns the CoM position Args: base_transform (T): The homogenous transform from base to world frame diff --git a/src/adam/jax/computations.py b/src/adam/jax/computations.py index 42a38e6..e2d6618 100644 --- a/src/adam/jax/computations.py +++ b/src/adam/jax/computations.py @@ -216,7 +216,7 @@ def gravity_term( def CoM_position( self, base_transform: jnp.array, joint_positions: jnp.array ) -> jnp.array: - """Returns the CoM positon + """Returns the CoM position Args: base_transform (jnp.array): The homogenous transform from base to world frame diff --git a/src/adam/numpy/computations.py b/src/adam/numpy/computations.py index 1a68f05..6dccdca 100644 --- a/src/adam/numpy/computations.py +++ b/src/adam/numpy/computations.py @@ -149,7 +149,7 @@ def jacobian_dot( def CoM_position( self, base_transform: np.ndarray, joint_positions: np.ndarray ) -> np.ndarray: - """Returns the CoM positon + """Returns the CoM position Args: base_transform (np.ndarray): The homogenous transform from base to world frame diff --git a/src/adam/parametric/casadi/computations_parametric.py b/src/adam/parametric/casadi/computations_parametric.py index 4149662..76ed45a 100644 --- a/src/adam/parametric/casadi/computations_parametric.py +++ b/src/adam/parametric/casadi/computations_parametric.py @@ -179,7 +179,7 @@ def jacobian_dot_fun(self, frame: str) -> cs.Function: ) def CoM_position_fun(self) -> cs.Function: - """Returns the CoM positon + """Returns the CoM position Returns: com (casADi function): The CoM position diff --git a/src/adam/parametric/jax/computations_parametric.py b/src/adam/parametric/jax/computations_parametric.py index 7920006..5104029 100644 --- a/src/adam/parametric/jax/computations_parametric.py +++ b/src/adam/parametric/jax/computations_parametric.py @@ -397,7 +397,7 @@ def CoM_position( length_multiplier: jnp.array, densities: jnp.array, ) -> jnp.array: - """Returns the CoM positon + """Returns the CoM position Args: base_transform (jnp.array): The homogenous transform from base to world frame diff --git a/src/adam/parametric/numpy/computations_parametric.py b/src/adam/parametric/numpy/computations_parametric.py index f703037..a1cd3d9 100644 --- a/src/adam/parametric/numpy/computations_parametric.py +++ b/src/adam/parametric/numpy/computations_parametric.py @@ -266,7 +266,7 @@ def CoM_position( length_multiplier: np.ndarray, densities: np.ndarray, ) -> np.ndarray: - """Returns the CoM positon + """Returns the CoM position Args: base_transform (np.ndarray): The homogenous transform from base to world frame diff --git a/src/adam/parametric/pytorch/computations_parametric.py b/src/adam/parametric/pytorch/computations_parametric.py index 63568d4..33e3c6e 100644 --- a/src/adam/parametric/pytorch/computations_parametric.py +++ b/src/adam/parametric/pytorch/computations_parametric.py @@ -271,7 +271,7 @@ def CoM_position( length_multiplier: torch.Tensor, densities: torch.Tensor, ) -> torch.Tensor: - """Returns the CoM positon + """Returns the CoM position Args: base_transform (torch.tensor): The homogenous transform from base to world frame diff --git a/tests/test_casadi.py b/tests/test_casadi.py index b8836b9..f136bfa 100644 --- a/tests/test_casadi.py +++ b/tests/test_casadi.py @@ -50,9 +50,7 @@ def test_CoM_jacobian(setup_test): adam_kin_dyn.CoM_jacobian_fun()(state.H, state.joints_pos) ) assert adam_com_jacobian - idyn_com_jacobian == pytest.approx(0.0, abs=1e-5) - adam_com_jacobian = cs.DM( - adam_kin_dyn.CoM_jacobian(state.H, state.joints_pos) - ) + adam_com_jacobian = cs.DM(adam_kin_dyn.CoM_jacobian(state.H, state.joints_pos)) assert adam_com_jacobian - idyn_com_jacobian == pytest.approx(0.0, abs=1e-5) diff --git a/tests/test_jax.py b/tests/test_jax.py index 6387527..7a35681 100644 --- a/tests/test_jax.py +++ b/tests/test_jax.py @@ -36,6 +36,7 @@ def test_CoM_pos(setup_test): adam_com = adam_kin_dyn.CoM_position(state.H, state.joints_pos) assert adam_com - idyn_com == pytest.approx(0.0, abs=1e-5) + def test_CoM_jacobian(setup_test): adam_kin_dyn, robot_cfg, state = setup_test idyn_com_jacobian = robot_cfg.idyn_function_values.CoM_jacobian diff --git a/tests/test_numpy.py b/tests/test_numpy.py index bb3f8de..2b2aa16 100644 --- a/tests/test_numpy.py +++ b/tests/test_numpy.py @@ -40,6 +40,7 @@ def test_CoM_jacobian(setup_test): adam_com_jacobian = adam_kin_dyn.CoM_jacobian(state.H, state.joints_pos) assert adam_com_jacobian - idyn_com_jacobian == pytest.approx(0.0, abs=1e-5) + def test_total_mass(setup_test): adam_kin_dyn, robot_cfg, state = setup_test idyn_total_mass = robot_cfg.idyn_function_values.total_mass From 8f49128c95105b754ed339ce40905b4c66998844 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 15:14:23 +0100 Subject: [PATCH 15/17] Fix comments in RBDAlgorithms to clarify CoM velocity computation --- 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 8aa5a65..a10e810 100644 --- a/src/adam/core/rbd_algorithms.py +++ b/src/adam/core/rbd_algorithms.py @@ -359,7 +359,7 @@ def CoM_jacobian( Returns: J_com (T): The CoM Jacobian """ - # The com velocity can be computed as dot_x = J_cm_mixed * nu_mixed = J_cm_body * nu_body + # The com velocity can be computed as dot_x * m = J_cm_mixed * nu_mixed = J_cm_body * nu_body # For this reason we compute the centroidal momentum matrix in mixed representation and then we convert it to body fixed if needed # Save the original frame velocity representation ori_frame_velocity_representation = self.frame_velocity_representation @@ -371,7 +371,7 @@ def CoM_jacobian( == Representations.BODY_FIXED_REPRESENTATION ): # if the frame velocity representation is body fixed we need to convert the centroidal momentum matrix to body fixed - # dot_x = J_cm_mixed * mixed_to_body * nu_body + # dot_x * m = J_cm_mixed * mixed_to_body * nu_body X = self.math.factory.eye(6 + self.NDoF) X[:6, :6] = self.math.adjoint_mixed(base_transform) Jcm = Jcm @ X From 7a0d0e18698a84f120f719b5bf0d497b14dbafc5 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 16:42:30 +0100 Subject: [PATCH 16/17] Add test for CoM Jacobian in test_idyntree_conversion.py --- tests/test_idyntree_conversion.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/tests/test_idyntree_conversion.py b/tests/test_idyntree_conversion.py index ceb5f77..df25c1e 100644 --- a/tests/test_idyntree_conversion.py +++ b/tests/test_idyntree_conversion.py @@ -46,6 +46,13 @@ def test_CoM_pos(setup_test): adam_com = cs.DM(adam_kin_dyn.CoM_position_fun()(state.H, state.joints_pos)) assert adam_com - idyn_com == pytest.approx(0.0, abs=1e-5) +def test_CoM_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_com_jac = robot_cfg.idyn_function_values.CoM_jacobian + adam_com_jac = cs.DM(adam_kin_dyn.CoM_jacobian_fun()(state.H, state.joints_pos)) + assert adam_com_jac - idyn_com_jac == pytest.approx(0.0, abs=1e-5) + adam_com_jac = cs.DM(adam_kin_dyn.CoM_jacobian_fun()(state.H, state.joints_pos)) + assert adam_com_jac - idyn_com_jac == pytest.approx(0.0, abs=1e-5) def test_total_mass(setup_test): adam_kin_dyn, robot_cfg, state = setup_test From ce886fc38832e6dd2349143f91be63f3f677f546 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 8 Jan 2025 17:29:48 +0100 Subject: [PATCH 17/17] Formatting with black --- tests/test_idyntree_conversion.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tests/test_idyntree_conversion.py b/tests/test_idyntree_conversion.py index df25c1e..063126a 100644 --- a/tests/test_idyntree_conversion.py +++ b/tests/test_idyntree_conversion.py @@ -46,6 +46,7 @@ def test_CoM_pos(setup_test): adam_com = cs.DM(adam_kin_dyn.CoM_position_fun()(state.H, state.joints_pos)) assert adam_com - idyn_com == pytest.approx(0.0, abs=1e-5) + def test_CoM_jacobian(setup_test): adam_kin_dyn, robot_cfg, state = setup_test idyn_com_jac = robot_cfg.idyn_function_values.CoM_jacobian @@ -54,6 +55,7 @@ def test_CoM_jacobian(setup_test): adam_com_jac = cs.DM(adam_kin_dyn.CoM_jacobian_fun()(state.H, state.joints_pos)) assert adam_com_jac - idyn_com_jac == pytest.approx(0.0, abs=1e-5) + def test_total_mass(setup_test): adam_kin_dyn, robot_cfg, state = setup_test idyn_total_mass = robot_cfg.idyn_function_values.total_mass