Cleanup casadi and numpy mixed tests

tests/mixed/test_CasADi_mixed.py

@@ -78,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)
 
@@ -115,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)
 
@@ -141,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)
 
@@ -152,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)
 
@@ -162,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)
 
@@ -175,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)
@@ -188,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)
@@ -203,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.MIXED_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/mixed/test_NumPy_mixed.py

@@ -76,45 +76,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.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():
     mass_mx = idyntree.MatrixDynSize()
     kinDyn.getFreeFloatingMassMatrix(mass_mx)
     mass_mxNumpy = mass_mx.toNumPy()
-    mass_test = comp.mass_matrix(H_b, s_)
+    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_)
+    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)
 
@@ -129,31 +113,31 @@ 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_)
+    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_)
+    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_)
+    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)
 
@@ -162,7 +146,7 @@ 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)
 
@@ -173,20 +157,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)
 
 
@@ -195,13 +178,13 @@ def test_gravity_term():
     kinDyn2.loadRobotModel(robot_iDyn.model())
     kinDyn2.setFloatingBase(root_link)
     kinDyn2.setFrameVelocityRepresentation(idyntree.MIXED_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)