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)