From 64e2440c53d45379b07aae33aa1ee44fb7ed6635 Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 12 Sep 2020 15:42:25 +0200 Subject: [PATCH] simplify LinkMotion computation review feedback --- core/src/cost_terms.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 612ff7b3..d22ec5d4 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -116,10 +116,10 @@ double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { } double distance{ 0.0 }; - Eigen::Translation3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() }; + Eigen::Vector3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() }; for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) { - Eigen::Translation3d new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() }; - distance += (new_position.vector() - position.vector()).norm(); + const auto& new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() }; + distance += (new_position - position).norm(); position = new_position; } return distance;