Class SwerveDriveKinematics

java.lang.Object
org.wpilib.math.kinematics.SwerveDriveKinematics
All Implemented Interfaces:
Interpolator<SwerveModulePosition[]>, Kinematics<SwerveModulePosition[], SwerveModuleVelocity[], SwerveModuleAcceleration[]>, ProtobufSerializable, StructSerializable, WPISerializable
public class SwerveDriveKinematics extends Object implements Kinematics<SwerveModulePosition[], SwerveModuleVelocity[], SwerveModuleAcceleration[]>, ProtobufSerializable, StructSerializable
Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual module states (velocity and angle).

The inverse kinematics (converting from a desired chassis velocity to individual module states) uses the relative locations of the modules with respect to the center of rotation. The center of rotation for inverse kinematics is also variable. This means that you can set your center of rotation in a corner of the robot to perform special evasion maneuvers.

Forward kinematics (converting an array of module states into the overall chassis motion) is performs the exact opposite of what inverse kinematics does. Since this is an overdetermined system (more equations than variables), we use a least-squares approximation.

The inverse kinematics: [moduleVelocities] = [moduleLocations] * [chassisVelocities] We take the Moore-Penrose pseudoinverse of [moduleLocations] and then multiply by [moduleVelocities] to get our chassis velocities.

