diff --git a/src/IK/include/BipedalLocomotion/IK/GravityTask.h b/src/IK/include/BipedalLocomotion/IK/GravityTask.h index 0083d2bc23..5a0cf5af43 100644 --- 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 index 490241c8ce..ccfd446c8c 100644 --- 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 index de79deff63..34ae776995 100644 --- 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 index 814bf5812a..5f08195af0 100644 --- a/src/IK/tests/QPInverseKinematicsTest.cpp +++ b/src/IK/tests/QPInverseKinematicsTest.cpp @@ -131,7 +131,7 @@ std::shared_ptr createParameterHandler() auto distanceTaskHandler = std::make_shared(); 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 createParameterHandler() auto gravityTaskHandler = std::make_shared(); 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 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(); 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));