Pathfinder - Fast Motion Profiling for Java and C++, Tank and Swerve Drive


I was able to reproduce the bug on my computer. The source trajectory that is generated is fine, but when the TankModifier is applied, the really huge velocities appear. My testing has shown that it happens whether or not multithreading is used.


Try this newest release. It fixes successive generations of trajectories and may solve this issue as a result. If you have any further issues, please direct them to the Github Issues page


I finally got the time to get Pathfinder working (hopefully we will be able to use it at BattleCry@WPI), and here is a demo video of our robot following a simple trajectory that is similar to what we will use to get over a defense and line up for our auto shot.

Also, I attached a picture of a web interface I wrote (using pynetworktables2js) that allows me to see how well the robot is following the trajectory and easily tune the parameters. (Also, disregard the weird looking data, I was working on a problem when I took the screenshot)

Web Interface.png

Web Interface.png


Looks good! What are you using to generate the graphs?

Once our robot lands back in Aus I’ll see if I can do a demo with it, after my exams of course :stuck_out_tongue:


I am using Plotly.js, which is being fed data from NetworkTables. I’m starting to wonder if there is a better alternative, though, because it seems to have trouble dealing with data that updates many times per second.


Have you tried flot?


Is there a way to generate a profile that goes BACKWARDS? (Meaning that it would result in -ve velocities/positions, etc.)


On my team’s robot I implemented something similar to this. Assuming you want your robot to drive the same path, but with the robot facing backwards, I simply sent the right trajectory to the left side of the robot and the left trajectory to right side of the robot and flipped which side was inverted (Because one side already had to go opposite the other side to go forward, I had to switch which side was inverted).

I also took this one step further and said that if I wanted to run a predefined path in reverse (meaning I started at the end and wanted to drive backwards), I took the original path, inverted the order of the points and and flipped the angles, and then followed the previous method to drive the new path backwards.

EDIT: Handy method for creating Talon SRX TrajectoryPoints from a Trajectory assuming the Trajectory was created with units of revolutions and that the Talon expects units of revolutions.

