6th-order jerk-controlled motion planning

Danal

@djdemond said in 6th-order jerk-controlled motion planning:

In the spirit of being conciliatory a lot of this comes down to the age-old debate that was had when auto bed-levelling first came about. We seem to be in two camps, those with stiff, precise machines which don't need it, and those with less solid machines (or less experience or resources) who do need it.

And the third set: Those who have nice, stiff, precise machines that know that their bed is not in the EXACT same place after it has been removed and replaced (delta), or who have verticals long enough for room temperature to have at least some effect (XL to XXL delta), and who therefore run a basic G32 at the beginning of every print.

Yeah, there is a FANTASTIC parallel here with auto-bed-level. Much of the 'meat' of that parallel being those who state that Auto-Level it is for those with imprecise mechanics ONLY... phrasing that contains more than a little snobbery.

Danal

@acmeanvil said in 6th-order jerk-controlled motion planning:

However, this thread is a mirror of where the machine tool industry found itself in the 80's. Trajectory planning, ball screw compensation tables, etc. were the new and bleeding edge, and the same arguments were had. Long story short, we take these things for granted now.

YES YES YES

DjDemonD

@danal said in 6th-order jerk-controlled motion planning:

@djdemond said in 6th-order jerk-controlled motion planning:

In the spirit of being conciliatory a lot of this comes down to the age-old debate that was had when auto bed-levelling first came about. We seem to be in two camps, those with stiff, precise machines which don't need it, and those with less solid machines (or less experience or resources) who do need it.

And the third set: Those who have nice, stiff, precise machines that know that their bed is not in the EXACT same place after it has been removed and replaced (delta), or who have verticals long enough for room temperature to have at least some effect (XL to XXL delta), and who therefore run a basic G32 at the beginning of every print.

Yeah, there is a FANTASTIC parallel here with auto-bed-level. Much of the 'meat' of that parallel being those who state that Auto-Level it is for those with imprecise mechanics ONLY... phrasing that contains more than a little snobbery.

Agreed. I did (shamefully) not also mention that there are many cases (deltas and large machines where flat beds are not achieveable) where probing is an extremely useful tool and not a compensation for a lack of precision or rigidity.

But the comparison still stands.

deckingman

On the topic of building a mechanically sound machine, there seems to be a consensus that it's difficult and/or expensive to achieve. To be clear, I know nothing about Deltas, so they may be a special case. But for Cartesians or CoreXY, it really isn't difficult or expensive and in fact you can save money.

To illustrate.......

T3P3Tony and DC42 will vouch for the fact that I took the top off my machine, stuck the whole thing in the back of my van, bumped it up the motorway for about 50 miles to the TCT show, took it out, put the top back on, made a slight adjustment to the bed because it was about half a mm low at the back (due to being bumped about in the back of my van) turned it on and checked that everything worked (which it did). Then for the rest of the show, at the start of each day, I turned it on, picked a file, hit print and left it alone. Without doing anything apart from homing, it spent 3 days making 5 colour objects for staff on the E3D stand (which was a pretty cool thing to do) and one that went to museum in Switzerland. I did the same in reverse after the show only this time I didn't have to adjust the bed level when I got home as it was spot on, and I haven't adjusted it since (that was last September ish)

I'm not trying to boast or brag. I say all that just to illustrate that my machine is pretty robust. However, I don't have access to any fancy machining or fabrication facilities. I'm just an old guy pottering about in a spare bedroom. The frame is all stock V-slot extrusion and the mounts, plates, gantry parts etc are all printed PLA parts (it's a RepRap as far as possible).

Of course, I've been making things and fixing things all my life and I sometimes forget that there are people who don't know which end of screwdriver to hold and which end goes on the screw. I don't mean that in a disparaging way. I'm sure many people would laugh at my feeble attempts to write code - it's just a different skill set. So I apologise for forgetting that some people just don't have the skills.

But if you have a modicum of DIY skills, it really isn't difficult or expensive to build a printer that needs very little in the way of software compensation and it can actually be cheaper. Plus it's fun

