PathFinder in Python

My team has recently been working on implementing PathFinder in Python. Unfortunately, we seem to be running into some unit conversion issues between meters and feet. We can either make the trajectories the right unit or the PID setpoint/process variable the right unit, but not both.

As an example, in the pathfinder-example code on Github, when the P gain is set to 0 and the heading improvements are removed, the robot spins out of control in the simulator. Shouldn’t its behavior be at least somewhat correct, with these corrections only being slight modifiers?

This leads me to wonder, is the unit conversion an issue in our code, or the Python PathFinder implementation? If the Python PathFinder implementation is correct, can someone explain/show me how to make the example code work decently without P gain or heading correction? Thanks in advance!

pathfinder doesn’t actually use any kind of units internally, so as long as you’re consistent you should be fine. The python library is a thin wrapper around the pathfinder C code, so it’ll be just as correct as the C implementation.

For why the heading correction is needed, see

Now, keep in mind this doesn’t account for heading of your robot, meaning it won’t track a curved path. To adjust for this, you can use your Gyroscope and the desired heading of the robot to create a simple, proportional gain that will turn your tracks. A full example, including the calculations for each side of the drive train is given below.

When I set P=0 and set turn=0, the robot only traveled in a half circle, and didn’t spin out of control. IIRC, that’s fairly close to what happens with a real robot.

Thank you for your quick reply. Correct me if I am wrong, but PathFinder will not work to medium/high accuracy unless heading corrections and P gains are added? That could explain my previous issues of severe inaccuracy being mistakenly corresponded to unit issues.

Yes, they are both in the example code for a reason. :slight_smile:

Okay thanks! My team was trying to tune the various aspects individually starting with just the path, and ran into these issues. I will look into somehow tuning them in parallel.

I am new to the world of PID, but could you run a separate PID loop on the motors to tune them first, and use those values later in pathfinder? Would that work?

If you tuned a position following PID loop separately and then wanted to use a position PID loop to follow a trajectory, then yes. The key thing here is you shouldn’t be using a position PID loop to follow a path, since a path has position, velocity, and acceleration components. A standard PID loop also doesn’t have feed forward control, which is critical for accurate path following.

The most common closed loop controller for a trajectory is a PIDVA controller, which takes position, velocity, and acceleration setpoints.

u(t) = KpposError + KiposError + Kd*(derror/dx) +KatargetAccel + KvtargetVel

where Kp, Ki, Kd, Ka, and Kv are gains.

For more information on feed forward control and determining the feed forward gains I’d look at these resources:

1 Like

Yeah that makes sense. I currently have set Ka to 1/a_max and Kv to 1/v_max, although I will definitely check out the DT characterization paper. for better estimates of these two quantities Thanks for the resource.

Although using your estimate for Kv will probably be fine, simply doing 1/a_max is NOT how Ka works. I would set Ka to zero until you run a proper characterization run on your drivetrain.

For reference,
v_app = Kv * velocity + Ka * acceleration

Hopefully you should see why 1/a_max won’t suffice.

Yeah my bad. Will definitely do the drive train characterization ASAP. Thanks for that clarification.

I created some code to do the characterization automatically… would love for you to try it and give feedback. Versions in Java and Python.

Also, post your results at

This looks much better than what I was planning on doing myself. Thanks so much for creating this resource! We will test it out at our next meeting, and let you know our results.


FYI, there were some computation bugs in the data analysis code (the data logging is fine), so if you’re using the analysis I’d recommend pulling the latest code and re-analyze your JSON.

I’m reposting my reply on this thread here.

We’re having trouble getting our Ka and Kv with the RobotPy characterization tool. We’ve followed the instructions in the README, and confirmed that everything is working properly, it just seems the resulting data is completely off. It’s probably something we’re doing wrong, but we can’t figure out what.

Here’s a picture of the output, and a video of us walking through using the characterization tool.

Robot Characterization Video