From 0634392253c7bfb018fbce9bc36b92a61e59920c Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 5 Dec 2024 17:06:49 +0100 Subject: [PATCH 01/20] Add pytest fixtures and data classes for robot state and idyntree computations --- tests/conftest.py | 273 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 273 insertions(+) create mode 100644 tests/conftest.py diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000..03405ea --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,273 @@ +import pytest +import numpy as np +import icub_models +import idyntree.bindings as idyntree +from adam import Representations +from adam.numpy.numpy_like import SpatialMath +import dataclasses +from itertools import product +import logging + + +@dataclasses.dataclass +class State: + H: np.ndarray + joints_pos: np.ndarray + base_vel: np.ndarray + joints_vel: np.ndarray + gravity: np.ndarray + + +@dataclasses.dataclass +class IDynFunctionValues: + mass_matrix: np.ndarray + centroidal_momentum_matrix: np.ndarray + CoM_position: np.ndarray + total_mass: float + jacobian: np.ndarray + jacobian_non_actuated: np.ndarray + jacobian_dot_nu: np.ndarray + relative_jacobian: np.ndarray + forward_kinematics: np.ndarray + forward_kinematics_non_actuated: np.ndarray + bias_forces: np.ndarray + coriolis_term: np.ndarray + gravity_term: np.ndarray + + +@dataclasses.dataclass +class RobotCfg: + robot_name: str + velocity_representation: Representations + model_path: str + joints_name_list: list + n_dof: int + kin_dyn: idyntree.KinDynComputations + idyn_function_values: IDynFunctionValues + + +VELOCITY_REPRESENTATIONS = [ + Representations.MIXED_REPRESENTATION, + Representations.BODY_FIXED_REPRESENTATION, +] + +ROBOTS = [ + "iCubGazeboV2_5", + "iCubGenova02", +] + + +TEST_CONFIGURATIONS = list(product(VELOCITY_REPRESENTATIONS, ROBOTS)) + + +@pytest.fixture(scope="module", params=TEST_CONFIGURATIONS, ids=str) +def tests_setup(request) -> RobotCfg | State: + + velocity_representation, robot_name = request.param + + np.random.seed(42) + + model_path = str(icub_models.get_model_file(robot_name)) + + joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", + ] + + logging.basicConfig(level=logging.DEBUG) + logging.debug("Showing the robot tree.") + + robot_iDyn = idyntree.ModelLoader() + robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) + + kin_dyn = idyntree.KinDynComputations() + kin_dyn.loadRobotModel(robot_iDyn.model()) + + if velocity_representation == Representations.BODY_FIXED_REPRESENTATION: + idyn_representation = idyntree.BODY_FIXED_REPRESENTATION + elif velocity_representation == Representations.MIXED_REPRESENTATION: + idyn_representation = idyntree.MIXED_REPRESENTATION + else: + raise ValueError(f"Unknown velocity representation: {velocity_representation}") + kin_dyn.setFrameVelocityRepresentation(idyn_representation) + + n_dofs = len(joints_name_list) + # base quantities + xyz = (np.random.rand(3) - 0.5) * 5 + rpy = (np.random.rand(3) - 0.5) * 5 + base_vel = (np.random.rand(6) - 0.5) * 5 + # joints quantitites + joints_val = (np.random.rand(n_dofs) - 0.5) * 5 + joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 + + g = np.array([0, 0, -9.80665]) + H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array + + state = State( + H=H_b, + joints_pos=joints_val, + base_vel=base_vel, + joints_vel=joints_dot_val, + gravity=g, + ) + + idyn_function_values = compute_idyntree_values(kin_dyn, state) + + robot_cfg = RobotCfg( + robot_name=robot_name, + velocity_representation=velocity_representation, + model_path=model_path, + joints_name_list=joints_name_list, + n_dof=n_dofs, + kin_dyn=kin_dyn, + idyn_function_values=idyn_function_values, + ) + + return robot_cfg, state + + +def compute_idyntree_values( + kin_dyn: idyntree.KinDynComputations, state: State +) -> IDynFunctionValues: + kin_dyn.setRobotState( + state.H, state.joints_pos, state.base_vel, state.joints_vel, state.gravity + ) + + # mass matrix + idyn_mass_matrix = idyntree.MatrixDynSize() + kin_dyn.getFreeFloatingMassMatrix(idyn_mass_matrix) + idyn_mass_matrix = idyn_mass_matrix.toNumPy() + + # centroidal momentum matrix + idyn_cmm = idyntree.MatrixDynSize() + kin_dyn.getCentroidalTotalMomentumJacobian(idyn_cmm) + idyn_cmm = idyn_cmm.toNumPy() + + # CoM position + idyn_com = kin_dyn.getCenterOfMassPosition().toNumPy() + + # total mass + total_mass = kin_dyn.model().getTotalMass() + + # jacobian + idyn_jacobian = idyntree.MatrixDynSize(6, kin_dyn.model().getNrOfDOFs() + 6) + kin_dyn.getFrameFreeFloatingJacobian("l_sole", idyn_jacobian) + idyn_jacobian = idyn_jacobian.toNumPy() + + # jacobian_non_actuated + idyn_jacobian_non_actuated = idyntree.MatrixDynSize( + 6, kin_dyn.model().getNrOfDOFs() + 6 + ) + kin_dyn.getFrameFreeFloatingJacobian("head", idyn_jacobian_non_actuated) + idyn_jacobian_non_actuated = idyn_jacobian_non_actuated.toNumPy() + + # jacobian_dot_nu + idyn_jacobian_dot_nu = kin_dyn.getFrameBiasAcc("l_sole").toNumPy() + + # relative_jacobian + # set the base pose to the identity to get the relative jacobian + kin_dyn.setRobotState( + np.eye(4), + state.joints_pos, + state.base_vel, + state.joints_vel, + state.gravity, + ) + idyn_relative_jacobian = idyntree.MatrixDynSize( + 6, kin_dyn.model().getNrOfDOFs() + 6 + ) + kin_dyn.getFrameFreeFloatingJacobian("l_sole", idyn_relative_jacobian) + idyn_relative_jacobian = idyn_relative_jacobian.toNumPy()[:, 6:] + + # forward_kinematics + # set back the state to the original one + kin_dyn.setRobotState( + state.H, state.joints_pos, state.base_vel, state.joints_vel, state.gravity + ) + idyn_forward_kinematics = ( + kin_dyn.getWorldTransform("l_sole").asHomogeneousTransform().toNumPy() + ) + + # forward_kinematics_non_actuated + idyn_forward_kinematics_non_actuated = ( + kin_dyn.getWorldTransform("head").asHomogeneousTransform().toNumPy() + ) + + # bias_forces + idyn_bias_forces = idyntree.FreeFloatingGeneralizedTorques(kin_dyn.model()) + assert kin_dyn.generalizedBiasForces(idyn_bias_forces) + idyn_bias_forces = np.concatenate( + ( + idyn_bias_forces.baseWrench().toNumPy(), + idyn_bias_forces.jointTorques().toNumPy(), + ) + ) + + # coriolis_term + # set gravity to zero to get only the coriolis term + kin_dyn.setRobotState( + state.H, state.joints_pos, state.base_vel, state.joints_vel, np.zeros(3) + ) + idyn_coriolis_term = idyntree.FreeFloatingGeneralizedTorques(kin_dyn.model()) + assert kin_dyn.generalizedBiasForces(idyn_coriolis_term) + idyn_coriolis_term = np.concatenate( + ( + idyn_coriolis_term.baseWrench().toNumPy(), + idyn_coriolis_term.jointTorques().toNumPy(), + ) + ) + + # gravity_term + # set gravity to the actual value and velocities to zero to get only the gravity term + kin_dyn.setRobotState( + state.H, + state.joints_pos, + np.zeros(6), + np.zeros(kin_dyn.model().getNrOfDOFs()), + state.gravity, + ) + idyn_gravity_term = idyntree.FreeFloatingGeneralizedTorques(kin_dyn.model()) + assert kin_dyn.generalizedBiasForces(idyn_gravity_term) + idyn_gravity_term = np.concatenate( + ( + idyn_gravity_term.baseWrench().toNumPy(), + idyn_gravity_term.jointTorques().toNumPy(), + ) + ) + + return IDynFunctionValues( + mass_matrix=idyn_mass_matrix, + centroidal_momentum_matrix=idyn_cmm, + CoM_position=idyn_com, + total_mass=total_mass, + jacobian=idyn_jacobian, + jacobian_non_actuated=idyn_jacobian_non_actuated, + jacobian_dot_nu=idyn_jacobian_dot_nu, + relative_jacobian=idyn_relative_jacobian, + forward_kinematics=idyn_forward_kinematics, + forward_kinematics_non_actuated=idyn_forward_kinematics_non_actuated, + bias_forces=idyn_bias_forces, + coriolis_term=idyn_coriolis_term, + gravity_term=idyn_gravity_term, + ) From 9801d76964fde27c7e21622d0fb629b08bd87b81 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 5 Dec 2024 18:38:58 +0100 Subject: [PATCH 02/20] Change pytest fixture scope from module to session for improved test efficiency --- tests/conftest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/conftest.py b/tests/conftest.py index 03405ea..2d7ba21 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -60,7 +60,7 @@ class RobotCfg: TEST_CONFIGURATIONS = list(product(VELOCITY_REPRESENTATIONS, ROBOTS)) -@pytest.fixture(scope="module", params=TEST_CONFIGURATIONS, ids=str) +@pytest.fixture(scope="session", params=TEST_CONFIGURATIONS, ids=str) def tests_setup(request) -> RobotCfg | State: velocity_representation, robot_name = request.param From 485787ce1c406c4d7d0212984542628b7d47e462 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 5 Dec 2024 18:39:13 +0100 Subject: [PATCH 03/20] Add casadi tests --- tests/test_casadi.py | 172 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 172 insertions(+) create mode 100644 tests/test_casadi.py diff --git a/tests/test_casadi.py b/tests/test_casadi.py new file mode 100644 index 0000000..272130c --- /dev/null +++ b/tests/test_casadi.py @@ -0,0 +1,172 @@ +import casadi as cs +import numpy as np +import pytest +from adam.casadi import KinDynComputations +from conftest import State, RobotCfg + + +@pytest.fixture(scope="module") +def setup_test(tests_setup) -> KinDynComputations | RobotCfg | State: + robot_cfg, state = tests_setup + adam_kin_dyn = KinDynComputations(robot_cfg.model_path, robot_cfg.joints_name_list) + adam_kin_dyn.set_frame_velocity_representation(robot_cfg.velocity_representation) + return adam_kin_dyn, robot_cfg, state + + +def test_mass_matrix(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix + adam_mass_matrix = cs.DM(adam_kin_dyn.mass_matrix(state.H, state.joints_pos)) + assert adam_mass_matrix - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) + adam_mass_matrix = cs.DM(adam_kin_dyn.mass_matrix_fun()(state.H, state.joints_pos)) + assert adam_mass_matrix - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_cmm = robot_cfg.idyn_function_values.centroidal_momentum_matrix + adam_cmm = cs.DM(adam_kin_dyn.centroidal_momentum_matrix(state.H, state.joints_pos)) + assert adam_cmm - idyn_cmm == pytest.approx(0.0, abs=1e-5) + adam_cmm = cs.DM( + adam_kin_dyn.centroidal_momentum_matrix_fun()(state.H, state.joints_pos) + ) + assert adam_cmm - idyn_cmm == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_com = robot_cfg.idyn_function_values.CoM_position + 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) + 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_total_mass(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_total_mass = robot_cfg.idyn_function_values.total_mass + assert ( + adam_kin_dyn.get_total_mass() - idyn_total_mass + == pytest.approx(0.0, abs=1e-5) + ) + + +def test_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian + adam_jacobian = cs.DM(adam_kin_dyn.jacobian("l_sole", state.H, state.joints_pos)) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + adam_jacobian = cs.DM( + adam_kin_dyn.jacobian_fun("l_sole")(state.H, state.joints_pos) + ) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian_non_actuated + adam_jacobian = cs.DM(adam_kin_dyn.jacobian("head", state.H, state.joints_pos)) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + adam_jacobian = cs.DM(adam_kin_dyn.jacobian_fun("head")(state.H, state.joints_pos)) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian_dot_nu = robot_cfg.idyn_function_values.jacobian_dot_nu + adam_jacobian_dot_nu = cs.DM( + adam_kin_dyn.jacobian_dot( + "l_sole", state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu == pytest.approx(0.0, abs=1e-5) + adam_jacobian_dot_nu = cs.DM( + adam_kin_dyn.jacobian_dot_fun("l_sole")( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu == pytest.approx(0.0, abs=1e-5) + + +def test_relative_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.relative_jacobian + adam_jacobian = cs.DM(adam_kin_dyn.relative_jacobian("l_sole", state.joints_pos)) + assert idyn_jacobian - adam_jacobian == pytest.approx(0.0, abs=1e-5) + adam_jacobian = cs.DM( + adam_kin_dyn.relative_jacobian_fun("l_sole")(state.joints_pos) + ) + assert idyn_jacobian - adam_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_fk(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics + adam_H = cs.DM(adam_kin_dyn.forward_kinematics("l_sole", state.H, state.joints_pos)) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + adam_H = cs.DM( + adam_kin_dyn.forward_kinematics_fun("l_sole")(state.H, state.joints_pos) + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics_non_actuated + adam_H = cs.DM(adam_kin_dyn.forward_kinematics("head", state.H, state.joints_pos)) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + adam_H = cs.DM( + adam_kin_dyn.forward_kinematics_fun("head")(state.H, state.joints_pos) + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_h = robot_cfg.idyn_function_values.bias_forces + adam_h = cs.DM( + adam_kin_dyn.bias_force( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) + assert idyn_h - adam_h == pytest.approx(0.0, abs=1e-4) + adam_h = cs.DM( + adam_kin_dyn.bias_force_fun()( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) + assert idyn_h - adam_h == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_matrix(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_coriolis = robot_cfg.idyn_function_values.coriolis_term + adam_coriolis = cs.DM( + adam_kin_dyn.coriolis_term( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) + assert idyn_coriolis - adam_coriolis == pytest.approx(0.0, abs=1e-4) + adam_coriolis = cs.DM( + adam_kin_dyn.coriolis_term_fun()( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) + assert idyn_coriolis - adam_coriolis == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_gravity = robot_cfg.idyn_function_values.gravity_term + adam_gravity = cs.DM( + adam_kin_dyn.gravity_term( + state.H, state.joints_pos + ) + ) + assert idyn_gravity - adam_gravity == pytest.approx(0.0, abs=1e-4) + adam_gravity = cs.DM( + adam_kin_dyn.gravity_term_fun()( + state.H, state.joints_pos + ) + ) + assert idyn_gravity - adam_gravity == pytest.approx(0.0, abs=1e-4) From a442601295900656ca4ba3c5894c17fff49e4450 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 5 Dec 2024 18:39:22 +0100 Subject: [PATCH 04/20] Add jax tests --- tests/test_jax.py | 113 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 113 insertions(+) create mode 100644 tests/test_jax.py diff --git a/tests/test_jax.py b/tests/test_jax.py new file mode 100644 index 0000000..ce04aa3 --- /dev/null +++ b/tests/test_jax.py @@ -0,0 +1,113 @@ +import numpy as np +import pytest +from adam.jax import KinDynComputations +from conftest import State, RobotCfg +from jax import config + +config.update("jax_enable_x64", True) + + +@pytest.fixture(scope="module") +def setup_test(tests_setup) -> KinDynComputations | RobotCfg | State: + robot_cfg, state = tests_setup + adam_kin_dyn = KinDynComputations(robot_cfg.model_path, robot_cfg.joints_name_list) + adam_kin_dyn.set_frame_velocity_representation(robot_cfg.velocity_representation) + return adam_kin_dyn, robot_cfg, state + + +def test_mass_matrix(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix + adam_mass_matrix = adam_kin_dyn.mass_matrix(state.H, state.joints_pos) + assert adam_mass_matrix - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_cmm = robot_cfg.idyn_function_values.centroidal_momentum_matrix + adam_cmm = adam_kin_dyn.centroidal_momentum_matrix(state.H, state.joints_pos) + assert adam_cmm - idyn_cmm == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_com = robot_cfg.idyn_function_values.CoM_position + 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_total_mass(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_total_mass = robot_cfg.idyn_function_values.total_mass + assert adam_kin_dyn.get_total_mass() - idyn_total_mass == pytest.approx( + 0.0, abs=1e-5 + ) + + +def test_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian + adam_jacobian = adam_kin_dyn.jacobian("l_sole", state.H, state.joints_pos) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian_non_actuated + adam_jacobian = adam_kin_dyn.jacobian("head", state.H, state.joints_pos) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian_dot_nu = robot_cfg.idyn_function_values.jacobian_dot_nu + adam_jacobian_dot_nu = adam_kin_dyn.jacobian_dot( + "l_sole", state.H, state.joints_pos, state.base_vel, state.joints_vel + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu == pytest.approx(0.0, abs=1e-5) + + +def test_relative_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.relative_jacobian + adam_jacobian = adam_kin_dyn.relative_jacobian("l_sole", state.joints_pos) + assert idyn_jacobian - adam_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_fk(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics + adam_H = adam_kin_dyn.forward_kinematics("l_sole", state.H, state.joints_pos) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics_non_actuated + adam_H = adam_kin_dyn.forward_kinematics("head", state.H, state.joints_pos) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_h = robot_cfg.idyn_function_values.bias_forces + adam_h = adam_kin_dyn.bias_force( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + assert idyn_h - adam_h == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_matrix(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_coriolis = robot_cfg.idyn_function_values.coriolis_term + adam_coriolis = adam_kin_dyn.coriolis_term( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + assert idyn_coriolis - adam_coriolis == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_gravity = robot_cfg.idyn_function_values.gravity_term + adam_gravity = adam_kin_dyn.gravity_term(state.H, state.joints_pos) + assert idyn_gravity - adam_gravity == pytest.approx(0.0, abs=1e-4) From 7cd0d2412bc91d87b8f66e98e21556c9fd97e11e Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 5 Dec 2024 18:39:38 +0100 Subject: [PATCH 05/20] Add numpy tests --- tests/test_numpy.py | 110 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 110 insertions(+) create mode 100644 tests/test_numpy.py diff --git a/tests/test_numpy.py b/tests/test_numpy.py new file mode 100644 index 0000000..c022fe5 --- /dev/null +++ b/tests/test_numpy.py @@ -0,0 +1,110 @@ +import numpy as np +import pytest +from adam.numpy import KinDynComputations +from conftest import State, RobotCfg + + +@pytest.fixture(scope="module") +def setup_test(tests_setup) -> KinDynComputations | RobotCfg | State: + robot_cfg, state = tests_setup + adam_kin_dyn = KinDynComputations(robot_cfg.model_path, robot_cfg.joints_name_list) + adam_kin_dyn.set_frame_velocity_representation(robot_cfg.velocity_representation) + return adam_kin_dyn, robot_cfg, state + + +def test_mass_matrix(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix + adam_mass_matrix = adam_kin_dyn.mass_matrix(state.H, state.joints_pos) + assert adam_mass_matrix - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_cmm = robot_cfg.idyn_function_values.centroidal_momentum_matrix + adam_cmm = adam_kin_dyn.centroidal_momentum_matrix(state.H, state.joints_pos) + assert adam_cmm - idyn_cmm == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_com = robot_cfg.idyn_function_values.CoM_position + 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_total_mass(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_total_mass = robot_cfg.idyn_function_values.total_mass + assert adam_kin_dyn.get_total_mass() - idyn_total_mass == pytest.approx( + 0.0, abs=1e-5 + ) + + +def test_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian + adam_jacobian = adam_kin_dyn.jacobian("l_sole", state.H, state.joints_pos) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian_non_actuated + adam_jacobian = adam_kin_dyn.jacobian("head", state.H, state.joints_pos) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian_dot_nu = robot_cfg.idyn_function_values.jacobian_dot_nu + adam_jacobian_dot_nu = adam_kin_dyn.jacobian_dot( + "l_sole", state.H, state.joints_pos, state.base_vel, state.joints_vel + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu == pytest.approx(0.0, abs=1e-5) + + +def test_relative_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.relative_jacobian + adam_jacobian = adam_kin_dyn.relative_jacobian("l_sole", state.joints_pos) + assert idyn_jacobian - adam_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_fk(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics + adam_H = adam_kin_dyn.forward_kinematics("l_sole", state.H, state.joints_pos) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics_non_actuated + adam_H = adam_kin_dyn.forward_kinematics("head", state.H, state.joints_pos) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_h = robot_cfg.idyn_function_values.bias_forces + adam_h = adam_kin_dyn.bias_force( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + assert idyn_h - adam_h == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_matrix(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_coriolis = robot_cfg.idyn_function_values.coriolis_term + adam_coriolis = adam_kin_dyn.coriolis_term( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + assert idyn_coriolis - adam_coriolis == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_gravity = robot_cfg.idyn_function_values.gravity_term + adam_gravity = adam_kin_dyn.gravity_term(state.H, state.joints_pos) + assert idyn_gravity - adam_gravity == pytest.approx(0.0, abs=1e-4) From 827e137c9dccb916512096f85edc365b2cabda70 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 5 Dec 2024 18:39:46 +0100 Subject: [PATCH 06/20] Add pytorch tests --- tests/test_pytorch.py | 124 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 124 insertions(+) create mode 100644 tests/test_pytorch.py diff --git a/tests/test_pytorch.py b/tests/test_pytorch.py new file mode 100644 index 0000000..256d37c --- /dev/null +++ b/tests/test_pytorch.py @@ -0,0 +1,124 @@ +import numpy as np +import pytest +from adam.pytorch import KinDynComputations +from conftest import State, RobotCfg +import torch + +torch.set_default_dtype(torch.float64) + + +@pytest.fixture(scope="module") +def setup_test(tests_setup) -> KinDynComputations | RobotCfg | State: + robot_cfg, state = tests_setup + adam_kin_dyn = KinDynComputations(robot_cfg.model_path, robot_cfg.joints_name_list) + adam_kin_dyn.set_frame_velocity_representation(robot_cfg.velocity_representation) + # convert state quantities to torch tensors + state.H = torch.tensor(state.H) + state.joints_pos = torch.tensor(state.joints_pos) + state.base_vel = torch.tensor(state.base_vel) + state.joints_vel = torch.tensor(state.joints_vel) + state.gravity = torch.tensor(state.gravity) + + return adam_kin_dyn, robot_cfg, state + + +def test_mass_matrix(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix + adam_mass_matrix = adam_kin_dyn.mass_matrix(state.H, state.joints_pos) + assert adam_mass_matrix.numpy() - idyn_mass_matrix == pytest.approx( + 0.0, abs=1e-5 + ) + + +def test_CMM(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_cmm = robot_cfg.idyn_function_values.centroidal_momentum_matrix + adam_cmm = adam_kin_dyn.centroidal_momentum_matrix(state.H, state.joints_pos) + assert adam_cmm.numpy() - idyn_cmm == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_com = robot_cfg.idyn_function_values.CoM_position + adam_com = adam_kin_dyn.CoM_position(state.H, state.joints_pos) + assert adam_com.numpy() - idyn_com == 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 + assert adam_kin_dyn.get_total_mass() - idyn_total_mass == pytest.approx( + 0.0, abs=1e-5 + ) + + +def test_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian + adam_jacobian = adam_kin_dyn.jacobian("l_sole", state.H, state.joints_pos) + assert adam_jacobian.numpy() - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian_non_actuated + adam_jacobian = adam_kin_dyn.jacobian("head", state.H, state.joints_pos) + assert adam_jacobian.numpy() - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian_dot_nu = robot_cfg.idyn_function_values.jacobian_dot_nu + adam_jacobian_dot_nu = adam_kin_dyn.jacobian_dot( + "l_sole", state.H, state.joints_pos, state.base_vel, state.joints_vel + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu.numpy() == pytest.approx( + 0.0, abs=1e-5 + ) + + +def test_relative_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.relative_jacobian + adam_jacobian = adam_kin_dyn.relative_jacobian("l_sole", state.joints_pos) + assert idyn_jacobian - adam_jacobian.numpy() == pytest.approx(0.0, abs=1e-5) + + +def test_fk(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics + adam_H = adam_kin_dyn.forward_kinematics("l_sole", state.H, state.joints_pos) + assert idyn_H - adam_H.numpy() == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics_non_actuated + adam_H = adam_kin_dyn.forward_kinematics("head", state.H, state.joints_pos) + assert idyn_H - adam_H.numpy() == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_h = robot_cfg.idyn_function_values.bias_forces + adam_h = adam_kin_dyn.bias_force( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + assert idyn_h - adam_h.numpy() == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_matrix(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_coriolis = robot_cfg.idyn_function_values.coriolis_term + adam_coriolis = adam_kin_dyn.coriolis_term( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + assert idyn_coriolis - adam_coriolis.numpy() == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_gravity = robot_cfg.idyn_function_values.gravity_term + adam_gravity = adam_kin_dyn.gravity_term(state.H, state.joints_pos) + assert idyn_gravity - adam_gravity.numpy() == pytest.approx(0.0, abs=1e-4) From a2bfbf3a250dee8f0370ae4d89d9979a2e21b178 Mon Sep 17 00:00:00 2001 From: giulero Date: Tue, 10 Dec 2024 12:15:21 +0100 Subject: [PATCH 07/20] Fix typo --- tests/conftest.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tests/conftest.py b/tests/conftest.py index 2d7ba21..bc70d88 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -112,14 +112,14 @@ def tests_setup(request) -> RobotCfg | State: raise ValueError(f"Unknown velocity representation: {velocity_representation}") kin_dyn.setFrameVelocityRepresentation(idyn_representation) - n_dofs = len(joints_name_list) + n_dof = len(joints_name_list) # base quantities xyz = (np.random.rand(3) - 0.5) * 5 rpy = (np.random.rand(3) - 0.5) * 5 base_vel = (np.random.rand(6) - 0.5) * 5 # joints quantitites - joints_val = (np.random.rand(n_dofs) - 0.5) * 5 - joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 + joints_val = (np.random.rand(n_dof) - 0.5) * 5 + joints_dot_val = (np.random.rand(n_dof) - 0.5) * 5 g = np.array([0, 0, -9.80665]) H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array @@ -139,7 +139,7 @@ def tests_setup(request) -> RobotCfg | State: velocity_representation=velocity_representation, model_path=model_path, joints_name_list=joints_name_list, - n_dof=n_dofs, + n_dof=n_dof, kin_dyn=kin_dyn, idyn_function_values=idyn_function_values, ) From 56b4b0ee8ef7dc78695ca3f66b8cc35efb04aa45 Mon Sep 17 00:00:00 2001 From: giulero Date: Tue, 10 Dec 2024 12:15:48 +0100 Subject: [PATCH 08/20] Add pytorch batch tests --- tests/test_pytorch_batch.py | 199 ++++++++++++++++++++++++++++++++++++ 1 file changed, 199 insertions(+) create mode 100644 tests/test_pytorch_batch.py diff --git a/tests/test_pytorch_batch.py b/tests/test_pytorch_batch.py new file mode 100644 index 0000000..3d58ec4 --- /dev/null +++ b/tests/test_pytorch_batch.py @@ -0,0 +1,199 @@ +import numpy as np +import pytest +from adam.pytorch import KinDynComputationsBatch +from conftest import State, RobotCfg +import torch +from jax import config + +config.update("jax_enable_x64", True) + + +@pytest.fixture(scope="module") +def setup_test(tests_setup) -> KinDynComputationsBatch | RobotCfg | State: + robot_cfg, state = tests_setup + adam_kin_dyn = KinDynComputationsBatch( + robot_cfg.model_path, robot_cfg.joints_name_list + ) + adam_kin_dyn.set_frame_velocity_representation(robot_cfg.velocity_representation) + # convert state quantities to torch tensors and tile them + n_samples = 2 + state.H = torch.tile(torch.tensor(state.H), (n_samples, 1, 1)).requires_grad_() + state.joints_pos = torch.tile( + torch.tensor(state.joints_pos), (n_samples, 1) + ).requires_grad_() + state.base_vel = torch.tile( + torch.tensor(state.base_vel), (n_samples, 1) + ).requires_grad_() + state.joints_vel = torch.tile( + torch.tensor(state.joints_vel), (n_samples, 1) + ).requires_grad_() + return adam_kin_dyn, robot_cfg, state, n_samples + + +def test_mass_matrix(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix + adam_mass_matrix = adam_kin_dyn.mass_matrix(state.H, state.joints_pos) + try: + adam_mass_matrix.sum().backward() + except: + raise ValueError(adam_mass_matrix) + assert adam_mass_matrix[0].detach().numpy() - idyn_mass_matrix == pytest.approx( + 0.0, abs=1e-4 + ) + assert adam_mass_matrix.shape == ( + n_samples, + robot_cfg.n_dof + 6, + robot_cfg.n_dof + 6, + ) + + +def test_CMM(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_cmm = robot_cfg.idyn_function_values.centroidal_momentum_matrix + adam_cmm = adam_kin_dyn.centroidal_momentum_matrix(state.H, state.joints_pos) + try: + adam_cmm.sum().backward() + except: + raise ValueError(adam_cmm) + assert adam_cmm[0].detach().numpy() - idyn_cmm == pytest.approx(0.0, abs=1e-4) + assert adam_cmm.shape == (n_samples, 6, robot_cfg.n_dof + 6) + + +def test_CoM_pos(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_com = robot_cfg.idyn_function_values.CoM_position + adam_com = adam_kin_dyn.CoM_position(state.H, state.joints_pos) + try: + adam_com.sum().backward() + except: + raise ValueError(adam_com) + assert adam_com[0].detach().numpy() - idyn_com == pytest.approx(0.0, abs=1e-4) + assert adam_com.shape == (n_samples, 3) + + +def test_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian + adam_jacobian = adam_kin_dyn.jacobian("l_sole", state.H, state.joints_pos) + try: + adam_jacobian.sum().backward() + except: + raise ValueError(adam_jacobian) + assert adam_jacobian[0].detach().numpy() - idyn_jacobian == pytest.approx( + 0.0, abs=1e-4 + ) + assert adam_jacobian.shape == (n_samples, 6, robot_cfg.n_dof + 6) + + +def test_jacobian_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian_non_actuated + adam_jacobian = adam_kin_dyn.jacobian("head", state.H, state.joints_pos) + try: + adam_jacobian.sum().backward() + except: + raise ValueError(adam_jacobian) + assert adam_jacobian[0].detach().numpy() - idyn_jacobian == pytest.approx( + 0.0, abs=1e-4 + ) + assert adam_jacobian.shape == (n_samples, 6, robot_cfg.n_dof + 6) + + +def test_jacobian_dot(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_jacobian_dot_nu = robot_cfg.idyn_function_values.jacobian_dot_nu + adam_jacobian_dot = adam_kin_dyn.jacobian_dot( + "l_sole", state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + try: + adam_jacobian_dot.sum().backward() + except: + raise ValueError(adam_jacobian_dot) + adam_jacobian_dot_nu = adam_jacobian_dot[0].detach().numpy() @ np.concatenate( + (state.base_vel[0].detach().numpy(), state.joints_vel[0].detach().numpy()) + ) + assert adam_jacobian_dot_nu - idyn_jacobian_dot_nu == pytest.approx(0.0, abs=1e-4) + assert adam_jacobian_dot.shape == (n_samples, 6, robot_cfg.n_dof + 6) + + +def test_relative_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.relative_jacobian + adam_jacobian = adam_kin_dyn.relative_jacobian("l_sole", state.joints_pos) + try: + adam_jacobian.sum().backward() + except: + raise ValueError(adam_jacobian) + assert adam_jacobian[0].detach().numpy() - idyn_jacobian == pytest.approx( + 0.0, abs=1e-4 + ) + assert adam_jacobian.shape == (n_samples, 6, robot_cfg.n_dof) + + +def test_fk(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics + adam_H = adam_kin_dyn.forward_kinematics("l_sole", state.H, state.joints_pos) + try: + adam_H.sum().backward() + except: + raise ValueError(adam_H) + assert adam_H[0].detach().numpy() - idyn_H == pytest.approx(0.0, abs=1e-4) + assert adam_H.shape == (n_samples, 4, 4) + + +def test_fk_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics_non_actuated + adam_H = adam_kin_dyn.forward_kinematics("head", state.H, state.joints_pos) + try: + adam_H.sum().backward() + except: + raise ValueError(adam_H) + assert adam_H[0].detach().numpy() - idyn_H == pytest.approx(0.0, abs=1e-4) + assert adam_H.shape == (n_samples, 4, 4) + + +def test_bias_force(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_h = robot_cfg.idyn_function_values.bias_forces + adam_h = adam_kin_dyn.bias_force( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + try: + adam_h.sum().backward() + except: + raise ValueError(adam_h) + assert adam_h[0].detach().numpy() - idyn_h == pytest.approx(0.0, abs=1e-4) + assert adam_h.shape == (n_samples, robot_cfg.n_dof + 6) + + +def test_coriolis_matrix(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_coriolis = robot_cfg.idyn_function_values.coriolis_term + adam_coriolis = adam_kin_dyn.coriolis_term( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + try: + adam_coriolis.sum().backward() + except: + raise ValueError(adam_coriolis) + assert adam_coriolis[0].detach().numpy() - idyn_coriolis == pytest.approx( + 0.0, abs=1e-4 + ) + assert adam_coriolis.shape == (n_samples, robot_cfg.n_dof + 6) + + +def test_gravity_term(setup_test): + adam_kin_dyn, robot_cfg, state, n_samples = setup_test + idyn_gravity = robot_cfg.idyn_function_values.gravity_term + adam_gravity = adam_kin_dyn.gravity_term(state.H, state.joints_pos) + try: + adam_gravity.sum().backward() + except: + raise ValueError(adam_gravity) + assert adam_gravity[0].detach().numpy() - idyn_gravity == pytest.approx( + 0.0, abs=1e-4 + ) + assert adam_gravity.shape == (n_samples, robot_cfg.n_dof + 6) From ee51c330719eef47f6c0146fa884bd143ccd141b Mon Sep 17 00:00:00 2001 From: giulero Date: Tue, 10 Dec 2024 12:17:14 +0100 Subject: [PATCH 09/20] Comply with black --- tests/test_casadi.py | 17 ++++------------- tests/test_pytorch.py | 4 +--- 2 files changed, 5 insertions(+), 16 deletions(-) diff --git a/tests/test_casadi.py b/tests/test_casadi.py index 272130c..2736602 100644 --- a/tests/test_casadi.py +++ b/tests/test_casadi.py @@ -45,9 +45,8 @@ def test_CoM_pos(setup_test): def test_total_mass(setup_test): adam_kin_dyn, robot_cfg, state = setup_test idyn_total_mass = robot_cfg.idyn_function_values.total_mass - assert ( - adam_kin_dyn.get_total_mass() - idyn_total_mass - == pytest.approx(0.0, abs=1e-5) + assert adam_kin_dyn.get_total_mass() - idyn_total_mass == pytest.approx( + 0.0, abs=1e-5 ) @@ -158,15 +157,7 @@ def test_coriolis_matrix(setup_test): def test_gravity_term(setup_test): adam_kin_dyn, robot_cfg, state = setup_test idyn_gravity = robot_cfg.idyn_function_values.gravity_term - adam_gravity = cs.DM( - adam_kin_dyn.gravity_term( - state.H, state.joints_pos - ) - ) + adam_gravity = cs.DM(adam_kin_dyn.gravity_term(state.H, state.joints_pos)) assert idyn_gravity - adam_gravity == pytest.approx(0.0, abs=1e-4) - adam_gravity = cs.DM( - adam_kin_dyn.gravity_term_fun()( - state.H, state.joints_pos - ) - ) + adam_gravity = cs.DM(adam_kin_dyn.gravity_term_fun()(state.H, state.joints_pos)) assert idyn_gravity - adam_gravity == pytest.approx(0.0, abs=1e-4) diff --git a/tests/test_pytorch.py b/tests/test_pytorch.py index 256d37c..21d84f7 100644 --- a/tests/test_pytorch.py +++ b/tests/test_pytorch.py @@ -26,9 +26,7 @@ def test_mass_matrix(setup_test): adam_kin_dyn, robot_cfg, state = setup_test idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix adam_mass_matrix = adam_kin_dyn.mass_matrix(state.H, state.joints_pos) - assert adam_mass_matrix.numpy() - idyn_mass_matrix == pytest.approx( - 0.0, abs=1e-5 - ) + assert adam_mass_matrix.numpy() - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) def test_CMM(setup_test): From c8c3f1fa3b7df56be31a5445433fb743674341da Mon Sep 17 00:00:00 2001 From: giulero Date: Tue, 10 Dec 2024 14:06:12 +0100 Subject: [PATCH 10/20] move temporary the file --- .../{test_pytorch_batch.py => test_pytorch_batch_bak.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename tests/pytorch_batch/{test_pytorch_batch.py => test_pytorch_batch_bak.py} (100%) diff --git a/tests/pytorch_batch/test_pytorch_batch.py b/tests/pytorch_batch/test_pytorch_batch_bak.py similarity index 100% rename from tests/pytorch_batch/test_pytorch_batch.py rename to tests/pytorch_batch/test_pytorch_batch_bak.py From 0cad0c794d9d8fa461e729a03117edec965d0934 Mon Sep 17 00:00:00 2001 From: giulero Date: Mon, 16 Dec 2024 15:31:20 +0100 Subject: [PATCH 11/20] Refactor idyntree conversion tests --- tests/test_idyntree_conversion.py | 167 ++++++++++++++++++++++++++++++ 1 file changed, 167 insertions(+) create mode 100644 tests/test_idyntree_conversion.py diff --git a/tests/test_idyntree_conversion.py b/tests/test_idyntree_conversion.py new file mode 100644 index 0000000..da76323 --- /dev/null +++ b/tests/test_idyntree_conversion.py @@ -0,0 +1,167 @@ +import casadi as cs +import numpy as np +import pytest +from adam.casadi import KinDynComputations +from conftest import State, RobotCfg, compute_idyntree_values +from adam.model.conversions.idyntree import to_idyntree_model + +@pytest.fixture(scope="module") +def setup_test(tests_setup) -> KinDynComputations | RobotCfg | State: + robot_cfg, state = tests_setup + adam_kin_dyn = KinDynComputations(robot_cfg.model_path, robot_cfg.joints_name_list) + adam_kin_dyn.set_frame_velocity_representation(robot_cfg.velocity_representation) + robot_cfg.kin_dyn.loadRobotModel(to_idyntree_model(adam_kin_dyn.rbdalgos.model)) + idyn_function_values = compute_idyntree_values(robot_cfg.kin_dyn, state) + robot_cfg.idyn_function_values = idyn_function_values + return adam_kin_dyn, robot_cfg, state + + + +def test_mass_matrix(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix + adam_mass_matrix = cs.DM(adam_kin_dyn.mass_matrix(state.H, state.joints_pos)) + assert adam_mass_matrix - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) + adam_mass_matrix = cs.DM(adam_kin_dyn.mass_matrix_fun()(state.H, state.joints_pos)) + assert adam_mass_matrix - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_cmm = robot_cfg.idyn_function_values.centroidal_momentum_matrix + adam_cmm = cs.DM(adam_kin_dyn.centroidal_momentum_matrix(state.H, state.joints_pos)) + assert adam_cmm - idyn_cmm == pytest.approx(0.0, abs=1e-5) + adam_cmm = cs.DM( + adam_kin_dyn.centroidal_momentum_matrix_fun()(state.H, state.joints_pos) + ) + assert adam_cmm - idyn_cmm == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_com = robot_cfg.idyn_function_values.CoM_position + 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) + 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_total_mass(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_total_mass = robot_cfg.idyn_function_values.total_mass + assert adam_kin_dyn.get_total_mass() - idyn_total_mass == pytest.approx( + 0.0, abs=1e-5 + ) + + +def test_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian + adam_jacobian = cs.DM(adam_kin_dyn.jacobian("l_sole", state.H, state.joints_pos)) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + adam_jacobian = cs.DM( + adam_kin_dyn.jacobian_fun("l_sole")(state.H, state.joints_pos) + ) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian_non_actuated + adam_jacobian = cs.DM(adam_kin_dyn.jacobian("head", state.H, state.joints_pos)) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + adam_jacobian = cs.DM(adam_kin_dyn.jacobian_fun("head")(state.H, state.joints_pos)) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian_dot_nu = robot_cfg.idyn_function_values.jacobian_dot_nu + adam_jacobian_dot_nu = cs.DM( + adam_kin_dyn.jacobian_dot( + "l_sole", state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu == pytest.approx(0.0, abs=1e-5) + adam_jacobian_dot_nu = cs.DM( + adam_kin_dyn.jacobian_dot_fun("l_sole")( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu == pytest.approx(0.0, abs=1e-5) + + +def test_relative_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.relative_jacobian + adam_jacobian = cs.DM(adam_kin_dyn.relative_jacobian("l_sole", state.joints_pos)) + assert idyn_jacobian - adam_jacobian == pytest.approx(0.0, abs=1e-5) + adam_jacobian = cs.DM( + adam_kin_dyn.relative_jacobian_fun("l_sole")(state.joints_pos) + ) + assert idyn_jacobian - adam_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_fk(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics + adam_H = cs.DM(adam_kin_dyn.forward_kinematics("l_sole", state.H, state.joints_pos)) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + adam_H = cs.DM( + adam_kin_dyn.forward_kinematics_fun("l_sole")(state.H, state.joints_pos) + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics_non_actuated + adam_H = cs.DM(adam_kin_dyn.forward_kinematics("head", state.H, state.joints_pos)) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + adam_H = cs.DM( + adam_kin_dyn.forward_kinematics_fun("head")(state.H, state.joints_pos) + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_h = robot_cfg.idyn_function_values.bias_forces + adam_h = cs.DM( + adam_kin_dyn.bias_force( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) + assert idyn_h - adam_h == pytest.approx(0.0, abs=1e-4) + adam_h = cs.DM( + adam_kin_dyn.bias_force_fun()( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) + assert idyn_h - adam_h == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_matrix(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_coriolis = robot_cfg.idyn_function_values.coriolis_term + adam_coriolis = cs.DM( + adam_kin_dyn.coriolis_term( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) + assert idyn_coriolis - adam_coriolis == pytest.approx(0.0, abs=1e-4) + adam_coriolis = cs.DM( + adam_kin_dyn.coriolis_term_fun()( + state.H, state.joints_pos, state.base_vel, state.joints_vel + ) + ) + assert idyn_coriolis - adam_coriolis == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(setup_test): + adam_kin_dyn, robot_cfg, state = setup_test + idyn_gravity = robot_cfg.idyn_function_values.gravity_term + adam_gravity = cs.DM(adam_kin_dyn.gravity_term(state.H, state.joints_pos)) + assert idyn_gravity - adam_gravity == pytest.approx(0.0, abs=1e-4) + adam_gravity = cs.DM(adam_kin_dyn.gravity_term_fun()(state.H, state.joints_pos)) + assert idyn_gravity - adam_gravity == pytest.approx(0.0, abs=1e-4) From 0309c3355ce931e2077b5067198d8779979bdb81 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 19 Dec 2024 12:41:18 +0100 Subject: [PATCH 12/20] Fix typo: bias_forces -> bias_force --- tests/conftest.py | 37 ++++++++++++++++++++++--------- tests/test_casadi.py | 2 +- tests/test_idyntree_conversion.py | 2 +- tests/test_jax.py | 2 +- tests/test_numpy.py | 2 +- tests/test_pytorch.py | 2 +- tests/test_pytorch_batch.py | 2 +- 7 files changed, 32 insertions(+), 17 deletions(-) diff --git a/tests/conftest.py b/tests/conftest.py index bc70d88..5596a94 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -7,6 +7,8 @@ import dataclasses from itertools import product import logging +import os +import requests @dataclasses.dataclass @@ -30,7 +32,7 @@ class IDynFunctionValues: relative_jacobian: np.ndarray forward_kinematics: np.ndarray forward_kinematics_non_actuated: np.ndarray - bias_forces: np.ndarray + bias_force: np.ndarray coriolis_term: np.ndarray gravity_term: np.ndarray @@ -53,10 +55,23 @@ class RobotCfg: ROBOTS = [ "iCubGazeboV2_5", - "iCubGenova02", + "StickBot", ] +def get_robot_model_path(robot_name: str) -> str: + if robot_name == "StickBot": + model_path = "stickbot.urdf" + if not os.path.exists(model_path): + url = "https://raw.githubusercontent.com/icub-tech-iit/ergocub-gazebo-simulations/master/models/stickBot/model.urdf" + response = requests.get(url) + with open(model_path, "wb") as file: + file.write(response.content) + else: + model_path = str(icub_models.get_model_file(robot_name)) + return model_path + + TEST_CONFIGURATIONS = list(product(VELOCITY_REPRESENTATIONS, ROBOTS)) @@ -67,7 +82,7 @@ def tests_setup(request) -> RobotCfg | State: np.random.seed(42) - model_path = str(icub_models.get_model_file(robot_name)) + model_path = get_robot_model_path(robot_name) joints_name_list = [ "torso_pitch", @@ -144,7 +159,7 @@ def tests_setup(request) -> RobotCfg | State: idyn_function_values=idyn_function_values, ) - return robot_cfg, state + yield robot_cfg, state def compute_idyntree_values( @@ -214,13 +229,13 @@ def compute_idyntree_values( kin_dyn.getWorldTransform("head").asHomogeneousTransform().toNumPy() ) - # bias_forces - idyn_bias_forces = idyntree.FreeFloatingGeneralizedTorques(kin_dyn.model()) - assert kin_dyn.generalizedBiasForces(idyn_bias_forces) - idyn_bias_forces = np.concatenate( + # bias_force + idyn_bias_force = idyntree.FreeFloatingGeneralizedTorques(kin_dyn.model()) + assert kin_dyn.generalizedBiasForces(idyn_bias_force) + idyn_bias_force = np.concatenate( ( - idyn_bias_forces.baseWrench().toNumPy(), - idyn_bias_forces.jointTorques().toNumPy(), + idyn_bias_force.baseWrench().toNumPy(), + idyn_bias_force.jointTorques().toNumPy(), ) ) @@ -267,7 +282,7 @@ def compute_idyntree_values( relative_jacobian=idyn_relative_jacobian, forward_kinematics=idyn_forward_kinematics, forward_kinematics_non_actuated=idyn_forward_kinematics_non_actuated, - bias_forces=idyn_bias_forces, + bias_force=idyn_bias_force, coriolis_term=idyn_coriolis_term, gravity_term=idyn_gravity_term, ) diff --git a/tests/test_casadi.py b/tests/test_casadi.py index 2736602..466c16f 100644 --- a/tests/test_casadi.py +++ b/tests/test_casadi.py @@ -122,7 +122,7 @@ def test_fk_non_actuated(setup_test): def test_bias_force(setup_test): adam_kin_dyn, robot_cfg, state = setup_test - idyn_h = robot_cfg.idyn_function_values.bias_forces + idyn_h = robot_cfg.idyn_function_values.bias_force adam_h = cs.DM( adam_kin_dyn.bias_force( state.H, state.joints_pos, state.base_vel, state.joints_vel diff --git a/tests/test_idyntree_conversion.py b/tests/test_idyntree_conversion.py index da76323..2379171 100644 --- a/tests/test_idyntree_conversion.py +++ b/tests/test_idyntree_conversion.py @@ -126,7 +126,7 @@ def test_fk_non_actuated(setup_test): def test_bias_force(setup_test): adam_kin_dyn, robot_cfg, state = setup_test - idyn_h = robot_cfg.idyn_function_values.bias_forces + idyn_h = robot_cfg.idyn_function_values.bias_force adam_h = cs.DM( adam_kin_dyn.bias_force( state.H, state.joints_pos, state.base_vel, state.joints_vel diff --git a/tests/test_jax.py b/tests/test_jax.py index ce04aa3..94c788e 100644 --- a/tests/test_jax.py +++ b/tests/test_jax.py @@ -90,7 +90,7 @@ def test_fk_non_actuated(setup_test): def test_bias_force(setup_test): adam_kin_dyn, robot_cfg, state = setup_test - idyn_h = robot_cfg.idyn_function_values.bias_forces + idyn_h = robot_cfg.idyn_function_values.bias_force adam_h = adam_kin_dyn.bias_force( state.H, state.joints_pos, state.base_vel, state.joints_vel ) diff --git a/tests/test_numpy.py b/tests/test_numpy.py index c022fe5..f5619ed 100644 --- a/tests/test_numpy.py +++ b/tests/test_numpy.py @@ -87,7 +87,7 @@ def test_fk_non_actuated(setup_test): def test_bias_force(setup_test): adam_kin_dyn, robot_cfg, state = setup_test - idyn_h = robot_cfg.idyn_function_values.bias_forces + idyn_h = robot_cfg.idyn_function_values.bias_force adam_h = adam_kin_dyn.bias_force( state.H, state.joints_pos, state.base_vel, state.joints_vel ) diff --git a/tests/test_pytorch.py b/tests/test_pytorch.py index 21d84f7..a85c829 100644 --- a/tests/test_pytorch.py +++ b/tests/test_pytorch.py @@ -99,7 +99,7 @@ def test_fk_non_actuated(setup_test): def test_bias_force(setup_test): adam_kin_dyn, robot_cfg, state = setup_test - idyn_h = robot_cfg.idyn_function_values.bias_forces + idyn_h = robot_cfg.idyn_function_values.bias_force adam_h = adam_kin_dyn.bias_force( state.H, state.joints_pos, state.base_vel, state.joints_vel ) diff --git a/tests/test_pytorch_batch.py b/tests/test_pytorch_batch.py index 3d58ec4..51435cb 100644 --- a/tests/test_pytorch_batch.py +++ b/tests/test_pytorch_batch.py @@ -157,7 +157,7 @@ def test_fk_non_actuated(setup_test): def test_bias_force(setup_test): adam_kin_dyn, robot_cfg, state, n_samples = setup_test - idyn_h = robot_cfg.idyn_function_values.bias_forces + idyn_h = robot_cfg.idyn_function_values.bias_force adam_h = adam_kin_dyn.bias_force( state.H, state.joints_pos, state.base_vel, state.joints_vel ) From de3b68d418237b5fcc09fa2a98292b83ef66e504 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 19 Dec 2024 12:55:55 +0100 Subject: [PATCH 13/20] Add requests dependency --- ci_env.yml | 1 + ci_env_win.yml | 1 + setup.cfg | 1 + 3 files changed, 3 insertions(+) diff --git a/ci_env.yml b/ci_env.yml index e97eff7..9f2da6e 100644 --- a/ci_env.yml +++ b/ci_env.yml @@ -20,3 +20,4 @@ dependencies: - jax - pytorch - jax2torch + - requests diff --git a/ci_env_win.yml b/ci_env_win.yml index d62e0a4..c04f1bf 100644 --- a/ci_env_win.yml +++ b/ci_env_win.yml @@ -17,3 +17,4 @@ dependencies: - icub-models - idyntree >=11.0.0 - gitpython + - requests diff --git a/setup.cfg b/setup.cfg index b77e541..7279340 100644 --- a/setup.cfg +++ b/setup.cfg @@ -58,6 +58,7 @@ test = black gitpython jax2torch + requests conversions = idyntree From b4327289f5fc7256385aefd859715eaa7485a388 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 19 Dec 2024 13:10:47 +0100 Subject: [PATCH 14/20] Add parametric tests --- tests/parametric/test_casadi.py | 178 +++++++++++++++++ tests/parametric/test_idyntree_conversion.py | 196 +++++++++++++++++++ tests/parametric/test_jax.py | 154 +++++++++++++++ tests/parametric/test_numpy.py | 154 +++++++++++++++ tests/parametric/test_pytorch.py | 165 ++++++++++++++++ 5 files changed, 847 insertions(+) create mode 100644 tests/parametric/test_casadi.py create mode 100644 tests/parametric/test_idyntree_conversion.py create mode 100644 tests/parametric/test_jax.py create mode 100644 tests/parametric/test_numpy.py create mode 100644 tests/parametric/test_pytorch.py diff --git a/tests/parametric/test_casadi.py b/tests/parametric/test_casadi.py new file mode 100644 index 0000000..43dbae8 --- /dev/null +++ b/tests/parametric/test_casadi.py @@ -0,0 +1,178 @@ +import casadi as cs +import numpy as np +import pytest +from adam.parametric.casadi import KinDynComputationsParametric +from conftest import State, RobotCfg + + +@pytest.fixture(scope="module") +def setup_test(tests_setup) -> KinDynComputationsParametric | RobotCfg | State: + robot_cfg, state = tests_setup + # skip the tests if the model is not the StickBot + if robot_cfg.robot_name != "StickBot": + pytest.skip("Skipping the test because the model is not StickBot") + link_name_list = ["chest"] + adam_kin_dyn = KinDynComputationsParametric( + robot_cfg.model_path, robot_cfg.joints_name_list, link_name_list + ) + # This is the original density value associated to the chest link, computed as mass/volume + original_density = [628.0724496264945] + original_length = np.ones(len(link_name_list)) + adam_kin_dyn.set_frame_velocity_representation(robot_cfg.velocity_representation) + return adam_kin_dyn, robot_cfg, state, original_density, original_length + + +def test_mass_matrix(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix + adam_mass_matrix = cs.DM( + adam_kin_dyn.mass_matrix_fun()( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert adam_mass_matrix - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_cmm = robot_cfg.idyn_function_values.centroidal_momentum_matrix + adam_cmm = cs.DM( + adam_kin_dyn.centroidal_momentum_matrix_fun()( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert adam_cmm - idyn_cmm == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_com = robot_cfg.idyn_function_values.CoM_position + adam_com = cs.DM( + adam_kin_dyn.CoM_position_fun()( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert adam_com - idyn_com == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_total_mass = robot_cfg.idyn_function_values.total_mass + adam_total_mass = cs.DM( + adam_kin_dyn.get_total_mass()(original_length, original_density) + ) + assert adam_total_mass - idyn_total_mass == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian + adam_jacobian = cs.DM( + adam_kin_dyn.jacobian_fun("l_sole")( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian_non_actuated + adam_jacobian = cs.DM( + adam_kin_dyn.jacobian_fun("head")( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian_dot_nu = robot_cfg.idyn_function_values.jacobian_dot_nu + adam_jacobian_dot_nu = cs.DM( + adam_kin_dyn.jacobian_dot_fun("l_sole")( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu == pytest.approx(0.0, abs=1e-5) + + +def test_relative_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.relative_jacobian + adam_jacobian = cs.DM( + adam_kin_dyn.relative_jacobian_fun("l_sole")( + state.joints_pos, original_length, original_density + ) + ) + assert idyn_jacobian - adam_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_fk(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics + adam_H = cs.DM( + adam_kin_dyn.forward_kinematics_fun("l_sole")( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics_non_actuated + adam_H = cs.DM( + adam_kin_dyn.forward_kinematics_fun("head")( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_h = robot_cfg.idyn_function_values.bias_force + adam_h = cs.DM( + adam_kin_dyn.bias_force_fun()( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + ) + assert idyn_h - adam_h == pytest.approx(0.0, abs=1e-5) + + +def test_coriolis_matrix(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_coriolis = robot_cfg.idyn_function_values.coriolis_term + adam_coriolis = cs.DM( + adam_kin_dyn.coriolis_term_fun()( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + ) + assert idyn_coriolis - adam_coriolis == pytest.approx(0.0, abs=1e-5) + + +def test_gravity_term(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_gravity = robot_cfg.idyn_function_values.gravity_term + adam_gravity = cs.DM( + adam_kin_dyn.gravity_term_fun()( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert idyn_gravity - adam_gravity == pytest.approx(0.0, abs=1e-5) diff --git a/tests/parametric/test_idyntree_conversion.py b/tests/parametric/test_idyntree_conversion.py new file mode 100644 index 0000000..c50ce41 --- /dev/null +++ b/tests/parametric/test_idyntree_conversion.py @@ -0,0 +1,196 @@ +import casadi as cs +import numpy as np +import pytest +from adam.parametric.casadi import KinDynComputationsParametric +from conftest import State, RobotCfg, compute_idyntree_values +from adam.model.conversions.idyntree import to_idyntree_model +from adam.parametric.model.parametric_factories.parametric_model import ( + URDFParametricModelFactory, +) +from adam.model import Model +from adam.numpy.numpy_like import SpatialMath + + +@pytest.fixture(scope="module") +def setup_test(tests_setup) -> KinDynComputationsParametric | RobotCfg | State: + robot_cfg, state = tests_setup + # skip the tests if the model is not the StickBot + if robot_cfg.robot_name != "StickBot": + pytest.skip("Skipping the test because the model is not StickBot") + link_name_list = ["chest"] + adam_kin_dyn = KinDynComputationsParametric( + robot_cfg.model_path, robot_cfg.joints_name_list, link_name_list + ) + adam_kin_dyn.set_frame_velocity_representation(robot_cfg.velocity_representation) + original_density = [628.0724496264945] + original_length = np.ones(len(link_name_list)) + factory = URDFParametricModelFactory( + path=robot_cfg.model_path, + math=SpatialMath(), + links_name_list=link_name_list, + length_multiplier=original_length, + densities=original_density, + ) + model = Model.build(factory=factory, joints_name_list=robot_cfg.joints_name_list) + + robot_cfg.kin_dyn.loadRobotModel(to_idyntree_model(model)) + robot_cfg.idyn_function_values = compute_idyntree_values(robot_cfg.kin_dyn, state) + return adam_kin_dyn, robot_cfg, state, original_density, original_length + + + + +def test_mass_matrix(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix + adam_mass_matrix = cs.DM( + adam_kin_dyn.mass_matrix_fun()( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert adam_mass_matrix - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_cmm = robot_cfg.idyn_function_values.centroidal_momentum_matrix + adam_cmm = cs.DM( + adam_kin_dyn.centroidal_momentum_matrix_fun()( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert adam_cmm - idyn_cmm == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_com = robot_cfg.idyn_function_values.CoM_position + adam_com = cs.DM( + adam_kin_dyn.CoM_position_fun()( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert adam_com - idyn_com == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_total_mass = robot_cfg.idyn_function_values.total_mass + adam_total_mass = cs.DM( + adam_kin_dyn.get_total_mass()(original_length, original_density) + ) + assert adam_total_mass - idyn_total_mass == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian + adam_jacobian = cs.DM( + adam_kin_dyn.jacobian_fun("l_sole")( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian_non_actuated + adam_jacobian = cs.DM( + adam_kin_dyn.jacobian_fun("head")( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian_dot_nu = robot_cfg.idyn_function_values.jacobian_dot_nu + adam_jacobian_dot_nu = cs.DM( + adam_kin_dyn.jacobian_dot_fun("l_sole")( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu == pytest.approx(0.0, abs=1e-5) + + +def test_relative_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.relative_jacobian + adam_jacobian = cs.DM( + adam_kin_dyn.relative_jacobian_fun("l_sole")( + state.joints_pos, original_length, original_density + ) + ) + assert idyn_jacobian - adam_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_fk(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics + adam_H = cs.DM( + adam_kin_dyn.forward_kinematics_fun("l_sole")( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics_non_actuated + adam_H = cs.DM( + adam_kin_dyn.forward_kinematics_fun("head")( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_h = robot_cfg.idyn_function_values.bias_force + adam_h = cs.DM( + adam_kin_dyn.bias_force_fun()( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + ) + assert idyn_h - adam_h == pytest.approx(0.0, abs=1e-5) + + +def test_coriolis_term(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_coriolis = robot_cfg.idyn_function_values.coriolis_term + adam_coriolis = cs.DM( + adam_kin_dyn.coriolis_term_fun()( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + ) + assert idyn_coriolis - adam_coriolis == pytest.approx(0.0, abs=1e-5) + + +def test_gravity_term(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_gravity = robot_cfg.idyn_function_values.gravity_term + adam_gravity = cs.DM( + adam_kin_dyn.gravity_term_fun()( + state.H, state.joints_pos, original_length, original_density + ) + ) + assert idyn_gravity - adam_gravity == pytest.approx(0.0, abs=1e-5) diff --git a/tests/parametric/test_jax.py b/tests/parametric/test_jax.py new file mode 100644 index 0000000..d3ab181 --- /dev/null +++ b/tests/parametric/test_jax.py @@ -0,0 +1,154 @@ +import numpy as np +import pytest +from adam.parametric.jax import KinDynComputationsParametric +from conftest import State, RobotCfg + + +@pytest.fixture(scope="module") +def setup_test(tests_setup) -> KinDynComputationsParametric | RobotCfg | State: + robot_cfg, state = tests_setup + # skip the tests if the model is not the StickBot + if robot_cfg.robot_name != "StickBot": + pytest.skip("Skipping the test because the model is not StickBot") + link_name_list = ["chest"] + adam_kin_dyn = KinDynComputationsParametric( + robot_cfg.model_path, robot_cfg.joints_name_list, link_name_list + ) + # This is the original density value associated to the chest link, computed as mass/volume + original_density = [628.0724496264945] + original_length = np.ones(len(link_name_list)) + adam_kin_dyn.set_frame_velocity_representation(robot_cfg.velocity_representation) + return adam_kin_dyn, robot_cfg, state, original_density, original_length + + +def test_mass_matrix(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix + adam_mass_matrix = adam_kin_dyn.mass_matrix( + state.H, state.joints_pos, original_length, original_density + ) + assert adam_mass_matrix - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_cmm = robot_cfg.idyn_function_values.centroidal_momentum_matrix + adam_cmm = adam_kin_dyn.centroidal_momentum_matrix( + state.H, state.joints_pos, original_length, original_density + ) + assert adam_cmm - idyn_cmm == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_com = robot_cfg.idyn_function_values.CoM_position + adam_com = adam_kin_dyn.CoM_position( + state.H, state.joints_pos, original_length, original_density + ) + assert adam_com - idyn_com == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_total_mass = robot_cfg.idyn_function_values.total_mass + assert adam_kin_dyn.get_total_mass( + original_length, original_density + ) - idyn_total_mass == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian + adam_jacobian = adam_kin_dyn.jacobian( + "l_sole", state.H, state.joints_pos, original_length, original_density + ) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian_non_actuated + adam_jacobian = adam_kin_dyn.jacobian( + "head", state.H, state.joints_pos, original_length, original_density + ) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian_dot_nu = robot_cfg.idyn_function_values.jacobian_dot_nu + adam_jacobian_dot_nu = adam_kin_dyn.jacobian_dot( + "l_sole", + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu == pytest.approx(0.0, abs=1e-5) + + +def test_relative_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.relative_jacobian + adam_jacobian = adam_kin_dyn.relative_jacobian( + "l_sole", state.joints_pos, original_length, original_density + ) + assert idyn_jacobian - adam_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_fk(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics + adam_H = adam_kin_dyn.forward_kinematics( + "l_sole", state.H, state.joints_pos, original_length, original_density + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics_non_actuated + adam_H = adam_kin_dyn.forward_kinematics( + "head", state.H, state.joints_pos, original_length, original_density + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_bias_force = robot_cfg.idyn_function_values.bias_force + adam_bias_force = adam_kin_dyn.bias_force( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + assert idyn_bias_force - adam_bias_force == pytest.approx(0.0, abs=1e-4) + # assert np.allclose(idyn_bias_force, adam_bias_force, atol=1e-5) + + +def test_coriolis_term(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_coriolis_gravity = robot_cfg.idyn_function_values.coriolis_term + adam_coriolis_gravity = adam_kin_dyn.coriolis_term( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + assert idyn_coriolis_gravity - adam_coriolis_gravity == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_gravity = robot_cfg.idyn_function_values.gravity_term + adam_gravity = adam_kin_dyn.gravity_term( + state.H, state.joints_pos, original_length, original_density + ) + assert np.allclose(idyn_gravity, adam_gravity, atol=1e-4) diff --git a/tests/parametric/test_numpy.py b/tests/parametric/test_numpy.py new file mode 100644 index 0000000..54e2c47 --- /dev/null +++ b/tests/parametric/test_numpy.py @@ -0,0 +1,154 @@ +import numpy as np +import pytest +from adam.parametric.numpy import KinDynComputationsParametric +from conftest import State, RobotCfg + + +@pytest.fixture(scope="module") +def setup_test(tests_setup) -> KinDynComputationsParametric | RobotCfg | State: + robot_cfg, state = tests_setup + # skip the tests if the model is not the StickBot + if robot_cfg.robot_name != "StickBot": + pytest.skip("Skipping the test because the model is not StickBot") + link_name_list = ["chest"] + adam_kin_dyn = KinDynComputationsParametric( + robot_cfg.model_path, robot_cfg.joints_name_list, link_name_list + ) + # This is the original density value associated to the chest link, computed as mass/volume + original_density = [628.0724496264945] + original_length = np.ones(len(link_name_list)) + adam_kin_dyn.set_frame_velocity_representation(robot_cfg.velocity_representation) + return adam_kin_dyn, robot_cfg, state, original_density, original_length + + +def test_mass_matrix(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix + adam_mass_matrix = adam_kin_dyn.mass_matrix( + state.H, state.joints_pos, original_length, original_density + ) + assert adam_mass_matrix - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_cmm = robot_cfg.idyn_function_values.centroidal_momentum_matrix + adam_cmm = adam_kin_dyn.centroidal_momentum_matrix( + state.H, state.joints_pos, original_length, original_density + ) + assert adam_cmm - idyn_cmm == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_com = robot_cfg.idyn_function_values.CoM_position + adam_com = adam_kin_dyn.CoM_position( + state.H, state.joints_pos, original_length, original_density + ) + assert adam_com - idyn_com == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_total_mass = robot_cfg.idyn_function_values.total_mass + assert adam_kin_dyn.get_total_mass( + original_length, original_density + ) - idyn_total_mass == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian + adam_jacobian = adam_kin_dyn.jacobian( + "l_sole", state.H, state.joints_pos, original_length, original_density + ) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian_non_actuated + adam_jacobian = adam_kin_dyn.jacobian( + "head", state.H, state.joints_pos, original_length, original_density + ) + assert adam_jacobian - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian_dot_nu = robot_cfg.idyn_function_values.jacobian_dot_nu + adam_jacobian_dot_nu = adam_kin_dyn.jacobian_dot( + "l_sole", + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu == pytest.approx(0.0, abs=1e-5) + + +def test_relative_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.relative_jacobian + adam_jacobian = adam_kin_dyn.relative_jacobian( + "l_sole", state.joints_pos, original_length, original_density + ) + assert idyn_jacobian - adam_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_fk(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics + adam_H = adam_kin_dyn.forward_kinematics( + "l_sole", state.H, state.joints_pos, original_length, original_density + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics_non_actuated + adam_H = adam_kin_dyn.forward_kinematics( + "head", state.H, state.joints_pos, original_length, original_density + ) + assert idyn_H - adam_H == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_bias_force = robot_cfg.idyn_function_values.bias_force + adam_bias_force = adam_kin_dyn.bias_force( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + assert idyn_bias_force - adam_bias_force == pytest.approx(0.0, abs=1e-4) + # assert np.allclose(idyn_bias_force, adam_bias_force, atol=1e-5) + + +def test_coriolis_term(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_coriolis_gravity = robot_cfg.idyn_function_values.coriolis_term + adam_coriolis_gravity = adam_kin_dyn.coriolis_term( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + assert idyn_coriolis_gravity - adam_coriolis_gravity == pytest.approx(0.0, abs=1e-4) + + +def test_gravity_term(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_gravity = robot_cfg.idyn_function_values.gravity_term + adam_gravity = adam_kin_dyn.gravity_term( + state.H, state.joints_pos, original_length, original_density + ) + assert np.allclose(idyn_gravity, adam_gravity, atol=1e-4) diff --git a/tests/parametric/test_pytorch.py b/tests/parametric/test_pytorch.py new file mode 100644 index 0000000..00efecf --- /dev/null +++ b/tests/parametric/test_pytorch.py @@ -0,0 +1,165 @@ +import numpy as np +import pytest +from adam.parametric.pytorch import KinDynComputationsParametric +from conftest import State, RobotCfg +import torch + +torch.set_default_dtype(torch.float64) + +@pytest.fixture(scope="module") +def setup_test(tests_setup) -> KinDynComputationsParametric | RobotCfg | State: + robot_cfg, state = tests_setup + # skip the tests if the model is not the StickBot + if robot_cfg.robot_name != "StickBot": + pytest.skip("Skipping the test because the model is not StickBot") + link_name_list = ["chest"] + adam_kin_dyn = KinDynComputationsParametric( + robot_cfg.model_path, robot_cfg.joints_name_list, link_name_list + ) + # This is the original density value associated to the chest link, computed as mass/volume + original_density = [628.0724496264945] + original_length = torch.ones(len(link_name_list)) + adam_kin_dyn.set_frame_velocity_representation(robot_cfg.velocity_representation) + # convert state quantities to torch tensors + state.H = torch.tensor(state.H) + state.joints_pos = torch.tensor(state.joints_pos) + state.base_vel = torch.tensor(state.base_vel) + state.joints_vel = torch.tensor(state.joints_vel) + state.gravity = torch.tensor(state.gravity) + return adam_kin_dyn, robot_cfg, state, original_density, original_length + + +def test_mass_matrix(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix + adam_mass_matrix = adam_kin_dyn.mass_matrix( + state.H, state.joints_pos, original_length, original_density + ) + assert adam_mass_matrix.numpy() - idyn_mass_matrix == pytest.approx(0.0, abs=1e-5) + + +def test_CMM(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_cmm = robot_cfg.idyn_function_values.centroidal_momentum_matrix + adam_cmm = adam_kin_dyn.centroidal_momentum_matrix( + state.H, state.joints_pos, original_length, original_density + ) + assert adam_cmm.numpy() - idyn_cmm == pytest.approx(0.0, abs=1e-5) + + +def test_CoM_pos(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_com = robot_cfg.idyn_function_values.CoM_position + adam_com = adam_kin_dyn.CoM_position( + state.H, state.joints_pos, original_length, original_density + ) + assert adam_com.numpy() - idyn_com == pytest.approx(0.0, abs=1e-5) + + +def test_total_mass(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_total_mass = robot_cfg.idyn_function_values.total_mass + assert adam_kin_dyn.get_total_mass( + original_length, original_density + ) - idyn_total_mass == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian + adam_jacobian = adam_kin_dyn.jacobian( + "l_sole", state.H, state.joints_pos, original_length, original_density + ) + assert adam_jacobian.numpy() - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.jacobian_non_actuated + adam_jacobian = adam_kin_dyn.jacobian( + "head", state.H, state.joints_pos, original_length, original_density + ) + assert adam_jacobian.numpy() - idyn_jacobian == pytest.approx(0.0, abs=1e-5) + + +def test_jacobian_dot(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian_dot_nu = robot_cfg.idyn_function_values.jacobian_dot_nu + adam_jacobian_dot_nu = adam_kin_dyn.jacobian_dot( + "l_sole", + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) @ np.concatenate((state.base_vel, state.joints_vel)) + assert idyn_jacobian_dot_nu - adam_jacobian_dot_nu.numpy() == pytest.approx( + 0.0, abs=1e-5 + ) + + +def test_relative_jacobian(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_jacobian = robot_cfg.idyn_function_values.relative_jacobian + adam_jacobian = adam_kin_dyn.relative_jacobian( + "l_sole", state.joints_pos, original_length, original_density + ) + assert idyn_jacobian - adam_jacobian.numpy() == pytest.approx(0.0, abs=1e-5) + + +def test_fk(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics + adam_H = adam_kin_dyn.forward_kinematics( + "l_sole", state.H, state.joints_pos, original_length, original_density + ) + assert idyn_H - adam_H.numpy() == pytest.approx(0.0, abs=1e-5) + + +def test_fk_non_actuated(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_H = robot_cfg.idyn_function_values.forward_kinematics_non_actuated + adam_H = adam_kin_dyn.forward_kinematics( + "head", state.H, state.joints_pos, original_length, original_density + ) + assert idyn_H - adam_H.numpy() == pytest.approx(0.0, abs=1e-5) + + +def test_bias_force(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_bias_force = robot_cfg.idyn_function_values.bias_force + adam_bias_force = adam_kin_dyn.bias_force( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + assert idyn_bias_force - adam_bias_force.numpy() == pytest.approx(0.0, abs=1e-4) + + +def test_coriolis_term(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_coriolis_gravity = robot_cfg.idyn_function_values.coriolis_term + adam_coriolis_gravity = adam_kin_dyn.coriolis_term( + state.H, + state.joints_pos, + state.base_vel, + state.joints_vel, + original_length, + original_density, + ) + assert idyn_coriolis_gravity - adam_coriolis_gravity.numpy() == pytest.approx( + 0.0, abs=1e-4 + ) + + +def test_gravity_term(setup_test): + adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test + idyn_gravity = robot_cfg.idyn_function_values.gravity_term + adam_gravity = adam_kin_dyn.gravity_term( + state.H, state.joints_pos, original_length, original_density + ) + assert np.allclose(idyn_gravity, adam_gravity.numpy(), atol=1e-4) From 8deff447f6e1fca6c3c503c03a202eae47f48801 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 19 Dec 2024 13:10:58 +0100 Subject: [PATCH 15/20] Remove old tests --- tests/body_fixed/test_CasADi_body_fixed.py | 242 ----------------- tests/body_fixed/test_Jax_body_fixed.py | 195 -------------- tests/body_fixed/test_NumPy_body_fixed.py | 192 -------------- tests/body_fixed/test_pytorch_body_fixed.py | 207 --------------- tests/conversions/test_idyntree.py | 245 ----------------- tests/conversions/test_idyntree_parametric.py | 247 ------------------ tests/mixed/test_CasADi_mixed.py | 242 ----------------- tests/mixed/test_Jax_mixed.py | 195 -------------- tests/mixed/test_NumPy_mixed.py | 191 -------------- tests/mixed/test_pytorch_mixed.py | 207 --------------- .../test_CasADi_computations_parametric.py | 219 ---------------- .../test_Jax_computations_parametric.py | 202 -------------- .../test_NumPy_computations_parametric.py | 201 -------------- .../test_pytorch_computations_parametric.py | 211 --------------- tests/pytorch_batch/test_pytorch_batch_bak.py | 186 ------------- 15 files changed, 3182 deletions(-) delete mode 100644 tests/body_fixed/test_CasADi_body_fixed.py delete mode 100644 tests/body_fixed/test_Jax_body_fixed.py delete mode 100644 tests/body_fixed/test_NumPy_body_fixed.py delete mode 100644 tests/body_fixed/test_pytorch_body_fixed.py delete mode 100644 tests/conversions/test_idyntree.py delete mode 100644 tests/conversions/test_idyntree_parametric.py delete mode 100644 tests/mixed/test_CasADi_mixed.py delete mode 100644 tests/mixed/test_Jax_mixed.py delete mode 100644 tests/mixed/test_NumPy_mixed.py delete mode 100644 tests/mixed/test_pytorch_mixed.py delete mode 100644 tests/parametric/test_CasADi_computations_parametric.py delete mode 100644 tests/parametric/test_Jax_computations_parametric.py delete mode 100644 tests/parametric/test_NumPy_computations_parametric.py delete mode 100644 tests/parametric/test_pytorch_computations_parametric.py delete mode 100644 tests/pytorch_batch/test_pytorch_batch_bak.py diff --git a/tests/body_fixed/test_CasADi_body_fixed.py b/tests/body_fixed/test_CasADi_body_fixed.py deleted file mode 100644 index 8d5b8c0..0000000 --- a/tests/body_fixed/test_CasADi_body_fixed.py +++ /dev/null @@ -1,242 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging - -import casadi as cs -import icub_models -import idyntree.swig as idyntree -import numpy as np -import pytest - -from adam.casadi import KinDynComputations -from adam.numpy.numpy_like import SpatialMath -from adam import Representations - - -np.random.seed(42) - -model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -def H_from_Pos_RPY_idyn(xyz, rpy): - T = idyntree.Transform.Identity() - R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) - p = idyntree.Position() - [p.setVal(i, xyz[i]) for i in range(3)] - T.setRotation(R) - T.setPosition(p) - return T - - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) -comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) -robot_iDyn = idyntree.ModelLoader() -robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) - -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(robot_iDyn.model()) -kinDyn.setFloatingBase(root_link) -kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) -n_dofs = len(joints_name_list) - -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -g = np.array([0, 0, -9.80665]) -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) - - -def test_mass_matrix(): - M = comp.mass_matrix_fun() - mass_mx = idyntree.MatrixDynSize() - kinDyn.getFreeFloatingMassMatrix(mass_mx) - mass_mxNumpy = mass_mx.toNumPy() - mass_test = cs.DM(M(H_b, joints_val)) - mass_test2 = cs.DM(comp.mass_matrix(H_b, joints_val)) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - assert mass_test2 - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - Jcm = comp.centroidal_momentum_matrix_fun() - cmm_idyntree = idyntree.MatrixDynSize() - kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) - cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = cs.DM(Jcm(H_b, joints_val)) - Jcm_test2 = cs.DM(comp.centroidal_momentum_matrix(H_b, joints_val)) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - assert Jcm_test2 - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - com_f = comp.CoM_position_fun() - CoM_test = cs.DM(com_f(H_b, joints_val)) - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - CoM_test2 = cs.DM(comp.CoM_position(H_b, joints_val)) - assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - assert CoM_test2 - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( - 0.0, abs=1e-5 - ) - - -def test_jacobian(): - J_tot = comp.jacobian_fun("l_sole") - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = cs.DM(J_tot(H_b, joints_val)) - J_test2 = cs.DM(comp.jacobian("l_sole", H_b, joints_val)) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - J_tot = comp.jacobian_fun("head") - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = cs.DM(J_tot(H_b, joints_val)) - J_test2 = cs.DM(comp.jacobian("head", H_b, joints_val)) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot_fun("l_sole") - Jdotnu = kinDyn.getFrameBiasAcc("l_sole") - Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot(H_b, joints_val, base_vel, joints_dot_val) @ np.concatenate( - (base_vel, joints_dot_val) - ) - J_dot_nu_test2 = cs.DM( - comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - ) @ np.concatenate((base_vel, joints_dot_val)) - assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) - assert Jdot_nu - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_idyntree = kinDyn.getWorldTransform("l_sole") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - T = comp.forward_kinematics_fun("l_sole") - H_test = cs.DM(T(H_b, joints_val)) - H_test2 = cs.DM(comp.forward_kinematics("l_sole", H_b, joints_val)) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_idyntree = kinDyn.getWorldTransform("head") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - T = comp.forward_kinematics_fun("head") - H_test = cs.DM(T(H_b, joints_val)) - H_test2 = cs.DM(comp.forward_kinematics("head", H_b, joints_val)) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(h_iDyn) - h_iDyn_np = np.concatenate( - (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) - ) - h = comp.bias_force_fun() - h_test = cs.DM(h(H_b, joints_val, base_vel, joints_dot_val)) - h_test2 = cs.DM(comp.bias_force(H_b, joints_val, base_vel, joints_dot_val)) - assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) - assert h_iDyn_np - h_test2 == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) - C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(C_iDyn) - C_iDyn_np = np.concatenate( - (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) - ) - C = comp.coriolis_term_fun() - C_test = cs.DM(C(H_b, joints_val, base_vel, joints_dot_val)) - C_test2 = cs.DM(comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val)) - assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) - assert C_iDyn_np - C_test2 == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - base_vel_0 = np.zeros(6) - joints_dot_val_0 = np.zeros(n_dofs) - kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) - kinDyn.setRobotState(H_b, joints_val, base_vel_0, joints_dot_val_0, g) - G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(G_iDyn) - G_iDyn_np = np.concatenate( - (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) - ) - G = comp.gravity_term_fun() - G_test = cs.DM(G(H_b, joints_val)) - G_test2 = cs.DM(comp.gravity_term(H_b, joints_val)) - assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) - assert G_iDyn_np - G_test2 == pytest.approx(0.0, abs=1e-4) - - -def test_relative_jacobian(): - eye = np.eye(4) - kinDyn.setRobotState(eye, joints_val, base_vel, joints_dot_val, g) - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] - J_fun = comp.relative_jacobian_fun("l_sole") - J_test = cs.DM(J_fun(joints_val)) - 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) diff --git a/tests/body_fixed/test_Jax_body_fixed.py b/tests/body_fixed/test_Jax_body_fixed.py deleted file mode 100644 index 1bfc8eb..0000000 --- a/tests/body_fixed/test_Jax_body_fixed.py +++ /dev/null @@ -1,195 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging - -import icub_models -import idyntree.swig as idyntree -import jax.numpy as jnp -import numpy as np -import pytest -from jax import config - -import adam -from adam.jax import KinDynComputations -from adam.numpy.numpy_like import SpatialMath - - -np.random.seed(42) -config.update("jax_enable_x64", True) - -model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -def H_from_Pos_RPY_idyn(xyz, rpy): - T = idyntree.Transform.Identity() - R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) - p = idyntree.Position() - [p.setVal(i, xyz[i]) for i in range(3)] - T.setRotation(R) - T.setPosition(p) - return T - - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) -comp.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION) -robot_iDyn = idyntree.ModelLoader() -robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) - -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(robot_iDyn.model()) -kinDyn.setFloatingBase(root_link) -kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) -n_dofs = len(joints_name_list) - -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -g = np.array([0, 0, -9.80665]) -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) - - -def test_mass_matrix(): - mass_mx = idyntree.MatrixDynSize() - kinDyn.getFreeFloatingMassMatrix(mass_mx) - mass_mxNumpy = mass_mx.toNumPy() - mass_test = comp.mass_matrix(H_b, joints_val) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - cmm_idyntree = idyntree.MatrixDynSize() - kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) - cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, joints_val) - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( - 0.0, abs=1e-5 - ) - - -def test_jacobian(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, joints_val) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("head", H_b, joints_val) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - Jdotnu = kinDyn.getFrameBiasAcc("l_sole") - Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) - assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_idyntree = kinDyn.getWorldTransform("l_sole") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("l_sole", H_b, joints_val) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_idyntree = kinDyn.getWorldTransform("head") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("head", H_b, joints_val) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(h_iDyn) - h_iDyn_np = np.concatenate( - (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) - ) - h_test = comp.bias_force(H_b, joints_val, base_vel, joints_dot_val) - assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = np.zeros(3) - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) - C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(C_iDyn) - C_iDyn_np = np.concatenate( - (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) - ) - C_test = comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) - assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - kinDyn2 = idyntree.KinDynComputations() - kinDyn2.loadRobotModel(robot_iDyn.model()) - kinDyn2.setFloatingBase(root_link) - kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) - base_vel0 = np.zeros(6) - joints_dot_val0 = np.zeros(n_dofs) - kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) - G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) - assert kinDyn2.generalizedBiasForces(G_iDyn) - G_iDyn_np = np.concatenate( - (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) - ) - G_test = comp.gravity_term(H_b, joints_val) - assert G_iDyn_np - G_test == 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 deleted file mode 100644 index 51b297f..0000000 --- a/tests/body_fixed/test_NumPy_body_fixed.py +++ /dev/null @@ -1,192 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging - -import icub_models -import idyntree.swig as idyntree -import numpy as np -import pytest - -from adam import Representations -from adam.numpy import KinDynComputations -from adam.numpy.numpy_like import SpatialMath - - -np.random.seed(42) - -model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -def H_from_Pos_RPY_idyn(xyz, rpy): - T = idyntree.Transform.Identity() - R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) - p = idyntree.Position() - [p.setVal(i, xyz[i]) for i in range(3)] - T.setRotation(R) - T.setPosition(p) - return T - - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) -comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) -robot_iDyn = idyntree.ModelLoader() -robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) - -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(robot_iDyn.model()) -kinDyn.setFloatingBase(root_link) -kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) -n_dofs = len(joints_name_list) - -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -g = np.array([0, 0, -9.80665]) -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) - - -def test_mass_matrix(): - mass_mx = idyntree.MatrixDynSize() - kinDyn.getFreeFloatingMassMatrix(mass_mx) - mass_mxNumpy = mass_mx.toNumPy() - mass_test = comp.mass_matrix(H_b, joints_val) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - cmm_idyntree = idyntree.MatrixDynSize() - kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) - cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, joints_val) - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( - 0.0, abs=1e-5 - ) - - -def test_jacobian(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, joints_val) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("head", H_b, joints_val) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - Jdotnu = kinDyn.getFrameBiasAcc("l_sole") - Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) - assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_idyntree = kinDyn.getWorldTransform("l_sole") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("l_sole", H_b, joints_val) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_idyntree = kinDyn.getWorldTransform("head") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("head", H_b, joints_val) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(h_iDyn) - h_iDyn_np = np.concatenate( - (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) - ) - h_test = comp.bias_force(H_b, joints_val, base_vel, joints_dot_val) - assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = np.zeros(3) - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) - C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(C_iDyn) - C_iDyn_np = np.concatenate( - (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) - ) - C_test = comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) - assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - kinDyn2 = idyntree.KinDynComputations() - kinDyn2.loadRobotModel(robot_iDyn.model()) - kinDyn2.setFloatingBase(root_link) - kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) - base_vel0 = np.zeros(6) - joints_dot_val0 = np.zeros(n_dofs) - kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) - G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) - assert kinDyn2.generalizedBiasForces(G_iDyn) - G_iDyn_np = np.concatenate( - (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) - ) - G_test = comp.gravity_term(H_b, joints_val) - assert G_iDyn_np - G_test == 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 deleted file mode 100644 index a6a1bba..0000000 --- a/tests/body_fixed/test_pytorch_body_fixed.py +++ /dev/null @@ -1,207 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging - -import icub_models -import idyntree.swig as idyntree -import numpy as np -import pytest -import torch - -from adam import Representations -from adam.pytorch import KinDynComputations -from adam.pytorch.torch_like import SpatialMath - - -np.random.seed(42) -torch.set_default_dtype(torch.float64) - -model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -def H_from_Pos_RPY_idyn(xyz, rpy): - T = idyntree.Transform.Identity() - R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) - p = idyntree.Position() - [p.setVal(i, xyz[i]) for i in range(3)] - T.setRotation(R) - T.setPosition(p) - return T - - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) -comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) -robot_iDyn = idyntree.ModelLoader() -robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) - -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(robot_iDyn.model()) -kinDyn.setFloatingBase(root_link) -kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) -n_dofs = len(joints_name_list) - -xyz = (torch.rand(3) - 0.5) * 5 -rpy = (torch.rand(3) - 0.5) * 5 -base_vel = (torch.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (torch.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (torch.rand(n_dofs) - 0.5) * 5 - -g = torch.tensor([0, 0, -9.80665]) -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -kinDyn.setRobotState( - H_b.numpy(), joints_val.numpy(), base_vel.numpy(), joints_dot_val.numpy(), g.numpy() -) - - -def test_mass_matrix(): - mass_mx = idyntree.MatrixDynSize() - kinDyn.getFreeFloatingMassMatrix(mass_mx) - mass_mxNumpy = mass_mx.toNumPy() - mass_test = comp.mass_matrix(H_b, joints_val) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - cmm_idyntree = idyntree.MatrixDynSize() - kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) - cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, joints_val) - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( - 0.0, abs=1e-5 - ) - - -def test_jacobian(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, joints_val) - assert iDynNumpyJ_ - np.asarray(J_test) == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("head", H_b, joints_val) - assert iDynNumpyJ_ - np.asarray(J_test) == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - Jdotnu = kinDyn.getFrameBiasAcc("l_sole") - Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) - assert Jdot_nu - np.asarray(J_dot_nu_test) == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_idyntree = kinDyn.getWorldTransform("l_sole") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = np.asarray(comp.forward_kinematics("l_sole", H_b, joints_val)) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_idyntree = kinDyn.getWorldTransform("head") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = np.asarray(comp.forward_kinematics("head", H_b, joints_val)) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(h_iDyn) - h_iDyn_np = np.concatenate( - (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) - ) - h_test = comp.bias_force(H_b, joints_val, base_vel, joints_dot_val) - assert h_iDyn_np - np.asarray(h_test) == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = torch.zeros(3) - kinDyn.setRobotState( - H_b.numpy(), - joints_val.numpy(), - base_vel.numpy(), - joints_dot_val.numpy(), - g0.numpy(), - ) - C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(C_iDyn) - C_iDyn_np = np.concatenate( - (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) - ) - C_test = comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) - assert C_iDyn_np - np.asarray(C_test) == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - kinDyn2 = idyntree.KinDynComputations() - kinDyn2.loadRobotModel(robot_iDyn.model()) - kinDyn2.setFloatingBase(root_link) - kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) - base_vel0 = torch.zeros(6) - joints_dot_val0 = torch.zeros(n_dofs) - kinDyn2.setRobotState( - H_b.numpy(), - joints_val.numpy(), - base_vel0.numpy(), - joints_dot_val0.numpy(), - g.numpy(), - ) - G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) - assert kinDyn2.generalizedBiasForces(G_iDyn) - G_iDyn_np = np.concatenate( - (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) - ) - G_test = comp.gravity_term(H_b, joints_val) - assert G_iDyn_np - np.asarray(G_test) == pytest.approx(0.0, abs=1e-4) diff --git a/tests/conversions/test_idyntree.py b/tests/conversions/test_idyntree.py deleted file mode 100644 index 975e33b..0000000 --- a/tests/conversions/test_idyntree.py +++ /dev/null @@ -1,245 +0,0 @@ -# Copyright (C) 2024 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging - -import casadi as cs -import icub_models -import idyntree.swig as idyntree -import numpy as np -import pytest - -from adam.casadi import KinDynComputations -from adam.numpy.numpy_like import SpatialMath -from adam import Representations -from adam.model.conversions.idyntree import to_idyntree_model - - -np.random.seed(42) - -model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -def H_from_Pos_RPY_idyn(xyz, rpy): - T = idyntree.Transform.Identity() - R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) - p = idyntree.Position() - [p.setVal(i, xyz[i]) for i in range(3)] - T.setRotation(R) - T.setPosition(p) - return T - - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) -comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) - -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(to_idyntree_model(comp.rbdalgos.model)) -kinDyn.setFloatingBase(root_link) -kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) -n_dofs = len(joints_name_list) - -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -g = np.array([0, 0, -9.80665]) -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) - - -def test_mass_matrix(): - M = comp.mass_matrix_fun() - mass_mx = idyntree.MatrixDynSize() - kinDyn.getFreeFloatingMassMatrix(mass_mx) - mass_mxNumpy = mass_mx.toNumPy() - mass_test = cs.DM(M(H_b, joints_val)) - mass_test2 = cs.DM(comp.mass_matrix(H_b, joints_val)) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - assert mass_test2 - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - Jcm = comp.centroidal_momentum_matrix_fun() - cmm_idyntree = idyntree.MatrixDynSize() - kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) - cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = cs.DM(Jcm(H_b, joints_val)) - Jcm_test2 = cs.DM(comp.centroidal_momentum_matrix(H_b, joints_val)) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - assert Jcm_test2 - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - com_f = comp.CoM_position_fun() - CoM_test = cs.DM(com_f(H_b, joints_val)) - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - CoM_test2 = cs.DM(comp.CoM_position(H_b, joints_val)) - assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - assert CoM_test2 - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - assert comp.get_total_mass() - kinDyn.model().getTotalMass() == pytest.approx( - 0.0, abs=1e-5 - ) - - -def test_jacobian(): - J_tot = comp.jacobian_fun("l_sole") - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = cs.DM(J_tot(H_b, joints_val)) - J_test2 = cs.DM(comp.jacobian("l_sole", H_b, joints_val)) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - J_tot = comp.jacobian_fun("head") - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = cs.DM(J_tot(H_b, joints_val)) - J_test2 = cs.DM(comp.jacobian("head", H_b, joints_val)) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot_fun("l_sole") - Jdotnu = kinDyn.getFrameBiasAcc("l_sole") - Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot(H_b, joints_val, base_vel, joints_dot_val) @ np.concatenate( - (base_vel, joints_dot_val) - ) - J_dot_nu_test2 = cs.DM( - comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - ) @ np.concatenate((base_vel, joints_dot_val)) - assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) - assert Jdot_nu - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_idyntree = kinDyn.getWorldTransform("l_sole") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - T = comp.forward_kinematics_fun("l_sole") - H_test = cs.DM(T(H_b, joints_val)) - H_test2 = cs.DM(comp.forward_kinematics("l_sole", H_b, joints_val)) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_idyntree = kinDyn.getWorldTransform("head") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - T = comp.forward_kinematics_fun("head") - H_test = cs.DM(T(H_b, joints_val)) - H_test2 = cs.DM(comp.forward_kinematics("head", H_b, joints_val)) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(h_iDyn) - h_iDyn_np = np.concatenate( - (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) - ) - h = comp.bias_force_fun() - h_test = cs.DM(h(H_b, joints_val, base_vel, joints_dot_val)) - h_test2 = cs.DM(comp.bias_force(H_b, joints_val, base_vel, joints_dot_val)) - assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) - assert h_iDyn_np - h_test2 == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) - C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(C_iDyn) - C_iDyn_np = np.concatenate( - (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) - ) - C = comp.coriolis_term_fun() - C_test = cs.DM(C(H_b, joints_val, base_vel, joints_dot_val)) - C_test2 = cs.DM(comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val)) - assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) - assert C_iDyn_np - C_test2 == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - base_vel_0 = np.zeros(6) - joints_dot_val_0 = np.zeros(n_dofs) - kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION) - kinDyn.setRobotState(H_b, joints_val, base_vel_0, joints_dot_val_0, g) - G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(G_iDyn) - G_iDyn_np = np.concatenate( - (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) - ) - G = comp.gravity_term_fun() - G_test = cs.DM(G(H_b, joints_val)) - G_test2 = cs.DM(comp.gravity_term(H_b, joints_val)) - assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) - assert G_iDyn_np - G_test2 == pytest.approx(0.0, abs=1e-4) - - -def test_relative_jacobian(): - eye = np.eye(4) - kinDyn.setRobotState(eye, joints_val, base_vel, joints_dot_val, g) - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] - J_fun = comp.relative_jacobian_fun("l_sole") - J_test = cs.DM(J_fun(joints_val)) - 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) diff --git a/tests/conversions/test_idyntree_parametric.py b/tests/conversions/test_idyntree_parametric.py deleted file mode 100644 index 6b6862b..0000000 --- a/tests/conversions/test_idyntree_parametric.py +++ /dev/null @@ -1,247 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging -import casadi as cs -import idyntree.swig as idyntree -import numpy as np -import pytest - -import adam.numpy -from adam.parametric.casadi import KinDynComputationsParametric -from adam.parametric.model.parametric_factories.parametric_model import ( - URDFParametricModelFactory, -) -from adam.model.conversions.idyntree import to_idyntree_model -from adam.core.constants import Representations -from adam.numpy.numpy_like import SpatialMath - -import tempfile -from git import Repo - - -# Getting stickbot urdf file -temp_dir = tempfile.TemporaryDirectory() -git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" -Repo.clone_from(git_url, temp_dir.name) -model_path = temp_dir.name + "/models/stickBot/model.urdf" - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - - -root_link = "root_link" - -link_name_list = ["chest"] -comp_w_hardware = KinDynComputationsParametric( - model_path, joints_name_list, link_name_list, root_link -) -comp_w_hardware.set_frame_velocity_representation(Representations.MIXED_REPRESENTATION) - -original_density = [ - 628.0724496264945 -] # This is the original density value associated to the chest link, computed as mass/volume -original_length = np.ones(len(link_name_list)) - -# TODO: the following two commands should be moved to a proper function/method -factory = URDFParametricModelFactory( - path=model_path, - math=adam.numpy.numpy_like.SpatialMath(), - links_name_list=link_name_list, - length_multiplier=original_length, - densities=original_density, -) -model = adam.model.Model.build(factory=factory, joints_name_list=joints_name_list) - - -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(to_idyntree_model(model)) -kinDyn.setFloatingBase(root_link) -kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) -n_dofs = len(joints_name_list) - - -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantities -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val - -g = np.array([0, 0, -9.80665]) -kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) - - -def test_mass_matrix(): - mass_mx = idyntree.MatrixDynSize() - kinDyn.getFreeFloatingMassMatrix(mass_mx) - mass_mxNumpy = mass_mx.toNumPy() - M_with_hardware = comp_w_hardware.mass_matrix_fun() - mass_test_hardware = cs.DM( - M_with_hardware(H_b, s_, original_length, original_density) - ) - assert mass_mxNumpy - mass_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - cmm_idyntree = idyntree.MatrixDynSize() - kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) - cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_with_hardware = comp_w_hardware.centroidal_momentum_matrix_fun() - Jcm_test_hardware = cs.DM( - Jcm_with_hardware(H_b, s_, original_length, original_density) - ) - assert cmm_idyntreeNumpy - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - com_with_hardware_f = comp_w_hardware.CoM_position_fun() - CoM_hardware = cs.DM( - com_with_hardware_f(H_b, s_, original_length, original_density) - ) - assert CoM_iDynTree - CoM_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - mass = kinDyn.model().getTotalMass() - mass_hardware_fun = comp_w_hardware.get_total_mass() - mass_hardware = cs.DM(mass_hardware_fun(original_length, original_density)) - assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_tot_with_hardware = comp_w_hardware.jacobian_fun("l_sole") - J_test_hardware = cs.DM( - J_tot_with_hardware(H_b, s_, original_length, original_density) - ) - assert iDynNumpyJ_ - J_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_tot_with_hardware = comp_w_hardware.jacobian_fun("head") - J_tot_with_hardware_test = cs.DM( - J_tot_with_hardware(H_b, s_, original_length, original_density) - ) - assert iDynNumpyJ_ - J_tot_with_hardware_test == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - Jdotnu = kinDyn.getFrameBiasAcc("l_sole") - Jdot_nu = Jdotnu.toNumPy() - J_dot_hardware = comp_w_hardware.jacobian_dot_fun("l_sole") - J_dot_nu_test2 = cs.DM( - J_dot_hardware( - H_b, joints_val, base_vel, joints_dot_val, original_length, original_density - ) - @ np.concatenate((base_vel, joints_dot_val)) - ) - assert Jdot_nu - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_idyntree = kinDyn.getWorldTransform("l_sole") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - T_with_hardware = comp_w_hardware.forward_kinematics_fun("l_sole") - H_with_hardware_test = cs.DM( - T_with_hardware(H_b, s_, original_length, original_density) - ) - assert H_with_hardware_test[:3, :3] - R_idy2np == pytest.approx(0.0, abs=1e-5) - assert H_with_hardware_test[:3, 3] - p_idy2np == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_idyntree = kinDyn.getWorldTransform("head") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - T_with_hardware = comp_w_hardware.forward_kinematics_fun("head") - H_with_hardware_test = cs.DM( - T_with_hardware(H_b, s_, original_length, original_density) - ) - assert H_with_hardware_test[:3, :3] - R_idy2np == pytest.approx(0.0, abs=1e-5) - assert H_with_hardware_test[:3, 3] - p_idy2np == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(h_iDyn) - h_iDyn_np = np.concatenate( - (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) - ) - - h_with_hardware = comp_w_hardware.bias_force_fun() - h_with_hardware_test = cs.DM( - h_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density) - ) - assert h_with_hardware_test - h_iDyn_np == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) - C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(C_iDyn) - C_iDyn_np = np.concatenate( - (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) - ) - C_with_hardware = comp_w_hardware.coriolis_term_fun() - C_with_hardware_test = cs.DM( - C_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density) - ) - assert C_with_hardware_test - C_iDyn_np == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - base_vel_0 = np.zeros(6) - joints_dot_val_0 = np.zeros(n_dofs) - kinDyn.setRobotState(H_b, joints_val, base_vel_0, joints_dot_val_0, g) - G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(G_iDyn) - G_iDyn_np = np.concatenate( - (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) - ) - G_with_hardware = comp_w_hardware.gravity_term_fun() - G_with_hardware_test = G_with_hardware(H_b, s_, original_length, original_density) - assert G_with_hardware_test - G_iDyn_np == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_CasADi_mixed.py b/tests/mixed/test_CasADi_mixed.py deleted file mode 100644 index bc0134f..0000000 --- a/tests/mixed/test_CasADi_mixed.py +++ /dev/null @@ -1,242 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging - -import casadi as cs -import icub_models -import idyntree.swig as idyntree -import numpy as np -import pytest - -from adam.casadi import KinDynComputations -from adam.numpy.numpy_like import SpatialMath -from adam import Representations - - -np.random.seed(42) - -model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -def H_from_Pos_RPY_idyn(xyz, rpy): - T = idyntree.Transform.Identity() - R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) - p = idyntree.Position() - [p.setVal(i, xyz[i]) for i in range(3)] - T.setRotation(R) - T.setPosition(p) - return T - - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) -comp.set_frame_velocity_representation(Representations.MIXED_REPRESENTATION) -robot_iDyn = idyntree.ModelLoader() -robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) - -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(robot_iDyn.model()) -kinDyn.setFloatingBase(root_link) -kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) -n_dofs = len(joints_name_list) - -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -g = np.array([0, 0, -9.80665]) -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) - - -def test_mass_matrix(): - M = comp.mass_matrix_fun() - mass_mx = idyntree.MatrixDynSize() - kinDyn.getFreeFloatingMassMatrix(mass_mx) - mass_mxNumpy = mass_mx.toNumPy() - mass_test = cs.DM(M(H_b, joints_val)) - mass_test2 = cs.DM(comp.mass_matrix(H_b, joints_val)) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - assert mass_test2 - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - Jcm = comp.centroidal_momentum_matrix_fun() - cmm_idyntree = idyntree.MatrixDynSize() - kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) - cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = cs.DM(Jcm(H_b, joints_val)) - Jcm_test2 = cs.DM(comp.centroidal_momentum_matrix(H_b, joints_val)) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - assert Jcm_test2 - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - com_f = comp.CoM_position_fun() - CoM_test = cs.DM(com_f(H_b, joints_val)) - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - CoM_test2 = cs.DM(comp.CoM_position(H_b, joints_val)) - assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - assert CoM_test2 - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( - 0.0, abs=1e-5 - ) - - -def test_jacobian(): - J_tot = comp.jacobian_fun("l_sole") - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = cs.DM(J_tot(H_b, joints_val)) - J_test2 = cs.DM(comp.jacobian("l_sole", H_b, joints_val)) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - J_tot = comp.jacobian_fun("head") - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = cs.DM(J_tot(H_b, joints_val)) - J_test2 = cs.DM(comp.jacobian("head", H_b, joints_val)) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - assert iDynNumpyJ_ - J_test2 == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot_fun("l_sole") - Jdotnu = kinDyn.getFrameBiasAcc("l_sole") - Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot(H_b, joints_val, base_vel, joints_dot_val) @ np.concatenate( - (base_vel, joints_dot_val) - ) - J_dot_nu_test2 = cs.DM( - comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - ) @ np.concatenate((base_vel, joints_dot_val)) - assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) - assert Jdot_nu - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_idyntree = kinDyn.getWorldTransform("l_sole") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - T = comp.forward_kinematics_fun("l_sole") - H_test = cs.DM(T(H_b, joints_val)) - H_test2 = cs.DM(comp.forward_kinematics("l_sole", H_b, joints_val)) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_idyntree = kinDyn.getWorldTransform("head") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - T = comp.forward_kinematics_fun("head") - H_test = cs.DM(T(H_b, joints_val)) - H_test2 = cs.DM(comp.forward_kinematics("head", H_b, joints_val)) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - assert R_idy2np - H_test2[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test2[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(h_iDyn) - h_iDyn_np = np.concatenate( - (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) - ) - h = comp.bias_force_fun() - h_test = cs.DM(h(H_b, joints_val, base_vel, joints_dot_val)) - h_test2 = cs.DM(comp.bias_force(H_b, joints_val, base_vel, joints_dot_val)) - assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) - assert h_iDyn_np - h_test2 == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = idyntree.Vector3() - g0.zero() - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) - C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(C_iDyn) - C_iDyn_np = np.concatenate( - (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) - ) - C = comp.coriolis_term_fun() - C_test = cs.DM(C(H_b, joints_val, base_vel, joints_dot_val)) - C_test2 = cs.DM(comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val)) - assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) - assert C_iDyn_np - C_test2 == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - base_vel_0 = np.zeros(6) - joints_dot_val_0 = np.zeros(n_dofs) - kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) - kinDyn.setRobotState(H_b, joints_val, base_vel_0, joints_dot_val_0, g) - G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(G_iDyn) - G_iDyn_np = np.concatenate( - (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) - ) - G = comp.gravity_term_fun() - G_test = cs.DM(G(H_b, joints_val)) - G_test2 = cs.DM(comp.gravity_term(H_b, joints_val)) - assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) - assert G_iDyn_np - G_test2 == pytest.approx(0.0, abs=1e-4) - - -def test_relative_jacobian(): - eye = np.eye(4) - kinDyn.setRobotState(eye, joints_val, base_vel, joints_dot_val, g) - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyRelativeJ = (iDyntreeJ_.toNumPy())[:, 6:] - J_fun = comp.relative_jacobian_fun("l_sole") - J_test = cs.DM(J_fun(joints_val)) - 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) diff --git a/tests/mixed/test_Jax_mixed.py b/tests/mixed/test_Jax_mixed.py deleted file mode 100644 index af4bd7b..0000000 --- a/tests/mixed/test_Jax_mixed.py +++ /dev/null @@ -1,195 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging - -import icub_models -import idyntree.swig as idyntree -import jax.numpy as jnp -import numpy as np -import pytest -from jax import config - -import adam -from adam.jax import KinDynComputations -from adam.numpy.numpy_like import SpatialMath - - -np.random.seed(42) -config.update("jax_enable_x64", True) - -model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -def H_from_Pos_RPY_idyn(xyz, rpy): - T = idyntree.Transform.Identity() - R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) - p = idyntree.Position() - [p.setVal(i, xyz[i]) for i in range(3)] - T.setRotation(R) - T.setPosition(p) - return T - - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) -comp.set_frame_velocity_representation(adam.Representations.MIXED_REPRESENTATION) -robot_iDyn = idyntree.ModelLoader() -robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) - -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(robot_iDyn.model()) -kinDyn.setFloatingBase(root_link) -kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) -n_dofs = len(joints_name_list) - -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -g = np.array([0, 0, -9.80665]) -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) - - -def test_mass_matrix(): - mass_mx = idyntree.MatrixDynSize() - kinDyn.getFreeFloatingMassMatrix(mass_mx) - mass_mxNumpy = mass_mx.toNumPy() - mass_test = comp.mass_matrix(H_b, joints_val) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - cmm_idyntree = idyntree.MatrixDynSize() - kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) - cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, joints_val) - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( - 0.0, abs=1e-5 - ) - - -def test_jacobian(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, joints_val) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("head", H_b, joints_val) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - Jdotnu = kinDyn.getFrameBiasAcc("l_sole") - Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) - assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_idyntree = kinDyn.getWorldTransform("l_sole") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("l_sole", H_b, joints_val) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_idyntree = kinDyn.getWorldTransform("head") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("head", H_b, joints_val) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(h_iDyn) - h_iDyn_np = np.concatenate( - (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) - ) - h_test = comp.bias_force(H_b, joints_val, base_vel, joints_dot_val) - assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = np.zeros(3) - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) - C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(C_iDyn) - C_iDyn_np = np.concatenate( - (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) - ) - C_test = comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) - assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - kinDyn2 = idyntree.KinDynComputations() - kinDyn2.loadRobotModel(robot_iDyn.model()) - kinDyn2.setFloatingBase(root_link) - kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) - base_vel0 = np.zeros(6) - joints_dot_val0 = np.zeros(n_dofs) - kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) - G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) - assert kinDyn2.generalizedBiasForces(G_iDyn) - G_iDyn_np = np.concatenate( - (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) - ) - G_test = comp.gravity_term(H_b, joints_val) - assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_NumPy_mixed.py b/tests/mixed/test_NumPy_mixed.py deleted file mode 100644 index 1a651be..0000000 --- a/tests/mixed/test_NumPy_mixed.py +++ /dev/null @@ -1,191 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging - -import icub_models -import idyntree.swig as idyntree -import numpy as np -import pytest - -from adam import Representations -from adam.numpy import KinDynComputations -from adam.numpy.numpy_like import SpatialMath - - -np.random.seed(42) - -model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -def H_from_Pos_RPY_idyn(xyz, rpy): - T = idyntree.Transform.Identity() - R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) - p = idyntree.Position() - [p.setVal(i, xyz[i]) for i in range(3)] - T.setRotation(R) - T.setPosition(p) - return T - - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) -robot_iDyn = idyntree.ModelLoader() -robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) - -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(robot_iDyn.model()) -kinDyn.setFloatingBase(root_link) -kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) -n_dofs = len(joints_name_list) - -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -g = np.array([0, 0, -9.80665]) -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g) - - -def test_mass_matrix(): - mass_mx = idyntree.MatrixDynSize() - kinDyn.getFreeFloatingMassMatrix(mass_mx) - mass_mxNumpy = mass_mx.toNumPy() - mass_test = comp.mass_matrix(H_b, joints_val) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - cmm_idyntree = idyntree.MatrixDynSize() - kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) - cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, joints_val) - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( - 0.0, abs=1e-5 - ) - - -def test_jacobian(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, joints_val) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("head", H_b, joints_val) - assert iDynNumpyJ_ - J_test == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - Jdotnu = kinDyn.getFrameBiasAcc("l_sole") - Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) - assert Jdot_nu - J_dot_nu_test == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_idyntree = kinDyn.getWorldTransform("l_sole") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("l_sole", H_b, joints_val) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_idyntree = kinDyn.getWorldTransform("head") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = comp.forward_kinematics("head", H_b, joints_val) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(h_iDyn) - h_iDyn_np = np.concatenate( - (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) - ) - h_test = comp.bias_force(H_b, joints_val, base_vel, joints_dot_val) - assert h_iDyn_np - h_test == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = np.zeros(3) - kinDyn.setRobotState(H_b, joints_val, base_vel, joints_dot_val, g0) - C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(C_iDyn) - C_iDyn_np = np.concatenate( - (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) - ) - C_test = comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) - assert C_iDyn_np - C_test == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - kinDyn2 = idyntree.KinDynComputations() - kinDyn2.loadRobotModel(robot_iDyn.model()) - kinDyn2.setFloatingBase(root_link) - kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) - base_vel0 = np.zeros(6) - joints_dot_val0 = np.zeros(n_dofs) - kinDyn2.setRobotState(H_b, joints_val, base_vel0, joints_dot_val0, g) - G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) - assert kinDyn2.generalizedBiasForces(G_iDyn) - G_iDyn_np = np.concatenate( - (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) - ) - G_test = comp.gravity_term(H_b, joints_val) - assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/mixed/test_pytorch_mixed.py b/tests/mixed/test_pytorch_mixed.py deleted file mode 100644 index 3ce33b2..0000000 --- a/tests/mixed/test_pytorch_mixed.py +++ /dev/null @@ -1,207 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging - -import icub_models -import idyntree.swig as idyntree -import numpy as np -import pytest -import torch - -from adam import Representations -from adam.pytorch import KinDynComputations -from adam.pytorch.torch_like import SpatialMath - - -np.random.seed(42) -torch.set_default_dtype(torch.float64) - -model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -def H_from_Pos_RPY_idyn(xyz, rpy): - T = idyntree.Transform.Identity() - R = idyntree.Rotation.RPY(rpy[0], rpy[1], rpy[2]) - p = idyntree.Position() - [p.setVal(i, xyz[i]) for i in range(3)] - T.setRotation(R) - T.setPosition(p) - return T - - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) -comp.set_frame_velocity_representation(Representations.MIXED_REPRESENTATION) -robot_iDyn = idyntree.ModelLoader() -robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list) - -kinDyn = idyntree.KinDynComputations() -kinDyn.loadRobotModel(robot_iDyn.model()) -kinDyn.setFloatingBase(root_link) -kinDyn.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) -n_dofs = len(joints_name_list) - -xyz = (torch.rand(3) - 0.5) * 5 -rpy = (torch.rand(3) - 0.5) * 5 -base_vel = (torch.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (torch.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (torch.rand(n_dofs) - 0.5) * 5 - -g = torch.tensor([0, 0, -9.80665]) -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -kinDyn.setRobotState( - H_b.numpy(), joints_val.numpy(), base_vel.numpy(), joints_dot_val.numpy(), g.numpy() -) - - -def test_mass_matrix(): - mass_mx = idyntree.MatrixDynSize() - kinDyn.getFreeFloatingMassMatrix(mass_mx) - mass_mxNumpy = mass_mx.toNumPy() - mass_test = comp.mass_matrix(H_b, joints_val) - assert mass_test - mass_mxNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - cmm_idyntree = idyntree.MatrixDynSize() - kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree) - cmm_idyntreeNumpy = cmm_idyntree.toNumPy() - Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) - assert Jcm_test - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, joints_val) - CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy() - assert CoM_test - CoM_iDynTree == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - assert comp.get_total_mass() - robot_iDyn.model().getTotalMass() == pytest.approx( - 0.0, abs=1e-5 - ) - - -def test_jacobian(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("l_sole", H_b, joints_val) - assert iDynNumpyJ_ - np.asarray(J_test) == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6) - kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_) - iDynNumpyJ_ = iDyntreeJ_.toNumPy() - J_test = comp.jacobian("head", H_b, joints_val) - assert iDynNumpyJ_ - np.asarray(J_test) == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - Jdotnu = kinDyn.getFrameBiasAcc("l_sole") - Jdot_nu = Jdotnu.toNumPy() - J_dot_nu_test = J_dot @ np.concatenate((base_vel, joints_dot_val)) - assert Jdot_nu - np.asarray(J_dot_nu_test) == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_idyntree = kinDyn.getWorldTransform("l_sole") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = np.asarray(comp.forward_kinematics("l_sole", H_b, joints_val)) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_idyntree = kinDyn.getWorldTransform("head") - p_idy2np = H_idyntree.getPosition().toNumPy() - R_idy2np = H_idyntree.getRotation().toNumPy() - H_test = np.asarray(comp.forward_kinematics("head", H_b, joints_val)) - assert R_idy2np - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert p_idy2np - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(h_iDyn) - h_iDyn_np = np.concatenate( - (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy()) - ) - h_test = comp.bias_force(H_b, joints_val, base_vel, joints_dot_val) - assert h_iDyn_np - np.asarray(h_test) == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - g0 = torch.zeros(3) - kinDyn.setRobotState( - H_b.numpy(), - joints_val.numpy(), - base_vel.numpy(), - joints_dot_val.numpy(), - g0.numpy(), - ) - C_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn.model()) - assert kinDyn.generalizedBiasForces(C_iDyn) - C_iDyn_np = np.concatenate( - (C_iDyn.baseWrench().toNumPy(), C_iDyn.jointTorques().toNumPy()) - ) - C_test = comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val) - assert C_iDyn_np - np.asarray(C_test) == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - kinDyn2 = idyntree.KinDynComputations() - kinDyn2.loadRobotModel(robot_iDyn.model()) - kinDyn2.setFloatingBase(root_link) - kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_REPRESENTATION) - base_vel0 = torch.zeros(6) - joints_dot_val0 = torch.zeros(n_dofs) - kinDyn2.setRobotState( - H_b.numpy(), - joints_val.numpy(), - base_vel0.numpy(), - joints_dot_val0.numpy(), - g.numpy(), - ) - G_iDyn = idyntree.FreeFloatingGeneralizedTorques(kinDyn2.model()) - assert kinDyn2.generalizedBiasForces(G_iDyn) - G_iDyn_np = np.concatenate( - (G_iDyn.baseWrench().toNumPy(), G_iDyn.jointTorques().toNumPy()) - ) - G_test = comp.gravity_term(H_b, joints_val) - assert G_iDyn_np - np.asarray(G_test) == pytest.approx(0.0, abs=1e-4) diff --git a/tests/parametric/test_CasADi_computations_parametric.py b/tests/parametric/test_CasADi_computations_parametric.py deleted file mode 100644 index 51dc788..0000000 --- a/tests/parametric/test_CasADi_computations_parametric.py +++ /dev/null @@ -1,219 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging -from os import link -import casadi as cs -import numpy as np -import pytest -import math -from adam.parametric.casadi import KinDynComputationsParametric -from adam.casadi import KinDynComputations -from adam.numpy.numpy_like import SpatialMath - -import tempfile -from git import Repo -from adam import Representations - - -# Getting stickbot urdf file -temp_dir = tempfile.TemporaryDirectory() -git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" -Repo.clone_from(git_url, temp_dir.name) -model_path = temp_dir.name + "/models/stickBot/model.urdf" - -## Hack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49 -with open(model_path, "r", encoding="utf-8") as robot_file: - robot_urdf_string = ( - robot_file.read() - .replace("", "") - ) - -with open(model_path, "w") as robot_file: - robot_file.write(robot_urdf_string) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) - -link_name_list = ["chest"] -comp_w_hardware = KinDynComputationsParametric( - model_path, joints_name_list, link_name_list, root_link -) - -original_density = [ - 628.0724496264945 -] # This is the original density value associated to the chest link, computed as mass/volume -original_length = np.ones(len(link_name_list)) - - -n_dofs = len(joints_name_list) -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val - - -def test_mass_matrix(): - M = comp.mass_matrix_fun() - M_with_hardware = comp_w_hardware.mass_matrix_fun() - mass_test = cs.DM(M(H_b, s_)) - mass_test_hardware = cs.DM( - M_with_hardware(H_b, s_, original_length, original_density) - ) - assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - Jcm = comp.centroidal_momentum_matrix_fun() - Jcm_with_hardware = comp_w_hardware.centroidal_momentum_matrix_fun() - Jcm_test = cs.DM(Jcm(H_b, s_)) - Jcm_test_hardware = cs.DM( - Jcm_with_hardware(H_b, s_, original_length, original_density) - ) - assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - com_f = comp.CoM_position_fun() - com_with_hardware_f = comp_w_hardware.CoM_position_fun() - CoM_cs = cs.DM(com_f(H_b, s_)) - CoM_hardware = cs.DM( - com_with_hardware_f(H_b, s_, original_length, original_density) - ) - assert CoM_cs - CoM_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - mass = comp.get_total_mass() - mass_hardware_fun = comp_w_hardware.get_total_mass() - mass_hardware = cs.DM(mass_hardware_fun(original_length, original_density)) - assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian(): - J_tot = comp.jacobian_fun("l_sole") - J_tot_with_hardware = comp_w_hardware.jacobian_fun("l_sole") - J_test = cs.DM(J_tot(H_b, s_)) - J_test_hardware = cs.DM( - J_tot_with_hardware(H_b, s_, original_length, original_density) - ) - assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - J_tot = comp.jacobian_fun("head") - J_test = cs.DM(J_tot(H_b, s_)) - J_tot_with_hardware = comp_w_hardware.jacobian_fun("head") - J_tot_with_hardware_test = cs.DM( - J_tot_with_hardware(H_b, s_, original_length, original_density) - ) - assert J_test - J_tot_with_hardware_test == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot_fun("l_sole") - J_dot_hardware = comp_w_hardware.jacobian_dot_fun("l_sole") - J_dot_nu_test = cs.DM( - J_dot(H_b, joints_val, base_vel, joints_dot_val) - @ np.concatenate((base_vel, joints_dot_val)) - ) - J_dot_nu_test2 = cs.DM( - J_dot_hardware( - H_b, joints_val, base_vel, joints_dot_val, original_length, original_density - ) - @ np.concatenate((base_vel, joints_dot_val)) - ) - assert J_dot_nu_test - J_dot_nu_test2 == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - T = comp.forward_kinematics_fun("l_sole") - H_test = cs.DM(T(H_b, s_)) - T_with_hardware = comp_w_hardware.forward_kinematics_fun("l_sole") - H_with_hardware_test = cs.DM( - T_with_hardware(H_b, s_, original_length, original_density) - ) - assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - T = comp.forward_kinematics_fun("head") - H_test = cs.DM(T(H_b, s_)) - T_with_hardware = comp_w_hardware.forward_kinematics_fun("head") - H_with_hardware_test = cs.DM( - T_with_hardware(H_b, s_, original_length, original_density) - ) - assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h = comp.bias_force_fun() - h_test = cs.DM(h(H_b, s_, vb_, s_dot_)) - - h_with_hardware = comp_w_hardware.bias_force_fun() - h_with_hardware_test = cs.DM( - h_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density) - ) - assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - C = comp.coriolis_term_fun() - C_test = cs.DM(C(H_b, s_, vb_, s_dot_)) - C_with_hardware = comp_w_hardware.coriolis_term_fun() - C_with_hardware_test = cs.DM( - C_with_hardware(H_b, s_, vb_, s_dot_, original_length, original_density) - ) - assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - G = comp.gravity_term_fun() - G_test = cs.DM(G(H_b, s_)) - G_with_hardware = comp_w_hardware.gravity_term_fun() - G_with_hardware_test = G_with_hardware(H_b, s_, original_length, original_density) - assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/parametric/test_Jax_computations_parametric.py b/tests/parametric/test_Jax_computations_parametric.py deleted file mode 100644 index 0aed8b5..0000000 --- a/tests/parametric/test_Jax_computations_parametric.py +++ /dev/null @@ -1,202 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging -from os import link -import urdf_parser_py.urdf -import jax.numpy as jnp -from jax import config -import numpy as np -import pytest -import math -from adam.parametric.jax import KinDynComputationsParametric -from adam.jax import KinDynComputations -from adam.numpy.numpy_like import SpatialMath - -import tempfile -from git import Repo -from adam import Representations - - -np.random.seed(42) -config.update("jax_enable_x64", True) - -# Getting stickbot urdf file -temp_dir = tempfile.TemporaryDirectory() -git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" -Repo.clone_from(git_url, temp_dir.name) -model_path = temp_dir.name + "/models/stickBot/model.urdf" - -## Hack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49 -with open(model_path, "r", encoding="utf-8") as robot_file: - robot_urdf_string = ( - robot_file.read() - .replace("", "") - ) - -with open(model_path, "w") as robot_file: - robot_file.write(robot_urdf_string) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) - -link_name_list = ["chest"] -comp_w_hardware = KinDynComputationsParametric( - model_path, joints_name_list, link_name_list, root_link -) - -original_density = [ - 628.0724496264945 -] # This is the original density value associated to the chest link, computed as mass/volume -original_length = np.ones(len(link_name_list)) - -n_dofs = len(joints_name_list) -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val - - -def test_mass_matrix(): - mass_test = comp.mass_matrix(H_b, s_) - mass_test_hardware = comp_w_hardware.mass_matrix( - H_b, s_, original_length, original_density - ) - assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) - Jcm_test_hardware = comp_w_hardware.centroidal_momentum_matrix( - H_b, s_, original_length, original_density - ) - - assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, s_) - CoM_hardware = comp_w_hardware.CoM_position( - H_b, s_, original_length, original_density - ) - - assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - mass = comp.get_total_mass() - mass_hardware = comp_w_hardware.get_total_mass(original_length, original_density) - assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian(): - J_test = comp.jacobian("l_sole", H_b, s_) - J_test_hardware = comp_w_hardware.jacobian( - "l_sole", H_b, s_, original_length, original_density - ) - assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - J_test = comp.jacobian("head", H_b, s_) - J_test_hardware = comp_w_hardware.jacobian( - "head", H_b, s_, original_length, original_density - ) - assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - J_dot_hardware = comp_w_hardware.jacobian_dot( - "l_sole", - H_b, - joints_val, - base_vel, - joints_dot_val, - original_length, - original_density, - ) - assert J_dot - J_dot_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_test = comp.forward_kinematics("l_sole", H_b, s_) - H_with_hardware_test = comp_w_hardware.forward_kinematics( - "l_sole", H_b, s_, original_length, original_density - ) - assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_test = comp.forward_kinematics("head", H_b, s_) - H_with_hardware_test = comp_w_hardware.forward_kinematics( - "head", H_b, s_, original_length, original_density - ) - assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_test = comp.bias_force(H_b, s_, vb_, s_dot_) - h_with_hardware_test = comp_w_hardware.bias_force( - H_b, s_, vb_, s_dot_, original_length, original_density - ) - assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - C_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) - C_with_hardware_test = comp_w_hardware.coriolis_term( - H_b, s_, vb_, s_dot_, original_length, original_density - ) - assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - G_test = comp.gravity_term(H_b, s_) - G_with_hardware_test = comp_w_hardware.gravity_term( - H_b, s_, original_length, original_density - ) - assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/parametric/test_NumPy_computations_parametric.py b/tests/parametric/test_NumPy_computations_parametric.py deleted file mode 100644 index 1354aa8..0000000 --- a/tests/parametric/test_NumPy_computations_parametric.py +++ /dev/null @@ -1,201 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging -from os import link -import urdf_parser_py.urdf -import pytest -import math -import numpy as np -from adam.parametric.numpy import KinDynComputationsParametric -from adam.numpy import KinDynComputations -from adam.numpy.numpy_like import SpatialMath - -import tempfile -from git import Repo -from adam import Representations - - -np.random.seed(42) - -# Getting stickbot urdf file -temp_dir = tempfile.TemporaryDirectory() -git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" -Repo.clone_from(git_url, temp_dir.name) -model_path = temp_dir.name + "/models/stickBot/model.urdf" - -## Hack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49 -with open(model_path, "r", encoding="utf-8") as robot_file: - robot_urdf_string = ( - robot_file.read() - .replace("", "") - ) - -with open(model_path, "w") as robot_file: - robot_file.write(robot_urdf_string) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) - -link_name_list = ["chest"] -comp_w_hardware = KinDynComputationsParametric( - model_path, joints_name_list, link_name_list, root_link -) -# comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) -# comp_w_hardware.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION) - -original_density = [ - 628.0724496264945 -] # This is the original density value associated to the chest link, computed as mass/volume -original_length = np.ones(len(link_name_list)) - -n_dofs = len(joints_name_list) -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -vb_ = base_vel -s_ = joints_val -s_dot_ = joints_dot_val - - -def test_mass_matrix(): - mass_test = comp.mass_matrix(H_b, s_) - mass_test_hardware = comp_w_hardware.mass_matrix( - H_b, s_, original_length, original_density - ) - - assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - Jcm_test = comp.centroidal_momentum_matrix(H_b, s_) - Jcm_test_hardware = comp_w_hardware.centroidal_momentum_matrix( - H_b, s_, original_length, original_density - ) - - assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, s_) - CoM_hardware = comp_w_hardware.CoM_position( - H_b, s_, original_length, original_density - ) - assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - mass = comp.get_total_mass() - mass_hardware = comp_w_hardware.get_total_mass(original_length, original_density) - assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian(): - J_test = comp.jacobian("l_sole", H_b, s_) - J_test_hardware = comp_w_hardware.jacobian( - "l_sole", H_b, s_, original_length, original_density - ) - assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - J_test = comp.jacobian("head", H_b, s_) - J_test_hardware = comp_w_hardware.jacobian( - "head", H_b, s_, original_length, original_density - ) - assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - J_dot_w_hardware = comp_w_hardware.jacobian_dot( - "l_sole", - H_b, - joints_val, - base_vel, - joints_dot_val, - original_length, - original_density, - ) - assert J_dot - J_dot_w_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_test = comp.forward_kinematics("l_sole", H_b, s_) - H_with_hardware_test = comp_w_hardware.forward_kinematics( - "l_sole", H_b, s_, original_length, original_density - ) - assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_test = comp.forward_kinematics("head", H_b, s_) - H_with_hardware_test = comp_w_hardware.forward_kinematics( - "head", H_b, s_, original_length, original_density - ) - assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_test = comp.bias_force(H_b, s_, vb_, s_dot_) - h_with_hardware_test = comp_w_hardware.bias_force( - H_b, s_, vb_, s_dot_, original_length, original_density - ) - assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - C_test = comp.coriolis_term(H_b, s_, vb_, s_dot_) - C_with_hardware_test = comp_w_hardware.coriolis_term( - H_b, s_, vb_, s_dot_, original_length, original_density - ) - assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - G_test = comp.gravity_term(H_b, s_) - G_with_hardware_test = comp_w_hardware.gravity_term( - H_b, s_, original_length, original_density - ) - assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/parametric/test_pytorch_computations_parametric.py b/tests/parametric/test_pytorch_computations_parametric.py deleted file mode 100644 index 73fad13..0000000 --- a/tests/parametric/test_pytorch_computations_parametric.py +++ /dev/null @@ -1,211 +0,0 @@ -# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -import logging -from os import link -import urdf_parser_py.urdf -import pytest -import math -import torch -import numpy as np -from adam.parametric.pytorch import KinDynComputationsParametric -from adam.pytorch import KinDynComputations -from adam.pytorch.torch_like import SpatialMath - -import tempfile -from git import Repo -from adam import Representations - - -np.random.seed(42) -torch.set_default_dtype(torch.float64) - -# Getting stickbot urdf file -temp_dir = tempfile.TemporaryDirectory() -git_url = "https://github.com/icub-tech-iit/ergocub-gazebo-simulations.git" -Repo.clone_from(git_url, temp_dir.name) -model_path = temp_dir.name + "/models/stickBot/model.urdf" - -## Hack to remove the encoding urdf, see https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/49 -with open(model_path, "r", encoding="utf-8") as robot_file: - robot_urdf_string = ( - robot_file.read() - .replace("", "") - ) - -with open(model_path, "w") as robot_file: - robot_file.write(robot_urdf_string) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - -logging.basicConfig(level=logging.DEBUG) -logging.debug("Showing the robot tree.") - - -root_link = "root_link" -comp = KinDynComputations(model_path, joints_name_list, root_link) - -link_name_list = ["chest"] -comp_w_hardware = KinDynComputationsParametric( - model_path, joints_name_list, link_name_list, root_link -) - -original_density = [ - 628.0724496264945 -] # This is the original density value associated to the chest link, computed as mass/volume -original_length = np.ones(len(link_name_list)) - -n_dofs = len(joints_name_list) -# base pose quantities -xyz = (torch.rand(3) - 0.5) * 5 -rpy = (torch.rand(3) - 0.5) * 5 -base_vel = (torch.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (torch.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (torch.rand(n_dofs) - 0.5) * 5 - -g = torch.tensor([0, 0, -9.80665]) -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array - - -def test_mass_matrix(): - mass_test = comp.mass_matrix(H_b, joints_val) - mass_test_hardware = np.array( - comp_w_hardware.mass_matrix(H_b, joints_val, original_length, original_density) - ) - assert mass_test - mass_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_CMM(): - Jcm_test = comp.centroidal_momentum_matrix(H_b, joints_val) - Jcm_test_hardware = np.array( - comp_w_hardware.centroidal_momentum_matrix( - H_b, joints_val, original_length, original_density - ) - ) - assert Jcm_test - Jcm_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_CoM_pos(): - CoM_test = comp.CoM_position(H_b, joints_val) - CoM_hardware = np.array( - comp_w_hardware.CoM_position(H_b, joints_val, original_length, original_density) - ) - assert CoM_test - CoM_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_total_mass(): - mass = comp.get_total_mass() - mass_hardware = comp_w_hardware.get_total_mass(original_length, original_density) - assert mass - mass_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian(): - J_test = comp.jacobian("l_sole", H_b, joints_val) - J_test_hardware = np.array( - comp_w_hardware.jacobian( - "l_sole", H_b, joints_val, original_length, original_density - ) - ) - assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_non_actuated(): - J_test = comp.jacobian("head", H_b, joints_val) - J_test_hardware = np.array( - comp_w_hardware.jacobian( - "head", H_b, joints_val, original_length, original_density - ) - ) - assert J_test - J_test_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_jacobian_dot(): - J_dot = comp.jacobian_dot("l_sole", H_b, joints_val, base_vel, joints_dot_val) - J_dot_hardware = comp_w_hardware.jacobian_dot( - "l_sole", - H_b, - joints_val, - base_vel, - joints_dot_val, - original_length, - original_density, - ) - assert J_dot - J_dot_hardware == pytest.approx(0.0, abs=1e-5) - - -def test_fk(): - H_test = np.array(comp.forward_kinematics("l_sole", H_b, joints_val)) - H_with_hardware_test = np.array( - comp_w_hardware.forward_kinematics( - "l_sole", H_b, joints_val, original_length, original_density - ) - ) - assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_fk_non_actuated(): - H_test = np.array(comp.forward_kinematics("head", H_b, joints_val)) - H_with_hardware_test = np.array( - comp_w_hardware.forward_kinematics( - "head", H_b, joints_val, original_length, original_density - ) - ) - assert H_with_hardware_test[:3, :3] - H_test[:3, :3] == pytest.approx(0.0, abs=1e-5) - assert H_with_hardware_test[:3, 3] - H_test[:3, 3] == pytest.approx(0.0, abs=1e-5) - - -def test_bias_force(): - h_test = np.array(comp.bias_force(H_b, joints_val, base_vel, joints_dot_val)) - h_with_hardware_test = np.array( - comp_w_hardware.bias_force( - H_b, joints_val, base_vel, joints_dot_val, original_length, original_density - ) - ) - assert h_with_hardware_test - h_test == pytest.approx(0.0, abs=1e-4) - - -def test_coriolis_term(): - C_test = np.array(comp.coriolis_term(H_b, joints_val, base_vel, joints_dot_val)) - C_with_hardware_test = np.array( - comp_w_hardware.coriolis_term( - H_b, joints_val, base_vel, joints_dot_val, original_length, original_density - ) - ) - assert C_with_hardware_test - C_test == pytest.approx(0.0, abs=1e-4) - - -def test_gravity_term(): - G_test = comp.gravity_term(H_b, joints_val) - G_with_hardware_test = comp_w_hardware.gravity_term( - H_b, joints_val, original_length, original_density - ) - assert G_with_hardware_test - G_test == pytest.approx(0.0, abs=1e-4) diff --git a/tests/pytorch_batch/test_pytorch_batch_bak.py b/tests/pytorch_batch/test_pytorch_batch_bak.py deleted file mode 100644 index 7625181..0000000 --- a/tests/pytorch_batch/test_pytorch_batch_bak.py +++ /dev/null @@ -1,186 +0,0 @@ -import logging - -import icub_models -import idyntree.swig as idyntree -import jax.numpy as jnp -import numpy as np -import pytest -from jax import config - -import adam -from adam.pytorch import KinDynComputationsBatch -from adam.numpy import KinDynComputations -from adam.numpy.numpy_like import SpatialMath - -import torch - - -np.random.seed(42) -config.update("jax_enable_x64", True) - -model_path = str(icub_models.get_model_file("iCubGazeboV2_5")) - -joints_name_list = [ - "torso_pitch", - "torso_roll", - "torso_yaw", - "l_shoulder_pitch", - "l_shoulder_roll", - "l_shoulder_yaw", - "l_elbow", - "r_shoulder_pitch", - "r_shoulder_roll", - "r_shoulder_yaw", - "r_elbow", - "l_hip_pitch", - "l_hip_roll", - "l_hip_yaw", - "l_knee", - "l_ankle_pitch", - "l_ankle_roll", - "r_hip_pitch", - "r_hip_roll", - "r_hip_yaw", - "r_knee", - "r_ankle_pitch", - "r_ankle_roll", -] - - -comp = KinDynComputationsBatch(model_path, joints_name_list) -comp.set_frame_velocity_representation(adam.Representations.MIXED_REPRESENTATION) - -comp_np = KinDynComputations(model_path, joints_name_list) -comp_np.set_frame_velocity_representation(adam.Representations.MIXED_REPRESENTATION) - -n_dofs = len(joints_name_list) -# base pose quantities -xyz = (np.random.rand(3) - 0.5) * 5 -rpy = (np.random.rand(3) - 0.5) * 5 -base_vel = (np.random.rand(6) - 0.5) * 5 -# joints quantitites -joints_val = (np.random.rand(n_dofs) - 0.5) * 5 -joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5 - -g = np.array([0, 0, -9.80665]) -H_b = SpatialMath().H_from_Pos_RPY(xyz, rpy).array -n_samples = 10 - -H_b_batch = torch.tile(torch.tensor(H_b), (n_samples, 1, 1)).requires_grad_() -joints_val_batch = torch.tile(torch.tensor(joints_val), (n_samples, 1)).requires_grad_() -base_vel_batch = torch.tile(torch.tensor(base_vel), (n_samples, 1)).requires_grad_() -joints_dot_val_batch = torch.tile( - torch.tensor(joints_dot_val), (n_samples, 1) -).requires_grad_() - - -# Check if the quantities are the correct testing against the numpy implementation -# Check if the dimensions are correct (batch dimension) -# Check if the gradient is computable - - -def test_mass_matrix(): - mass_matrix = comp.mass_matrix(H_b_batch, joints_val_batch) - mass_matrix_np = comp_np.mass_matrix(H_b, joints_val) - assert np.allclose(mass_matrix[0].detach().numpy(), mass_matrix_np) - assert mass_matrix.shape == (n_samples, n_dofs + 6, n_dofs + 6) - mass_matrix.sum().backward() - - -def test_centroidal_momentum_matrix(): - centroidal_momentum_matrix = comp.centroidal_momentum_matrix( - H_b_batch, joints_val_batch - ) - centroidal_momentum_matrix_np = comp_np.centroidal_momentum_matrix(H_b, joints_val) - assert np.allclose( - centroidal_momentum_matrix[0].detach().numpy(), centroidal_momentum_matrix_np - ) - assert centroidal_momentum_matrix.shape == (n_samples, 6, n_dofs + 6) - centroidal_momentum_matrix.sum().backward() - - -def test_relative_jacobian(): - frame = "l_sole" - relative_jacobian = comp.relative_jacobian(frame, joints_val_batch) - assert np.allclose( - relative_jacobian[0].detach().numpy(), - comp_np.relative_jacobian(frame, joints_val), - ) - assert relative_jacobian.shape == (n_samples, 6, n_dofs) - relative_jacobian.sum().backward() - - -def test_jacobian_dot(): - frame = "l_sole" - jacobian_dot = comp.jacobian_dot( - frame, H_b_batch, joints_val_batch, base_vel_batch, joints_dot_val_batch - ) - assert np.allclose( - jacobian_dot[0].detach().numpy(), - comp_np.jacobian_dot(frame, H_b, joints_val, base_vel, joints_dot_val), - ) - assert jacobian_dot.shape == (n_samples, 6, n_dofs + 6) - jacobian_dot.sum().backward() - - -def test_forward_kineamtics(): - frame = "l_sole" - forward_kinematics = comp.forward_kinematics(frame, H_b_batch, joints_val_batch) - assert np.allclose( - forward_kinematics[0].detach().numpy(), - comp_np.forward_kinematics(frame, H_b, joints_val), - ) - assert forward_kinematics.shape == (n_samples, 4, 4) - forward_kinematics.sum().backward() - - -def test_jacobian(): - frame = "l_sole" - jacobian = comp.jacobian(frame, H_b_batch, joints_val_batch) - assert np.allclose( - jacobian[0].detach().numpy(), comp_np.jacobian(frame, H_b, joints_val) - ) - assert jacobian.shape == (n_samples, 6, n_dofs + 6) - jacobian.sum().backward() - - -def test_bias_force(): - bias_force = comp.bias_force( - H_b_batch, joints_val_batch, base_vel_batch, joints_dot_val_batch - ) - assert np.allclose( - bias_force[0].detach().numpy(), - comp_np.bias_force(H_b, joints_val, base_vel, joints_dot_val), - ) - assert bias_force.shape == (n_samples, n_dofs + 6) - bias_force.sum().backward() - - -def test_coriolis_term(): - coriolis_term = comp.coriolis_term( - H_b_batch, joints_val_batch, base_vel_batch, joints_dot_val_batch - ) - assert np.allclose( - coriolis_term[0].detach().numpy(), - comp_np.coriolis_term(H_b, joints_val, base_vel, joints_dot_val), - ) - assert coriolis_term.shape == (n_samples, n_dofs + 6) - coriolis_term.sum().backward() - - -def test_gravity_term(): - gravity_term = comp.gravity_term(H_b_batch, joints_val_batch) - assert np.allclose( - gravity_term[0].detach().numpy(), comp_np.gravity_term(H_b, joints_val) - ) - assert gravity_term.shape == (n_samples, n_dofs + 6) - gravity_term.sum().backward() - - -def test_CoM_position(): - CoM_position = comp.CoM_position(H_b_batch, joints_val_batch) - assert np.allclose( - CoM_position[0].detach().numpy(), comp_np.CoM_position(H_b, joints_val) - ) - assert CoM_position.shape == (n_samples, 3) - CoM_position.sum().backward() From 824a57a71222046f80409be8ec9d602aa9ee9883 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 19 Dec 2024 13:12:47 +0100 Subject: [PATCH 16/20] Remove gitpython as depenency --- ci_env.yml | 1 - ci_env_win.yml | 1 - setup.cfg | 1 - 3 files changed, 3 deletions(-) diff --git a/ci_env.yml b/ci_env.yml index 9f2da6e..9073a8e 100644 --- a/ci_env.yml +++ b/ci_env.yml @@ -16,7 +16,6 @@ dependencies: - pytest-repeat - icub-models - idyntree >=11.0.0 - - gitpython - jax - pytorch - jax2torch diff --git a/ci_env_win.yml b/ci_env_win.yml index c04f1bf..5837040 100644 --- a/ci_env_win.yml +++ b/ci_env_win.yml @@ -16,5 +16,4 @@ dependencies: - pytest-repeat - icub-models - idyntree >=11.0.0 - - gitpython - requests diff --git a/setup.cfg b/setup.cfg index 7279340..c9c5767 100644 --- a/setup.cfg +++ b/setup.cfg @@ -56,7 +56,6 @@ test = idyntree icub-models black - gitpython jax2torch requests From 3459788da6f5e082110d60a580a28c3aa06f70db Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 19 Dec 2024 13:13:21 +0100 Subject: [PATCH 17/20] Formatting with black --- tests/parametric/test_idyntree_conversion.py | 2 -- tests/parametric/test_pytorch.py | 1 + tests/test_idyntree_conversion.py | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/tests/parametric/test_idyntree_conversion.py b/tests/parametric/test_idyntree_conversion.py index c50ce41..29c8f70 100644 --- a/tests/parametric/test_idyntree_conversion.py +++ b/tests/parametric/test_idyntree_conversion.py @@ -38,8 +38,6 @@ def setup_test(tests_setup) -> KinDynComputationsParametric | RobotCfg | State: return adam_kin_dyn, robot_cfg, state, original_density, original_length - - def test_mass_matrix(setup_test): adam_kin_dyn, robot_cfg, state, original_density, original_length = setup_test idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix diff --git a/tests/parametric/test_pytorch.py b/tests/parametric/test_pytorch.py index 00efecf..9ec765b 100644 --- a/tests/parametric/test_pytorch.py +++ b/tests/parametric/test_pytorch.py @@ -6,6 +6,7 @@ torch.set_default_dtype(torch.float64) + @pytest.fixture(scope="module") def setup_test(tests_setup) -> KinDynComputationsParametric | RobotCfg | State: robot_cfg, state = tests_setup diff --git a/tests/test_idyntree_conversion.py b/tests/test_idyntree_conversion.py index 2379171..ea4bc1f 100644 --- a/tests/test_idyntree_conversion.py +++ b/tests/test_idyntree_conversion.py @@ -5,6 +5,7 @@ from conftest import State, RobotCfg, compute_idyntree_values from adam.model.conversions.idyntree import to_idyntree_model + @pytest.fixture(scope="module") def setup_test(tests_setup) -> KinDynComputations | RobotCfg | State: robot_cfg, state = tests_setup @@ -16,7 +17,6 @@ def setup_test(tests_setup) -> KinDynComputations | RobotCfg | State: return adam_kin_dyn, robot_cfg, state - def test_mass_matrix(setup_test): adam_kin_dyn, robot_cfg, state = setup_test idyn_mass_matrix = robot_cfg.idyn_function_values.mass_matrix From 1cd63beade1814926a931e5d65bc1cbb7e7a42db Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 19 Dec 2024 13:19:48 +0100 Subject: [PATCH 18/20] Change robot --- tests/conftest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/conftest.py b/tests/conftest.py index 5596a94..e40a9f0 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -54,7 +54,7 @@ class RobotCfg: ] ROBOTS = [ - "iCubGazeboV2_5", + "iCubGenova04", "StickBot", ] From 5e3095239a99487371267f204c248a479efe2aa8 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 19 Dec 2024 13:20:15 +0100 Subject: [PATCH 19/20] Renaming tests to avoid duplicates --- tests/parametric/{test_casadi.py => test_casadi_parametric.py} | 0 ...ntree_conversion.py => test_idyntree_conversion_parametric.py} | 0 tests/parametric/{test_jax.py => test_jax_parametric.py} | 0 tests/parametric/{test_numpy.py => test_numpy_parametric.py} | 0 tests/parametric/{test_pytorch.py => test_pytorch_parametric.py} | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename tests/parametric/{test_casadi.py => test_casadi_parametric.py} (100%) rename tests/parametric/{test_idyntree_conversion.py => test_idyntree_conversion_parametric.py} (100%) rename tests/parametric/{test_jax.py => test_jax_parametric.py} (100%) rename tests/parametric/{test_numpy.py => test_numpy_parametric.py} (100%) rename tests/parametric/{test_pytorch.py => test_pytorch_parametric.py} (100%) diff --git a/tests/parametric/test_casadi.py b/tests/parametric/test_casadi_parametric.py similarity index 100% rename from tests/parametric/test_casadi.py rename to tests/parametric/test_casadi_parametric.py diff --git a/tests/parametric/test_idyntree_conversion.py b/tests/parametric/test_idyntree_conversion_parametric.py similarity index 100% rename from tests/parametric/test_idyntree_conversion.py rename to tests/parametric/test_idyntree_conversion_parametric.py diff --git a/tests/parametric/test_jax.py b/tests/parametric/test_jax_parametric.py similarity index 100% rename from tests/parametric/test_jax.py rename to tests/parametric/test_jax_parametric.py diff --git a/tests/parametric/test_numpy.py b/tests/parametric/test_numpy_parametric.py similarity index 100% rename from tests/parametric/test_numpy.py rename to tests/parametric/test_numpy_parametric.py diff --git a/tests/parametric/test_pytorch.py b/tests/parametric/test_pytorch_parametric.py similarity index 100% rename from tests/parametric/test_pytorch.py rename to tests/parametric/test_pytorch_parametric.py From bcce799e39e2d59a9c78e154d3122099f03dbb41 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 19 Dec 2024 15:31:04 +0100 Subject: [PATCH 20/20] Change pytest fixture scope from session to module for test isolation --- tests/conftest.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/conftest.py b/tests/conftest.py index e40a9f0..0e9b491 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -75,7 +75,7 @@ def get_robot_model_path(robot_name: str) -> str: TEST_CONFIGURATIONS = list(product(VELOCITY_REPRESENTATIONS, ROBOTS)) -@pytest.fixture(scope="session", params=TEST_CONFIGURATIONS, ids=str) +@pytest.fixture(scope="module", params=TEST_CONFIGURATIONS, ids=str) def tests_setup(request) -> RobotCfg | State: velocity_representation, robot_name = request.param