Optimisation error

[MutexMakhano]

In your dubins base functions e.g. dubins_LRL(), you are outputting for 'out' either [angle,distance,angle] or [angle,angle,angle] depending on whether it is CSC or CCC respectively.
In your cost function, dubins_shortest_path(), you are just adding these outputs together, irrespective of their units. It is not clear that you can add an angle (in radians) to a distance and have something meaningful. This appears to be an error.