Trapezoidal Trajectory Planner


Hello everyone,

Over the last few weeks I’ve been working on implementing a trajectory planner for use with ODrive, and I’m excited to say that stage one - Trapezoidal (acceleration limited) trajectories - is ready for testing. There is a pull request for it here, and you can pull the branch directly here


Results first! Everyone loves graphs, and thanks to @Richard_Parsons we were able to get graphs from real-time testing (not just Python simulations). Richard used his flywheel setup to create a challenging high-inertia system for ODrive.

Unplanned move with high gains:

Same gains, with planner

Long distance move after tuning:

Calling move_to_pos in the middle of a move:

Using The Planner

To use it, you need to first setup your trajectory planner limits (in encoder counts):

<odrv>.<axis>.trap_traj.config.vel_limit = <Float>
<odrv>.<axis>.trap_traj.config.accel_limit= <Float>
<odrv>.<axis>.trap_traj.config.decel_limit = <Float>

Note: You may want to set your controller.vel_limit slightly higher than your planned move velocity limit, to act as a safety without getting in the way.

Then, you can simply make function calls to move_to_pos() as follows:

The move_to_pos command takes a position in encoder counts, computes a trajectory, and then switches the ODrive controller into CTRL_MODE_PLANNED_MOVE_CONTROL (4). In this mode, ODrive computes a new position and velocity setpoint during every update cycle. Current setpoint will also be used in the future when we have a good way to convert from acceleration to current/torque. When the move is complete, pos_setpoint is set to the final position, and vel_setpoint is set to 0. The controller then returns to CTRL_MODE_POSITION_CONTROL to maintain position.

Changes to the trajectory kinematic limits can be made during a move without breaking the planner, but move_to_pos must be called immediately after to recompute the trajectory.


The algorithm itself is an implementation of this paper (PDF Warning), which goes on to describe a fast time-optimal jerk-limited planner (which we plan to finish in the future). The implementation was first tested thoroughly in Python to ensure that all corner cases were handled gracefully, including short moves, overshoots, and mid-move kinematic limit changes . The C++ code came directly from the Python, with slight modifications to improve real-time behaviour. The python code is included under the “Tools” folder in the ODrive repository.

The algorithm is very fast, robust, and can be expanded to multi-axis synchronization by computing all trajectories in time-optimal mode, then using the slowest time and recomputing the trajectories in fixed-time mode. They demonstrate this with a 6 axis robot


I can’t stress enough just how big a difference this trajectory planner makes to the usefulness of odrive. It allows you to set very high PID gains so that the motor behaves like a stepper motor in terms of having a high stiffness when holding position while also allowing you to make rapid moves without overshooting your target position. I’m sure this feature will be very useful for almost everyone in the odrive community.

Great work @Wetmelon and I’m looking forward to trying out your multaxis planer when that ready :grinning:

Oh and for anyone wanting to learn more about trajectory planners I found this course to be quite informative.


A post was merged into an existing topic: How to use github?


Super curious about that flywheel, Richard… Is it part of some project?


The flywheel is for no specific project. Its just something I had on hand and so I thought it could be useful to for testing with the trajectory generator among other things. If you were ever wanting to make your own you could making a motor adaptor for one of these weights provided you kept the RPM low for dynamic balancing reasons.

If your curious, my flywheel came out of a machine a bit like this that was being thrown out at my place of work and so it was as easy as printing an adaptor for the motor in order to get up and running.