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 https://github.com/madcowswe/ODrive/pull/210, and you can pull the branch directly here https://github.com/Wetmelon/ODrive/tree/TrajPlan.
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:
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:
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