private static TrajectoryPoint] generatePoints(Trajectory t, boolean invert) {
		TrajectoryPoint] points = new TrajectoryPoint[t.length()];
		for (int i = 0; i < points.length; i++) {
			Segment s = t.get(i);
			TrajectoryPoint point = new TrajectoryPoint();
			point.position = s.position;
			point.velocity = s.velocity * 60;
			point.timeDurMs = (int) (s.dt * 1000.0);
			point.profileSlotSelect = 1;
			point.velocityOnly = false;
			point.zeroPos = i == 0;
			point.isLastPoint = i == t.length() - 1;

			if (invert) {
				point.position = -point.position;
				point.velocity = -point.velocity;

			points* = point;
		return points;


I’m new to motion profiling, but would love if our robot is able to do it this year. Could someone enlighten me on whether it is possible (and if so, how) to use these generated trajectories with a Talon SRX in motion profiling mode?

EDIT: Ignore this, I missed the post right above mine. I’ll see what I can do with the code they provided. Thanks!


I would like to ask a few questions about this really cool motion profiling program. I am comply new to motion profiling. In this program you reference the wheel base. I am confused as to what you are measuring. Is it the width of the wheel? Also when you plan your trajectory what units are the points in? I think I understand that the trajectory produced is on a graph where they y-axis is velocity and the x-axis is the time. One last question. In the wiki it shows setting a PIDVA for each side of the robot. How precise do those values need to be and how do do you find them?

Thanks for the help,


Did you try searching on the internet


Wow I feel stupid. I should have thought of that in the first place. Didn’t think it was that simple. I still have some unanswered questions though. Also what is the units the wheel base is measured in when using this program?



wheelbase_width: The width between individual sides of the wheelbase

This is conventionally called the “trackwidth”.


Units don’t matter at all as long as you are consistent. You need to choose one unit of measurement and use it for everything: Coordinates, wheelbase, velocity (unit/s), acceleration (unit/s^2), and jerk (unit/s^3).

I personally have been creating waypoints in revolutions and converting my wheelbase to revolutions. This makes the output friendly for Talon SRXs by feeding in distances in revolutions and easily converting to revolutions per min for velocities.


Could you explain on how you make these calculation? Are you saying that your waypoints would be how many rotations the wheel needs to make? So for example one waypoint would be:

new Waypoint(5, 0, 0),

So the robot would move 5 wheel rotations. In a PID loop you would set the distance per pulse to how many counts the encoder is per one rotation of the wheel. Is that what you would sent as the wheel base?


There’s a lot of jerking at the beginning and end of the path. Can that be optimized further?

Or would that require reducing your center drop a bit?


You can use whatever unit values you want. Pathfinder on it’s own is unitless, but the units that you pass to it will be consistent throughout. Most commonly used are waypoints in terms of metres, velocities in terms of metres/sec and acceleration in metres/sec/sec. For those across the pond, this also works in feet and inches, or yards, or kilometres, or parsecs or lightyears or anything. As long as you keep your units in the same format, it will generate just fine.

I wouldn’t recommend doing this in wheel rotations, though. I’d generate the trajectory in real units first, and convert it after path generation. This will make it much easier on you when you decide to write the trajectory


Not sure if this is the case here (or if the robot in question is even still driving), but for the sake of discussion, two of the challenges to getting smooth starts and precise stops in robot drivetrains are backlash and stiction.

Backlash exists because of the stackup of clearances between the wheels and the motors in most drivetrains - chain/belt slack, gear tooth clearance, and slop in hubs. Dog gears in shifters in particular contribute a lot of backlash (because they are designed to allow for easy shifting). It’s not usually possible (or even desirable) to remove all of the backlash from your drivetrain, so you better deal with it. The problem with backlash for precise control is there is a “deadband” around zero velocity with hysteresis depending on which side of the slop the mechanism begins. This means there is a small period of time before your robot can change direction, during which your controller might be falling behind and overcompensates (especially if there is an integral term). This hurts especially when stopping, as your controller might “stick” the landing only for the inertia of the robot to continue through all of the backlash.

One easy way to get smoother starts is to always “preload” your robot into the backlash - if you are going to drive forward at the start of autonomous mode, make sure the last thing you do before you walk away is roll your robot backwards (if possible). (It might be counter intuitive, but think carefully about what happens to the backlash when you do this). You can also try to incorporate backlash into your controller, but this gets complicated beyond using the backlash of your system as a good starting point for an “on target” tolerance. (Otherwise you can try to estimate which side of the backlash you are on - or even use one encoder on the motor and another on the output shaft and measure it. Way overkill for FRC.)

Stiction is “static friction”, which must be overcome before the robot can begin moving. It’s a threshold, so your drivetrain needs to apply at least this much force before you start rolling. Your profile might be nice and smooth (limited jerk and acceleration), but profile following works well because of feedforward, and linear feedforward on velocity and acceleration does not work well at really low speeds (assuming you tuned feedforward at a higher speed) because of this nonlinearity. The result is that your feedforward terms gradually ramp up voltage, but the robot doesn’t move until you cross the threshold, during which time once again your robot is accumulating error and increasing its throttle until it shoots off the starting line.

So stiction is a problem because our feedforward is linear, but our system is not. So let’s make our feedforward nonlinear! A great way to deal with stiction is to measure the voltage necessary to get the mechanism to just begin to move, and add a tiny bit less than this to the output of your controller if the commanded voltage is >0 but < this voltage (and visa versa for reverse). Now your feedforward has a step at zero voltage, but is linear once the robot gets moving. This is a pretty good model for a DC motor-driven mechanism! Note that the Talon SRX supports this concept (nominal output voltage) in its closed-loop control modes.


I feel like I’m missing something here but after a few hours of trial and error, we could only get our last years robot to spin in circles. Here is our java code:

//Motion Profiling Setup
	Waypoint] points = new Waypoint] {
		new Waypoint(-4, -1, Pathfinder.d2r(-45)),
		new Waypoint(-2, -2, 0),
		new Waypoint(0, 0, 0)
	double _wheelDiameter = 0.1016; //In Meters
	double _ticksPerRevolution = 2905.51921716; //Encoder Ticks Per Revolution
	double _maxVelocity = 1; //Default 2.0
	double _maxAcceleration = 1; //Default 2.0
	double _maxJerk = 30; //Default 60
	double _wheelBaseWidth = 0.775; //Archer .775 (in meters from inside of both wheels)
	Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH, 0.05, _maxVelocity, _maxAcceleration, _maxJerk);
	Trajectory trajectory = Pathfinder.generate(points, config);
	TankModifier modifier = new TankModifier(trajectory).modify(_wheelBaseWidth);
	EncoderFollower left = new EncoderFollower(modifier.getLeftTrajectory());
	EncoderFollower right = new EncoderFollower(modifier.getRightTrajectory());
	TalonSRX talonL1 = new TalonSRX(11);
	TalonSRX talonL2 = new TalonSRX(12);
	TalonSRX talonR1 = new TalonSRX(13);
	TalonSRX talonR2 = new TalonSRX(14);

	TalonFactory TalonFactory = new TalonFactory( new int]{ 11, 12, 13, 14 } ); //This is just a custom class reset all talons under the specified ids
	ADXRS450_Gyro Gyro = new ADXRS450_Gyro();
	Joystick controller = new Joystick(0);
	public void robotInit() {

	public void autonomousInit() {
		talonL2.set(ControlMode.Follower, talonL1.getDeviceID()); //Make the secondary motors followers
		talonR2.set(ControlMode.Follower, talonR1.getDeviceID());
		talonL1.set(ControlMode.PercentOutput, 0); //Make sure the main motors are stopped
		talonR1.set(ControlMode.PercentOutput, 0);
		left.configureEncoder(talonL1.getSelectedSensorPosition(0), (int) Math.round(_ticksPerRevolution), _wheelDiameter); //Configure the encoder
		left.configurePIDVA(1.0, 0.0, 0.0, 1 / _maxVelocity, 0); //Configure the pidva
		right.configureEncoder(talonR1.getSelectedSensorPosition(0), (int) Math.round(_ticksPerRevolution), _wheelDiameter);
		right.configurePIDVA(1.0, 0.0, 0.0, 1 / _maxVelocity, 0);

	public void autonomousPeriodic() {
		if (!left.isFinished()) {
			//Calculate using the new encoder values
			double l = left.calculate(-talonL1.getSelectedSensorPosition(0));
			double r = right.calculate(talonR1.getSelectedSensorPosition(0));
			//Calculate turning
			double gyro_heading = Gyro.getAngle();
			double desired_heading = Pathfinder.r2d(left.getHeading());
			double angleDifference = Pathfinder.boundHalfDegrees(desired_heading - gyro_heading);
			double turn = 0.8 * (-1.0/80.0) * angleDifference;
			//System.out.println("Left: " +  (l + turn) + ", Right: " + (r - turn) + " Angle: " + Gyro.getAngle() + "");
			talonL1.set(ControlMode.PercentOutput, l + turn);
			talonR1.set(ControlMode.PercentOutput, r - turn);
		} else {
			//Make sure the robot is stopped at the end
			talonL1.set(ControlMode.PercentOutput, 0);
			talonR1.set(ControlMode.PercentOutput, 0);


Pathfinder is counter-clockwise for turning (left is positive, right is negative), while gyros are clockwise (right is positive, left is negative). The simplest way would be to add your gyro heading to your desired heading to get the correct angle difference.

//Calculate turning
double gyro_heading = Gyro.getAngle();
double desired_heading = Pathfinder.r2d(left.getHeading());
**double angleDifference = Pathfinder.boundHalfDegrees(desired_heading + gyro_heading);**