scurve trajectory planner

@Arciera,

Thanks for all the work!
Pff. If you don't know what you are doing. It's kind off keeping the patience for a long long time.

updated source
I removed the dogleg dependencies for now. it's not used at this moment. Later on will activate it again.

To know more about eq44 and how it works, i started at your item 7, and used not the dogleg, but a initial guess value.
Then to item 8.

It seems now to plot a clothoid 4 compound in 3d.
I used eq.14 as a startpoint to get succes.

Using xy angle 45 degrees at start.
Using z angle 45 degrees at end.


Basicly i understand how the dogleg algo works.
It run's a given function until the function result is within treshold, <1e-6.
It uses one or more variables with a initial guess value. It also updates a inverse jacobian "J" in the iteration, wich
i think we need, but not for sure.

However, if you compute function eq44_G123, the function error is alway's near absolute zero.
As a result of that, the dogleg solver does nothing at this moment.

1. Basicly eq44 is not ok. It should return a error value, then dogleg can optimize.
2. I don't understand yet, how to get the clothoid compound to fit the given end point.

And for info. I did not use any fresnel header file, fresnels are not used, maybe also wrong.

Basicly eq44 is not ok. It should return a error value, then dogleg can optimize

I guess the error is in the end position (se)

I don't understand yet, how to get the clothoid compound to fit the given end point.

In my opinion that is what the dogleg is supposed to work with:
 

subtract the right side from the left we get 3 equations :

x_traj(s0) + integral_term - x_traj(se) = 0
y_traj(s0) + integral_term - y_traj(se) = 0
z_traj(s0) + integral_term - z_traj(se) = 0

However I don't know how to calculate the integral_terms in the algorithm.


Also we need dogleg to solve for 3 unknown variables using the system of 3 equations above which it apparently can do but I have no idea how to set that up.

Looking at the readme in the dogleg library:
github.com/dkogan/libdogleg?tab=readme-ov-file

It seems that we might want to start with 'dogleg_optimize_dense' or 'dogleg_optimize_dense2' as these calls don't expect a Jacobian.

 

'p' would be a vector with 3 elements [gamma_1,1, gamma_2,1, s_1] containing our initial guesses when calling and the optimized results in the end.

'Nstate' = 3

'f' would be the system of 3 equations in the last post. This is where I'm stuck at the moment.
 

@Arciera,

Thanks for your responce.

Indeed, the error is likely the endpos. I was thinking the same yesterday night.

error = distance "requested endpos" - "guess endpos"

When playing with different Y11, 21 gues values using the same length s1:
Changing the guess values Y11, Y21 will influence the rigid of the clothoid compound.

Picture values from left to right :      
Y11=1 Y21=1
Y11=0.5 Y21=0.5
Y11=0.1 Y21=0.1
 

Now i will try to setup the dogleg method. Using our new criteria.

The Jacobian usage seems to be more effiecient in finding the solution. But without is also no problem and
will result in more iterations to find the fit solution.

Your point at eq44 if solved here as a example : eq14
This eq14 is not a optimized function yet.  And for calculating the clothoid endpoint, i want to make a
few one liners today.

Plan :
1. Create a function to calculate the spline endpoint efficient, using y11, y21, s1.  Eq14 current code is not effiecient for this.
2. Create a Dogleg fit function to optimize y11, y21, s1 values that the spline compound fit's the given endpoint. (Magic function).

For info, to be aware off:
In linuxcnc we use G2,G3 as helix. This abstact isn't taking account of helixes. Only flat arc's for input,
We will create a solution that uses a end vector of any type of curve.

For info:
Deepseek is better in reading pdf files than Chatgpt.
Also the code example from Deepseek are better.
So i often just upload the clothoid_3d.pdf to deekseek and start asking questions.
This really helps the process. Deepseek just throw's out a full dogleg c implementation. It's amazing.

Let's hope the Magic Function will find the solution for endpos.

yes, eq 14 :
 

because we set s1=s2=s3=s4 we need to integrate from 0 to 4*s1.

It's still unclear to me how dogleg optimizes a system of 3 equations as it would not get back a single residual value but rather a vector with 3 residual elements.
 

I guess we could simply calculate the length of our 3D residual vector and pass that back as the error value to be minimized.