diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 276e45c37d..71b927daaf 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -292,7 +292,9 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( traj.insert(traj.end(), start, waypoint_traj.end()); if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) + { percentage_solved = static_cast(i + 1) / static_cast(waypoints.size()); + } else { percentage_solved += wp_percentage_solved / static_cast(waypoints.size()); @@ -494,7 +496,9 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath( path.insert(path.end(), start, waypoint_path.end()); if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) + { percentage_solved = static_cast((i + 1)) / static_cast(waypoints.size()); + } else { percentage_solved += wp_percentage_solved / static_cast(waypoints.size());