test separation branche temporaire pour faire la refonte du splinePath et de son optimisation
Minimisation distance spline avec evitement d'obstacle. Ajout de la forme finale des couts/gradient. Reste a remplir les calculs de distance min et deriv
Ajout des tests temporels ( ET dans problem solveur) plus log du cout et calcul jacobienne distance, compilation passed
no error at execution, without obstacle almost same result that basic cost, why ?)
Implementation of SplinePath and its optimization.Code Clean. TODO: fix infinite time optimizing with collision avoidance