6th-order jerk-controlled motion planning

carlosspr

I have finally managed to compile Reprapfirmware (had lots of problem with the ARM toolchain). I have tried to implement the S-Curve based in Bezier algorithm, but I cannot make it work.

I am posting here findings and reasoning to see if someone in the forum can see the errors and where it fails and/or clarify possible wrong concepts. i have the feeling that it might be related to unit conversions.... but I can not find the error.

There is not much documentation on how Reprap implements the Movement, so I have tried to understand it by reading the code. However, due to real-time optimizations the implementations are hard to follow and my findings can be wrong.

As far as I understood, the class Move controls all movement of the RepRap machine, both along its axes, and in its extruder drives.

The planner is implementer in DDA.h/DDa.cpp and the Control of the steppers in DriveMovement.h/Drivemovement.cpp

TRAJECTORY PLANNER (DDA): Implements a line tracing using Bresenham algorithm. In other words, it buffers movement commands and manages the movement profile plan. This ensure that our printer moves as fast as is possible within the kinematic constant constraints at the configuration.

This are the variables used by the motion planner to manage acceleration (defined in DDA.h).

           float accelDistance; 
           float steadyDistance; 
           float decelDistance;  
           float requestedSpeed;
           float startSpeed;  
           float topSpeed;  
           float endSpeed; 
           float targetNextSpeed;

All speed units are in mm/s

STEP CONTROL: The step control is performed at Movement.h and Movement.cpp. The process determines for each drive instants of the time t (in processor [cycles]) when pulses (steps) are generated based on the calculated profile.

There are two methods – two group of algorithms - for calculating instants of time when pulses must be generated. They are named as: “time per step” and “steps per time”. Reprap uses "time per step" with the following operation mode (as I could understand from the code):

1./ calculate time period to next pulse (nextCalcStepTime),
2./ wait until that much time period elapses,
3./ generate next pulse and
4./ go back to step (1) and repeat until desired number of pulses is over .

It is not very clear to me the algorithm followed to calculate nextCalcStepTime and I could see some compensation clocks on the formula which also I do not quite understand well.

In order to add an S-Curve profile, I followed Marlin approach to use the same trapezoid generated by DDA and only implemented changes in DriveMovement.

The first step I did was to draw the trapezoid using as speed units steps/s and the variables available in the code:

The trapezoid is the shape the speed curve over time in steps per second.

It starts at step=0, accelerates until accelStopStep,
then keeps going at topSpeed (constant speed) until it reaches decelStartStep
after which it decelerates until the trapezoid generator is reset to next DDA at totalSteps.

The first pulse (step) controller generates at the start of motion, at the start of the phase of acceleration, i.e. at the step time t=0. After the first pulse is generated, the controller needs to calculate the time period until the next pulse (nextCalcStepTime) wait until this period has elapsed, and then generate the next pulse, at step time t=1. This will go on until the desired position is achieved, or in other words, the desired number of pulses (steps) has been generated.

Since after each pulse the motor makes one step of distance 1/stepsPerMm, the time delay between two successive steps is given by

 *nextCalcStepTime* = 1 / ( stepsPerMm * V(t) )

