Rotational Swerve Simulation Angle Issues

[SirFire]

The Problem

Our team has been trying to get swerve working this year and we’ve run into a slight issue. That being that, when simulating the swerve module states with an Xbox controller, rotational inputs make the modules turn like so: (All speeds in all screenshots are positive and all angles are unoptimized.)

Translating works exactly as expected. For example, moving forward and to the left yields this:

However, while not touching the translation input and only moving the rotational input fully to the left, I get this:

Each angle is opposite of itself if I push the rotational input to the right.
Translating and rotating looks like this: (Forward and rotating left)

The Code

We’ve defined the kinematics object like so:

public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
        new Translation2d(kWheelBase / 2, kTrackWidth / 2), // Front left
        new Translation2d(kWheelBase / 2, -kTrackWidth / 2), // Front right
        new Translation2d(-kWheelBase / 2, kTrackWidth / 2), // Rear left
        new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); // Rear right

And later in our drive() method, we get the module states:

ChassisSpeeds chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotSpeed);

SwerveModuleState[] states = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds);

m_frontLeftModule.setModuleState(states[0]);
m_frontRightModule.setModuleState(states[1]);
m_rearLeftModule.setModuleState(states[2]);
m_rearRightModule.setModuleState(states[3]);

None of this has been run on an actual robot yet, only simulations. I’ve seen that a cause could be that the order of the modules was not the same across the code, but I believe this is all in the same order, that being: front left, front right, rear left, rear right.
Please inform me if there is any more information I can provide.

[Jizelda]

When we first started to code our swerve we faced the same problem.
If I remember correctly, it happened because the axis system that wpilib uses isn’t what we assumed it is, so when we constructed the kinematics object we defined each module in the wrong place.

Try going over wpilib coordinate system page, i remember it cleared a lot of things for us.

[jdao]

How are all of your motors/encoders inverted? The inversions should be consistent across all modules. Additionally, you may need to manually invert your encoders when simulating it instead of relying on the CTRE/Rev get/set inversion functions.

[SndMndBdy]

The pictures look as if you have left and right swapped somewhere. In particular, when everything is working correctly, when only rotating, you want to see a “diamond” shape for the wheel orientation, and if you swap your left and right pictures, you get that. I believe that your picture of translating while rotating is consistent with that, although that’s a little harder to visualize.

[SirFire]

The data shown in those pictures is only from SwerveModuleStates. Nothing to do with any motors yet. We’re simply putting our controller inputs into a ChassisSpeeds object which then gets converted into SwerveModuleStates.
One thing I neglected to mention is that I am using the controller’s getY(), getX(), and getZ() for the ChassisSpeeds vx, vy, and omega arguments in that order. When I used getX() for vx and getY() for vy, translating was 90 degrees off. Unsure if that has anything todo with the current problem or not.

[SirFire]

Thank you, I did take a look at that page and it looks like we are correctly following the local coordinate system unless I am missing something.

[SirFire]

I don’t understand this: If I change the code to get field relative chassis speeds with a constant heading of 45 degrees, translation again works as expected, being 45 degrees to the left of 0. However, this change doesn’t affect the rotation as rotating left still looks like this:

[SndMndBdy]

My strong suspicion is that the things you are outputting to your dashboard that you are labeling “left” are actually the right wheels, and Vice versa. But it is really hard to validate that without seeing all of the code. Perhaps you could paste it all in here, or share the code repo.

[SirFire]

Alright, here is the relevant code for that:

Constants.java

// Chassis configuration

// Distance between centers of right and left wheels on robot
public static final double kTrackWidth = Units.inchesToMeters(28.25); // TODO: Check these
// Distance between front and back wheels on robot
public static final double kWheelBase = Units.inchesToMeters(28.25); // TODO: Check these

public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
  new Translation2d(kWheelBase / 2, kTrackWidth / 2), // Front left
  new Translation2d(kWheelBase / 2, -kTrackWidth / 2), // Front right
  new Translation2d(-kWheelBase / 2, kTrackWidth / 2), // Rear left
  new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); // Rear right

DriveSubsystem.java

