From 4b714e252f443edc89af434fd44ad555a819e37a Mon Sep 17 00:00:00 2001
From: EnricoMingo <enricomingo@gmail.com>
Date: Fri, 9 Feb 2024 11:02:00 +0100
Subject: [PATCH] Removed TestKinematics

---
 tests/CMakeLists.txt           |   5 --
 tests/utils/TestKinematics.cpp | 112 ++++++++++++++-------------------
 2 files changed, 47 insertions(+), 70 deletions(-)

diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index df7e2b21..3dec4ae4 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -345,11 +345,6 @@ add_test(NAME OpenSoT_task_velocity_Postural COMMAND testPosturalVelocityTask)
 # add_dependencies(testCoMForceTask  OpenSoT)
 # add_test(NAME OpenSoT_task_force_CoM COMMAND testCoMForceTask)
 
-# ADD_EXECUTABLE(testKinematics utils/TestKinematics.cpp)
-# TARGET_LINK_LIBRARIES(testKinematics ${TestLibs})
-# add_dependencies(testKinematics   OpenSoT)
-# add_test(NAME OpenSoT_utils_test_kinematics COMMAND testKinematics)
-
 # if(${qpSWIFT_FOUND})
 #     ADD_EXECUTABLE(testqpSWIFTSolver solvers/TestqpSWIFT.cpp)
 #     TARGET_LINK_LIBRARIES(testqpSWIFTSolver ${TestLibs} qpSWIFT::qpSWIFT-static)
diff --git a/tests/utils/TestKinematics.cpp b/tests/utils/TestKinematics.cpp
index 82dc2767..7d734747 100644
--- a/tests/utils/TestKinematics.cpp
+++ b/tests/utils/TestKinematics.cpp
@@ -2,26 +2,18 @@
 #include <memory>
 #include <xbot2_interface/xbotinterface2.h>
 
