scurve trajectory planner

Arciera is hard working to try out new non-linear solvers

I'm afraid that is a bit of an overstatement, I have no experience with this and also not really the time to poke around in dark any longer. This requires somebody who actually knows what they are doing, at least somewhat.

What is needed is a robust numerical solver and then the next step is the max error fitting described in the paper.

Ceres template for our codebase

Ceres example

@Hi

There are now different solvers in the codebase, in c and c++.

Performance:
 
Test object:
 

Grotius wrote:

@Hi

There are now different solvers in the codebase, in c and c++.

Performance:
 

Attachment not found

Test object:
 

Attachment not found

that's a huge difference between the c/c++ but I imagine your using libraries for the c++ which are usually highly optimized for performance, but WOW. 

Hi Collins,

The ceres library is likely the Ferrari of the non-linear-solvers. It's given as a present from google.
The ceres_capi is in fact wrapped around c++.

Today i added algorithm1, page 9 from the abstract. Wich is to find the closest point to the clothoid, given a point in space.
This one also uses the ceres solver.
 

Tomorrow we start doing algorithm2, wich calculates max distance from gcode trajectory to clothoid.

Hi,

Related to the 3d clothoid corner fitting i had to do some extra programming work.
In the abstract a example is given for arc and line :

 
This involves eq30 page 5.
There it calculates theta, kappa, sharpness at a given arc point. So far so good.

However, sinds linuxcnc uses arc & arc-helix solutions given a G2 or G3, above eq30 is not valid to integrate in linuxcnc.
The clothoid should not fit ok on a helix. What now?

To make it valid, we have to code the theta, kappa and sharpness for a helix at a given helix point.
Then the 3d clothoid can be attached to the helix flawless.

I made a tiny picture to show the 3d helix properties, and what we are dealing with :
 

The detail is also that the 3d clothoid input requires theta_z and theta_xy input's.
These are derived from the tangent T in the picture. Wich is a tangent decomposition.

All this code is done from scratch and vector based. In this we are not limited to G17, G18, G19 planes.
Soon we can validate the helix code output.

Hi,

I had some real problems related to the clothoid fit.

It turned out eq.45 in the abstract is partially wrong.
In the abstract page.7 there is stated : DeltaTheta = Thetai4 - Thetai0.

Now this is most off time ok. But not alway's ok. The problem lies in the way DeltaTheta is calculated.
The input theta's are in range [0,2*pi], [0,-2*pi], derived from atan2(x,y). This are results of decomposed 3d vectors,
as the clothoid uses inputs for xy and z seperatly.

eq.45 Delta Theta [dt] uses - or + for direction. This is ok, so we know we must use - and +.
Simply substracting the theta's may lead's to incorrect values, wich results in a self intersecting clothoid for example,
caused by a extra + or - pi rotations.

I had to think a while how to solve this problem. To solve it, i calculated the theta's back into xy vectors and then
calculated the signed angle between these two xy vectors. Here is the solution in code.
So far above problem seems to be solved now.
I send a email to the abstract creator's and informed them about the problem.

Furthermore, the exact path deviation is used to fit the clothoid.

In total for one clothoid fit, ceres non-linear solver is used 3 times.

1. Ceres finds the clothoid xyz endpoint, given the 3 unknowns y11, y21, s1.
2. Ceres finds the closest point in space to the clothoid curve. (helper function to calculate path deviation).
3. Ceres finds the inperpolated points on curveA & curveB given the max path deviation.

The performance is about ~25ms to fit the clothoid.

Some fit clothoid curve fitting example's i used for validation :

// Line line fit.

// Arc line fit.

// Arc arc fit.

// No deviation is used here.

Hi,

At this point we can use the clothoid_3d.so library with the scurve motion planner.
What looked like a mission impossible to me, is succeeded in a functional 3d clothoid library.

I brewed an idea how to integrate this chunk of software into linuxcnc's motion planner.
Because one clothoid fit takes ~25 servo cycles we have to use an old men's trick.

1. We put the clothoid fitting algo in a seperate thread.

2. We need to have a modified segment buffer, wich request new segments from the interpreter when at ~10 segments of the buffer end.
If we don't do this, the machine will pause in the program for a short while, when recieving a new gcode load. We must avoid this.
We have to look how linuxcnc behaves when giving us the very last segment of the program.
I hope it will not stop the program directly but wait's until we are ready.

3. I think also to create some kind of segment ringbuffer. Wich is usefull for motion reverse. And usefull to avoid gcode dump wait periods.

For info:
Julian has done work on a back-lash solution using scurve motion. This is scurve on top of scurve, wich works!

I actually registered here to say: Wow, really impressive work with the trajectory planner! looking forward to see it evolve.

We put the clothoid fitting algo in a seperate thread.

Creating a lockless, thread-save producer-consumer queue has a lot of pitfalls, I would recommend looking at this implementation  Lock-Free Single-Producer - Single Consumer Circular Queue    or in C  

 Besides that can I ask if the points raised by hmnijp ( smoothed polyline segmentation causing deviations, page 3) are addressed with the use of clothoid-curves? I guess to really have jerk limiting you not only need to be smooth in the time domain but also in the path, thus the use of clothoid(?)

@Konrad,

Thank you!

Besides that can I ask if the points raised by hmnijp ... are addressed?
A few are adressed now.

The algo is still using local smoothing. as shown on page 3.
But now it uses a 3d-clothoid instead of a spline, and uses the correct path deviation to fit the clothoid.

I guess to really have jerk limiting you not only need to be smooth in the time domain but also in the path, thus the use of clothoid(?)
1. The 3d clothoid fit's the trajectory with G3 continuity, as explained in the abstract.
2. Then we have scurve motion profile, wich is used for jerk limiting.
3. Then the trajectory can use different algo's for path optimalisation, like flattening out a zigzag motion. This will also contribute to jerk.

The ringbuffer is working. And to my surprise i did'nt need a seperate thread so far to fit clothoids.