Added suggestions to improve the distance and gravity task computations

Co-authored-by: Giulio Romualdi <giulio.romualdi@gmail.com>
ami-iit · Sep 27, 2023 · 88eb49a · 88eb49a
1 parent 972ab04
commit 88eb49a
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git 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
@@ -164,7 +164,8 @@ 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 = feedforwardAbsolute.topRows<2>();
+ m_b.noalias() -= m_kp * m_bm * desiredZDirectionAbsolute;
 
  m_isValid = true;
  return m_isValid;