Fixed a sign mistake in GravityTask.

This might have been the reason why the IK was failing before
ami-iit · Aug 3, 2023 · a3baa19 · a3baa19
1 parent a6078ce
commit a3baa19
Show file tree

Hide file tree

Showing 4 changed files with 8 additions and 8 deletions.
diff --git a/src/IK/include/BipedalLocomotion/IK/GravityTask.h b/src/IK/include/BipedalLocomotion/IK/GravityTask.h
@@ -31,7 +31,7 @@ namespace IK
  * \begin{bmatrix}
  * 1 & 0 & 0 \\
  * 0 & 1 & 0
- * \end{bmatrix} J_\omega \nu = k \begin{bmatrix}
+ * \end{bmatrix} J_\omega \nu = -k \begin{bmatrix}
  * 0 &-1 & 0 \\
  * 1 & 0 & 0
  * \end{bmatrix} {} ^ W R_f {} ^ f g ^ * + {} ^ W R_f {} ^ f \omega_{ff}

diff --git a/src/IK/src/GravityTask.cpp b/src/IK/src/GravityTask.cpp
@@ -164,7 +164,7 @@ bool GravityTask::update()
  }
 
  toEigen(this->subA(m_robotVelocityVariable)) = m_Am * m_jacobian.bottomRows<3>();
- m_b = m_kp * m_bm * desiredZDirectionAbsolute + feedforwardAbsolute.topRows<2>();
+ m_b = -m_kp * m_bm * desiredZDirectionAbsolute + feedforwardAbsolute.topRows<2>();
 
  m_isValid = true;
  return m_isValid;

diff --git a/src/IK/tests/GravityTaskTest.cpp b/src/IK/tests/GravityTaskTest.cpp
@@ -124,8 +124,8 @@ TEST_CASE("Distance task")
  Eigen::Vector3d axisAbsolute, feedforwardAbsolute;
  axisAbsolute = targetRotation * desiredDirection;
  feedforwardAbsolute = targetRotation * feedforward;
- expectedB(0) = -kp * axisAbsolute(1) + feedforwardAbsolute(0);
- expectedB(1) = kp * axisAbsolute(0) + feedforwardAbsolute(1);
+ expectedB(0) = kp * axisAbsolute(1) + feedforwardAbsolute(0);
+ expectedB(1) = -kp * axisAbsolute(0) + feedforwardAbsolute(1);
  REQUIRE(b.isApprox(expectedB));
  }
  }

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", 1);
+ distanceTaskHandler->setParameter("priority", 0);
  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", 1);
+ gravityTaskHandler->setParameter("priority", 0);
  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", lowPriority, distanceWeight));
+ REQUIRE(out.ik->addTask(out.distanceTask, "distance_task", highPriority, 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", lowPriority, gravityWeight));
+ REQUIRE(out.ik->addTask(out.gravityTask, "gravity_task", highPriority, gravityWeight));
 
  REQUIRE(out.ik->finalize(variables));