WIP: fix asymptotically infinite loop with Dichotomy

[jmirabel]

In this draft PR, I add a test case that reproduce the asymptotically infinite loop in continuous path validation by dichotomy. The test simple most a sphere next to a box along a straight line. The min distance below is the true minimal distance between the two objects during the path.

In the table below, I report the number of iterations of the validation algo. The 0.00101 and 0.00099 are values close to the hard-coded 0.001 in the code. I can make the number of iteration increase (arbitrarily?) by reducing 1e-6.

min distance	0.00101	0.00099	1e-6	0
Fix excluded	612	612	564412	2
Fix included	612	2	2	2

I'm opening this as a draft PR as I'm not 100% sure we want this. For Dichotomy, we could use the parameter passed to the constructor as a distance lower bound threshold (making sure the break distance is larger). This means that a valid path with a minimal distance smaller than this threshold may or may not be considered invalid (depending on whether the path is evaluated at a given time).
In the context of gradient based optimization, it is problematic if the input path, assumed to be valid turns out not to be. However,

• we might avoid the issue by smartly playing with the threshold and security margin at different stages, by being more restrictive at planning time. This might not be very easy though.
• I prefer a failure to optimize than a unstoppable optimization.

[florent-lamiraux]

Thank you for opening this issue. The problem is well explained in https://laas.hal.science/hal-04056297v1 Section IV.C.
I agree that the best solution is to set different security margins at planning time and at optimization time.

With your proposition, what will happen if a path is tested as valid and later on as invalid in trajectory optimization, will it be detected and reported as a failure ?

[jmirabel]

With the current implementation, the only problem I see is when the path passed as input to optimization is invalid.

The following strategy would work:

• Compute an input path with a security margin Sp > 0, break distance Bp > 0 and lower bound threshold Lp < Bp.
• At optimization time, use security margin So in [ 0, Sp [, break distance Bo > Lo and lower bound threshold Lo <= Sp - So

This should ensure that the path will never be considered invalid at optimization time if it has been considered valid at planning time.

Going into numerical values, this could be
Lo = 1mm
So = 1mm
Bo = 2cm
Sp = 2mm
Lp = 1mm (a larger value might be better)
Bp = 2cm

[florent-lamiraux]

I understand security margin and break distance, but can you recall what lower bound threshold stands for ?

[florent-lamiraux]

From an implementation point of view, should we

• duplicate PathValidation instances, one for path planning, one for path optimization,
• store two sets of parameters and update the single instance of PathValidation before planning or optimizing ?

[jmirabel]

The lower bound threshold is this 0.001 in this draft PR. It would be made modifiable if the principle is accepted.

[jmirabel]

@florent-lamiraux could you have another look at this?

For dichotomy, I redirected the constructor parameter to the new distance threshold. The parameter has to be negative as it is the opposite of the tolerance.

I'm thinking to rename the distance lower bound threshold in distance threshold since it is expected that the threshold is lower than the break distance and so that the distance lower bound equals the actual distance.

Apart from this, the current method does not change the current behavior by default. I think this is sufficient for a first version since it is not clear how to automatically integrate it into the overall pipeline.

[florent-lamiraux]

I would call the threshold distance_uncertainty since it corresponds to the range of distance in which the result is uncertain.

Can you also update the documentation of the function contructors (and more if relevant) ?

[florent-lamiraux]

For a reason that does not seem related to this PR, I got some compilations errors that I fixed with the following patch

diff --git a/tests/test-gradient-based.cc b/tests/test-gradient-based.cc
index 8b8baeb1..e0168bc5 100644
--- a/tests/test-gradient-based.cc
+++ b/tests/test-gradient-based.cc
@@ -40,6 +40,7 @@
 #include <hpp/pinocchio/collision-object.hh>
 #include <hpp/pinocchio/device.hh>
 #include <hpp/pinocchio/joint.hh>
+#include <hpp/pinocchio/joint-collection.hh>
 #include <hpp/pinocchio/urdf/util.hh>
 #include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/fwd.hpp>
@@ -320,7 +321,7 @@ BOOST_AUTO_TEST_CASE(spline_optimization_obstacle) {
   ProblemPtr_t problem = Problem::create(robot);
   PathValidationPtr_t pv(continuousValidation::Dichotomy::create(robot, 0));
   problem->pathValidation(pv);
-  pinocchio::Model obstacleRModel;
+  hpp::pinocchio::Model obstacleRModel;
   hpp::pinocchio::GeomModelPtr_t obstacleModel(new hpp::pinocchio::GeomModel);
   hpp::pinocchio::GeomDataPtr_t obstacleData(
       new hpp::pinocchio::GeomData(*obstacleModel));