+#include "../common.h"
+
+
 
 namespace{
-class testKinematics: public ::testing::Test
+class testKinematics: public TestBase
 {
 protected:
-    XBot::ModelInterface::Ptr _model_ptr;
-    std::string _path_to_cfg;
 
-    testKinematics()
+    testKinematics():TestBase("coman_floating_base")
     {
-        std::string relative_path = OPENSOT_TEST_PATH "configs/coman/configs/config_coman_floating_base.yaml";
-
-        _path_to_cfg = relative_path;
 
-        _model_ptr = XBot::ModelInterface::getModel(_path_to_cfg);
-
-        if(_model_ptr)
-            std::cout<<"pointer address: "<<_model_ptr.get()<<std::endl;
-        else
-            std::cout<<"pointer is NULL "<<_model_ptr.get()<<std::endl;
     }
 
     virtual ~testKinematics() {
@@ -68,16 +60,6 @@ double getRandomAngle(const double min, const double max)
     return (double)rand()/RAND_MAX * (max-min) + min;
 }
 
-Eigen::VectorXd getRandomAngles(const Eigen::VectorXd &min, const Eigen::VectorXd &max, const int size)
-{
-    initializeIfNeeded();
-    Eigen::VectorXd q(size);
-    assert(min.size() >= size);
-    assert(max.size() >= size);
-    for(unsigned int i = 0; i < size; ++i)
-        q(i) = getRandomAngle(min[i],max[i]);
-    return q;
-}
 
 Eigen::Matrix3d skew(const Eigen::Vector3d& p)
 {
@@ -122,12 +104,12 @@ TEST_F(testKinematics, testFloatingBaseJacobian)
 
         //std::cout<<"# Joints: "<<_model_ptr->getJointNum()<<std::endl;
 
-        _model_ptr->setJointPosition(getRandomAngles(qmin, qmax, qmin.size()));
+        _model_ptr->setJointPosition(_model_ptr->generateRandomQ());
         _model_ptr->update();
 
-        urdf::ModelInterface urdf_model = _model_ptr->getUrdf();
+        urdf::ModelConstSharedPtr urdf_model = _model_ptr->getUrdf();
         std::vector<urdf::LinkSharedPtr> links;
-        urdf_model.getLinks(links);
+        urdf_model->getLinks(links);
 
         std::string base_link =   "Waist";
         std::string distal_link = links[getRandomInt(0, links.size()-1)]->name; //"RSoftHand";
@@ -166,59 +148,59 @@ TEST_F(testKinematics, testFloatingBaseJacobian)
 
 }
 
-TEST_F(testKinematics, testRelativeJacobian)
-{
-    for(unsigned int k = 0; k < 100; ++k)
-    {
-        Eigen::VectorXd qmin, qmax;
-        _model_ptr->getJointLimits(qmin, qmax);
+//TEST_F(testKinematics, testRelativeJacobian)
+//{
+//    for(unsigned int k = 0; k < 100; ++k)
+//    {
+//        Eigen::VectorXd qmin, qmax;
+//        _model_ptr->getJointLimits(qmin, qmax);
 
-        //std::cout<<"# Joints: "<<_model_ptr->getJointNum()<<std::endl;
+//        //std::cout<<"# Joints: "<<_model_ptr->getJointNum()<<std::endl;
 
-        _model_ptr->setJointPosition(getRandomAngles(qmin, qmax, qmin.size()));
-        _model_ptr->update();
+//        _model_ptr->setJointPosition(getRandomAngles(qmin, qmax, qmin.size()));
+//        _model_ptr->update();
 
-        urdf::ModelInterface urdf_model = _model_ptr->getUrdf();
-        std::vector<urdf::LinkSharedPtr> links;
-        urdf_model.getLinks(links);
+//        urdf::ModelInterface urdf_model = _model_ptr->getUrdf();
+//        std::vector<urdf::LinkSharedPtr> links;
+//        urdf_model.getLinks(links);
 
-        std::string base_link =   links[getRandomInt(0, links.size()-1)]->name; //"LSoftHand";
-        std::string distal_link = links[getRandomInt(0, links.size()-1)]->name; //"RSoftHand";
-        //std::cout<<"base_link = "<<base_link<<"     distal_link = "<<distal_link<<std::endl;
+//        std::string base_link =   links[getRandomInt(0, links.size()-1)]->name; //"LSoftHand";
+//        std::string distal_link = links[getRandomInt(0, links.size()-1)]->name; //"RSoftHand";
+//        //std::cout<<"base_link = "<<base_link<<"     distal_link = "<<distal_link<<std::endl;
 
-        Eigen::MatrixXd J; _model_ptr->getRelativeJacobian(distal_link, base_link, J); //Jacobian of distal_link in base_link expressed in base_link
+//        Eigen::MatrixXd J; _model_ptr->getRelativeJacobian(distal_link, base_link, J); //Jacobian of distal_link in base_link expressed in base_link
 
-        //std::cout<<"Jacobian of "<<distal_link<<" wrt "<<base_link<<" expressed in "<<base_link<<": "<<std::endl<<J<<std::endl;
+//        //std::cout<<"Jacobian of "<<distal_link<<" wrt "<<base_link<<" expressed in "<<base_link<<": "<<std::endl<<J<<std::endl;
 
 
-        Eigen::Affine3d wTb; _model_ptr->getPose(base_link, wTb); //Pose of base_link in world
-        Eigen::Affine3d wTd; _model_ptr->getPose(distal_link, wTd); //Pose of distal_link in world
+//        Eigen::Affine3d wTb; _model_ptr->getPose(base_link, wTb); //Pose of base_link in world
+//        Eigen::Affine3d wTd; _model_ptr->getPose(distal_link, wTd); //Pose of distal_link in world
 
-        Eigen::MatrixXd Jdistal; _model_ptr->getJacobian(distal_link, Jdistal); //Jacobian of distal_link expressed in world
-        Eigen::MatrixXd Jbase; _model_ptr->getJacobian(base_link, Jbase); //Jacobian of base_link expressed in world
+//        Eigen::MatrixXd Jdistal; _model_ptr->getJacobian(distal_link, Jdistal); //Jacobian of distal_link expressed in world
+//        Eigen::MatrixXd Jbase; _model_ptr->getJacobian(base_link, Jbase); //Jacobian of base_link expressed in world
 
-        Eigen::Matrix6d Adj1; Adj1.setZero();
-        Adj1.block(0,0,3,3) = wTb.linear().transpose();
-        Adj1.block(3,3,3,3) = wTb.linear().transpose();
-        Eigen::MatrixXd J1 = Adj1*Jdistal; //Jacobian of distal_link expressed in base_link
-        //std::cout<<"J1: "<<std::endl<<J1<<std::endl;
+//        Eigen::Matrix6d Adj1; Adj1.setZero();
+//        Adj1.block(0,0,3,3) = wTb.linear().transpose();
+//        Adj1.block(3,3,3,3) = wTb.linear().transpose();
+//        Eigen::MatrixXd J1 = Adj1*Jdistal; //Jacobian of distal_link expressed in base_link
+//        //std::cout<<"J1: "<<std::endl<<J1<<std::endl;
 
-        Eigen::Vector3d p = wTd.translation() - wTb.translation(); //relative position between distal_link and base_link expressed in world
+//        Eigen::Vector3d p = wTd.translation() - wTb.translation(); //relative position between distal_link and base_link expressed in world
 
-        Eigen::Matrix6d Adj2; Adj2.setZero();
-        Adj2.block(0,0,3,3) = wTb.linear().transpose();
-        Adj2.block(0,3,3,3) = -wTb.linear().transpose()*skew(p);
-        Adj2.block(3,3,3,3) = wTb.linear().transpose();
-        //std::cout<<"Adj2: "<<std::endl<<Adj2<<std::endl;
-        Eigen::MatrixXd J2 = Adj2*Jbase; //Jacobian of base_link expressed in base_link computed at distal_link
-        //std::cout<<"J2: "<<std::endl<<J2<<std::endl;
+//        Eigen::Matrix6d Adj2; Adj2.setZero();
+//        Adj2.block(0,0,3,3) = wTb.linear().transpose();
+//        Adj2.block(0,3,3,3) = -wTb.linear().transpose()*skew(p);
+//        Adj2.block(3,3,3,3) = wTb.linear().transpose();
+//        //std::cout<<"Adj2: "<<std::endl<<Adj2<<std::endl;
+//        Eigen::MatrixXd J2 = Adj2*Jbase; //Jacobian of base_link expressed in base_link computed at distal_link
+//        //std::cout<<"J2: "<<std::endl<<J2<<std::endl;
 
-        Eigen::MatrixXd JR = J1 - J2;
-        //std::cout<<"Computed Jacobian of "<<distal_link<<" wrt "<<base_link<<" expressed in "<<base_link<<": "<<std::endl<<JR<<std::endl;
+//        Eigen::MatrixXd JR = J1 - J2;
+//        //std::cout<<"Computed Jacobian of "<<distal_link<<" wrt "<<base_link<<" expressed in "<<base_link<<": "<<std::endl<<JR<<std::endl;
 
-        EXPECT_MATRIX_NEAR(J, JR, 1e-9);
-    }
-}
+//        EXPECT_MATRIX_NEAR(J, JR, 1e-9);
+//    }
+//}
 
 }