Testl1HQP

ADVRHumanoids · Feb 18, 2024 · 061e160 · 061e160
1 parent 1eefe55
commit 061e160
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 29 deletions.
diff --git 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
@@ -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,21 +63,22 @@ 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);
     OpenSoT::constraints::velocity::JointLimits::Ptr joint_limits =
             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);