TestAffine Utils ported

ADVRHumanoids · Feb 9, 2024 · 87b985e · 87b985e
1 parent 4b714e2
commit 87b985e
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 17 deletions.
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
@@ -159,10 +159,10 @@ endif()
 # add_dependencies(testeiQuadProgSolver   OpenSoT)
 # add_test(NAME OpenSoT_solvers_eiquadprog COMMAND testeiQuadProgSolver)
 
-#ADD_EXECUTABLE(testAffineUtils utils/TestAffineUtils.cpp)
-#TARGET_LINK_LIBRARIES(testAffineUtils ${TestLibs})
-#add_dependencies(testAffineUtils   OpenSoT)
-#add_test(NAME OpenSoT_affine_utils COMMAND testAffineUtils)
+ADD_EXECUTABLE(testAffineUtils utils/TestAffineUtils.cpp)
+TARGET_LINK_LIBRARIES(testAffineUtils ${TestLibs})
+add_dependencies(testAffineUtils   OpenSoT)
+add_test(NAME OpenSoT_affine_utils COMMAND testAffineUtils)
 
 ADD_EXECUTABLE(testAffineHelper utils/TestAffineHelper.cpp)
 TARGET_LINK_LIBRARIES(testAffineHelper ${TestLibs})

diff --git a/tests/utils/TestAffineUtils.cpp b/tests/utils/TestAffineUtils.cpp
@@ -14,11 +14,11 @@ class testAffineUtils: public TestBase
 {
 protected:
 
-    testAffineUtils(): TestBase("coman")
+    testAffineUtils(): TestBase("coman_floating_base")
     {
         model_ptr = _model_ptr;
 
-        q.setZero(model_ptr->getJointNum());
+        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;
@@ -58,12 +58,12 @@ TEST_F(testAffineUtils, testConstraintsToAffine)
     com_vel_lims<<0.1, 0.1, 0.1;
     OpenSoT::constraints::velocity::CartesianVelocity::Ptr constraint =
             std::make_shared<OpenSoT::constraints::velocity::CartesianVelocity>(
-                com_vel_lims, 0.01, std::make_shared<OpenSoT::tasks::velocity::CoM>(this->q, *this->model_ptr.get()));
+                com_vel_lims, 0.01, std::make_shared<OpenSoT::tasks::velocity::CoM>(*this->model_ptr.get()));
 
     //2. Creates variables
     std::vector<std::pair<std::string, int>> name_size_pairs;
 
-    name_size_pairs.emplace_back("dq", this->q.size());
+    name_size_pairs.emplace_back("dq", this->model_ptr->getNv());
     int slack_size = 6;
     name_size_pairs.emplace_back("slack", slack_size);
     OpenSoT::OptvarHelper opt_helper(name_size_pairs);
@@ -85,7 +85,7 @@ TEST_F(testAffineUtils, testConstraintsToAffine)
     this->model_ptr->setJointPosition(this->q);
     this->model_ptr->update();
 
-    affine_constraint->update(this->q);
+    affine_constraint->update(Eigen::VectorXd(0));
     EXPECT_EQ(affine_constraint->getAineq().rows(), constraint->getAineq().rows());
     EXPECT_EQ(affine_constraint->getAineq().cols(), constraint->getAineq().cols() + slack_size);
     EXPECT_EQ(affine_constraint->getbLowerBound(), constraint->getbLowerBound());
@@ -99,13 +99,13 @@ TEST_F(testAffineUtils, testBoundsToAffine)
 {
     //1. Creates a Velocity Limits constraint
     OpenSoT::constraints::velocity::VelocityLimits::Ptr bound =
-            std::make_shared<OpenSoT::constraints::velocity::VelocityLimits>(
-                M_PI, 0.01, this->model_ptr->getJointNum());
+            std::make_shared<OpenSoT::constraints::velocity::VelocityLimits>(*this->model_ptr,
+                M_PI, 0.01);
 
     //2. Creates variables
     std::vector<std::pair<std::string, int>> name_size_pairs;
 
-    name_size_pairs.emplace_back("dq", this->q.size());
+    name_size_pairs.emplace_back("dq", this->model_ptr->getNv());
     int slack_size = 6;
     name_size_pairs.emplace_back("slack", slack_size);
     OpenSoT::OptvarHelper opt_helper(name_size_pairs);
@@ -121,8 +121,8 @@ TEST_F(testAffineUtils, testBoundsToAffine)
 
     //4. Update bound
     bound->setVelocityLimits(M_PI/4.);
-    bound->update(this->q);
-    affine_bound->update(this->q);
+    bound->update(Eigen::VectorXd(0));
+    affine_bound->update(Eigen::VectorXd(0));
 
     EXPECT_EQ(affine_bound->getAineq().rows(), bound->getLowerBound().rows());
     EXPECT_EQ(affine_bound->getAineq().cols(), bound->getLowerBound().rows() + slack_size);
@@ -138,12 +138,12 @@ TEST_F(testAffineUtils, testCartesianTaskToAffine)
     //1. Creates a Cartesian velocity Task
     OpenSoT::tasks::velocity::Cartesian::Ptr task =
             std::make_shared<OpenSoT::tasks::velocity::Cartesian>(
-                "LSOLE", this->q, *(this->model_ptr), "l_sole", "world");
+                "LSOLE", *this->model_ptr, "l_sole", "world");
 
     //2. Creates variables
     std::vector<std::pair<std::string, int>> name_size_pairs;
 
-    name_size_pairs.emplace_back("dq", this->q.size());
+    name_size_pairs.emplace_back("dq", this->model_ptr->getNv());
     int slack_size = 6;
     name_size_pairs.emplace_back("slack", slack_size);
     OpenSoT::OptvarHelper opt_helper(name_size_pairs);
@@ -171,7 +171,7 @@ TEST_F(testAffineUtils, testCartesianTaskToAffine)
     Eigen::Affine3d T = Eigen::Affine3d::Identity();
     task->setReference(T);
 
-    affine_task->update(Eigen::VectorXd(1));
+    affine_task->update(Eigen::VectorXd(0));
 
     Eigen::Affine3d T_ref; task->getReference(T_ref);
     EXPECT_EQ(T.matrix(), T_ref.matrix());