Instead of a linear speed in the acceleration and deceleration phases, V(t) is replaced by the quintic (fifth-degree) Bézier polynomial for the velocity curve:

       V(t) = V_f(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F

And this is calculated using the Bezier S-Curve with the same code developed by Eduardo José Tagle (Marlin).

The trapezoid generator state in DriveMovement.cpp contains the following information, that we will use to create and evaluate the Bézier curve:

   accelStopStep		    		[TS] = The total count of steps for this movement. (=distance)
   dda.startSpeed * stepsPerMm   [VI] = The initial steps per second (=velocity)
   dda.topSpeed * stepsPerMm     [VF] = The ending steps per second  (=velocity)
   nextStep 						[CS] = count of steps completed(=distance until now or current step)

In DriveMovement:Prepare, at the start of each trapezoid, I calculated the coefficients A,B,C,F and Advance [AV], as follows:

   *      A =  6*128*(VF - VI) =  768*(VF - VI)
   *      B = 15*128*(VI - VF) = 1920*(VI - VF)
   *      C = 10*128*(VF - VI) = 1280*(VF - VI)
   *      F =    128*VI        =  128*VI
   *     AV = (1<<32)/TS      ~= 0xFFFFFFFF / TS (To use ARM UDIV, that is 32 bits) (this is computed at the planner, to offload expensive calculations from the ISR)
  **/

For the deceleration part, we use the same approach, but adapted to the initial velocity and final velocity of the segment:

	   *  totalSteps - decelStartStep	[TS] = The total count of steps for this movement. (=distance)
	   *  dda.topSpeed * stepsPerMm     [VI] = The initial steps per second (=velocity)
	   *  dda.endSpeed * stepsPerMm     [VF] = The ending steps per second  (=velocity)
	   *  nextStep 			     [CS] = count of steps completed(=distance until now or current step)

Finally, in DriveMovement::CalcNextStepTime we calculate nextCalcStepTime for the S-Curve profile

nextCalcStepTime = 1 / (eval_bezier_curve(nextStep) * stepsPerMm);
nextCalcStepTime = nextCalcStepTime * StepClockRate;

The time is multiplied by the StepClockRate to get the number of clock cycles.
Probably I would need to add compensation Clocks.... but I do not understand why this is in the formula on the current firmware and could not test because it does not work properly.

I could provide the Source code from DriveMovement.cpp and DriveMovement.h which are the only parts changed in case that anyone wants to investigate further......

dc42

It sounds that you have done some good work. The compensationClocks parameter is only used when applying pressure advance. So it is zero for ordinary axis movements, and zero for extruder movements if the pressure advance for that extruder is set to zero.

RCarlyle

Bit late to the party, but adding some points that I think are important...

Yes, corner velocity jumps are a bigger deal than the straight-line acceleration scheme. But those are separate issues. You can improve print quality by changing cornering behavior OR straight-line acceleration behavior.

There IS value in going up to 4th-order motion. Specifically, this is what is required to get those nice "glass of beer doesn't slosh" or "pendulum stops nicely" motion profiles. Anything over 4th order is pointless in this application (and frankly EVERY application except satellite moment control) but the G2/Marlin Bezier curve implementation has more processor-friendly math with 6th order motion than it would with a 4th order implementation. So there was no downside for Synthetos to go straight to 6th-order in G2. If it weren't for implementation math quirks, 4th order would be where you'd stop.

There are two very real factors for why 4th-order (or higher) motion control will give better print quality:

When you traverse a corner via the normal decel-jump-accel scheme in RRF, the motor torque demands and drivetrain loads from the corner and from the straight-line acceleration out of the corner are ADDITIVE. The printer is still violently executing the corner velocity jump when the next segment's straight-line acceleration kicks in, so you have larger peak forces/deflections. Any S-curve profile has a brief moment of negligible straight-line acceleration out of the corner, which reduces peak forces and allows the system to settle a bit. (Note that you could emulate this by simply waiting a millisecond or whatever before acceleration starts on the next segment.)
As we all know, real drivetrains (and the stepper torque response as well) are elastic and have a position error that depends on drivetrain force. And force is mass * acceleration. And RRF's second-order motion control has instantaneous changes in acceleration. For a rigid body, that's fine, acceleration/force can change "instantly" (ignoring stuff too fast to worry about like speed of sound in the material anyway) and there's no problem. But it's an issue for elastic systems. Instantaneous change in acceleration means an instantaneous change in force, which for an elastic system means instantaneous change in drivetrain deflection, which means instantaneous change in nozzle position error. And THAT is impossible. If you try to instantly change force on an elastic system, you get oscillation and you get peak deflections ~2x higher than the equivalent steady-state force. Thus, more ringing.

Now, what people tend to forget in these discussions is that the motion controller isn't moving the load, it's sending step pulses to a stepper driver, which generates a target position (coil energization phase angle), which generates a torque in proportion to the rotor angle error. So you've got two problems that people don't like to talk about:

Position quantization and timing jitter -- there's no point in trying to control motion much more precisely than the fundamental precision of the step pulse train and resulting position stairstep profile.
Motor following error -- steppers only generate torque in proportion to POSITION ERROR (strictly the sine of error) so all your fancy motion control step pulse train timing is getting low-pass filtered by the motor. This is the main reason velocity jumps at corners are possible. Your printer is probably about a 1/16th microstep behind where you think it is, any time it's accelerating. (You could feed-forward control this error out if you know the load inertia and motor torque, but it's probably not worth the trouble.)

So, yeah, S-curves are better than constant-accel, but you also want to understand how the firmware behavior gets turned into nozzle motion so you don't go overboard.

