Update tests

tests/body_fixed/test_CasADi_body_fixed.py

@@ -10,9 +10,9 @@
 import numpy as np
 import pytest
 
-import adam
 from adam.casadi import KinDynComputations
 from adam.geometry import utils
+from adam import Representations
 
 np.random.seed(42)
 
@@ -60,8 +60,7 @@ def H_from_Pos_RPY_idyn(xyz, rpy):
 
 root_link = "root_link"
 comp = KinDynComputations(model_path, joints_name_list, root_link)
-comp.set_frame_velocity_representation(adam.Representations.BODY_FIXED_REPRESENTATION)
-
+comp.set_frame_velocity_representation(Representations.BODY_FIXED_REPRESENTATION)
 robot_iDyn = idyntree.ModelLoader()
 robot_iDyn.loadReducedModelFromFile(model_path, joints_name_list)
 
@@ -79,34 +78,18 @@ def H_from_Pos_RPY_idyn(xyz, rpy):
 joints_val = (np.random.rand(n_dofs) - 0.5) * 5
 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5
 
-# set iDynTree kinDyn
-H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy)
-vb = idyntree.Twist.Zero()
-[vb.setVal(i, base_vel[i]) for i in range(6)]
-
-s = idyntree.VectorDynSize(n_dofs)
-[s.setVal(i, joints_val[i]) for i in range(n_dofs)]
-s_dot = idyntree.VectorDynSize(n_dofs)
-[s_dot.setVal(i, joints_dot_val[i]) for i in range(n_dofs)]
-
-g = idyntree.Vector3()
-g.zero()
-g.setVal(2, -9.80665)
-kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g)
-# set ADAM
+g = np.array([0, 0, -9.80665])
 H_b = utils.H_from_Pos_RPY(xyz, rpy)
-vb_ = base_vel
-s_ = joints_val
-s_dot_ = joints_dot_val
+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, s_))
-    mass_test2 = cs.DM(comp.mass_matrix(H_b, s_))
+    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)
 
@@ -116,17 +99,17 @@ def test_CMM():
     cmm_idyntree = idyntree.MatrixDynSize()
     kinDyn.getCentroidalTotalMomentumJacobian(cmm_idyntree)
     cmm_idyntreeNumpy = cmm_idyntree.toNumPy()
-    Jcm_test = cs.DM(Jcm(H_b, s_))
-    Jcm_test2 = cs.DM(comp.centroidal_momentum_matrix(H_b, s_))
+    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, s_))
+    CoM_test = cs.DM(com_f(H_b, joints_val))
     CoM_iDynTree = kinDyn.getCenterOfMassPosition().toNumPy()
-    CoM_test2 = cs.DM(comp.CoM_position(H_b, s_))
+    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)
 
@@ -142,8 +125,8 @@ def test_jacobian():
     iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6)
     kinDyn.getFrameFreeFloatingJacobian("l_sole", iDyntreeJ_)
     iDynNumpyJ_ = iDyntreeJ_.toNumPy()
-    J_test = cs.DM(J_tot(H_b, s_))
-    J_test2 = cs.DM(comp.jacobian("l_sole", H_b, s_))
+    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)
 
@@ -153,8 +136,8 @@ def test_jacobian_non_actuated():
     iDyntreeJ_ = idyntree.MatrixDynSize(6, n_dofs + 6)
     kinDyn.getFrameFreeFloatingJacobian("head", iDyntreeJ_)
     iDynNumpyJ_ = iDyntreeJ_.toNumPy()
-    J_test = cs.DM(J_tot(H_b, s_))
-    J_test2 = cs.DM(comp.jacobian("head", H_b, s_))
+    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)
 
@@ -163,10 +146,12 @@ 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, s_, vb_, s_dot_) @ np.concatenate((vb_, s_dot_))
+    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, s_, vb_, s_dot_)
-    ) @ np.concatenate((vb_, s_dot_))
+        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)
 
@@ -176,8 +161,8 @@ def test_fk():
     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, s_))
-    H_test2 = cs.DM(comp.forward_kinematics("l_sole", H_b, s_))
+    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)
@@ -189,8 +174,8 @@ def test_fk_non_actuated():
     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, s_))
-    H_test2 = cs.DM(comp.forward_kinematics("head", H_b, s_))
+    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)
@@ -204,52 +189,53 @@ def test_bias_force():
         (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy())
     )
     h = comp.bias_force_fun()
