Assert in ReedsSheppPath

[jmirabel]

The following assert does not pass

hpp-core/src/reeds-shepp-path.cc

Line 591 in f367db0

assert (fabs (extraLength_ + currentLength_ - paramRange ().second) < 1e-8);

when building a ReedsSheppPath with the following parameters:

• init: 0, -1, 1, 0, 0, 0, 0.14999999999999999, 0.20000000000000001, -1.3400000000000001, -0.20000000000000001, 1.9399999999999999, -1.5700000000000001, 1.3700000000000001, 0, 0.029999999999999999, 0, 0
• goal: 5.7362982238346234, 9.4465579648718965, 0.98636702396464049, 0.16456030516238818, 0, 0, 0.1931115704789346, 2.4512899638467114, 0.64483866454209959, -2.5676909543696151, 2.238067950490557, -1.0640813964560949, -0.80595092889081654, -1.8540772172091715, 0.019575152211624737, -0.017281600618040516, 0.62502857597837558
• robot: tiago
  • rho: 0.20219999999999999
  • no wheels
  • xyId, rzId: 0, 2
• constraints: locked joint on the two wheels (dof 4 and 5).

[jmirabel]

The extra length is 2.7715421813461121

[jmirabel]

In the assert line:

• extraLength_: 2.7715421813461121
• currentLength_: 11.981989169651751
• paramRange (): { first = 0, second = 25.688923795498006 }