For example, putting the frame together. Say you want to join a horizontal cross member to a vertical leg.You can use 90 degree cast corners at about £1.50 a pop with a couple of T nuts at about 25p each plus a couple of bolts but the connection can easily be twisted. (Those are the cheap brackets btw - you can pay £3.00 or so for hidden ones). Alternatively, you can tap the ends of the cross member. The extrusion already has holes that will take an M5 tap. Then drill clearance holes for a hex key in the leg (I did all mine by hand with a cordless drill) and simply use two button head screws (assuming 2040 section), slide the heads into slot and tighten. You'd need the screws anyway for the bracket. So you have a much better connection and save £2.00 each time plus it looks a lot better. If all you have is 4 rails at the top and and 4 at the bottom that's still 16 brackets (one at each end of each rail) so £32 saving and you've got a much more robust connection. If you can't tap the holes, you could pay 50p a hole and still save money. OK so it take a bit longer but ones own time is generally free.

Another example where you could potentially negate the need for any compensation and save money at the same time. If you are considering automatic bed levelling using 3 (or God forbid 4) screws and multiple Z motors, consider just using a single motor and a continuous belt. If you use automatic bed levelling the software will probe the bed and adjust the screws by turning the motors. Just think about that - software is going to turn a screw for you. Every time you remove power from the motors, they'll jump to the nearest whole step (actually I think it's worse than that) but anyway, the motors will get out of sync. So every time you power on the printer you will have to check and adjust the level (by turning each screw). So why not use a single motor and continuous belt? To level it, you position the hot end near a screw (I put slip gauge under the nozzle), and move the bed until it just touches. Then move the nozzle close to another screw, slacken a grub screw holding the pulley to the screw, and turn the screw until the bed just touches. Repeat for the other screw(s) and maybe recheck the datum, and that's it. Job done - for ever (well almost). That's what I do. Nothing special. I did throw a thrust bearing into the bottom of the hole that takes the screw but the mounts are just cheep plastic printed parts. So no need to run auto levelling and I save the cost of two steppers, wiring connectors, etc.

As for a flat bed, well that will cost a bit more but not much. Generally people use a heat spreader which is usually aluminium. So if it's 3mm thick, throw it away and use 6, 8 or even 10mm tooling plate. Or if you are building from scratch, buy that thickness instead of the thin stuff. Yes it costs a bit more but not all that much - nothing like say the cost of a Duet board . But then that's it - no need to run mesh compensation and no need to probe the bed at multiple locations - ever. So you could probably save some of the cost of the plate by not having such an expensive probe.

I could go on, but I think I've illustrated that it really isn't difficult or expensive. I started out with RepRap Pro Mendel which needed all sorts of compensation applied - in fact there was no mechanical means of levelling the bed. So I know how much nicer it is just to pick a file and hit print without any faffing about. Printing a 3D object is literally as easy as printing a document on my PC (although it take longer of course) but it wasn't difficult or expensive to get there.

Dougal1957

@deckingman Well said Ian and my CoreXY when finished will be the same and I hope will not need any of the trickery apart from bed levelling as I plan on using independent driven leadscrews just cos it is easier to achieve on my build but using 3 leadscrews driven by a looped belt would work just as well,

Doug

Phaedrux

@deckingman Sometimes even best attempts at building a solid printer from the start end up missing the mark. Case in point, I spent a fairly exorbitant sum of money to obtain a 6mm thick MIC-6 aluminum tooling plate to use as my bed. Not easy to get in Canada. Along with the plate I got a sheet of PEI and silicone heater. I bonded the three together and mounted with a 3 point leveling system. And after all that planning and expense, I found out that the plate had been bent during shipping (I assume). You couldn't tell by eye that one corner was raised about 0.2mm, but you could tell from the failed leveling pattern prints and the mesh grid compensation map showed it clearly.

My options were to buy a new plate, PEI, and heater, or just use mesh bed compensation for free.

deckingman

@phaedrux said in 6th-order jerk-controlled motion planning:

@deckingman Sometimes even best attempts at building a solid printer from the start end up missing the mark. Case in point, I spent a fairly exorbitant sum of money to obtain a 6mm thick MIC-6 aluminum tooling plate to use as my bed. Not easy to get in Canada. Along with the plate I got a sheet of PEI and silicone heater. I bonded the three together and mounted with a 3 point leveling system. And after all that planning and expense, I found out that the plate had been bent during shipping (I assume). You couldn't tell by eye that one corner was raised about 0.2mm, but you could tell from the failed leveling pattern prints and the mesh grid compensation map showed it clearly.

