From 061e160083ef18ea8a253f5a80cc1b9d51d50d6c Mon Sep 17 00:00:00 2001
From: EnricoMingo <enricomingo@gmail.com>
Date: Sun, 18 Feb 2024 16:48:19 +0100
Subject: [PATCH] Testl1HQP

---
 tests/CMakeLists.txt        |  8 +++----
 tests/solvers/Testl1HQP.cpp | 45 +++++++++++++++++--------------------
 2 files changed, 24 insertions(+), 29 deletions(-)

diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 03bd358d..ce8683f5 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -91,10 +91,10 @@ if(${PCL_FOUND} AND ${moveit_core_FOUND})
 endif()
 
 
-#ADD_EXECUTABLE(testl1HQP     solvers/Testl1HQP.cpp)
-#TARGET_LINK_LIBRARIES(testl1HQP ${TestLibs})
-#add_dependencies(testl1HQP   OpenSoT)
-#add_test(NAME OpenSoT_solvers_l1HQP COMMAND testl1HQP)
+ADD_EXECUTABLE(testl1HQP     solvers/Testl1HQP.cpp)
+TARGET_LINK_LIBRARIES(testl1HQP ${TestLibs})
+add_dependencies(testl1HQP   OpenSoT)
+add_test(NAME OpenSoT_solvers_l1HQP COMMAND testl1HQP)
 
 # if(${OPENSOT_SOTH_FRONT_END})
 #     ADD_EXECUTABLE(testSOTH     solvers/TestSOTH.cpp)
diff --git a/tests/solvers/Testl1HQP.cpp b/tests/solvers/Testl1HQP.cpp
index d1086ae8..154bf2e3 100644
--- a/tests/solvers/Testl1HQP.cpp
+++ b/tests/solvers/Testl1HQP.cpp
@@ -16,7 +16,7 @@ class testl1HQP: public TestBase
 {
 protected:
 
-    testl1HQP(): TestBase("coman")
+    testl1HQP(): TestBase("coman_floating_base")
     {
 
     }
@@ -34,30 +34,24 @@ class testl1HQP: public TestBase
     }
 
     Eigen::VectorXd getGoodInitialPosition(XBot::ModelInterface::Ptr _model_ptr) {
-        Eigen::VectorXd _dq(_model_ptr->getNv());
-        _dq.setZero();
-        _dq[_model_ptr->getDofIndex("RHipSag")] = -25.0*M_PI/180.0;
-        _dq[_model_ptr->getDofIndex("RKneeSag")] = 50.0*M_PI/180.0;
-        _dq[_model_ptr->getDofIndex("RAnkSag")] = -25.0*M_PI/180.0;
+        Eigen::VectorXd _q = _model_ptr->getNeutralQ();
+        _q[_model_ptr->getDofIndex("RHipSag")] = -25.0*M_PI/180.0;
+        _q[_model_ptr->getDofIndex("RKneeSag")] = 50.0*M_PI/180.0;
+        _q[_model_ptr->getDofIndex("RAnkSag")] = -25.0*M_PI/180.0;
 
-        _dq[_model_ptr->getDofIndex("LHipSag")] = -25.0*M_PI/180.0;
-        _dq[_model_ptr->getDofIndex("LKneeSag")] = 50.0*M_PI/180.0;
-        _dq[_model_ptr->getDofIndex("LAnkSag")] = -25.0*M_PI/180.0;
+        _q[_model_ptr->getDofIndex("LHipSag")] = -25.0*M_PI/180.0;
+        _q[_model_ptr->getDofIndex("LKneeSag")] = 50.0*M_PI/180.0;
+        _q[_model_ptr->getDofIndex("LAnkSag")] = -25.0*M_PI/180.0;
 
-        _dq[_model_ptr->getDofIndex("LShSag")] =  20.0*M_PI/180.0;
-        _dq[_model_ptr->getDofIndex("LShLat")] = 10.0*M_PI/180.0;
-        _dq[_model_ptr->getDofIndex("LElbj")] = -80.0*M_PI/180.0;
+        _q[_model_ptr->getDofIndex("LShSag")] =  20.0*M_PI/180.0;
+        _q[_model_ptr->getDofIndex("LShLat")] = 10.0*M_PI/180.0;
+        _q[_model_ptr->getDofIndex("LElbj")] = -80.0*M_PI/180.0;
 
-        _dq[_model_ptr->getDofIndex("RShSag")] =  20.0*M_PI/180.0;
-        _dq[_model_ptr->getDofIndex("RShLat")] = -10.0*M_PI/180.0;
-        _dq[_model_ptr->getDofIndex("RElbj")] = -80.0*M_PI/180.0;
+        _q[_model_ptr->getDofIndex("RShSag")] =  20.0*M_PI/180.0;
+        _q[_model_ptr->getDofIndex("RShLat")] = -10.0*M_PI/180.0;
+        _q[_model_ptr->getDofIndex("RElbj")] = -80.0*M_PI/180.0;
 
-        auto q = _model_ptr->sum(_model_ptr->getNeutralQ(), _dq);
-
-        _model_ptr->setJointPosition(q);
-        _model_ptr->update();
-
-        return q;
+        return _q;
     }
 
 };
@@ -69,13 +63,14 @@ TEST_F(testl1HQP, testContructor)
     model_ptr = _model_ptr;
 
     Eigen::VectorXd q = this->getGoodInitialPosition(model_ptr);
+    _model_ptr->update();
 
     OpenSoT::tasks::velocity::Cartesian::Ptr l_sole =
-            std::make_shared<OpenSoT::tasks::velocity::Cartesian>("l_sole", q, *model_ptr, "l_sole", "world");
+            std::make_shared<OpenSoT::tasks::velocity::Cartesian>("l_sole", *model_ptr, "l_sole", "world");
     OpenSoT::tasks::velocity::Cartesian::Ptr r_sole =
-            std::make_shared<OpenSoT::tasks::velocity::Cartesian>("r_sole", q, *model_ptr, "r_sole", "world");
+            std::make_shared<OpenSoT::tasks::velocity::Cartesian>("r_sole", *model_ptr, "r_sole", "world");
     OpenSoT::tasks::velocity::CoM::Ptr CoM =
-            std::make_shared<OpenSoT::tasks::velocity::CoM>(q, *model_ptr);
+            std::make_shared<OpenSoT::tasks::velocity::CoM>(*model_ptr);
 
     Eigen::VectorXd qmin, qmax;
     model_ptr->getJointLimits(qmin, qmax);
@@ -83,7 +78,7 @@ TEST_F(testl1HQP, testContructor)
             std::make_shared<OpenSoT::constraints::velocity::JointLimits>(*_model_ptr, qmax, qmin);
 
     OpenSoT::AutoStack::Ptr stack = ((l_sole + r_sole)/CoM)<<joint_limits;
-    stack->update(q);
+    stack->update(Eigen::VectorXd(0));
 
     OpenSoT::solvers::l1HQP::Ptr l1_solver =
             std::make_shared<OpenSoT::solvers::l1HQP>(*stack);