Improve smooother2 time sampling

[snozawa]

Summary

• Improve the time sampling in ParabolicSmoother2.

Detail

• The current method to sample times is:
  1. Sample times : https://github.com/rdiankov/openrave/blob/master/plugins/rplanners/parabolicsmoother2.cpp#L2538-L2539, https://github.com/rdiankov/openrave/blob/master/plugins/rplanners/parabolicsmoother2.cpp#L2551-L2552
  2. If within minTimeStep, reject since it's too close: https://github.com/rdiankov/openrave/blob/master/plugins/rplanners/parabolicsmoother2.cpp#L2566
• Instead of sample+reject, directly sample the times from feasible range. to do so, combine the inequality about minTimeStep with the existing inequalities for t0 and t1.
  • Sample times from feasible range considering minTimeStep for usual case: https://github.com/rdiankov/openrave/blob/master/plugins/rplanners/parabolicsmoother2.cpp#L2551-L2552
  • t0 and t1 are always feasible.
  • Also consider the minTimeStep for zero-velocity case : https://github.com/rdiankov/openrave/blob/master/plugins/rplanners/parabolicsmoother2.cpp#L2538-L2539
  • t0 and t1 might be infeasible, due to specialShortcutCutoffTime. if feasible, use the computed values. if infeasible, remove specialShortcutCutoffTime and compute the times again as a fallback. This fallback computation is always feasible.
• Background
  • Without this PR, sampled times are simply rejected sometimes. Failure count nItersFromPrevSuccessful is unexpectedly accumulated, and thus, the iteration might quit too early.
  • This PR would very-slightly improve the computed duration. More than that, this PR will make the computational results deterministic because we can reduce the risk that the iteration quits too early.

[Puttichai]

If the main motivation is to prevent unnecessary accumulation of failure counts due to the sampled t0 and t1 being too close to each other, I wonder what differences will be if we just continue to the next iteration without incrementing the iteration count (iters in Shortcut function).

[snozawa]

Thanks for your comment!

I think your comment is equivalent as follows:

git diff
diff --git a/plugins/rplanners/parabolicsmoother2.cpp b/plugins/rplanners/parabolicsmoother2.cpp
index 9c6eaaaf9..d275d9415 100644
--- a/plugins/rplanners/parabolicsmoother2.cpp
+++ b/plugins/rplanners/parabolicsmoother2.cpp
@@ -2565,6 +2565,8 @@ protected:
                 ++vShortcutStats[SS_TimeInstantsTooClose];
                 shortcutprogress << SS_TimeInstantsTooClose << "\n";
 #endif
+                iters--;
+                nItersFromPrevSuccessful--;
                 continue;
             }

I originally tried this, and yes, it's reasonable. But, in this PR, I try to compute the times directly, since:

• In the above retrying of the iteration, if we are super unlucky, we can continue failing, e.g. no theoretical guarantee to get feasible t0 and t1 in a fixed time. Of course, I think such probability is extremely small. Meanwhile, the way in this PR will return feasible t0 and t1 in a fixed function call.
• More than that, zero-velocity code path (https://github.com/rdiankov/openrave/blob/master/plugins/rplanners/parabolicsmoother2.cpp#L2538-L2539) already contains Rand. So, retrying this situation would be bit complicated in terms of coding.

[rdiankov]

worth a try, thanks~