use min instead of multiplication to combine relative and absolute ch…

…eck (moveit#3556)
ubi-agni · Jan 26, 2024 · 9440572 · 9440572
1 parent 538360c
commit 9440572
Showing 1 changed file with 5 additions and 3 deletions.
diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp
@@ -219,12 +219,14 @@ double CartesianInterpolator::checkJointSpaceJump(const JointModelGroup* group,
     return percentage_solved;
 
   if (jump_threshold.factor > 0.0)
-    percentage_solved *= checkRelativeJointSpaceJump(group, traj, jump_threshold.factor);
+    percentage_solved = checkRelativeJointSpaceJump(group, traj, jump_threshold.factor);
 
+  double percentage_solved_absolute = 1.0;
   if (jump_threshold.revolute > 0.0 || jump_threshold.prismatic > 0.0)
-    percentage_solved *= checkAbsoluteJointSpaceJump(group, traj, jump_threshold.revolute, jump_threshold.prismatic);
+    percentage_solved_absolute =
+        checkAbsoluteJointSpaceJump(group, traj, jump_threshold.revolute, jump_threshold.prismatic);
 
-  return percentage_solved;
+  return std::min(percentage_solved, percentage_solved_absolute);
 }
 
 double CartesianInterpolator::checkRelativeJointSpaceJump(const JointModelGroup* group,