From 010bf500fb2548b3809ccebc488c215d52753aee Mon Sep 17 00:00:00 2001 From: Ines Date: Tue, 6 Apr 2021 15:45:51 +0200 Subject: [PATCH] Fix bug in QPFixedBaseTSIDTest --- src/TSID/tests/QPFixedBaseTSIDTest.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/TSID/tests/QPFixedBaseTSIDTest.cpp b/src/TSID/tests/QPFixedBaseTSIDTest.cpp index 802f2f77ba..e388c1472e 100644 --- a/src/TSID/tests/QPFixedBaseTSIDTest.cpp +++ b/src/TSID/tests/QPFixedBaseTSIDTest.cpp @@ -67,8 +67,8 @@ std::shared_ptr createParameterHandler() parameterHandler->setParameter("verbosity", false); /////// SE3 Task - constexpr double kp_se3task = 16.0; - constexpr double kd_se3task = 8.0; + constexpr double kp_se3task = 100.0; + constexpr double kd_se3task = 20.0; auto SE3ParameterHandler = std::make_shared(); SE3ParameterHandler->setParameter("robot_acceleration_variable_name", @@ -98,6 +98,7 @@ TSIDAndTasks createTSID(std::shared_ptr handler, const Eigen::VectorXd kpRegularization = 1 * Eigen::VectorXd::Ones(kinDyn->model().getNrOfDOFs()); const Eigen::VectorXd kdRegularization = 2 * Eigen::VectorXd::Ones(kinDyn->model().getNrOfDOFs()); const Eigen::VectorXd weightRegularization = 1 * Eigen::VectorXd::Ones(kinDyn->model().getNrOfDOFs()); + const Eigen::VectorXd weightSE3Task = 1 * Eigen::VectorXd::Ones(6); // 6 is the size of the end-effector DoF handler->getGroup("REGULARIZATION_TASK").lock()->setParameter("kp", kpRegularization); handler->getGroup("REGULARIZATION_TASK").lock()->setParameter("kd", kdRegularization); @@ -112,7 +113,7 @@ TSIDAndTasks createTSID(std::shared_ptr handler, out.se3Task = std::make_shared(); REQUIRE(out.se3Task->setKinDyn(kinDyn)); REQUIRE(out.se3Task->initialize(handler->getGroup("EE_SE3_TASK"))); - REQUIRE(out.tsid->addTask(out.se3Task, "se3_task", lowPriority)); + REQUIRE(out.tsid->addTask(out.se3Task, "se3_task", lowPriority, weightSE3Task)); out.regularizationTask = std::make_shared(); REQUIRE(out.regularizationTask->setKinDyn(kinDyn));