public void drive(double xSpeed, double ySpeed, double rotSpeed) {
    ChassisSpeeds chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rotSpeed, Rotation2d.fromDegrees(0));

    SwerveModuleState[] states = DriveConstants.kDriveKinematics.toSwerveModuleStates(chassisSpeeds);

    m_frontLeftModule.setModuleState(states[0]);
    m_frontRightModule.setModuleState(states[1]);
    m_rearLeftModule.setModuleState(states[2]);
    m_rearRightModule.setModuleState(states[3]);

    SmartDashboard.putNumber("FL Angle", states[0].angle.getDegrees());
    SmartDashboard.putNumber("FR Angle", states[1].angle.getDegrees());
    SmartDashboard.putNumber("RL Angle", states[2].angle.getDegrees());
    SmartDashboard.putNumber("RR Angle", states[3].angle.getDegrees());

    SmartDashboard.putNumber("FL Speed", states[0].speedMetersPerSecond);
    SmartDashboard.putNumber("FR Speed", states[1].speedMetersPerSecond);
    SmartDashboard.putNumber("RL Speed", states[2].speedMetersPerSecond);
    SmartDashboard.putNumber("RR Speed", states[3].speedMetersPerSecond);
  }

DriveWithJoystickCmd.java

@Override
  public void execute() {
    m_driveSubsystem.drive(
      -MathUtil.applyDeadband(m_controller.getLeftY(), 0.1),
      MathUtil.applyDeadband(m_controller.getLeftX(), 0.1),
      MathUtil.applyDeadband(m_controller.getRawAxis(2), 0.1)
    );
  }

I am using a controller instead of a joystick, so ignore the file name. I believe this is the only code that has anything to do with the topic at hand. Please inform me if there is more I should send.

[SirFire]

I did swap the left and right as suggested, and that gave the values we wanted which is great! But I’d still like to know why it happened in the first place so we can avoid this issue later down the road. Isn’t the order of the SwerveModuleStates supposed to be in the same order they’re provided?

[SndMndBdy]

After reading through your code, I can understand your earlier comment of

“One thing I neglected to mention is that I am using the controller’s getY(), getX(), and getZ() for the ChassisSpeeds vx, vy, and omega arguments in that order. When I used getX() for vx and getY() for vy, translating was 90 degrees off.”

The reason that is happening is because the meaning of “X” and “Y” for the ChassisSpeeds object is different than what it is for the controller. In particular, see:

ChassisSpeeds (WPILib API 2023.4.3)

Where it says:

|double|vxMetersPerSecond|Represents forward velocity w.r.t the robot frame of reference.|
|double|vyMetersPerSecond|Represents sideways velocity w.r.t the robot frame of reference.|

Whereas in your joystick, “X” refers to “sideways” and “Y” refers to forwards/backwards.

So that’s why you need to switch the meaning of X and Y on your way into the SwerveDriveKinematics.

All that being said, that shouldn’t matter as it relates to exclusively rotating, since both X and Y are zero.

I looked through your code to try to find what might be causing this, but I can’t find it. I even compared it to the MAXSwerve example code, and as far as I can tell it is using things in the same way you are. But I suppose it’s possible that they have things mislabeled there, as well! ( https://github.com/REVrobotics/MAXSwerve-Java-Template/blob/main/src/main/java/frc/robot/subsystems/DriveSubsystem.java ). Later today I will check against my team’s code from this year to see if I can see what’s different.

[SndMndBdy]

I took a look at my team’s code from this year, and as far as I can tell we are doing the same thing as you. But we weren’t having this problem. The only thing that comes to mind is the possibility that a bug has been introduced to WPLIB recently. And if you’re building your code with a newer version of WPILIB, perhaps there is something wrong in there that’s causing this. It does appear that there have been several changes to this area of the code recently:

History for wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java - wpilibsuite/allwpilib (github.com)

I perused them and didn’t see anything obviously wrong, but perhaps one of the WPILIB developers could comment. @Thad_House , any ideas?

[prensing]

As far as I can see, the full code repository was not posted here, so there could be lots of oddities hiding. I suggest the OP post a link to the actual code.

[SirFire]

Sorry, I didn’t have a remote repo for this yet. Here it is now: SirFireE/Swerve8551 (github.com)

This issue was solved by switching the right and left SwerveModuleStates, but logging the array of states to AdvantageScope is proving to be problematic due to the mix-up. I’d very much like to find out the origin of this issue.

[Quake]

Everything looks correct and cloning your code and logging the swerve module states from the drive method in simulation gives me the angles I expected:

    SmartDashboard.putNumber("FL Angle", states[0].angle.getDegrees());
    SmartDashboard.putNumber("FR Angle", states[1].angle.getDegrees());
    SmartDashboard.putNumber("RL Angle", states[2].angle.getDegrees());
    SmartDashboard.putNumber("RR Angle", states[3].angle.getDegrees());

Not sure if the wrong values are being displayed or how you are seeing your results. Could you verify you get these angles in the sim gui with the proper module order (0, 1, 2, 3) and a positive rotSpeed?

Also, if you are using an xbox controller, why use the Joystick class instead of XboxController/CommandXboxController? Currently getZ() maps to the left trigger axis on an xbox controller.