Forward kinematics is also used for odometry -- determining the position of the robot on the field using encoders and a gyro.

  • Field Details

  • Constructor Details

    • SwerveDriveKinematics

      public SwerveDriveKinematics(Translation2d... moduleTranslations)
      Constructs a swerve drive kinematics object. This takes in a variable number of module locations as Translation2d objects. The order in which you pass in the module 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:
      moduleTranslations - The locations of the modules relative to the physical center of the robot.

  • Method Details

    • resetHeadings

      public void resetHeadings(Rotation2d... moduleHeadings)
      Reset the internal swerve module headings.
      Parameters:
      moduleHeadings - The swerve module headings. The order of the module headings should be same as passed into the constructor of this class.

    • toSwerveModuleVelocities

      public SwerveModuleVelocity[] toSwerveModuleVelocities(ChassisVelocities chassisVelocities, Translation2d centerOfRotation)
      Performs inverse kinematics to return the module states from a desired chassis velocity. This method is often used to convert joystick values into module velocities 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 velocities are zero (i.e. the robot will be stationary), the previously calculated module angle will be maintained.

      Parameters:
      chassisVelocities - The desired chassis velocity.
      centerOfRotation - The center of rotation. For example, if you set the center of rotation at one corner of the robot and provide a chassis velocity 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 velocities to go above the attainable max velocity. Use the DesaturateWheelVelocities function to rectify this issue.

    • toSwerveModuleVelocities

      public SwerveModuleVelocity[] toSwerveModuleVelocities(ChassisVelocities chassisVelocities)
      Performs inverse kinematics. See toSwerveModuleVelocities(ChassisVelocities, Translation2d) toSwerveModuleVelocities for more information.
      Parameters:
      chassisVelocities - The desired chassis velocity.
      Returns:
      An array containing the module states.

    • toWheelVelocities

      public SwerveModuleVelocity[] toWheelVelocities(ChassisVelocities chassisVelocities)
      Description copied from interface: Kinematics
      Performs inverse kinematics to return the wheel velocities from a desired chassis velocity. This method is often used to convert joystick values into wheel velocities.
      Specified by:
      toWheelVelocities in interface Kinematics<SwerveModulePosition[], SwerveModuleVelocity[], SwerveModuleAcceleration[]>
      Parameters:
      chassisVelocities - The desired chassis velocity.
      Returns:
      The wheel velocities.

    • toChassisVelocities

      public ChassisVelocities toChassisVelocities(SwerveModuleVelocity... moduleVelocities)
      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 velocity and angle of each module on the robot.
      Specified by:
      toChassisVelocities in interface Kinematics<SwerveModulePosition[], SwerveModuleVelocity[], SwerveModuleAcceleration[]>
      Parameters:
      moduleVelocities - The state of the modules (as a SwerveModuleVelocity 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 velocity.

    • toTwist2d

      public Twist2d toTwist2d(SwerveModulePosition... moduleDeltas)
      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 velocity and angle of each module on the robot.
      Parameters:
      moduleDeltas - 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.

    • toTwist2d

      public Twist2d toTwist2d(SwerveModulePosition[] start, SwerveModulePosition[] end)
      Description copied from interface: Kinematics
      Performs forward kinematics to return the resulting Twist2d from the given change in wheel positions. This method is often used for odometry -- determining the robot's position on the field using changes in the distance driven by each wheel on the robot.
      Specified by:
      toTwist2d in interface Kinematics<SwerveModulePosition[], SwerveModuleVelocity[], SwerveModuleAcceleration[]>
      Parameters:
      start - The starting distances driven by the wheels.
      end - The ending distances driven by the wheels.
      Returns:
      The resulting Twist2d in the robot's movement.

    • desaturateWheelVelocities

      public static void desaturateWheelVelocities(SwerveModuleVelocity[] moduleVelocities, double attainableMaxVelocity)
      Renormalizes the wheel velocities if any individual velocity is above the specified maximum.

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

      Scaling down the module velocities rotates the direction of net motion in the opposite direction of rotational velocity, which makes discretizing the chassis velocities inaccurate because the discretization did not account for this translational skew.

      Parameters:
      moduleVelocities - Reference to array of module states. The array will be mutated with the normalized velocities!
      attainableMaxVelocity - The absolute max velocity in meters per second that a module can reach.

    • desaturateWheelVelocities

      public static void desaturateWheelVelocities(SwerveModuleVelocity[] moduleVelocities, LinearVelocity attainableMaxVelocity)
      Renormalizes the wheel velocities if any individual velocity is above the specified maximum.

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

      Scaling down the module velocities rotates the direction of net motion in the opposite direction of rotational velocity, which makes discretizing the chassis velocities inaccurate because the discretization did not account for this translational skew.

      Parameters:
      moduleVelocities - Reference to array of module states. The array will be mutated with the normalized velocities!
      attainableMaxVelocity - The absolute max velocity in meters per second that a module can reach.

    • desaturateWheelVelocities

      public static void desaturateWheelVelocities(SwerveModuleVelocity[] moduleVelocities, ChassisVelocities desiredChassisVelocity, double attainableMaxModuleVelocity, double attainableMaxTranslationalVelocity, double attainableMaxRotationalVelocity)
      Renormalizes the wheel velocities if any individual velocity is above the specified maximum, as well as getting rid of joystick saturation at edges of joystick.

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

      Scaling down the module velocities rotates the direction of net motion in the opposite direction of rotational velocity, which makes discretizing the chassis velocities inaccurate because the discretization did not account for this translational skew.

      Parameters:
      moduleVelocities - Reference to array of module states. The array will be mutated with the normalized velocities!
      desiredChassisVelocity - The desired velocity of the robot
      attainableMaxModuleVelocity - The absolute max velocity in meters per second that a module can reach
      attainableMaxTranslationalVelocity - The absolute max velocity in meters per second that your robot can reach while translating
      attainableMaxRotationalVelocity - The absolute max velocity in radians per second the robot can reach while rotating

    • desaturateWheelVelocities

      public static void desaturateWheelVelocities(SwerveModuleVelocity[] moduleVelocities, ChassisVelocities desiredChassisVelocity, LinearVelocity attainableMaxModuleVelocity, LinearVelocity attainableMaxTranslationalVelocity, AngularVelocity attainableMaxRotationalVelocity)
      Renormalizes the wheel velocities if any individual velocity is above the specified maximum, as well as getting rid of joystick saturation at edges of joystick.

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

      Scaling down the module velocities rotates the direction of net motion in the opposite direction of rotational velocity, which makes discretizing the chassis velocities inaccurate because the discretization did not account for this translational skew.

      Parameters:
      moduleVelocities - Reference to array of module states. The array will be mutated with the normalized velocities!
      desiredChassisVelocity - The desired velocity of the robot
      attainableMaxModuleVelocity - The absolute max velocity that a module can reach
      attainableMaxTranslationalVelocity - The absolute max velocity that your robot can reach while translating
      attainableMaxRotationalVelocity - The absolute max velocity the robot can reach while rotating

    • copy

      public SwerveModulePosition[] copy(SwerveModulePosition[] positions)
      Description copied from interface: Kinematics
      Returns a copy of the wheel positions object.
      Specified by:
      copy in interface Kinematics<SwerveModulePosition[], SwerveModuleVelocity[], SwerveModuleAcceleration[]>
      Parameters:
      positions - The wheel positions object to copy.
      Returns:
      A copy.

    • copyInto

      public void copyInto(SwerveModulePosition[] positions, SwerveModulePosition[] output)
      Description copied from interface: Kinematics
      Copies the value of the wheel positions object into the output.
      Specified by:
      copyInto in interface Kinematics<SwerveModulePosition[], SwerveModuleVelocity[], SwerveModuleAcceleration[]>
      Parameters:
      positions - The wheel positions object to copy. Will not be modified.
      output - The output variable of the copy operation. Will have the same value as the positions object after the call.

    • interpolate

      public SwerveModulePosition[] interpolate(SwerveModulePosition[] startValue, SwerveModulePosition[] endValue, double t)
      Description copied from interface: Interpolator
      Perform interpolation between two values.
      Specified by:
      interpolate in interface Interpolator<SwerveModulePosition[]>
      Parameters:
      startValue - The value to start at.
      endValue - The value to end at.
      t - How far between the two values to interpolate. This should be bounded to [0, 1].
      Returns:
      The interpolated value.

    • getModules

      public Translation2d[] getModules()
      Gets the locations of the modules relative to the center of rotation.
      Returns:
      The locations of the modules relative to the center of rotation. This array should not be modified.

    • getStruct

      public static final SwerveDriveKinematicsStruct getStruct(int numModules)
      Creates an implementation of the Struct interface for swerve drive kinematics objects.
      Parameters:
      numModules - The number of modules of the kinematics objects this serializer processes.
      Returns:
      The struct implementation.

    • toSwerveModuleAccelerations

      public SwerveModuleAcceleration[] toSwerveModuleAccelerations(ChassisAccelerations chassisAccelerations, double angularVelocity, Translation2d centerOfRotation)
      Performs inverse kinematics to return the module accelerations from a desired chassis acceleration. This method is often used for dynamics calculations -- converting desired robot accelerations into individual module accelerations.

      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.

      Parameters:
      chassisAccelerations - The desired chassis accelerations.
      angularVelocity - The desired robot angular velocity.
      centerOfRotation - The center of rotation. For example, if you set the center of rotation at one corner of the robot and provide a chassis acceleration that only has a dtheta component, the robot will rotate around that corner.
      Returns:
      An array containing the module accelerations.

    • toSwerveModuleAccelerations

      public SwerveModuleAcceleration[] toSwerveModuleAccelerations(ChassisAccelerations chassisAccelerations, double angularVelocity)
      Performs inverse kinematics. See toSwerveModuleAccelerations(ChassisAccelerations, double, Translation2d) toSwerveModuleAccelerations for more information.
      Parameters:
      chassisAccelerations - The desired chassis accelerations.
      angularVelocity - The desired robot angular velocity.
      Returns:
      An array containing the module accelerations.

    • toWheelAccelerations

      public SwerveModuleAcceleration[] toWheelAccelerations(ChassisAccelerations chassisAccelerations)
      Description copied from interface: Kinematics
      Performs inverse kinematics to return the wheel accelerations from a desired chassis acceleration. This method is often used for dynamics calculations -- converting desired robot accelerations into individual wheel accelerations.
      Specified by:
      toWheelAccelerations in interface Kinematics<SwerveModulePosition[], SwerveModuleVelocity[], SwerveModuleAcceleration[]>
      Parameters:
      chassisAccelerations - The desired chassis accelerations.
      Returns:
      The wheel accelerations.

    • toChassisAccelerations

      public ChassisAccelerations toChassisAccelerations(SwerveModuleAcceleration... moduleAccelerations)
      Performs forward kinematics to return the resulting chassis accelerations from the given module accelerations. This method is often used for dynamics calculations -- determining the robot's acceleration on the field using data from the real-world acceleration of each module on the robot.
      Specified by:
      toChassisAccelerations in interface Kinematics<SwerveModulePosition[], SwerveModuleVelocity[], SwerveModuleAcceleration[]>
      Parameters:
      moduleAccelerations - The accelerations of the modules as measured from respective encoders and gyros. The order of the swerve module accelerations should be same as passed into the constructor of this class.
      Returns:
      The resulting chassis accelerations.