Simple path planner


I’m trying to write a path planner for a 3-axis gantry, the goal being to move through a series of waypoints along a smooth path, respecting the velocity and acceleration limits of my servos. It occurred to me this a problem that must have been solved for 3D printer firmwares like Klipper, so I’ve been looking at the source code and documentation for inspritation. (In fact as far as I can Klipper’s path planner is based on Grbl, but I thought I’d ask here as this forum seems to be very active).

My requirements are much more forgiving than for a real CNC/3d printer controller. My paths consist of a set of waypoints, no more than 10, with curves of arbitrary radii at each junction. I start from rest and come to rest at the end of the path.

I’ve written some code that can build trapezoid segments given a travel distance, max velocity limit and arbitrary start and stop velocities, and can stitch these together so that the velocities are continuous at each junction.

What I’m struggling with is how to synchronise these segments across multiple axes so that each point is arrived at at the same time for each axis, and I can’t work how klipper etc solve this problem. One idea I had is to build the trapezoid for a pair of points for each of the three axes, see which is the slowest, then re generate the trapezoids for the faster axes but with that longer duration. I can see various challenges with this however and I wanted to ask here to see if I’m missing a simpler solution before I started trying to implement this.

Any input or pointers are gratefully received.



A couple of pennies dropped since my post above. Mainly that as I understand it, rather than computing trapezoids for each axis separately and somehow synchonising them, a single trapezoid is calculated for every move, and synchronisation happens when controlling the steppers using bresenham’s line algorithm. Using this method I have a matlab/octave script that produces a sequence of trapezoids which produce the desired trajectory.

My question now is about junction deviation calculations. As I understand this explanation, the goal is to set a maximum velocity at the junction between straight line segments, as a function of velocity in and out, and angle between the two segments. My issue is that this (as I’ve implemented it) produces very big jumps in velocity whenever an axis changes direction. The velocity (magnitude) never reaches zero, so when it switches direction, the axis simply goes off in the new direction with a velcity of the same magnitude but opposite direction, producing huge instantaneous accelerations. Here’s a plot of a two axis system:

In case the legend is not legible, blue is x velocity, red is y velocity, and yellow is the magnitude of the velocity. As you can see velocity magnitude is continuous, but indivual axis velocities can have big jumps.

I can’t post more than one image at a time so see below…

And a zoomed in view

And here’s a plot of the (2 axis) trajectory it’s following, designed to show the algorithm’s behaviour when going through various angles.


So anyway, is this the behaviour that klipper produces, or have I misunderstood something in my implementation?

Any pointers gratefully received.