Package frc.robot.subsystems.swervedrive2.swervelib.math

## Class SwerveKinematics2

- java.lang.Object
- - edu.wpi.first.math.kinematics.SwerveDriveKinematics
  - - frc.robot.subsystems.swervedrive2.swervelib.math.SwerveKinematics2

-     public class SwerveKinematics2
      extends edu.wpi.first.math.kinematics.SwerveDriveKinematics

  Clone of WPI SwerveKinematics, which implements second order kinematics when calculating modules states from chassis speed.

  Makes use of `SwerveModuleState2` to add the angular velocity that is required of the module as an output.

- - ### Field Summary

    Fields
    Modifier and Type | Field | Description
    --- | --- | ---
    `private org.ejml.simple.SimpleMatrix` | `bigInverseKinematics` |
    `private org.ejml.simple.SimpleMatrix` | `m_forwardKinematics` |
    `private org.ejml.simple.SimpleMatrix` | `m_inverseKinematics` |
    `private edu.wpi.first.math.geometry.Translation2d[]` | `m_modules` |
    `private SwerveModuleState2[]` | `m_moduleStates` |
    `private int` | `m_numModules` |
    `private edu.wpi.first.math.geometry.Translation2d` | `m_prevCoR` |

  - ### Constructor Summary

    Constructors
    Constructor | Description
    --- | ---
    `SwerveKinematics2​(edu.wpi.first.math.geometry.Translation2d... wheelsMeters)` | Constructs a swerve drive kinematics object.

  - ### Method Summary

    All Methods Static Methods Instance Methods Concrete Methods
    Modifier and Type | Method | Description
    --- | --- | ---
    `static void` | `desaturateWheelSpeeds​(SwerveModuleState2[] moduleStates, double attainableMaxSpeedMetersPerSecond)` | Renormalizes the wheel speeds if any individual speed is above the specified maximum.
    `static void` | `desaturateWheelSpeeds​(SwerveModuleState2[] moduleStates, edu.wpi.first.math.kinematics.ChassisSpeeds currentChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)` | Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well as getting rid of joystick saturation at edges of joystick.
    `edu.wpi.first.math.kinematics.ChassisSpeeds` | `toChassisSpeeds​(SwerveModuleState2... wheelStates)` | Performs forward kinematics to return the resulting chassis state from the given module states.
    `SwerveModuleState2[]` | `toSwerveModuleStates​(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)` | Performs inverse kinematics.
    `SwerveModuleState2[]` | `toSwerveModuleStates​(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds, edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters)` | Performs inverse kinematics to return the module states from a desired chassis velocity.
    `edu.wpi.first.math.geometry.Twist2d` | `toTwist2d​(edu.wpi.first.math.kinematics.SwerveModulePosition... wheelDeltas)` | Performs forward kinematics to return the resulting chassis state from the given module states.

    - ### Methods inherited from class edu.wpi.first.math.kinematics.SwerveDriveKinematics

      `desaturateWheelSpeeds, desaturateWheelSpeeds, toChassisSpeeds`

    - ### Methods inherited from class java.lang.Object

      `clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait`

