Trajectory Planner using Ruckig Lib

I just removed everything over c++17 so hopefully you're able to compile it without any problems now. I also added a comment to stress_test.cpp that explains how to actually use the solution that is generated. It's updated on github.

I also just changed the clothoid project to c++17 and added a type biclothoid_t to make things simpler.

@Hi Andrew,
Thanks for your efforts so far.

The stress test runs ok.

Now i would like to print the current velocity over the total periods.
I am able to get the periods from the *sol.
Can you tell me how to get curvel given a time 0 < ttot ?

I hadn't had a need for that yet. I just added it, but I haven't had a chance to test it.

github.com/a-downing/scurvy/commit/938b7...a75392ee1d14c722bf5f

Edit: Oops it's definitely wrong, let me fix it quick.

Edit: Should be good now: github.com/a-downing/scurvy/commit/f271a...40af0f51734534134e3f

Hi Andrew,

Everything is ok now.  Both libs compile as a charm!

The scurvy runs. Also a python graph is plotting after the matplotlib install.
I get the curvel at time printed. Nice !.
I was questioning myself, why is the "vt" print accepting "problem_t" in the function instead of " solution_t" ?

For the clothoid library i see a "t1=0.5" value.I suspect "t1" is the size of the clothoid. And you are also working on the path deviation wich is the fitsize.
This has no hurry.

Goal tomorrow is to work on the clothoid interface, so we can retrieve 3d clothoids fillets from your master piece of code.





 

Ok, awesome.

period_t::vt() takes a problem_t because besides the T1-T7 time periods that belong to it's own class, it needs J, A, and D from the problem. There is another solution_t::vt() function for convenience that passes its solution_t::prob member to period_t::vt().

t1 is a value that ranges from 0 to 1. It defines the point on the first shape that the clothoid starts at. The start point is held fixed while the length of the clothoid is adjusted to make the end of the clothoid land perfectly on the second shape. That function will be used by a second algorithm that adjusts t1 to bring it closer to the end of the first shape to get the deviation between the clothoid and the path under the tolerance. I couldn't get the second algorithm in the paper to work, but I was able to figure something out myself.

The current issue is that that second algorithm depends on a function to calculate the "directed Hausdorff distance" between the clothoid and the original path. Basically what you would call what the P param of G64 is measuring. For now I've fudged it by just calculating the min distance between the path corner that's being blended and the clothoid. The distance that this finds isn't always close to the actual distance that's needed. I think this will take some amount of brute forcing. An alternative is to just not parameterize on the deviation and instead just start the clothoid a given distance from the path corner and let it deviate however far it needs to, which if I'm thinking right should always be quite a bit less than the distance from the corner.

I'll push what I've done so far to github.

Grotius and Anderw2085
Fitting biclothoids for g2 continuity is a great addition for jerk limitation.
Another method used for CAM generated gcode which has many small line segments (polyline) is to use a line to spline converter. 
In OpenCN they have alluded to this algorithm as the CompressCurveStructs in this documentation link:
mecatronyx.gitlab.io/opencnc/opencn/CNC_...-planning-algorithms

More elaborate discussion in Section 6.2.5 in this document: mecatronyx.gitlab.io/opencnc/opencn/CNC_...tric_Operations.html

The BSpline calculator from short line segments from the OpenCN project can be found at: gitlab.com/mecatronyx/opencnc/opencn-mat...Lee.m?ref_type=heads

This compressor can be turned on and off with a parameter so it is not necessary to keep it on all the time. 

Linuxcnc has an offline compressor called g1tog23.py detailed in this link: wiki.linuxcnc.org/cgi-bin/wiki.pl?LinesToArcs.
This can be used to compress small line segments to lines and biarcs. It uses Douglas–Peucker algorithm and a biarc interpolator for the compression. The compression is done offline though.  

Regards,
automata

I was thinking B-spline smoothing would be good for what I'd call global smoothing where multiple g-code moves are replaced by a single spline segment. The clothoid stuff would be better for just smoothing motion between g-code moves where you don't eliminate any altogether except for maybe simple pre-processing where colinear lines are combined.

I started adding a path solver to the scurvy project where you give it a list of segments with constraints (j_max, a_max, d_max, v_max) to solve. The initial velocity of the first segment and final velocity of the last segment are respected and the initial and final velocities of the interior segments are adjusted to make the solution possible while respecting the other constraints. It's not perfect or efficient yet, but it's a start.

Grotuis, I'm not sure if you already solved this with the s-curve stuff you already made.

@Automata,

Thanks for your input. I looked at your links, will remember these.
Was also testing a 3d b-spline today, to try over the linuxcnc splash gcode. Somehow it fails at input point 33.
Will go on with this at another point in time as i have to work on the clothoids now.

@Andrew,
Yes the clothoid integration is most important now.
This will take me some effort now.

I have not been able to work on your scurvy any further. But i like to see you doing this.
If you have a complete solver, it will be better and faster to use and or test in the planner.
Once you are ready, we can make a trajectory component to test your planner.
It's a benefit to have more than one scurve library.

Grotuis, I'm not sure if you already solved this with the s-curve stuff you already made.
I made a scurve project, its for jogging & auto mode.
link to sample picture
performance vs ruckig

 

@Andrew,
I am looking also to other clothoid library to test some of it's classes.

@All,

I was totally new to clothoids. Never heard of them. I want to show you little info how they are used.

Here we show a test for line - line fillet using 3 clothoids (Class G2solve3arc) that are inter-connected. Link to lib.
The library used is big. You have to link 3 libs in total to your project.

The clothoids use a startpos & endpos in 2d. This are just x,y coordinates.

They also use a thing called "kappa" to specify begin and end curvature. So when connect to a line, you
must end with zero curvature as the line is also zero curvature.

The kappa "k0" is the curvature at the begin.
The kappa "k1" is the curvature at the end.

The kappa or curvature for a line = 0. In this example the kappa "k0" & "k1" are both zero.
For connecting to a arc, jou must end the clothoid matching the arc curvature or kappa,
arc kappa = 1/radius. (quite easy to find out)

Then they have a direction for startpos & endpos. This is calculated in 2d by a atan formula, result is in pi radians.

Info needed to generate a tri-clodhoid fillet:

The goal of this line-line test : solve the G2 problem.


documentation