Added TestqpSWIFT and TestCartesian (here one test is missing, added …

…a note in the CMakeLists.txt). Fixed test in TestQPOases
ADVRHumanoids · Feb 20, 2024 · 7c9ee91 · 7c9ee91
1 parent 84a468e
commit 7c9ee91
Show file tree

Hide file tree

Showing 4 changed files with 284 additions and 359 deletions.
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
@@ -283,10 +283,10 @@ add_test(NAME OpenSoT_task_velocity_Postural COMMAND testPosturalVelocityTask)
  add_dependencies(testContactFloatingBaseTask   OpenSoT)
  add_test(NAME OpenSoT_task_floating_base_Postural COMMAND testContactFloatingBaseTask)
 
-# ADD_EXECUTABLE(testCartesianVelocityTask tasks/velocity/TestCartesian.cpp)
-# TARGET_LINK_LIBRARIES(testCartesianVelocityTask ${TestLibs})
-# add_dependencies(testCartesianVelocityTask   OpenSoT)
-# add_test(NAME OpenSoT_task_velocity_Cartesian COMMAND testCartesianVelocityTask)
+ ADD_EXECUTABLE(testCartesianVelocityTask tasks/velocity/TestCartesian.cpp) #<-- missing testCartesianTask.testActiveJointsMask !
+ TARGET_LINK_LIBRARIES(testCartesianVelocityTask ${TestLibs})
+ add_dependencies(testCartesianVelocityTask   OpenSoT)
+ add_test(NAME OpenSoT_task_velocity_Cartesian COMMAND testCartesianVelocityTask)
 
 # ADD_EXECUTABLE(testCartesianAdmittanceVelocityTask tasks/velocity/TestCartesianAdmittance.cpp)
 # TARGET_LINK_LIBRARIES(testCartesianAdmittanceVelocityTask ${TestLibs})
@@ -330,12 +330,12 @@ add_test(NAME OpenSoT_task_velocity_Postural COMMAND testPosturalVelocityTask)
  add_dependencies(testCoMForceTask  OpenSoT)
  add_test(NAME OpenSoT_task_force_CoM COMMAND testCoMForceTask)
 
-# if(${qpSWIFT_FOUND})
-#     ADD_EXECUTABLE(testqpSWIFTSolver solvers/TestqpSWIFT.cpp)
-#     TARGET_LINK_LIBRARIES(testqpSWIFTSolver ${TestLibs} qpSWIFT::qpSWIFT-static)
-#     add_dependencies(testqpSWIFTSolver OpenSoT)
-#     add_test(NAME OpenSoT_solvers_qpswift COMMAND testqpSWIFTSolver)
-# endif()
+ if(${qpSWIFT_FOUND})
+     ADD_EXECUTABLE(testqpSWIFTSolver solvers/TestqpSWIFT.cpp)
+     TARGET_LINK_LIBRARIES(testqpSWIFTSolver ${TestLibs} qpSWIFT::qpSWIFT-static)
+     add_dependencies(testqpSWIFTSolver OpenSoT)
+     add_test(NAME OpenSoT_solvers_qpswift COMMAND testqpSWIFTSolver)
+ endif()
 
 #ADD_EXECUTABLE(testCartesianPositionVelocityConstraint constraints/velocity/TestCartesianPositionConstraint.cpp)
 #TARGET_LINK_LIBRARIES(testCartesianPositionVelocityConstraint ${TestLibs})

diff --git a/tests/solvers/TestQPOases.cpp b/tests/solvers/TestQPOases.cpp
@@ -482,8 +482,8 @@ TEST_F(testQPOasesTask, testQPOasesTask)
 using namespace OpenSoT::constraints::velocity;
 TEST_F(testQPOasesTask, testProblemWithConstraint)
 {
-        Eigen::VectorXd q(_model_ptr->getNq()); q = _model_ptr->getNeutralQ();
-        Eigen::VectorXd q_ref(q.size()); q_ref.setConstant(q.size(), M_PI);
+        Eigen::VectorXd q = _model_ptr->getNeutralQ();
+        Eigen::VectorXd q_ref = _model_ptr->generateRandomQ();
         _model_ptr->setJointPosition(q);
         _model_ptr->update();
 
@@ -498,7 +498,7 @@ TEST_F(testQPOasesTask, testProblemWithConstraint)
         JointLimits::Ptr joint_limits(
             new JointLimits(*_model_ptr, q_max, q_min));
         postural_task->getConstraints().push_back(joint_limits);
-        postural_task->setLambda(0.1);
+        postural_task->setLambda(1.);
 
         std::cout<<"postural_task->getXSize(): "<<postural_task->getXSize()<<std::endl;
 
@@ -558,7 +558,7 @@ TEST_F(testQPOasesTask, testProblemWithConstraint)
                 EXPECT_NEAR( q[i], q_min[i-1], 1E-4);
             }
             else
-                EXPECT_NEAR( q[i], q_ref[i-1], 1E-4);
+                EXPECT_NEAR( q[i], q_ref[i], 1E-4);
         }
 }
 
@@ -598,8 +598,8 @@ TEST_F(testQPOasesTask, test_on_eigen)
 
 TEST_F(testiHQP, testContructor1Problem)
 {
-    Eigen::VectorXd q(_model_ptr->getNq()); q = _model_ptr->getNeutralQ();
-    Eigen::VectorXd q_ref(q.size()); q_ref.setConstant(q.size(), M_PI);
+    Eigen::VectorXd q = _model_ptr->getNeutralQ();
+    Eigen::VectorXd q_ref = _model_ptr->generateRandomQ();
     _model_ptr->setJointPosition(q);
     _model_ptr->update();
 
@@ -665,7 +665,7 @@ TEST_F(testiHQP, testContructor1Problem)
             EXPECT_NEAR( q[i], q_min[i-1], 1E-4);
         }
         else
-            EXPECT_NEAR( q[i], q_ref[i-1], 1E-4);
+            EXPECT_NEAR( q[i], q_ref[i], 1E-4);
 
     }
 }