Add initial update checks in task test cases

ami-iit · Feb 26, 2025 · 5d91fc7 · 5d91fc7
1 parent 804226d
commit 5d91fc7
Show file tree

Hide file tree

Showing 7 changed files with 18 additions and 0 deletions.
diff --git a/src/TSID/tests/AngularMomentumTaskTest.cpp b/src/TSID/tests/AngularMomentumTaskTest.cpp
@@ -100,6 +100,9 @@ TEST_CASE("Angular momentum task")
                 REQUIRE(task.setKinDyn(kinDyn));
                 REQUIRE(task.initialize(parameterHandler));
                 REQUIRE(task.setVariablesHandler(variablesHandler));
+
+                REQUIRE_FALSE(task.update());
+
                 REQUIRE(task.setSetPoint(desiredAngularMomentum, //
                                          desiredAngularMomentumRateOfChange));
                 REQUIRE(task.update());

diff --git a/src/TSID/tests/CoMTaskTest.cpp b/src/TSID/tests/CoMTaskTest.cpp
@@ -90,6 +90,8 @@ TEST_CASE("CoM Task")
             const auto desiredVelocity = manif::R3d::Tangent::Random();
             const auto desiredAcceleration = manif::R3d::Tangent::Random();
 
+            REQUIRE_FALSE(task.update());
+
             REQUIRE(task.setSetPoint(desiredPosition.coeffs(),
                                      desiredVelocity.coeffs(),
                                      desiredAcceleration.coeffs()));

diff --git a/src/TSID/tests/JointTrackingTaskTest.cpp b/src/TSID/tests/JointTrackingTaskTest.cpp
@@ -89,6 +89,8 @@ TEST_CASE("Joint Regularization task")
             REQUIRE(task.initialize(parameterHandler));
             REQUIRE(task.setVariablesHandler(variablesHandler));
 
+            REQUIRE_FALSE(task.update());
+
             REQUIRE(task.setSetPoint(Eigen::VectorXd::Zero(model.getNrOfDOFs()),
                                      Eigen::VectorXd::Zero(model.getNrOfDOFs()),
                                      feedforwardDesiredJointAcc));

diff --git a/src/TSID/tests/R3TaskTest.cpp b/src/TSID/tests/R3TaskTest.cpp
@@ -89,6 +89,8 @@ TEST_CASE("SE3 Task")
             const auto desiredVelocity = manif::R3d::Tangent::Random();
             const auto desiredAcceleration = manif::R3d::Tangent::Random();
 
+            REQUIRE_FALSE(task.update());
+
             REQUIRE(task.setSetPoint(desiredPosition.coeffs(),
                                      desiredVelocity.coeffs(),
                                      desiredAcceleration.coeffs()));

diff --git a/src/TSID/tests/SE3TaskTest.cpp b/src/TSID/tests/SE3TaskTest.cpp
@@ -93,6 +93,8 @@ TEST_CASE("SE3 Task")
             const auto desiredVelocity = manif::SE3d::Tangent::Random();
             const auto desiredAcceleration = manif::SE3d::Tangent::Random();
 
+            REQUIRE_FALSE(task.update());
+
             REQUIRE(task.setSetPoint(desiredPose, desiredVelocity, desiredAcceleration));
 
             REQUIRE(task.update());

diff --git a/src/TSID/tests/SO3TaskTest.cpp b/src/TSID/tests/SO3TaskTest.cpp
@@ -89,6 +89,8 @@ TEST_CASE("SO3 Task")
             const auto desiredVelocity = manif::SO3d::Tangent::Random();
             const auto desiredAcceleration = manif::SO3d::Tangent::Random();
 
+            REQUIRE_FALSE(task.update());
+
             REQUIRE(task.setSetPoint(desiredOrientation, desiredVelocity, desiredAcceleration));
 
             REQUIRE(task.update());

diff --git a/src/TSID/tests/VariableRegularizationTaskTest.cpp b/src/TSID/tests/VariableRegularizationTaskTest.cpp
@@ -52,8 +52,13 @@ TEST_CASE("Variable Regularization task")
 
         Eigen::VectorXd regularize(elementNames.size());
         regularize.setRandom();
+
+        REQUIRE_FALSE(task.update());
+
         REQUIRE(task.setSetPoint(regularize));
 
+        REQUIRE(task.update());
+
         Eigen::Ref<const Eigen::MatrixXd> A = task.getA();
 
         // check the matrix A