My options were to buy a new plate, PEI, and heater, or just use mesh bed compensation for free.

Well for sure things go wrong. This may sound harsh but the lesson is to check things when they arrive. OK, you may not have a straight edge so...........

Actually, you could have fixed it for (almost) free. I'm assuming that you fix the plate to some sort of frame. In which case, you could use some semi rigid packing (under floor heating insulation panels would work well) between the plate and frame. Something that can be compressed ever so slightly. Then, by altering the tension of the screws that fix the plate to the fame, you'd be able to straighten it. You'd need to play around with the fixing screw positions maybe but it'd be possible.

There is almost always a mechanical solution to a mechanical problem.

deckingman

@dougal1957 said in 6th-order jerk-controlled motion planning:

@deckingman Well said Ian and my CoreXY when finished will be the same and I hope will not need any of the trickery apart from bed levelling as I plan on using independent driven leadscrews just cos it is easier to achieve on my build but using 3 leadscrews driven by a looped belt would work just as well,

Doug

Hi Doug,

I forgot, you were at the TCT show too.

Anyway, if my bed had been a bit bigger, I'd have been forced into using multiple motors too. The reason being that I'm using the longest continuous loop GT2 belt that I can find, and even then had to re-jig things a bit. (Damn - just realised that it doesn't have to be GT2 - could be any profile as long as the pulleys match).

I guess if you went seriously big, you'd likely need multiple motors to get the torque (or gearing). Mine (400mm x 400mm x 10mm) does OK in that respect with a single Nema 17 but I do use fine lead screws (1mm lead) which helps.

Larzarus

So, FWIW, I'll throw in my two cents. Three years ago I did some experiments with implementing s-curve / sinusoidal jerk in a Marlin based firmware (UM2) (I used a lookup table instead of calculating it). Let's see how much I remember...

I found it did not really improve print quality, at least not in any way I could see with the naked eye. But it did allow for faster usable movement without shaking the machine or wild oscillations. Especially non-printing moves, as my printer is mostly limited in printing speed by the extruder design. I was printing with good quality at 120mm/s, infill at 250mm/s, and non-printing moves in the 450-550mm/s range. Those are max speeds, but net average speed was within 80-90% of the max speed (depending on segment length mostly). I put up some videos (and a link to the code) here: https://www.youtube.com/watch?v=D9tGB-FtyJQ and a high speed non-printing movement / wobble test here https://www.youtube.com/watch?v=-L8scUiasVg (without S-curve, it was much more violent and that pen would fall over at less than half the max speed)

The main benefit of s-curves was I could use much much higher acceleration values. In theory I could get to those same speeds without s-curve, but I would have to keep the acceleration values low to prevent the machine shaking itself apart when it changed direction... but a high max speed with low acceleration doesn't help because the head would only reach those speeds on very long movement segments. The improvement to acceleration is what made those speeds achievable / beneficial, even on shorter movement segments.

550mm/s was pretty much the upper limit of my machine's design (belts, motor current, the rate at which the CPU could generate pulses, etc). Even then I had to make some dynamics optimizations to the Marlin movement stepper pulse generator code to move at that speed. But it would very very rarely skip steps. Like maybe once every couple hours; enough to mess up a print (it was an issue with the i2c comm interrupt blocking the stepper movement interrupts). Ultimately I think it came down to pushing hard against the limits of that 8 bit CPU. If I limited the speed to under 300mm/s, it was pretty rock solid.

I gave up on further code development when I switched to the Duet Wifi from the AVR based UM2 motherboard.

It's been three years since I did this code, so I've forgotten many of the implementation details, and I do not have the desire to debate. I am relaying my personal experience implementing this on a real world printer, with real world testing. I found the s-curve was well worthwhile in reducing oscillations, violence, noise, etc. and far less computational taxing than true 5-6th order calculations. I agree it should be optional.

I'll add that since I switched the the Duet and no longer use s-curve motion, I don't really miss it that much. Yes, it allowed faster net printing, but I'm still extruder-limited most of the time anyway.

carlosspr

@larzarus yours is a very interesting experience, which suggests me that there is still invaluable desirable effects on this S-Curve implementation. Beyond printing speeds, the hardware in the printers will be less exposed to wear and tear and the expected hardware life will increase. Also maintenance cycles will be lower. With less vibrations bolts will not become loose and steppers will not have to absorb the vibrations.

I have read the Movement code and it seems that DriveMovement.cpp also uses a trapezoid. There are references on the code to an "Acceleration phase", " Constant speed phase" and "Deceleration phase". In an analogy with TinyG implementation, they make a sub-division of the same trapezoid into 5 segments: First and second halves of the acceleration ramp (the concave and convex parts of the S curve in the "head" or "Acceleration Phase" in Duets movement.cpp"). Periods 3 and 4 are the first and second parts of the deceleration ramp (the tail), or "Deceleration phase" in Duets Movement.cpp. There is also a period for the "Constant Speed phase".

I wanted to give it a go, but adding this feature requires far more knowledge of the actual implementation that I have and the leaarning curve will take time. I am willing to help, but I would need the guidelines of the code maintainters.

dc42

To do it properly you need to limit the acceleration and deceleration. So the acceleration and deceleration phases need (in general) ramp up, constant acceleration/deceleration, and ramp down phases. That's 7 phases per move.

Larzarus

I've got mixed feelings about the benefit for normal use. I was doing experiments to push the limits of my machine. If you routinely run your machines close to its limits (extruder, mechanical, etc), then yes, definitely worthwhile and 'something for nothing' (except programmer effort!). I used it daily for well over a year with good results, squeezing some extra performance from my rig. Switching to the DuetWifi, I dialed back the speed and accel, but for daily use I don't really I miss it much. Would I use it if it was there? Sure. But it's a fringe nice to have, not a core requirement.

In terms of nitty gritties, I kept the 'phases' the same in the Marlin motion planner -- the 3 phase trapezoid of accel, coast, dec. To implement the sinusoidal jerk, I injected some code into the pulse generator loop, where the accel phase had a function of speed = speed + accel (similarly speed = speed - accel for dec phase). I replaced the accel term with a fixed-point math lookup table of a pre-generated sine curve** (multiplied by accel). The table has an almost-linear section in the middle, effectively making for three blended sub-phases (net 7 motion planner phases, as dc42 is refers to).

This shaped the accel and dec sections of the trapezoid into the desired s-curve. I wouldn't say it's proper by any means, but it was achievable on the 8 bit cpu in terms of performance and without an entire rewrite of the motion planning codebase. A quick hack, enough to test if s-curves provided any benefit...It did result in smoother movement (less violence / oscillation and quieter), allowing me to increase acceleration, which increased net average speed per segment.

I'm sure dc42 has a very clear picture of what would be needed to implement it: certainly a more correct, complicated and involved effort than my tests. There is a definite benefit, for some, but is it worth the effort? Dunno.

** taking care that my pre-generated curve maintained the same 'area under the curve' integral as the instantaneous jerk / line accel would, so the resulting phase resulted in the same total number of movement steps (at least to the limits of the fixed point math the wimpy 8 bit cpu is capable of)

carlosspr

Hi, I am trying to do some progress with this. Las post from Lazarus was very clarifying and opened some ideas that I have been exploring. I would like to share with the forum the process to add some light to the maths behind the algorithm so that I can get closer to an implementation.

The first thing that called my attention from Lazarus' last post was the use of sinusoidal functions to generate an S-Curve. It triggered a lot of crazy ideas to use signal processing algorithms to work on the frequency domain, so I crushed the numbers. Unfortunately the sinusoidal waveforms do not meet the requirements.

Which are the requirements that the ideal S-Curve shall meet?:

It shall have acceleration=0 (dv/dt =0) at the joints of the trapezoid.
lt shall have jerk =0 at the joints of the trapezoid (dv/dt2 =0 ).
It shall fit on the control points to the trapezoid acceleration and deceleration phases.

Sinusoidal waveforms can fit the trapezoid and have zero acceleration on the joints, but will fail on jerk. The maths behind this: If we use sin(t) to generate the shape, its derivative cos(t) will start and stop at 0, but its second derivative -sin(t) will not. the following graph shows an example on a Spreadsheet of Velocity, Acceleration and Jerk using sinusoidal waveforms where jerk is celarly non-zero on the trapezoidal joints:

Sadly I had to abandon the idea to use sinusoidal waves and started looking at the Bezier curves, but realized that not all Bezier curves could be fit to meet the requirements and at least a fifth order Bezier curve is required is we want to assure jerk=0 and higher orders would ensure also snap crack or pop. Since Bezier curves can be derived to infinite, it is a matter of increasing the order. However the mathematical complexity is probably not worth to compensate some effects that are physically hard to understand (I see them as harmonics of low effect). With a fifth order Bezier we can express the velocity in function of time with the following formula:

V(t) = At^5 + Bt^4 + Ct^3 + Dt^2 + E*t + F

Where t is normalized time (from 0 to 1). And the challenge is to solve the weights (A,B,C,D,E,F). For this we apply the constraints

Constraint #1: dv/dt =0 at t=0 produces the first solution E=0.
Constraint #2: dv/dt2 =0 at t=0 produces the second solution D=0
So having E0= and D=0 warrants a jerk-free curve on the joints.
Constraint #3: fit the curve to start speed and top speed.

Constraint #4,5: force dv/dt=0 and dv/dt2=0 at t=1 produce other equiations that combined will solve to the final result.

  A = -6*initialSpeed +  6*endSpeed
  B =  15*initialSpeed - 15*endSpeed
  C = -10*initialSpeed + 10*endSpeed
  D = 0
  E = 0
  F = initialSpeed

On the Acceleration Phase
initialSpeed=startSpeed
endSpeed=topSpeed

On the decceleration phase
initialSpeed=topSpeed
endSpeed=stopSpeed

The same formula applies, but if startSpeed is different that stopSpeed, the coefficients need to be recalculated to fit the trapezoid properly.

The following graph shows the graphical representation of the S Curve, acceleration and jerk using the brute force calculation on a Spreadsheet of the formula (i used startSpeed=50 and topSpeed=3000):

So these are the maths and are not so complex once understood. Now the challenge is the implementation. For that I have been reading the implementation of Movement in ReprapFirmware (and I will have to read another dozen times before I get comfortable with it and attempt to do any attempt to modify anything), so far, what I have understood is the following (and the experts can correct me if I misunderstood the code):

DDA.cpp deals with the motion planning.
DDA::Init(): would require a call to a function that calculates the coefficients of the S-Curve (A,B,C,F) each time there is a new value for "Startspeed","topSpeed" or "endSpeed".

DriveMovement.cpp
DriveMovement::CalcNextStepTime() contains the loop that define the frequency of calls to the ISR interrupts in nextCalcStepTime.
This is defined as follows for a cartesian printer:

// acceleration phase
const uint32_t adjustedStartSpeedTimesCdivA = dda.startSpeedTimesCdivA + mp.cart.compensationClocks;
nextCalcStepTime = isqrt64(isquare64(adjustedStartSpeedTimesCdivA) + (mp.cart.twoCsquaredTimesMmPerStepDivA * nextCalcStep)) - adjustedStartSpeedTimesCdivA;

I am still having trouble to understand this (it is a big challenge for me!), but looks like it is calculated as a function of A (meaning Acceleration?). If this is the case, it would be a matter of adding an "if (trajectoryPlanning=bezier) { nextCalcStepTime = a_function_of_the_variable_S_Curve_velocity_in_realtime}.

[the if... would allow to define an M code and make the feature configurable]

Another "major" challenge to reach the goal is too find a method to calculate v(t) [or (a(t)] in an efficient manner for a real-time system.
TinyG is using "Forward Difference Calculation" algorithm. Forward differencing is an efficient scheme for evaluating a polynomial at many uniform steps, but it will not work if the steps are not uniform.
Marlin is tickless, i.e., interrupts are only generated where a step (or some other event) has to be scheduled. Because of this Marlin implementation could not use TinyG approach and their solution consisted to transform the math to use fixed point values to be able to evaluate it in realtime and they implemented this portion in assembler code to be even more efficient.

In the case of Duet, I need to read further, but would appear that it is also tickless. We might explore other algorithms (probably we could not do better than Horner's method) or adopt Marlin's approach.

It's been a long post, but I would hope that it is useful and leads us to a successful implementation of a new feature that is already receiving lots of recognition by the Marlin community.

Carlos.

Larzarus

@carlosspr Take a look at this article: https://pdfs.semanticscholar.org/6539/02c3dbd1aa7afe4d5f3538524de149d4ecb2.pdf It explains how to achieve the desired 0 jerk / 0 accel at the crossover points. It's also got some empirical data on resulting forces and movement execution time, showing the benefit in quantifiable terms. It's worth the read.

dc42

I have been following this discussion, and improving the motion algorithm is on my list.

The Marlin implementation assumes zero acceleration at the start and end of each segment, and I think it also assumes zero rate of change of acceleration at the start and end. This is reasonable for a travel move on a printer with non-segmented kinematics. However, when accelerating over a number of segments, we definitely do not want to start and end with zero acceleration, we want uniform acceleration across several segments, except at the start and end of the acceleration. Otherwise the motion will be jerky, for example at the start and end of the outline when printing a cylinder.

So at present I favour traditional S-curve motion (3rd order motion profile). See https://pdfs.semanticscholar.org/a229/fdba63d8d68abd09f70604d56cc07ee50f7d.pdf. But before I make a decision, is anyone reading this thread aware of any evidence that higher order motion profiles are advantageous?

carlosspr

@dc42 yes, symetric higher motion profiles like trigonometric functions or Bezier curves are advantageous. They simplify the implementation greatly and do not require to change the planner. I will elaborate.

Third order moves is quicker to calculate, but would require recalculating the planner variables like "accelDistance" and "accelDistance" and it would require many changes in DDA.

The beauty of Marlin/TinyG implementation or sinusoids (versus parabolic) is that the area under the S-curve is exacly the same as using the constant acceleration model. This is because the areas above and below the equivalent linear speed are equal and it allows to use the calculations and planning as if it was a constant acceleration move. To illustrate this check the next figure: the blue line is the speed versus time. The distance traveled from startSpeed to Top Speed is equal to the area of the yellow figure, which is the area used to plan movements using constant acceleration.

0_1524239961699_Screenshot from 2018-04-20 19-57-41.png

So basically Marlin does not change the planner "at all". Only when they calculate the step pulse frequency, they use a Bezier curve to evaluate the time to the next steps on the acceleration and deceleration phases versus using a constant acceleration.

Traditional 3rd order motion profile (like the parabolic profile on the paper) is not a mathematically complete solution. It will also produce zero acceleration at the start and end of a segment, but the difference with Bezier curves is that it does not assure zero jerk. On the paper the "Figure 5. Parabolic profile" shows the velocity and acceleration, but not the jerk. The jerk is the derivative of the acceleration which can be seen as the slope of the tangent at each point of the curve. For this figure 5 the jerk will be constant (no zero) in all joints.

From a mathematical point of view, and ease of implementation, Bezier curves are the perfect solution and at the end a simpler approach.

dc42

Like I said, the Marlin implementation only works because it assumes zero acceleration and jerk at the start end new of each move - and that assumption will have nasty consequences. Once you drop that constraint, Bezier curves lose their appeal.

carlosspr

@dc42 just playing with the implementation on Duet, this might give some ideas:

on Drivemovement.h I have declared new variables on the struct PrepParams as follows:

	// Bezier coefficients for the acceleration phase
	float bezier_aA;   
	float bezier_aB; 
	float bezier_aC; 
	float bezier_aF; 
	bezier_aNormalizeTime;  //required to normalize time between [0,1]
	// Bezier coefficients for the deceleration phase
	float bezier_dA;   
	float bezier_dB; 
	float bezier_dC; 
	float bezier_dF; 
	bezier_dNormalizeTime;  //required to normalize time between [0,1]

The reason to have duplicated coefficients is that Prepare is called once for the whole move.
These variables are evaluated on DDA::Prepare

	params.bezier_aA = 6*(topSpeed - startSpeed);
	params.bezier_aB =  15*(startSpeed - topSpeed);
	params.bezier_aC = 10*(topSpeed - startSpeed);
	//bezier_D = 0
	//bezier_E = 0
	params.bezier_aF = startSpeed;
	params.bezier_aNormalizeTime =1/accelStopTime; //move starts at Time=0

For the deceleration phase just replace startSpeed by topSpeed and topSpeed by endSpeed. params.bezier_dAV =1/(totalMoveTime - decelStartTime)
totalMoveTime does not exist in the current code and needs to be calculated (part of the work pending).

On Drivemovement.cpp I have added a new function for the class DriveMovement that will calculate the instant velocity.

float instantVelocityBezierAccelerationPhase (const uint32_t currentTime) {
    // Instant velocity is defined by the formula
// V(t) = At^5 + Bt^4 + Ct^3 + F
// Implemented with Horner’s Method for Polynomial Evaluation
// Polinomial coefficients are {bezier_aA,bezier_aB,bezier_aC,0,0,bezier_aF}
// Current time shall be normalized between t=[0,1].
float t= currentTime*bezier_aNormalize; // this ensures time will be between 0 and 1
float result = bezier_aA;
result = result*t + bezier_aB;
result = result*t + bezier_aC;
result = result*t*t*t + bezier_aF;
return result;

}

Finally, on CalcNextStepTime functions, for the Acceleration phase

  nextCalcStepTime = 1/instantVelocityBezierAccelerationPhase(timeSinceMoveStart);

I still have not figured out how to calculate "timeSinceMoveStart". I am reading the code many times and getting more and more familiar with it, but is hard for me as I am new to this firmware.

It has errors. For example instant velocity is float and nextCalcStepTime is defined as uint32_t, and is not complete.

it is necessary to define an M code to be able to select between actual linear velocity profile and S-Curve and add the corresponding "if" on the relevant sections.

Finally I have not evaluated performance and it is more than probable that we need modify the math to use fixed point values the same way marlin has done (https://github.com/MarlinFirmware/Marlin/pull/10337/files).

Let me know if I can help you.

ejtagle opened this pull request in MarlinFirmware/Marlin

closed [2.0.x] 6th-order jerk-controlled motion planning in real-time #10337

carlosspr

@dc42 I do not see the nasty consequences.. Can you elaborate?.

I imagine this as a driving a car. I start the engine and start accelerating until I reach the maximum speed on the road (80Mps for the shake of the story). Then I maintain the velocity and to do that I do not apply any more acceleration to the car (acceleration =0). If I see a curve, I push the brake and reduce velocity a bit to 60Mps (decceleration) and at the exit of the curve I push the accelerator to increase speed again to 80Mps for the next road segment.
In this drive, there is an inflexion point when the car reaches the minimum speed and before it starts accelerating again (gains more velocity). If the movement is smooth the speed profile would be like a "U" with the minimum speed reached at 60Mps. On the bottom of the U, the tangent (acceleration) is horizontal and therefore the acceleration is 0, and this does not produce discontinuities or nasty consequences.

Also a third order motion profile will have 0 acceleration at the start end new of each move and will have the same consequences so I see no differences (except for the fact that it will have jerk).

Larzarus

@dc42 said in 6th-order jerk-controlled motion planning:
...

So at present I favour traditional S-curve motion (3rd order motion profile). See https://pdfs.semanticscholar.org/a229/fdba63d8d68abd09f70604d56cc07ee50f7d.pdf. But before I make a decision, is anyone reading this thread aware of any evidence that higher order motion profiles are advantageous?

@dc42 I'm gonna throw in this quote from the paper I linked to:

...the trigonometric S-curve trajectory is:
• simple as 3rd order polynomial model and
• capable of producing smooth trajectory as good as 5th order polynomial S-curves and quintic splines

That's what drew me to this solution in my testing -- a close approximation of 5th order with only 3rd order effort, by replacing this angular jerk curve:

0_1524258811822_e98cd887-dc4d-42fa-9a1c-12e20151f07a-image.png

with a sinusoidal jerk curve -- I mean look at it, it's beggin' for it! If you're considering limiting the implementation to 3rd order to keep it simple / maintain performance, this may be something easy and worth adding?

Honestly, anything that get us away from instantaneous jerk is going to be the largest single improvement (even the simplest 3rd order will help).

Beyond that, the debate over what's the best way to shape higher orders (splines, trig, polynomials, etc) can go on forever, and no answer will please everyone. I don't know how reasonable it is, as this is deeply tied into everything motion planning, but maybe consider if it's possible to encapsulate that shaping math enough that others can plug in their favorite method? You know, to get people off your back.