From 469f4b8722f16dfa4f2852bdaa86e285f9f6a293 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 27 Sep 2023 14:06:57 +0200 Subject: [PATCH] Added suggestions to improve the distance and gravity task computations Co-authored-by: Giulio Romualdi --- src/IK/src/DistanceTask.cpp | 5 ++--- src/IK/src/GravityTask.cpp | 5 +++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/IK/src/DistanceTask.cpp b/src/IK/src/DistanceTask.cpp index 182237753b..487a782247 100644 --- a/src/IK/src/DistanceTask.cpp +++ b/src/IK/src/DistanceTask.cpp @@ -199,10 +199,9 @@ bool DistanceTask::update() m_jacobian.rightCols(m_kinDyn->getNrOfDegreesOfFreedom()) = m_relativeJacobian; } - m_computedDistance = sqrt(pow(m_framePosition(0), 2) + pow(m_framePosition(1), 2) - + pow(m_framePosition(2), 2)); + m_computedDistance = m_framePosition.norm(); - toEigen(this->subA(m_robotVelocityVariable)) + toEigen(this->subA(m_robotVelocityVariable)).noalias() = (m_framePosition.transpose() * m_jacobian.topRows<3>()) / (std::max(0.001, m_computedDistance)); m_b(0) = m_kp * (m_desiredDistance - m_computedDistance); diff --git a/src/IK/src/GravityTask.cpp b/src/IK/src/GravityTask.cpp index fd4aee826d..aa408a13bc 100644 --- a/src/IK/src/GravityTask.cpp +++ b/src/IK/src/GravityTask.cpp @@ -163,8 +163,9 @@ bool GravityTask::update() return m_isValid; } - toEigen(this->subA(m_robotVelocityVariable)) = m_Am * m_jacobian.bottomRows<3>(); - m_b = -m_kp * m_bm * desiredZDirectionAbsolute + feedforwardAbsolute.topRows<2>(); + toEigen(this->subA(m_robotVelocityVariable)).noalias() = m_Am * m_jacobian.bottomRows<3>(); + m_b = feedforwardAbsolute.topRows<2>(); + m_b.noalias() -= m_kp * m_bm * desiredZDirectionAbsolute; m_isValid = true; return m_isValid;