Moved again distance and gravity tasks to cost

ami-iit · Aug 3, 2023 · 954b608 · 954b608
1 parent a3baa19
commit 954b608
Showing 1 changed file with 4 additions and 4 deletions.
diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp
@@ -131,7 +131,7 @@ std::shared_ptr<IParametersHandler> createParameterHandler()
     auto distanceTaskHandler = std::make_shared<StdImplementation>();
     distanceTaskHandler->setParameter("robot_velocity_variable_name", robotVelocity);
     distanceTaskHandler->setParameter("type", "DistanceTask");
-    distanceTaskHandler->setParameter("priority", 0);
+    distanceTaskHandler->setParameter("priority", 1);
     distanceTaskHandler->setParameter("kp", gain);
     Eigen::VectorXd distanceWeight(1);
     distanceWeight << additionalTasksWeight;
@@ -142,7 +142,7 @@ std::shared_ptr<IParametersHandler> createParameterHandler()
     auto gravityTaskHandler = std::make_shared<StdImplementation>();
     gravityTaskHandler->setParameter("robot_velocity_variable_name", robotVelocity);
     gravityTaskHandler->setParameter("type", "GravityTask");
-    gravityTaskHandler->setParameter("priority", 0);
+    gravityTaskHandler->setParameter("priority", 1);
     gravityTaskHandler->setParameter("kp", gain);
     const Eigen::Vector2d gravityWeight = additionalTasksWeight * Eigen::Vector2d::Ones();
     gravityTaskHandler->setParameter("weight", gravityWeight);
@@ -233,13 +233,13 @@ InverseKinematicsAndTasks createIK(std::shared_ptr<IParametersHandler> handler,
     REQUIRE(out.distanceTask->setKinDyn(kinDyn));
     REQUIRE(out.distanceTask->initialize(handler->getGroup("DISTANCE_TASK")));
     REQUIRE(handler->getGroup("DISTANCE_TASK").lock()->getParameter("weight", distanceWeight));
-    REQUIRE(out.ik->addTask(out.distanceTask, "distance_task", highPriority, distanceWeight));
+    REQUIRE(out.ik->addTask(out.distanceTask, "distance_task", lowPriority, distanceWeight));
 
     out.gravityTask = std::make_shared<GravityTask>();
     REQUIRE(out.gravityTask->setKinDyn(kinDyn));
     REQUIRE(out.gravityTask->initialize(handler->getGroup("GRAVITY_TASK")));
     REQUIRE(handler->getGroup("GRAVITY_TASK").lock()->getParameter("weight", gravityWeight));
-    REQUIRE(out.ik->addTask(out.gravityTask, "gravity_task", highPriority, gravityWeight));
+    REQUIRE(out.ik->addTask(out.gravityTask, "gravity_task", lowPriority, gravityWeight));
 
     REQUIRE(out.ik->finalize(variables));