Test the setTaskWeight and the getTaskWeight method in IK and TSID

ami-iit · Dec 2, 2021 · 7147ac0 · 7147ac0
1 parent 96dd232
commit 7147ac0
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 0 deletions.
diff --git a/src/IK/tests/QPInverseKinematicsTest.cpp b/src/IK/tests/QPInverseKinematicsTest.cpp
@@ -126,6 +126,17 @@ InverseKinematicsAndTasks createIK(std::shared_ptr<IParametersHandler> handler,
  lowPriority,
  weightRegularization));
 
+
+
+ Eigen::VectorXd newWeight = 10 * weightRegularization;
+ REQUIRE(out.ik->setTaskWeight("regularization_task", newWeight));
+
+ Eigen::VectorXd weight(newWeight.size());
+ REQUIRE(out.ik->getTaskWeight("regularization_task", weight));
+ REQUIRE(weight.isApprox(newWeight));
+
+ REQUIRE(out.ik->setTaskWeight("regularization_task", weightRegularization));
+
  REQUIRE(out.ik->finalize(variables));
 
  return out;

diff --git a/src/TSID/tests/QPFixedBaseTSIDTest.cpp b/src/TSID/tests/QPFixedBaseTSIDTest.cpp
@@ -124,6 +124,15 @@ TSIDAndTasks createTSID(std::shared_ptr<IParametersHandler> handler,
  lowPriority,
  weightRegularization));
 
+ Eigen::VectorXd newWeight = 10 * weightRegularization;
+ REQUIRE(out.tsid->setTaskWeight("regularization_task", newWeight));
+
+ Eigen::VectorXd weight(newWeight.size());
+ REQUIRE(out.tsid->getTaskWeight("regularization_task", weight));
+ REQUIRE(weight.isApprox(newWeight));
+ REQUIRE(out.tsid->setTaskWeight("regularization_task", weightRegularization));
+
+
  REQUIRE(out.tsid->finalize(variables));
 
  return out;