-    h_test = cs.DM(h(H_b, s_, vb_, s_dot_))
-    h_test2 = cs.DM(comp.bias_force(H_b, s_, vb_, s_dot_))
+    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_idyn, s, vb, s_dot, g0)
+    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, s_, vb_, s_dot_))
-    C_test2 = cs.DM(comp.coriolis_term(H_b, s_, vb_, s_dot_))
+    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():
-    vb0 = idyntree.Twist.Zero()
-    s_dot0 = idyntree.VectorDynSize(n_dofs)
+    base_vel_0 = np.zeros(6)
+    joints_dot_val_0 = np.zeros(n_dofs)
     kinDyn.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION)
-    kinDyn.setRobotState(H_b_idyn, s, vb0, s_dot0, g)
+    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, s_))
-    G_test2 = cs.DM(comp.gravity_term(H_b, s_))
+    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():
-    kinDyn.setRobotState(H_from_Pos_RPY_idyn(np.zeros(3), np.zeros(3)), s, vb, s_dot, g)
+    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(s_))
-    J_test2 = cs.DM(comp.relative_jacobian("l_sole", s_))
+    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)

tests/body_fixed/test_Jax_body_fixed.py

@@ -63,7 +63,6 @@ def H_from_Pos_RPY_idyn(xyz, rpy):
 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)
 
@@ -81,45 +80,29 @@ def H_from_Pos_RPY_idyn(xyz, rpy):
 joints_val = (np.random.rand(n_dofs) - 0.5) * 5
 joints_dot_val = (np.random.rand(n_dofs) - 0.5) * 5
 
-# set iDynTree kinDyn
-H_b_idyn = H_from_Pos_RPY_idyn(xyz, rpy)
-vb = idyntree.Twist.Zero()
-[vb.setVal(i, base_vel[i]) for i in range(6)]
-
-s = idyntree.VectorDynSize(n_dofs)
-s = s.FromPython(joints_val)
-s_dot = idyntree.VectorDynSize(n_dofs)
-s_dot = s_dot.FromPython(joints_dot_val)
-
-g = idyntree.Vector3()
-g.zero()
-g.setVal(2, -9.80665)
-kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g)
-# set ADAM
+g = np.array([0, 0, -9.80665])
 H_b = utils.H_from_Pos_RPY(xyz, rpy)
-vb_ = base_vel
-s_ = joints_val
-s_dot_ = joints_dot_val
+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, s_)
-assert np.asarray(mass_test) - mass_mxNumpy == pytest.approx(0.0, abs=1e-5)
+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, s_)
-    assert np.array(Jcm_test) - cmm_idyntreeNumpy == pytest.approx(0.0, abs=1e-5)
+    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, s_)
+    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)
 
@@ -134,40 +117,40 @@ 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, s_)
-    assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-5)
+    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, s_)
-    assert iDynNumpyJ_ - np.array(J_test) == pytest.approx(0.0, abs=1e-5)
+    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, s_, vb_, s_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((vb_, s_dot_))
+    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, s_)
-    assert R_idy2np - np.array(H_test[:3, :3]) == pytest.approx(0.0, abs=1e-5)
-    assert p_idy2np - np.array(H_test[:3, 3]) == pytest.approx(0.0, abs=1e-5)
+    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, s_)
+    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)
 
@@ -178,20 +161,19 @@ def test_bias_force():
     h_iDyn_np = np.concatenate(
         (h_iDyn.baseWrench().toNumPy(), h_iDyn.jointTorques().toNumPy())
     )
-    h_test = comp.bias_force(H_b, s_, vb_, s_dot_)
+    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 = idyntree.Vector3()
-    g0.zero()
-    kinDyn.setRobotState(H_b_idyn, s, vb, s_dot, g0)
+    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, s_, vb_, s_dot_)
+    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)
 
 
@@ -200,13 +182,13 @@ def test_gravity_term():
     kinDyn2.loadRobotModel(robot_iDyn.model())
     kinDyn2.setFloatingBase(root_link)
     kinDyn2.setFrameVelocityRepresentation(idyntree.BODY_FIXED_REPRESENTATION)
-    vb0 = idyntree.Twist.Zero()
-    s_dot0 = idyntree.VectorDynSize(n_dofs)
-    kinDyn2.setRobotState(H_b_idyn, s, vb0, s_dot0, g)
+    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, s_)
+    G_test = comp.gravity_term(H_b, joints_val)
     assert G_iDyn_np - G_test == pytest.approx(0.0, abs=1e-4)