As for the G2/Marlin Bezier 6th-order code specifically... I don't like it. I think it does something dumb on arcs, which will be brutal on segmented delta firmwares like Marlin: it drops to 0 acceleration at every segment corner. When you have axis direction reversals, like a >90 degree corner on a Cartesian printer, that's good and normal behavior. But that's undesirable on arcs where you have multiple segments in the same axis direction but at different speeds/accels. For example, say you're starting from zero into a gentle, finely-faceted arc with a lot of short segments. The cornering speed algorithm will not register any need for corner slowdowns on this kind of geometry. (Which is itself an issue with the code, but that's a separate discussion.) So RRF will try to accelerate through the arc over multiple segments until it reaches command speed. We WANT acceleration to continue through these gentle corners so you don't cause unnecessary loss of speed or unnecessary changes in drivetrain force. But the Bezier code implementation can't handle non-zero entry/exit acceleration values... which I think is a big implementation failure.

dc42

@rcarlyle, I agree with all of that. The fact that the Marlin algorithm forces zero acceleration at the segment boundaries was bothering me too.

I expect to start work on this in July. Using a 6th order Bezier curve, I think there are enough parameters to match target accelerations at the segment start and and as well as target speeds, but the maths to work out the parameters may be difficult. If it turns out to be too hard then I can try implementing classic 7-phase motion instead of the current 3-phase.

Any solution also needs to eliminate commands to change velocity instantaneously ("jerk" in the 3D printer sense), by planning a deliberate deviation from the commanded path instead of commanding instantaneous speed changes and getting an unplanned deviation as a result.

JoergS5

Dear @carlosspr , thanks for explaining part of the firmware source files. If you want, I can help you checking your changes.

My proposal:

isolate the formulas so they can be tested independently (unit testing)
take real world examples of parameters
calculate the values with your formulas
test plausibility by differential time slices, e.g. V-t diagram. E.g. the S curve should show up
if the formula are correct, at least some sums must be correct, like total distance in given time

It is possible to use mathematical simulation programs for the task, but to start easy you can use a spreadsheet program, each row a diff t. The lower diff t, the more exact the results.

RCarlyle

@dc42 I think you need to decide whether to work on addressing corner jerks or work on nth-order straight-line motion, because the solutions strike me as more or less mutually exclusive. Deviating from corners means your paths are no longer exclusively a series of straight lines and corners, so from what I understand the entire motion planner and step interpolation scheme have to be reworked. For long segments you can just clip off the start and end, insert a new radius-arc motion to replace the corner, and keep the segment-middles as straight-line paths like normal. But with short segments, I think there's some complexity around deciding when/how to merge multiple gcode commands into a single radius'd arc or what have you. (Maybe you have something in mind.)

Personally, I'm fine with corner velocity jumps for this application, and am even fine with position jumps on extruder axis (eg pressure advance motions), because they work just fine with real steppers and servos when you stop ignoring driver+motor behavior. I would prefer to see a better cornering-speed-determination algorithm invented rather than eliminate them entirely. You probably know this, but here's what I've seen done:

Limit speed change for each motor (Sailfish)
Limit nozzle velocity vector delta magnitude (older Marlin, Repetier)
Limit "junction deviation" which is some arbitrary hocus with no real physical significance, but gives pretty good results... the CONCEPT is combining a radial acceleration limit with an error tolerance to round off corners, but it doesn't actually follow the trajectory required to implement that; it just uses the derived speed you WOULD use if you did it

My opinion, what we really ought to do is take the motor's behavior into account. For example, 3d printers are typically built with utterly ridiculous torque safety factors (like 50:1 on nominal acceleration), and what really limits cornering speed is the force:error relationship for the printer versus acceptable print quality. (IE the sum of motor following error and drivetrain deflections.) So we should be using a calculation connected to how velocity jumps exert forces on the mechanism. If those forces are comparable to the desired acceleration forces, there's no issue whatsoever with the corner velocity jump. I think that can be reasonably approximated as an elastic impact model, ie the change in kinetic energy due to the velocity jump is converted into elastic energy stored in the drivetrain as a spring deflection. You can thus turn the velocity jump into a peak force and predicted error. The trick here -- what nobody has brought together yet to my knowledge -- is that a lot of the drivetrain inertia is in the rotor, and a lot is in the extruder/bed, so on printers like deltas where the drivetrain ratio is variable, you need to check BOTH the rotor delta-V and the nozzle delta-V and either limit both separately or limit some kind of summing function of the two.

Ideally you'd calculate the total system inertia (moving mass reflected through drivetrain ratio, plus rotor) and velocity changes at each corner to get an actual delta-Ke value, and also calculate the total drivetrain elasticity at each corner according to the machine position, and use those to model a corner error due to the jump, but I doubt that's worth all the trouble. There's probably a simplified approach that captures the various factors with an empirically-tuned limit parameter like we use today. For example, combine a junction-deviation type calculation with a Sailfish-style motor speed change limit. Or convert the cornering jump into an equivalent acceleration based on some heuristic. Etc.

Changing gears. Deviating from the path according to a defined tolerance is a viable and reasonable approach as long as the max error is user-configured. MachineKit has a 3-axis trajectory planner mode that does this. (People use velocity extrusion to slave extruder motion to the 3-axis planner for 3d printing, although I personally think this violates a Stratasys patent.) However, it's a huge change from the GRBL ancestry all the major 3d printer firmwares use. You also probably want to be able to switch the allowed error mid-print so you can run infill rougher and perimeters finer, for example. (There's no fundamental reason for infill to use higher nominal feedrates than perimeters -- we just don't have good motion planners for complex perimeter geometry yet, so we drop perimeter speed to compensate for GRBL sucking.)

What I would suggest with corner-rounding is apply either a RADIAL nozzle acceleration cap and an acceptable path deviation, or PER-AXIS acceleration caps (cartesian axis or motor) and position errors. The path and rounded-cornering speed is then whatever trajectory achieves those max accels and errors. You get different corner-rounding shapes with the different approaches -- circle arcs from a radial acceleration cap, elipses for cartesian axis acceleration caps, and who-the-hell-knows-what from delta motor acceleration caps.

This gets more complicated when the segment size is small enough (ie similar magnitude as allowable error) such that the rounded trajectory spans more than two segments. Machinekit starts by merging small segments wherever the merged path results in less path-following error than the defined limit. That helps a lot. But it's another source of lost path-following fidelity. Then you have to do some iterative work over a queue to generate the overall rounded path, but I'm fuzzy on the details of how that's done.

If you really want to get complicated, you corner more like a car does, and combine centripetal acceleration with radial acceleration along a conic profile or something to achieve a constant combined acceleration. But that seems like too much to try to tackle.

RCarlyle

Oh, I also want to point out that GRBL's biggest speed control shortcoming is that it only looks at corners, and not radial acceleration across lots of segments. Finely-faceted smooth curves are basically ignored by all the mainstream 3d printer firmwares (except MachineKit). We really ought to be checking radial acceleration through arcs so the printer doesn't overspeed through them. The challenge there is how the firmware identifies an arc and determines what run of segments to check for radial acceleration...

JoergS5

Somebody wrote above that it's not possible to test different methods easily without a new firmware version and uploading.

But there is a possibility of scripting inside a program: you can script on the fly and test different possible functions, like the jerk functions above. A true evolutionary approach would be script variations - print and measure - take best of the variations. Print + measure could be a laser or led through the nozzle and photodiodes instead of real filament.

burtoogle

Really interesting thread this but the detail is somewhat over my head. Nonetheless, here's an idea to consider:

If some of the calculations that you maybe would like to do during printing are too complex to do in real-time, how about pre-processing the gcode into an intermediate form that can be interpreted quickly. So arcs/curves and other features of interest could be identified and replaced/augmented with mcodes or comments (or whatever) so that the subsequent gcode++(tm) file could then be printed using simpler realtime calculations.

The pre-processing could be done by the Duet, it would not need to be done before uploading the gcode.

JoergS5

@burtoogle the information whether it's a circle is in the original 3d object*), so transfering this information parallel to the gcode would be probably easier. Because calculating 3d objects from g-code or stl is not trivial. Your idea would allow printing a circle if the printer is capable of this, while another printer prints linear segments.

*) in this case, 2d objects at every layer

