Swerve PathPlanner Trajectory ends and does not exit

Technical Java
1

Hi everyone, I am back again with another question after bashing my head against the wall for a week. We finally got our robot’s coordinate system correct and robot goes in the right direction every time, but at the end of a path, the robot keeps drifting in the direction that was last set. We tried using both HolonomicDriveController and PPHolonomicDriveController (or whatever the PathPlanner equivalent is called) and that did not change our results. Our odometry is printing the correct signs compared to the pathplanner goal. I am printing out our feed forwards and feedback values that are calculated in the HolonomicDriveController class and I can see that even after the robot supposedly reaches the end of the timed trajectory, the last value for the feed forward is still being included in the ChassisSpeed calculations. I expected that the generated routine would have a goal FF of 0 for both X and Y but I guess that assumption was wrong. Now, I don’t know if this is okay and the command would work if we were able to exit the command correctly. I know to exit the command it has to run for long enough AND it has to be within a threshold of the goal in all three DOF. The robot is currently not going the correct distance in X and Y, which we assumed is because of our wheel diameter after talking to a few teams. I tried inverting the output of our PID controllers in HolonomicDriveController thinking maybe they were working with the error instead of against it, but that produced a similar result. Any ideas? I can provide specific code snippets if required.

2

The drift is because PathPlanner does not update the module velocities to 0 at the end of its command. My recommendation is adding a .andThen() onto the command to do so wherever you generate a autonomous routine. For example (pseudocode, I don’t remember the exact naming): new PPSwerveControllerCommand(args).andThen(drives.setModuleSpeeds(drives.kinematics.toModuleStates(new ChassisSpeeds(0, 0)))

2 Likes
3

The command doesn’t end, as the routine does not think it has reached the goal I guess? So I don’t think that code would ever execute

1 Like
4

The command should end either way, it’s timer based, not position based. See: https://github.com/mjansen4857/pathplanner/blob/6a19300a031c23b7073d5b37bea3ea3265ec7f67/pathplannerlib/src/main/java/com/pathplanner/lib/commands/PPSwerveControllerCommand.java#L172

5

Can you share a link to your code? How do you know the command isn’t ending?

6

Let me talk to who runs our github so I can give total access. We have a printout inside the command that shows if it is still running or not.

7

I agree fully with @wanderingcarrot here. Pathpanner trajectories are timer based, so Are you absolutely sure that the command isnt ending? You could have some wierd unit issue that causes pathplanner to assume it will take an unreasonable amount of time to complete the trajectory. IIRC you can check the time it will after importing the trajectory, so you should be able to double check. Also you should be able to see the command ending in wpilib glass. I really doubt the command not ending is the issue. Do you set velocity to 0 after the pathplanner command? Are you sure that command is executing and functioning properly?

1 Like
8

Check and see what the isFinished() method is returning in your DriveController, should be doing something like this

  @Override
  public boolean isFinished() {
    return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
  }

when this is true the command should automatically exit and if you use .andThen() with a function to set the swervestates to 0 this should stop it.

1 Like
9

This is a bit of a Hail Mary, but we are seeing the same issue that the OP reported, where our path planner command never ends. We’ve tried a ton of different things, including wrapping it with a race or a deadline to hopefully terminate it in time. But in all cases, it finishes the path and then slowly drifts in the direction of last movement.

Link to current code is below (it tries to use path events, which have also never worked for us, and also tries to wrap the PP command in a deadline. No joy.

Any suggestions?

1 Like
10

We actually got this fixed a while ago, but in your case can you try running it in a ParallelRaceGroup with a WaitUntilCommand that is a lambda expression for a button input? I.e. it could look like
WaitUntilCommand(() → button.getAsBoolean()) inside the parallel race group, and see if you trigger the button if that stops the movement. I would use PPServer (look up on the Path Planner github, it is EXTREMELY useful for debugging) and see where the robot thinks its going, etc

11

Who needs a PP server when you’ve got inbuilt display on all of your favorite dashboards? The Field2d Widget — FIRST Robotics Competition documentation

12

I don’t like it as much, personal preference

13

Thanks guys! We figured it out - the PP command was in a parallel group with another command that wasn’t finishing. Once we fixed that, PP works as advertised.

Early on in debugging we assumed we were doing something wrong with PP and never stepped back to revisit the assumption after banging our heads on the wall. Good lesson for the team about tunnel vision.

14

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.