Swerve Trajectory Tracking with Heading Control

There is a swerve specific command/controller for doing so, see the sample WPILib Java code here:

Ramsete is specific to tank drive styled robots due to the fact that they cannot move sideways.

3 Likes

This is what we are currently using, and it is behaving weirdly. To start with If I feed it a end point of x = 1 , y=1 and theta = 0 I would expect it to end at the point 1 meter in the positive x and y directions. But it moves about 4.3 meters in both directions, but does so constantly. I also checked to see if the odomotry thought that it was going 1 meter in both directions, but it prints out as 4.3 in both. The odomotry is seemingly accurate. I tried printing out the x and y values of the translation 2d that we were feeding it immediately before they were received, and they were 1, 1 .That alone is concerning, but manageable if it always goes in a 4.3 to 1 ratio. On top of that if we use the other trajectory constructor that allows us to feed waypoints instead of 2d translations, then the rotation is all off. No matter what I do I can’t find a relationship between the fed rotation and the actual rotation it achieves. Again I printed the odomotry and the fed numbers and found that when I feed it 1 radian, it rotates about 2 radians. Then when I feed it .5 radians, it moves more than 2 radians. I couldn’t find any mathematical relationship between the radians it was receiving at the waypoints, and the radians it was going to. I do want to say that it is possible that it is not completely finishing the path, because when we go back from autonomous to telliop, it then seems to have the velocity limit of the point on the path that it was just on. I am thoroughly confused as to what is going on, and it is quite possible that I am just out of my depth. Any help would be appreciated.

1 Like

Yes, we have also been experiencing the same weirdness. Below I have attached a video of our autonomous. At the time we were using transform2d to generate the path, but when we changed to Pose2d and added a rotation, the robot would drive through the point at that heading causing some problems.

github robot2021 : PathWeaver Branch
First Successful Test Of Swerve Drive

2 Likes

My assumption, based on your code, is that you have a unit conversion error here:

You initialize the swerve kinematics in inches, but then feed it into the swerve controller. Since a lot of the kinematics/odometry libraries assume you are using SI units, I would bet that somewhere in the math, the conversion from inches to meters doesn’t happen, which is causing your issues. Try changing this to meters and see what happens.

Edit: Also, those values should be divided by 2, since all those translations will be relative to the center of your robot.

1 Like

Hmm… I’m not sure why but I seem to have issues generating a valid swerve trajectory with the points you used here:

If you remove the points from either path starting from Pose2d(8.2, 2, zero) onwards, do you still get issues? If so, a workaround might be to split your path into separate trajectories to get around this. You can then customize the start/end speeds of the parts so that you don’t stop when transitioning between trajectories.

2 Likes

I fixed the unit error, and it is still behaving weird with just translations as points. It still moves -4.3 times the fed unit according to the odometry, which still seems to be accurate. Any ideas? @Peter_Johnson?

1 Like

Thank You! I like the idea of using multiple trajectories. Currently, we are experimenting and learning by generating trajectories in different ways and see how they run. I also plan on using PathWeaver to generate trajectories. Motion profiling is also something I plan to look into. Thanks for your help!

1 Like

The trajectory generator was designed for tank drive, but can work with swerve. The important thing to realize is that the heading of the point on the trajectory is just the field-oriented translation vector’s direction. That is, that’s the direction the robot is moving. If you have swerve, the robot can be facing any other direction you want at that time.

Trajectory generation, at the moment, doesn’t allow you to specify the actual robot heading as a separate variable at each waypoint, so it’s not going to generate a nice continuous heading adjustment for you at each point in the trajectory. You will have to do this yourself.

One simple thing you can do is just set your heading to the trajectory heading, which essentially turns your fancy swerve drive into a tank drive. I will leave it as an exercise for you to determine what the best solution is. :slight_smile:

Our team uses Pure Pursuit for our path following pipeline (with swerve). We maintain a heading using a simple PID controller, and we specify the desired heading in our path generation program. Not sure if this is directly applicable to the path generation program you use, just my 2¢.

2 Likes

In the PID values using frc-characterization, did you specify the tool to generate PID values for Talons/Falcons? Additionally, did you verify that the configuration of your project was correct? (e.g. deploy the test project, verify that 1 wheel rotation is reported correctly by physically moving your drive wheel one full rotation and checking the SmartDashboard value).