burtoogle

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

@burtoogle the information whether it's a circle is in the original 3d object, so transfering this information parallel to the gcode would be probably easier. Because calculating 3d objects from g-code or stl is not trivial. Your idea would allow printing a circle if the printer is capable of this, while another printer prints linear segments.

Sure, but that requires the slicer to play a part and it would be nice to have a solution that doesn't require help from the slicer.

@RCarlyle said

This gets more complicated when the segment size is small enough (ie similar magnitude as allowable error) such that the rounded trajectory spans more than two segments. Machinekit starts by merging small segments wherever the merged path results in less path-following error than the defined limit. That helps a lot. But it's another source of lost path-following fidelity. Then you have to do some iterative work over a queue to generate the overall rounded path, but I'm fuzzy on the details of how that's done.

Some of that could be a candidate for being pre-processed.

I'm really out of my depth with this stuff, so I can't pursue this any further.

wilriker

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

Because calculating 3d objects from g-code or stl is not trivial.

It's not but there is some promising progress made in that area right now: https://github.com/slic3r/Slic3r/issues/23#issuecomment-385288531

Of course this would again make it more slicer-dependent but still an awesome feature if it works as good as described.

Anyway, I think this is a little derailing in the regard that we are now discussing about corner-to-curves-conversion instead of the original discussion about acceleration-types, isn't it?

