From 2a79e0c5c25fdbbc3a9362cecd2df18f8c28abdb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Thu, 8 Aug 2024 12:12:26 +0200 Subject: [PATCH] fix(cpp): removed isapprox for isclose comparisons According to the eigen documentation isApprox does not work for comparisons with another vector close to zero. That is why this commit changes it to l1 norm comparisons. The FR3 moving and arriving callbacks are performed on the l infinite norm (absolute max element) --- src/common/Pose.cpp | 2 +- src/common/Pose.h | 2 +- src/sim/FR3.cpp | 11 +++++++---- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/common/Pose.cpp b/src/common/Pose.cpp index e49955d2..51ba9d22 100644 --- a/src/common/Pose.cpp +++ b/src/common/Pose.cpp @@ -177,7 +177,7 @@ Pose Pose::inverse() const { } bool Pose::is_close(const Pose &other, double eps_r, double eps_t) const { - return (this->translation() - other.translation()).norm() < eps_t && + return (this->translation() - other.translation()).lpNorm<1>() < eps_t && this->quaternion().angularDistance(other.quaternion()) < eps_r; } diff --git a/src/common/Pose.h b/src/common/Pose.h index 26d958c3..92e317ff 100644 --- a/src/common/Pose.h +++ b/src/common/Pose.h @@ -55,7 +55,7 @@ struct RPY { } bool is_close(const RPY &other, double eps = 1e-8) const { - return this->as_vector().isApprox(other.as_vector(), eps); + return (this->as_vector() - other.as_vector()).lpNorm<1>() < eps; } }; diff --git a/src/sim/FR3.cpp b/src/sim/FR3.cpp index 16e405e7..5fa5880c 100644 --- a/src/sim/FR3.cpp +++ b/src/sim/FR3.cpp @@ -187,15 +187,18 @@ void FR3::set_cartesian_position(const common::Pose& pose) { } void FR3::is_moving_callback() { common::Vector7d current_angles = this->get_joint_position(); - this->state.is_moving = not this->state.previous_angles.isApprox( - current_angles, 0.0001); // TODO: careful with isapprox + // difference of the largest element is smaller than threshold + this->state.is_moving = + (current_angles - this->state.previous_angles).cwiseAbs().maxCoeff() > + 0.0001; this->state.previous_angles = current_angles; } void FR3::is_arrived_callback() { common::Vector7d current_angles = this->get_joint_position(); - this->state.is_arrived = this->state.target_angles.isApprox( - current_angles, this->cfg.joint_rotational_tolerance); + this->state.is_arrived = + (current_angles - this->state.target_angles).cwiseAbs().maxCoeff() < + this->cfg.joint_rotational_tolerance; } bool FR3::collision_callback() {