Extra loops in a u-turn case

[jcsantamaria]

Hi Maintainers,

I found a u-turn case that generates extra loops during a u-turn case:

`

// construct cc dubins engine
CC_Dubins_State_Space cc_dubins_forwards_ss(0.699249625, 0.0363610275, 1.0, true);

// initial start
steer::State start(0.0, 3.0480000972747803, 3.1415927410125732, 0.0);
steer::State goal(0.0, 0.0, 0.0, 0.0);

std::vector<steer::State> path = cc_dubins_forwards_ss.get_path(start, goal);

`

The resulting path has extra loops:

I found out that removing the twopify call at line 48 of hh_cc_state_space.cpp fixes the issue.

double delta_min = 0.5 * pow(kappa, 2) / sigma; //twopify(0.5 * pow(kappa, 2) / sigma);

The resulting path with this modification is:

It appears that delta_min should remain above 2pi for some combinations of kappa and sigma. The twopify() function brings this value unnecessary down (inside the [0,2pi) interval) and messes up some conditions inside HC_CC_Circle::cc_turn_length().

I hope this helps.

[jcsantamaria]

Hi, any comments on this? I can create a pull request with this change, but I would like to know if this is an appropriate fix. Thanks!

[hbanzhaf]

Hi Juan, Thank you for your feedback. Your issues are definitely on my list. Unfortunately, I will not be able to work on this issue before the end of May. So you might want to keep working with your modifications until we can clarify the problem. Best regards, Holger

[jcsantamaria]

Hi Holger, Thank you for your reply. Just to let you know I am happy to help and contribute. If you can point me in the right direction about the purpose of TiST and TeST I may be able to discern if my modifications makes sense and submit a pull request. :-)

[hbanzhaf]

The difference between TiST and TeST is shown in Fig. 10 in [1]. The family TeST denotes the case where the tangent between the start and goal circle runs parallel to the line connecting both circle centers (shown on the top of Fig. 10), and the family TiST describes the case where the tangent crosses that line (shown on the bottom of Fig. 10). Let me know if you need any additional information.

[1] T. Fraichard and A. Scheuer, "From Reeds and Shepp's to Continuous-Curvature Paths," in IEEE Transactions on Robotics, 2004.

[ktahiliani3]

Hi Holger, Any update on this issue? Thanks, Kapil

[jcsantamaria]

Hi Holger, here are some additional insights. It appears that the formula for computing sigma during an elementary path needs some guards. In certain cases (for example this one) the sigma computed using the formula when in elementary path condition results in a sigma value way beyond the maximum, which yields an unfeasible trajectory. I hope this helps.

[hbanzhaf]

Hi Kapil and Juan, Yes Juan, you are right. As stated in [2], a unique elementary path can only be found if the deflection is smaller than 1.462595458 pi. This needs to be added to the implementation including a fallback to a regular/irregular CC turn in case the elementary path can not be computed.

[2] A. Scheuer and T. Fraichard, "Continuous-Curvature Path Planning for Car-Like Vehicles," in IEEE/RSJ International Conference on Intelligent Robots and Systems, 1997.

[hbanzhaf]

The implementation has been extended (f3564e2ad53259485e7eebe91d674d211783be61) such that it can now also cope with large minimal deflections (as is the case with your parameterization). This should remove the issues that you described above:

[ktahiliani3]

Thank you for the update! This really helps.

[jcsantamaria]

Hi Holger, Thank you for the update. The fix solves issues #3 and #4. However, we found a new issue with extra loops. I'll post it shortly.