scurve trajectory planner

Aciera
Offline
Administrator

20 Feb 2025 12:49 - 20 Feb 2025 13:06 #322057 by Aciera

Replied by Aciera on topic scurve trajectory planner

See my last post, looks like dogleg is not solving for gamma11 and gamma21.

[edit]

Just checked and it does seem to be adjusting gamma11 and gamma 21 but it always ends up being zero which does not quite look right to me.

Last edit: 20 Feb 2025 13:06 by Aciera.

The following user(s) said Thank You: Grotius

Please Log in or Create an account to join the conversation.

Aciera
Offline
Administrator

20 Feb 2025 13:14 #322060 by Aciera

Replied by Aciera on topic scurve trajectory planner

So the gamma11 and gamma21 depend on the start values, however there is another problem.
I'm running single_clothoid_fit and I changed kappa10 from 0 to 0.1 and now everything is off:

The following user(s) said Thank You: Grotius

Please Log in or Create an account to join the conversation.

Grotius
Topic Author
Away
Platinum Member

20 Feb 2025 13:23 #322062 by Grotius

Replied by Grotius on topic scurve trajectory planner

@Arciera,

I found the buggybug.

. Was not alien obstruction.

Glad in eq14 we can choose 2 interpolation models:
1. eq14_drift (archived)
2. eq14_gauss

It turns out nr. 1 works good and shows correct clothoid compound.
So nr.2 contains a buggybug. Will try to find it now.

The following user(s) said Thank You: Aciera

Please Log in or Create an account to join the conversation.

Grotius
Topic Author
Away
Platinum Member

20 Feb 2025 13:30 #322063 by Grotius

Replied by Grotius on topic scurve trajectory planner

@Arciera,

Yes, kappa10 is start curvature in z direction.

Gamma11 = y11, is the rigid property of the clothoid. if 0, then straight line. When 0.001 then clothoid begins to curve in a direction depending the sign off y11.

and now everything is off:
Keep also an eye on the GNU plot display max values. "set xrange [-20:80]\n"

The following user(s) said Thank You: Aciera

Please Log in or Create an account to join the conversation.

Aciera
Offline
Administrator

20 Feb 2025 13:46 - 20 Feb 2025 14:01 #322065 by Aciera

Replied by Aciera on topic scurve trajectory planner

We also might want to call it a fail if max_iterations have been reached:

Solution found after 1000000 iterations: gamma10 = -0.000148, gamma20 = 0.000011, s1 = 66.624893

Strange that it thinks it has succeeded when the residual is still way off (looks like its happy to have found a local minimum or something):

Last edit: 20 Feb 2025 14:01 by Aciera.

Please Log in or Create an account to join the conversation.

Aciera
Offline
Administrator

20 Feb 2025 14:22 #322070 by Aciera

Replied by Aciera on topic scurve trajectory planner

Is there a particular reason for using 'Multidimensional Minimization' instead of 'Nonlinear Least-Squares Fitting' which would contain the dogleg method?

// Set up the Nelder-Mead solver
const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2;
gsl_multimin_fminimizer *s = gsl_multimin_fminimizer_alloc(T, 3);

The following user(s) said Thank You: Grotius

Please Log in or Create an account to join the conversation.

Aciera
Offline
Administrator

20 Feb 2025 14:25 #322071 by Aciera

Replied by Aciera on topic scurve trajectory planner

reading this might explain why it is returning a local minimum:
www.gnu.org/software/gsl/doc/html/multimin.html

So in my opinion we are using the wrong solver.

The following user(s) said Thank You: Grotius

Please Log in or Create an account to join the conversation.

Aciera
Offline
Administrator

20 Feb 2025 14:33 #322073 by Aciera

Replied by Aciera on topic scurve trajectory planner

I'm way out of my depth here but this may help:
www.gnu.org/software/gsl/doc/html/nls.ht...east-squares-example

The following user(s) said Thank You: Grotius

Please Log in or Create an account to join the conversation.

Grotius
Topic Author
Away
Platinum Member

20 Feb 2025 15:02 #322075 by Grotius

Replied by Grotius on topic scurve trajectory planner

Hi Arciera,

So in my opinion we are using the wrong solver.
Haha yes indeed we now use : Nelder-Mead simplex method

For now this solver prooved our codebase works ok.
Maybe we can switch solvers with a flag.

Then we can also measure different solvers performance time and choose the best one.

We will add a new dogleg solver soon, as libdogleg was kind of problematic. Github repository
also say's better to use seres solver.

The gauss now works without junction flaws, error solved in eq14_gauss.
It was kind off trickie error. It needed the theta, kappa, sharpness at the given distance.
This in combination with the gauss method, is tricky.
Result:

The drift vs gauss function in the example without fit method:

github updated

The following user(s) said Thank You: Aciera

Please Log in or Create an account to join the conversation.

Aciera
Offline
Administrator

20 Feb 2025 15:17 #322076 by Aciera

Replied by Aciera on topic scurve trajectory planner

Why not try the dogleg solver in the GSL library?
Playing around with one of the examples linked above, it looks like we could possibly run 'levenberg-marquard', 'dogleg' and 'double dogleg' and '2D subspace' methods with our function and the Jacobian:

  /* define function to be minimized */
  fdf.f = func_f;
  fdf.df = func_df;
  //fdf.fvv = func_fvv;
  fdf.n = n;
  fdf.p = p;
  fdf.params = &params;

  /* starting point */
  gsl_vector_set(x, 0, 6.0);
  gsl_vector_set(x, 1, 14.5);

  fprintf(stderr, "%-25s %-6s %-5s %-5s %-13s %-12s %-13s %-15s\n",
          "Method", "NITER", "NFEV", "NJEV", "Initial Cost",
          "Final cost", "Final cond(J)", "Final x");

  fdf_params.trs = gsl_multifit_nlinear_trs_lm;
  solve_system(x, &fdf, &fdf_params);

  //fdf_params.trs = gsl_multifit_nlinear_trs_lmaccel;
  //solve_system(x, &fdf, &fdf_params);

  fdf_params.trs = gsl_multifit_nlinear_trs_dogleg;
  solve_system(x, &fdf, &fdf_params);

  fdf_params.trs = gsl_multifit_nlinear_trs_ddogleg;
  solve_system(x, &fdf, &fdf_params);

  fdf_params.trs = gsl_multifit_nlinear_trs_subspace2D;
  solve_system(x, &fdf, &fdf_params);

  gsl_vector_free(f);
  gsl_vector_free(x);

  return 0;

The following user(s) said Thank You: Grotius

Please Log in or Create an account to join the conversation.

Time to create page: 0.151 seconds

scurve trajectory planner

Attachments:

Attachments:

Attachments:

Attachments: