PathPlanner - swerve trajectories with holonomic rotation stops too early

We’re testing PathPlanner trajectories, and the ones that do not use holonomic rotation work just fine. The robot moves sideways etc without any issues.

Here is a trajectory that does a simple 90 degree counterclockwise turn -

image

image

image

The turn command stops when the robot turns around 34-37 degrees counterclockwise rather than 90 degrees.

To put it in perspective - here is the log of odometry updates:

f: false 
 Odometry Command Rotation: Rotation2d(Rads: 0.36, Deg: 20.41) 
 Odometry Command Module: 0 SwerveModulePosition(Distance: 0.21 m, Angle: Rotation2d(Rads: 2.26, Deg: 129.64)) 
 Odometry Command Module: 1 SwerveModulePosition(Distance: -0.99 m, Angle: Rotation2d(Rads: -2.33, Deg: -133.68)) 
 Odometry Command Module: 2 SwerveModulePosition(Distance: -0.19 m, Angle: Rotation2d(Rads: -2.26, Deg: -129.38)) 
 Odometry Command Module: 3 SwerveModulePosition(Distance: -0.08 m, Angle: Rotation2d(Rads: -0.74, Deg: -42.54)) 
 Odometry: Pose2d(Translation2d(X: 2.00, Y: 2.00), Rotation2d(Rads: 0.36, Deg: 20.41)) 
 f: false 
 Odometry Command Rotation: Rotation2d(Rads: 0.36, Deg: 20.39) 
 Odometry Command Module: 0 SwerveModulePosition(Distance: 0.21 m, Angle: Rotation2d(Rads: 2.26, Deg: 129.64)) 
 Odometry Command Module: 1 SwerveModulePosition(Distance: -0.99 m, Angle: Rotation2d(Rads: -2.33, Deg: -133.33)) 
 Odometry Command Module: 2 SwerveModulePosition(Distance: -0.19 m, Angle: Rotation2d(Rads: -2.26, Deg: -129.38)) 
 Odometry Command Module: 3 SwerveModulePosition(Distance: -0.08 m, Angle: Rotation2d(Rads: -0.74, Deg: -42.45)) 
 Odometry: Pose2d(Translation2d(X: 2.00, Y: 2.00), Rotation2d(Rads: 0.36, Deg: 20.39)) 
 f: false 
 CommandScheduler loop overrun 
Warning  1  Loop time of 0.02s overrun
 edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:387) 
 Warning at edu.wpi.first.wpilibj.IterativeRobotBase.printLoopOverrunMessage(IterativeRobotBase.java:387): Loop time of 0.02s overrun 
 
 Odometry Command Rotation: Rotation2d(Rads: 0.35, Deg: 20.30) 
 Odometry Command Module: 0 SwerveModulePosition(Distance: 0.21 m, Angle: Rotation2d(Rads: 2.26, Deg: 129.55)) 
 Odometry Command Module: 1 SwerveModulePosition(Distance: -0.99 m, Angle: Rotation2d(Rads: -2.33, Deg: -133.24)) 
 Odometry Command Module: 2 SwerveModulePosition(Distance: -0.19 m, Angle: Rotation2d(Rads: -2.26, Deg: -129.29)) 
 Odometry Command Module: 3 SwerveModulePosition(Distance: -0.08 m, Angle: Rotation2d(Rads: -0.74, Deg: -42.63)) 
 Odometry: Pose2d(Translation2d(X: 2.00, Y: 2.00), Rotation2d(Rads: 0.35, Deg: 20.30)) 
 f: true 
 *** End trajectory command. Interrupted:false 
 End trajectory

The “Interrupted” tells me that the trajectory ended on its own and was not interrupted

The “f” value is the result of the "isFinished method.

I am using PPSwerveControllerCommand specifically. Technically I designed my own command, extending PPSwerveControllerCommand so I can do extra telemetry as I run trajectory.

As part of further testing I removed the command finish check, and noted that it’s indeed not the timing issue. Once the rotation reaches about 1/2 of the final angle, the controller does not believe that the robot should turn anymore even though the end condition angle-wise is not reached yet.
So, it looks like it’s just not calculating the trajectory outputs correctly for holonomic moves. Any suggestions?

Also I generated the CSV and JSON files via PathPlanner, and they clearly indicate that the trajectory SUPPOSE to stop at 89.9999 degrees (rather than 40 or so where it’s stopping now)

What kind of control are you using? Id it’s PID I’d recommend checking your gains
If you have too low of a kP it won’t give enough of a correction to overcome the friction in the system

2 Likes

Here are the controls:

public AutonomousTrajectoryRioCommand(PathPlannerTrajectory trajectoryPath) {
// Use addRequirements() here to declare subsystem dependencies.

super(
  trajectoryPath,
  RobotContainer.driveSubsystem::getPose,
  Constants.Swerve.SWERVE_KINEMATICS,
  new PIDController(Constants.Swerve.DRIVE_MOTOR_KP,
                    Constants.Swerve.DRIVE_MOTOR_KI,
                    Constants.Swerve.DRIVE_MOTOR_KD),
  new PIDController(Constants.Swerve.DRIVE_MOTOR_KP,
                    Constants.Swerve.DRIVE_MOTOR_KI,
                    Constants.Swerve.DRIVE_MOTOR_KD),
  new PIDController(Constants.Swerve.ANGLE_MOTOR_KP,
                    Constants.Swerve.ANGLE_MOTOR_KI,
                    Constants.Swerve.ANGLE_MOTOR_KD),
  RobotContainer.driveSubsystem::setDesiredStates,
  false,
  RobotContainer.driveSubsystem
);
this.trajectoryPath = trajectoryPath;

}

And the constants are:

public static final double DRIVE_MOTOR_KP = 3.0;
public static final double DRIVE_MOTOR_KI = 0.005;
public static final double DRIVE_MOTOR_KD = 0.01;
public static final double ANGLE_MOTOR_KP = 0.75;
public static final double ANGLE_MOTOR_KI = 0.005;
public static final double ANGLE_MOTOR_KD = 0.01;

They definitely move the robot. Any suggestions on a change to try?

And as I said, the robot turns. It just finishes the turn way too early, and not even trying to go further)

First of all I’d recommend separating your auton PID constants from swerve module constants.

After that, raise your P by a significant factor to make sure it’s affecting things. Raise it to something like 8 and see how it does.

My initial guess, as well as with the PID constants being too low possibly, would be that the trajectory is “ending” when it gets to the end of the path, and does not wait for the angle is correct. Is that what you are observing?

It is much easier for those smarter than I to help troubleshoot if you provide a link to a github repo or something of the sort for others to search through your code for any pitfalls.

The PathPlanner ends trajectories on the estimated run time, rather than completion of the PATH.

In other words, once the time expires, even if it’s not done yet, you’re done :slight_smile:

Here is from the PathPlanner library code to prove is:

@Override
public boolean isFinished() {
return this.timer.hasElapsed(transformedTrajectory.getTotalTimeSeconds());
}

A full code repo is here: GitHub - FRC999/2024Swerve at master

The command is AutonomousTrajectoryRioCommand

We’re actually making yet another community Swerve project, with a goal to fully document all aspects of programming swerve, so it will not be a “black box” for the team, but something that all programming students will understand in detail. We’re almost done and ready for a reveal, and that’s the last item we need to check/fix.

The PID constants are separate. We do not use software PID for teleop driving (only hardware PID, see the code link I posted).
Our code has A LOT of documentation built into it, which explains our programming decisions among other items.

Then, you really should consider better names for those PID constants (which presumably is what triggered the comment). “DRIVE_MOTOR_KP” and “ANGLE_MOTOR_KP” distinctly sounds like constants used to control a single swerve module. You also have them in “Constants.Swerve” which again sounds like a swerve module and not your full drivetrain/chassis.

I would suggest names like “CHASSIS_DRIVE” and “CHASSIS_TURN”, and maybe use “Drivetrain” or “Chassis” for the container.

Names do matter, not because the programmmer does not understand, but because everyone else needs to understand.

2 Likes

Pathplanner is not meant for doing turn in place stuff. Use a separate command with a PID or profiled PID controller for turn in place.

The reason you’re seeing it stop even when removing the isFinished check is that most of the rotation is velocity based. Once the timer is past what the end of the path should be, the rotational velocity commanded is going to be 0, with only a small amount of correction from your pid constants.

4 Likes

Thank you. I THINK we saw something similar on trajectories that had both linear and holonomic components. But I will retest that. For instance, going forward, and turning 90 degrees at the same time.
What was strange is that the CSV representation of the trajectory I mentioned indeed indicated the final position at the 90 degree angle.
I am curious - why not cover in-place holonomic turns? Why would that be different than linear? I mean - ultimately all you do is producing a set of the swerve module states over time…
I will also test PID increase. It appears in a current implementation the PID essentially dictates what the trajectory controller think about the “speed of reaction” from the actual motors. In other words, while the trajectory controller dictates the power output and angles, very high PID seems to indicate that your robot reacts very fast to the changing module states (which is indeed the case with our robot). So, hopefully we can fix/tune that.
The linear motion tracking without holonomic component in the current controller version is working well (in any direction including sideways), with the slight exception of chassis counter-rotation on slippery surfaces (on such surfaces synchronous wheel turns cause slight chassis counter-rotation in the opposite direction, which can accumulate over longer distances - known issue with the swerve).

It just wasn’t built for that. Holonomic rotation was added on to a generator that was initially made just for differential drive trains. It should technically be possible in next years version, but you’d really get better results doing turn in place stuff with a separate command instead of path planner. It’s really made for following paths with rotation as a component instead of just rotation exclusively, so something does does handle rotation exclusively would be better for that purpose.

Thank you. I will try it.
Any suggestions on PID? Some recommended to try really high PID values. Also what’s your take on Ki?

Your constants look like they should be mostly fine. If the robot is failing to complete a rotation I would guess it’s just not being given enough time to complete it through following a path.

I think we only used P for most of our controllers this year and didn’t need to use any I/D. I also think I causes more problems than it solves most of the time. It’s really annoying to work with most of the time. Any time we’ve tried using it we ended up just getting rid of it after messing around with it for a while.

1 Like

Thanks.

We will do some more in-depth testing of various PID constants. Based on our observations, currently I believe that the trajectory simply ends earlier than it should, like you said, with the primary reason being - the robot reports itself to the controller being much less responsive than it really is. In other words, PathPlanner I believe does not take into consideration if the PID constants are set too low, regardless whether the robot can actually respond fast to the requests.

On the note of I and D - I’ve read a very interesting scientific paper done by university students that designed swerve-drive hospital bed. Essentially their bed was a regular swerve chassis that we use, and they did in-depth analysis of PID for it. They came up with a controversial result that for precision “I” may need to exceed “P” for holomonic trajectories. Anyway - we will try that and see if we can make some headway.
On that note - I realize that current implementation primarily uses velocity PID. I was wondering, however, if you would be willing to consider putting a slightly different algorithm using position PID depending on an extra flag. I realize that it may be much work, so perhaps, we can help in some way? We have been using your product last year with the tank drive, and I have to say without it we would not have gotten as high as we got. So, we want to continue using it as the team thinks very highly of it.

It is a position PID. It uses what the velocity at a given point should be as a feedforward and then runs PID on top of that to minimize positional error.