scurve trajectory planner

github.com/dkogan/libdogleg

Dogleg runs.
You basicly provide the equatation in a callback function. Then it iterates until result is within treshold.

Startvalue a=1
Startvalue b= 1
Increment a= 1
Increment b = 1

10 = 3 + a x b

This will do
result = left hand - right hand
0 = 10 - 3 + 1 x 1
Finally
0 = 0

Hi,

At this stage, we are able to plot something with gnu plot.

In the main.c there is a 2d & 3d example.

When having problems, i decided to revert to implement the 2d solution from eq.46, to get a more simpler overall calculation.

To focus on the 2d solution. The plot is not as should be.
It plots 4 connected clothoids. But the start angle of upcoming clothoid looks to be inverted.
The clothoid compound start, end angle seems to be ok. Start at 45 degrees, end at -45 degrees.

This 2d examples does not focus on the transformation equations. As these transformations can be seen completely seperate.
And it does not use the dogleg solver. It turns out without dogleg solver, initial guess values are most of time ok.

For 2d, there are 2 unknown variables. y21 & s1. In the sample i set a value for y21=1 & for s1=1, this seems to work ok.

Reviewing my code would be appreciated.

updated source
Here is the 2d plot.


 

@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.

@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.