Trapezoidal Trajectory Planner


#1

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

Results

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:
image

Same gains, with planner
image

Long distance move after tuning:
image

Calling move_to_pos in the middle of a move:
image

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:
<odrv>.<axis>.controller.move_to_pos(<Float>)

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.

Algorithm

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


Flywheel jig to test trapezoidal trajectory planner
#2

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.


#3

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


#4

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


#5

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.


#6

I understand you’re working off the jerk-constrained trajectory generation paper and I’m getting more accustomed to the code as it sits now. But purely out of curiosity, what would be the down- (and/or up-)side of generating a trajectory of positions over time based on the desired acceleration and velocity parameters and letting the positional PID do the work? It seems this would allow strong PIDs to be used for both move accuracy (as it “chases” the generated positions) and strong holding.


#7

I think this is exactly what we do with the trajectory control mode.


#8

I’ve spent some time cleaning up and enhancing @Wetmelon’s original implementation. If anyone is interested to test it out, it’s available here:

Please let us know if everything works, or if there are some issues.
Thanks


#9

@madcowswe - Hmmm, I think i see it now. Y is the new position returned on each call to evalTrapTraj.

I was thrown off by the Yd and Ydd terms seemingly sending velocity and acceleration terms back. I guess I was just thinking of it more simply as a series of moves to a series of (potentially) arbitrary positions, letting the normal position-seeking logic deal with chasing down each “point”.

So what is the logic of sending the Yd and Ydd terms back and setting both vel_setpoint and current_setpoint?
Is it just an ODrive controller convention?

(And, man, does the English language fail us here - “current” as in “present time” and “current” as in “Amperage” - argh)


#10

Yes so Yd and Ydd are the first and second derivatives of the trajectory at the present timestep. We do assist the controller from falling behind the trajectory using these; we pass the values as feed-forward to the controller. This means that the feedback (what you called PID (it’s not exactly PID, it’s close though)) only has to work to correct disturbances and unmodelled dynamics.

FYI: in this context, current_setpoint refers to electrical current (which produces torque, and hence proportional to the acceleration).

EDIT: Here is a diagram to help illustrate what we are doing:

The Source seems to be an interesting read on the subject. (note our implementation is not exactly like this, but the concept of the feed forward based on the trajectory is similar).


#11

Just so everyone is aware, the Trajectory Planner module for trapezoidal trajectories is now available in FW 4.6.

Parameters

To use it, you have a whole new module and four new configuration values:

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

vel_limit is the maximum planned trajectory speed. This sets your coasting speed.
accel_limit is the maximum acceleration in counts / sec^2
decel_limit is the maximum deceleration in counts / sec^2
A_per_css is a value which correlates acceleration (in counts / sec^2) and motor current. It is 0 by default, and is not strictly necessary, but can improve response of your system if correctly tuned. Keep in mind this will need to change with the load / mass of your system.

All values should be strictly positive (>= 0).

Keep in mind that you must still set your safety limits as before. I recommend you set these a little higher ( > 10%) than the planner values, to give the PID controller enough control authority.

<odrv>.<axis>.motor.config.current_lim = <Float>
<odrv>.<axis>.controller.config.vel_limit = <Float>

Functions

Use the move_to_pos function to move to an absolute position:

<odrv>.<axis>.controller.move_to_pos(<Float>)

move_incremental is not yet implemented.
ramp_to_vel is not yet implemented.


#12

These commands, are they available via UART Ascii protocol? Binary protocol?
My next step would be to send the commands from the CableCam Controller.

Second question is about motor slip. The CableCam has an additional Hall-Sensor on one of the two running wheels. Hence the motor position and the cablecam position have different scaling. But more important, the motor might slip a bit when accelerating or braking. Any idea how to merge the running wheel position into the position calculation?


#13

The commands are available on the ASCII protocol.

t <axis> <position>

I don’t currently have a good answer for you re the 2nd encoder. This is something that’s done in other industries too (high precision PnP for instance), but we don’t support it yet.


#14

beautiful! cant wait to try it!