GRBL's cornering algorithm

Updated on 2022-11-26.

Cornering is how a machine tool head follows a path through a vertex at speed given a machine’s physical constraints. Sonny Jeon’s article on improving GRBL’s cornering algorithm describes a heuristic that keeps a machine’s centripetal acceleration constant by approximating the corner with a circle. It’s implemented in GRBL’s planner C code and widely copied by other motion planning systems.

GRBL is a G-code interpreter and receives high-level instructions for where to move the tool and converts them to low-level stepper motor movements. In contrast, EiBotBoard’s “low-level move” commands and Klipper leave motion planning up to control software running on more powerful devices and communicate detailed moves to simple firmware. Regardless, any open source motion planning system I could find use Jeon’s algorithm to lower speed going into corners.

Theory

This algorithm finds a cornering velocity based on the maximum acceleration the machine is capable of and a user-defined constant factor. In mathematical terms, cornering velocity $v_c$ with constant centripetal acceleration $a$ around a circle of radius $r$ is given by $v_c = \sqrt{ar}$ . This is derived from analyzing the change in velocity for circular motion.

The algorithm uses a circle that touches the edges of the corner and a deviation factor $\delta$ to control the minimum distance between the corner and the circle. A smaller $\delta$ translates to a smaller $r$ and a proportionally smaller $v_c$ . Bisecting the angle $\theta$ of the two edges creates a right triangle with the opposite side having length $r$ and the hypotenuse length $r + \delta$ . From trigonometry:

\sin{\theta \over 2} = {r \over {r + \delta}}

And rearranging the terms to solve for $r$ :

r = r \sin{\theta \over 2} + \delta \sin{\theta \over 2}

r - r \sin{\theta \over 2} = \delta \sin{\theta \over 2}

r (1 - \sin{\theta \over 2}) = \delta \sin{\theta \over 2}

r = \delta {\sin{\theta \over 2} \over {1 - \sin{\theta \over 2}}}

The angle $\theta$ is given by the dot product of the incoming edge $\vec e_i$ and the outgoing edge $\vec e_o$ :

\cos{\theta} = {\vec e_i \cdot \vec e_o \over |\vec e_i| |\vec e_o|}

To avoid the expensive $\cos^{-1}$ and $\sin$ operations, Sonny used the half-angle identity for $\sin$ :

\sin{\theta \over 2} = \pm \sqrt{1 - \cos{\theta} \over 2}

Where the sign of the expression is the sign of:

2 \pi - \theta + 4 \pi {\left\lfloor {\theta \over 4 \pi} \right\rfloor}

For an angle $\theta$ in $[\theta, 2 \pi]$ , the expression is always positive. Keeping the expression in terms of cosine allows substitution of the dot product, which is just a sum of products, which in 2-dimensions is:

\vec e_i \cdot \vec e_o = e_{ix} e_{ox} + e_{iy} e_{oy}

To compute the cosine:

\cos{\theta} = {e_{ix} e_{ox} + e_{iy} e_{oy} \over |\vec e_i| |\vec e_o|}

Combining the equations above to find $v_c$ :

c = \sqrt{1 - {e_{ix} e_{ox} + e_{iy} e_{oy} \over |\vec e_i| |\vec e_o|} \over 2}

r = \delta {c \over 1 - c}

v_c = \sqrt{a \delta {c \over 1 - c}}

This approach requires just two square root operations for each vertex. The division by the two vector’s magnitudes are usually avoided by normalzing the vectors to the unit vectors.

The cornering velocity is used as the ending and starting velocities for the motion pieces leading to and from the vertex in Constant acceleration motion planning.

Implementations

GRBL’s code is particularly terse, but benefits from thorough commenting. Instead of discrete operators for the vector components of motion, the operations are done incrementally, without any wasted steps.

Saxi overall has a more understandable structure to the code, with ASCII art comments to describe trapezoidal motion planning.

Klipper’s code is accompanied by a Kinematics document.