Thinking about it again, your 4.3 magnitude difference is close to 4, which is what you should multiply 2048 by to set the encoder counts in the frc-characterization project to be (See [this post](https://www.chiefdelphi.com/t/robot-. characterization-trajectory-tracking-help/391685/4?u=jdao)). Did you do this?

This is incorrect. You can specify the headings of each point if you generate a trajectory with just a list of Pose2d() points. As an example:

    m_trajectory = TrajectoryGenerator.generateTrajectory(
            new Pose2d(1,1, new Rotation2d()),
            List.of(new Translation2d(3, 2),
                    new Translation2d(5, 3),
                    new Translation2d(7, 2)),
            new Pose2d(9, 1, new Rotation2d()),
            new TrajectoryConfig(4, 4)
    );

translationPath

    m_trajectory = TrajectoryGenerator.generateTrajectory(
            List.of(new Pose2d(1,1, new Rotation2d()),
                    new Pose2d(3,2, new Rotation2d()),
                    new Pose2d(5,3, new Rotation2d()),
                    new Pose2d(6,2, new Rotation2d()),
                    new Pose2d(9,1, new Rotation2d())),
            new TrajectoryConfig(4, 4)
    );

pose2dPath

FRC-Docs Page

This allows you to specify waypoint heading, not the robot’s heading independent of the movement. If you see in your first example, I want to follow that path, but have the robot constantly ‘looking’ towards the bottom. This isn’t possible with these tools because it is controlling robot heading as if it is tank drive, and not swerve drive. Again trajectory doesn’t support swerve drive heading.

1 Like

We didn’t use the characterization tool that you mentioned.

Thank you for the verification, I thought this might be the case. Do you have any ideas as to why we are having about a 4.3 magnitude difference between the points we specify and where we are going?

Sorry it took so long for me to reply, I wanted to read up on the Pure Pursuit that you mentioned. The paper that I read was interesting, but unless I’m mistaken it still seems as though it was intended for a tank drive. I am interested in how you converted the tank drive outputs into swerve and how you then manipulate them into heading control.

I’m guessing your read this paper, which is what we based our Pure Pursuit implementation off of.

Long story short, we don’t convert the tank drive outputs. What we did instead was just drive towards the point where our look ahead radius intersected the path. Imagine that as your [X,Y] vector, with the magnitude of the vector being the velocity of the nearest point in the path. Here’s a link to our (unfinished) technical write-up.

For Swerve drives, if you want to specify a heading independent of the path, you need to use the other Swerve4ControllerCommand() constructor, where you specify the heading target you want after the the theta controller.

Regardless of if you used the frc-characterization tool, can you confirm that aphysical rotation of one of the swerve drive’s module returns a distrance traveled value equal to the circumference of the wheel?

Hi @benjiboy2539, this is Matt from team 2052 KnightKrawler. Our team recently starting working with the WPILib swerve library to try and see if it is feasible and similar to your question we have been looking for a way to change our robot’s heading while following a set path. Did your team ever solve your heading problem where you could follow a path, but couldn’t specify a heading?

Below I’ve included an auto path which, without the () -> Rotation2d.fromDegrees(_) line, goes forward and then turn left. ( () -> Rotation2d.fromDegrees(_) is included in both commands because otherwise the frame rotation gets set back to 0 degreese) However, Once we include what should rotate the frame of the robot while following the same path our robot begins to have unexpected behavior.

Specifically using a 45 degree rotation results in the path going straight forward, pausing, and then continuing along the same straight path. Using a 90 degree rotation results in the path going straight pausing and then going right relative to the field. We’ve tried using the Trajectory.transformBy(new Transform2d(new Translation2d(0, 0), new Rotation2d(_))) to adjust our path, but it just seems to cause more unwanted behavior.

Even more troubling, checking our Glass field odometry tracker it shows the same path being followed regardless of rotation, meaning WPI thinks it’s executing the right behavior.

SwerveControllerCommand forward = new SwerveControllerCommand(
  TrajectoryGenerator.generateTrajectory(
    new Pose2d(0, 0, new Rotation2d()),
    new ArrayList<Translation2d>(),
    new Pose2d(2, 0, new Rotation2d()),
    defaultConfig
  ), drivetrain::getPose, DrivetrainConstants.kSwerveDriveKinematics,
  new PIDController(AutonomousConstants.kPXController, 0, 0),
  new PIDController(AutonomousConstants.kPYController, 0, 0),
  thetaController, drivetrain::setModuleStates,
  drivetrain
);

SwerveControllerCommand turn = new SwerveControllerCommand(
  TrajectoryGenerator.generateTrajectory(
    new Pose2d(2, 0, new Rotation2d()),
    new ArrayList<Translation2d>(),
    new Pose2d(2.3, 0, Rotation2d.fromDegrees(0)),
    defaultConfig
  ),
  drivetrain::getPose,
  DrivetrainConstants.kSwerveDriveKinematics,
  new PIDController(AutonomousConstants.kPXController, 0, 0),
  new PIDController(AutonomousConstants.kPYController, 0, 0),
  thetaController,
  () -> Rotation2d.fromDegrees(45),
  drivetrain::setModuleStates,
  drivetrain
);

SwerveControllerCommand left = new SwerveControllerCommand(
  TrajectoryGenerator.generateTrajectory(
    new Pose2d(2.3, 0, Rotation2d.fromDegrees(0)),
    new ArrayList<Translation2d>(),
    new Pose2d(2.3, 2, Rotation2d.fromDegrees(0)),
    defaultConfig
  ),
  drivetrain::getPose,
  DrivetrainConstants.kSwerveDriveKinematics,
  new PIDController(AutonomousConstants.kPXController, 0, 0),
  new PIDController(AutonomousConstants.kPYController, 0, 0),
  thetaController,
  () -> Rotation2d.fromDegrees(45), // <- default behavior is 0
  drivetrain::setModuleStates,
  drivetrain
);

If you have any questions about our code or want clarifications please let me know.

Does this mean your robot odometry appears to follow the intended path, but the actual robot does not(e.g. going right instead of left)? What does your odometry update look like?

Have you confirmed the odometry correctly tracks while driving in teleop, and that supplying ChassisSpeeds in teleop controls the swerve modules to the correct orientation and speed?

We switched to the SDS Swerve Lib and rebuilt our code which seemed to fix our problems. We’re currently thinking there was either something slightly wrong with our code or a bug in the WPI swerve library given how new it is. However, we are still using WPI’s swerve odometry and kinematics which leads me to believe it was most likely an error in our code.