From 8deff447f6e1fca6c3c503c03a202eae47f48801 Mon Sep 17 00:00:00 2001 From: giulero Date: Thu, 19 Dec 2024 13:10:58 +0100 Subject: [PATCH] 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()