- - ### Field Detail

    - #### m_inverseKinematics

          private final org.ejml.simple.SimpleMatrix m_inverseKinematics

    - #### m_forwardKinematics

          private final org.ejml.simple.SimpleMatrix m_forwardKinematics

    - #### bigInverseKinematics

          private final org.ejml.simple.SimpleMatrix bigInverseKinematics

    - #### m_numModules

          private final int m_numModules

    - #### m_modules

          private final edu.wpi.first.math.geometry.Translation2d[] m_modules

    - #### m_moduleStates

          private final SwerveModuleState2[] m_moduleStates

    - #### m_prevCoR

          private edu.wpi.first.math.geometry.Translation2d m_prevCoR

  - ### Constructor Detail

    - #### SwerveKinematics2

          public SwerveKinematics2​(edu.wpi.first.math.geometry.Translation2d... wheelsMeters)

      Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations as Translation2ds. The order in which you pass in the wheel locations is the same order that you will receive the module states when performing inverse kinematics. It is also expected that you pass in the module states in the same order when calling the forward kinematics methods.
      Parameters:
        `wheelsMeters` - The locations of the wheels relative to the physical center of the robot.

  - ### Method Detail

    - #### desaturateWheelSpeeds

          public static void desaturateWheelSpeeds​(SwerveModuleState2[] moduleStates,
                                                   double attainableMaxSpeedMetersPerSecond)

      Renormalizes the wheel speeds if any individual speed is above the specified maximum.

      Sometimes, after inverse kinematics, the requested speed from one or more modules may be above the max attainable speed for the driving motor on that module. To fix this issue, one can reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the absolute threshold, while maintaining the ratio of speeds between modules.

      Parameters:
        `moduleStates` - Reference to array of module states. The array will be mutated with the normalized speeds!
        `attainableMaxSpeedMetersPerSecond` - The absolute max speed that a module can reach.

    - #### desaturateWheelSpeeds

          public static void desaturateWheelSpeeds​(SwerveModuleState2[] moduleStates,
                                                   edu.wpi.first.math.kinematics.ChassisSpeeds currentChassisSpeed,
                                                   double attainableMaxModuleSpeedMetersPerSecond,
                                                   double attainableMaxTranslationalSpeedMetersPerSecond,
                                                   double attainableMaxRotationalVelocityRadiansPerSecond)

      Renormalizes the wheel speeds if any individual speed is above the specified maximum, as well as getting rid of joystick saturation at edges of joystick.

      Sometimes, after inverse kinematics, the requested speed from one or more modules may be above the max attainable speed for the driving motor on that module. To fix this issue, one can reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the absolute threshold, while maintaining the ratio of speeds between modules.

      Parameters:
        `moduleStates` - Reference to array of module states. The array will be mutated with the normalized speeds!
        `currentChassisSpeed` - The current speed of the robot
        `attainableMaxModuleSpeedMetersPerSecond` - The absolute max speed that a module can reach
        `attainableMaxTranslationalSpeedMetersPerSecond` - The absolute max speed that your robot can reach while translating
        `attainableMaxRotationalVelocityRadiansPerSecond` - The absolute max speed the robot can reach while rotating

    - #### toSwerveModuleStates

          public SwerveModuleState2[] toSwerveModuleStates​(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds,
                                                           edu.wpi.first.math.geometry.Translation2d centerOfRotationMeters)

      Performs inverse kinematics to return the module states from a desired chassis velocity. This method is often used to convert joystick values into module speeds and angles.

      This function also supports variable centers of rotation. During normal operations, the center of rotation is usually the same as the physical center of the robot; therefore, the argument is defaulted to that use case. However, if you wish to change the center of rotation for evasive maneuvers, vision alignment, or for any other use case, you can do so.

      In the case that the desired chassis speeds are zero (i.e. the robot will be stationary), the previously calculated module angle will be maintained.

      Overrides:
        `toSwerveModuleStates` in class `edu.wpi.first.math.kinematics.SwerveDriveKinematics`
      Parameters:
        `chassisSpeeds` - The desired chassis speed.
        `centerOfRotationMeters` - The center of rotation. For example, if you set the center of rotation at one corner of the robot and provide a chassis speed that only has a dtheta component, the robot will rotate around that corner.
      Returns:
        An array containing the module states. Use caution because these module states are not normalized. Sometimes, a user input may cause one of the module speeds to go above the attainable max velocity. Use the `DesaturateWheelSpeeds` function to rectify this issue.

    - #### toSwerveModuleStates

          public SwerveModuleState2[] toSwerveModuleStates​(edu.wpi.first.math.kinematics.ChassisSpeeds chassisSpeeds)

      Performs inverse kinematics. See `toSwerveModuleStates(ChassisSpeeds, Translation2d)` toSwerveModuleStates for more information.
      Overrides:
        `toSwerveModuleStates` in class `edu.wpi.first.math.kinematics.SwerveDriveKinematics`
      Parameters:
        `chassisSpeeds` - The desired chassis speed.
      Returns:
        An array containing the module states.

    - #### toChassisSpeeds

          public edu.wpi.first.math.kinematics.ChassisSpeeds toChassisSpeeds​(SwerveModuleState2... wheelStates)

      Performs forward kinematics to return the resulting chassis state from the given module states. This method is often used for odometry -- determining the robot's position on the field using data from the real-world speed and angle of each module on the robot.
      Parameters:
        `wheelStates` - The state of the modules (as a SwerveModuleState type) as measured from respective encoders and gyros. The order of the swerve module states should be same as passed into the constructor of this class.
      Returns:
        The resulting chassis speed.

    - #### toTwist2d

          public edu.wpi.first.math.geometry.Twist2d toTwist2d​(edu.wpi.first.math.kinematics.SwerveModulePosition... wheelDeltas)

      Performs forward kinematics to return the resulting chassis state from the given module states. This method is often used for odometry -- determining the robot's position on the field using data from the real-world speed and angle of each module on the robot.
      Overrides:
        `toTwist2d` in class `edu.wpi.first.math.kinematics.SwerveDriveKinematics`
      Parameters:
        `wheelDeltas` - The latest change in position of the modules (as a SwerveModulePosition type) as measured from respective encoders and gyros. The order of the swerve module states should be same as passed into the constructor of this class.
      Returns:
        The resulting Twist2d.