alranel created this issue in slic3r/Slic3r

open Detect arcs and generate G2/G3 #23

JoergS5

@wilriker wow, that's really interesting, thank you for the link!!

wilriker

@joergs5 Yeah, I was really excited when I found this myself.

RCarlyle

The reason why this arc stuff struggles to go anywhere is that there's a chicken-vs-egg problem with the slicer and firmware needing to change at the same time. Implementing G2/G3 arcs in the slicer is borderline pointless when the firmware handles arc commands by breaking them back into facets and running normal straight-line-and-corner motion control on the re-segmented path. (There's some benefit in gcode file size and command stream processing overhead but that's kinda "meh.")

STLs are faceted, and firmwares do everything faceted, so there's no point in slicers making up arcs when the upstream and downstream toolchains don't cooperate.

If you're going to make up arcs, you might as well do it in firmware. The Klipper model (RasPi does slicing AND trajectory planning, RAMPS does step pulses) is probably the easiest way to go on that front, but there are other things you can potentially do, like run a post-processing script between the slicer and firmware to do whatever conversion work you want.

Given the complexity of rounding off corners, I think working on straight-line motion and corner speed determination is pretty low-hanging fruit in comparison. That's my two cents.

Phaedrux

@rcarlyle I've been wanting to try playing around with Klipper using the duet as the MCU. Best of both worlds if you will. Best in class electronics and very high steps rates. If klipper succeeds in implementing s curve motion planning before the duet I will probably give it a shot.

wilriker

@rcarlyle Regarding whether arcs are making sense or not I was also torn between yes and no but I am now at "yes, they are useful". Let me explain why:

You are right that it seems to be pointless to have a command that defines an arc but the firmware will break it back into lots of linear movements. But there are at least three advantages of this approach I can see

Gcode files are smaller (more of a minor side-effect but still there)
The firmware usually knows on what hardware is running and it can optimize the breaking the arcs apart based on the capabilities of the processor, i.e. slower processors will have fewer segments compared to faster ones (this is already the case comparing Marlin vs. RRF where RRF uses a significantly smaller segment-size by default)
It cannot happen this way that the slicer is doing something strange in the middle of an arc (there are multiple posts in this forum where this happened) preventing the segmented arc being one fluid move. This can lead to lots of shaking, rattling, vibrations that can have a negative effect on print quality

So in my view having the slicer produce G2/G3 commands is desirable even though it seems absurd to have a total workflow where you design arcs in CAD, export them to STL which does not have arcs only to have them being reconstructed in the slicer be broken apart again inside the firmware.

This could in theory also be done in firmware (I mean reconstructing the arcs) but in the most cases the slicer runs on a much more powerful machine and that makes it a much better candidate for this step.

JoergS5

@wilriker I see a forth advantage

there are 3d printers being able to print arcs directly like printers with robot arms, they can print perfectly with arc g-code if the radius of the arm matches the radius of the g-code arc.

(I happen to be building a scara printer with variable radius.;-))

dc42

If G2/G3 arc printing becomes commonplace then I might find a way to generate arc movements without segmenting them.

Catalin_RO

@dc42 I use them extensively for CNC milling and all the motion related issues that I have (detailed in other threads!) are related to G2/G3, more specific, to G2/G3 movements followed by a G1. Maybe for 3D printing G2/G3 is not that important as the slicers usually start from a STL model, but CNC usually milling starts from a STEP model or something like that.

I will give just a simple example: if I want a 5mm hole, when made out of segments it will be slightly smaller!

I use extensively FreeCAD and CamBam. While FreeCAD can export both STL and STEP, CamBam really supports only STL. For complex items I tend to extract the edges and do simple profile and pocket operation based on them instead of an actual 3D milling - sometimes it requires less than half of the machining time! But for simple items I just draw 2D shapes in CamBam and then do operations on them with different Z start/stop heights. Either case, CamBam issues a lot of G2/G3 commands.