Package me.wobblyyyy.pathfinder.kinematics

Class SwerveKinematics

java.lang.Object
me.wobblyyyy.pathfinder.kinematics.SwerveKinematics
public class SwerveKinematics
extends java.lang.Object
A variety of mathematical functionality related to kinematics for a swerve chassis. Both forwards and backwards (inverse) kinematic utilities are provided here, allowing for translation from desired translation to desired swerve module states as well as translation from desired swerve module states. The math behind this class is largely based upon the Efficient Java Matrix Library SimpleMatrix with a bit of fancy intervention here and there on our part. This class DOES NOT modify the drivetrain's states - this class simply provides a mathematical interface for determining module states and translations for a swerve chassis with a set of manually inputted wheel positions.
Since:
0.3.0
Author:
Colin Robertson

  • Field Summary

    Fields
    Modifier and Type Field Description
    private org.ejml.simple.SimpleMatrix kinematicsBackwards
    Inverse kinematics - used for odometry purposes.
    private org.ejml.simple.SimpleMatrix kinematicsForwards
    Forwards kinematics - used for getting module states from a translation.
    private me.wobblyyyy.edt.Arrayable<Point> wheelPositions
    A container for each of the wheel positions on the robot.

  • Constructor Summary

    Constructors
    Constructor Description
    SwerveKinematics​(me.wobblyyyy.edt.Arrayable<Point> wheelPositions)
    Create a new SwerveKinematics instance.

  • Method Summary

    Modifier and Type Method Description
    private org.ejml.simple.SimpleMatrix getChassisTransformVector​(org.ejml.simple.SimpleMatrix stateMatrix)
    Get a transformation vector based on a matrix of swerve states.
    private org.ejml.simple.SimpleMatrix getModuleMatrix​(org.ejml.simple.SimpleMatrix transformationVector)
    Get a matrix of swerve module states based on a transformation vector generated elsewhere.
    private org.ejml.simple.SimpleMatrix getModuleStateMatrix​(me.wobblyyyy.edt.StaticArray<SwerveState> states)
    Get a matrix of swerve module states based on an array of swerve module states.
    me.wobblyyyy.edt.StaticArray<SwerveState> getStates​(RTransform transform)
    Get swerve module states based on the desired transformation and the wheel positions passed to this instance of SwerveKinematics on construction.
    private me.wobblyyyy.edt.StaticArray<SwerveState> getSwerveModuleStates​(org.ejml.simple.SimpleMatrix moduleMatrix)
    Get an array of swerve module states based on a matrix of swerve module states.
    RTransform getTransform​(me.wobblyyyy.edt.StaticArray<SwerveState> states)
    Get a positional transformation based on a set of swerve module states.
    private org.ejml.simple.SimpleMatrix getTransformationVector​(RTransform transform)
    Get a transformation vector based on a robot transformation.
    private RTransform swapXY​(RTransform original)
    Swap X and Y values of a translation.

    Methods inherited from class java.lang.Object

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

  • Field Details

    • kinematicsForwards

      private final org.ejml.simple.SimpleMatrix kinematicsForwards
      Forwards kinematics - used for getting module states from a translation.

    • kinematicsBackwards

      private final org.ejml.simple.SimpleMatrix kinematicsBackwards
      Inverse kinematics - used for odometry purposes.

    • wheelPositions

      private final me.wobblyyyy.edt.Arrayable<Point> wheelPositions
      A container for each of the wheel positions on the robot.

  • Constructor Details

    • SwerveKinematics

      public SwerveKinematics​(me.wobblyyyy.edt.Arrayable<Point> wheelPositions)
      Create a new SwerveKinematics instance. This class doesn't actually do anything other than provide a mathematical interface for kinematics.
      Parameters:
      wheelPositions - an Arrayable instance containing relative swerve module positions. This position is defined as the center axis on which the swerve module both drives and rotates. This array must be greater than two in size. Please note: the order of this array determines the order in which kinematic operations output power or positional values - remember the order you put these modules in, such as FR/FL/BR/BL.

  • Method Details

    • swapXY

      private RTransform swapXY​(RTransform original)
      Swap X and Y values of a translation.
      Parameters:
      original - the original translation.
      Returns:
      a translation with swapped X and Y values.

    • getTransformationVector

      private org.ejml.simple.SimpleMatrix getTransformationVector​(RTransform transform)
      Get a transformation vector based on a robot transformation.
      Parameters:
      transform - the desired robot transformation.
      Returns:
      a transformation matrix/vector based on the desired robot transformation - once again, my favorite!

    • getModuleMatrix

      private org.ejml.simple.SimpleMatrix getModuleMatrix​(org.ejml.simple.SimpleMatrix transformationVector)
      Get a matrix of swerve module states based on a transformation vector generated elsewhere.
      Parameters:
      transformationVector - the transformation vector to base the matrix of states upon.
      Returns:
      a matrix of swerve module state information based on the requested transformation vector.

    • getSwerveModuleStates

      private me.wobblyyyy.edt.StaticArray<SwerveState> getSwerveModuleStates​(org.ejml.simple.SimpleMatrix moduleMatrix)
      Get an array of swerve module states based on a matrix of swerve module states.
      Parameters:
      moduleMatrix - the matrix of swerve module state information.
      Returns:
      an array of swerve module states.

    • getStates

      public me.wobblyyyy.edt.StaticArray<SwerveState> getStates​(RTransform transform)
      Get swerve module states based on the desired transformation and the wheel positions passed to this instance of SwerveKinematics on construction. These states are outputted in the SAME order as the wheels are inputted in upon construction. For example, if you created this swerve kinematics with the order of FR/FL/BR/BL, the outputted states will be outputted in the same order.
      Parameters:
      transform - the robot's desired transformation. In most cases, this transformation should fit within the range of (-1, 1).
      Returns:
      a StaticArray of each of the module states that would represent the desired translation. These module states are returned in the same order as the wheel positions that were inputted upon this instance's construction.

    • getModuleStateMatrix

      private org.ejml.simple.SimpleMatrix getModuleStateMatrix​(me.wobblyyyy.edt.StaticArray<SwerveState> states)
      Get a matrix of swerve module states based on an array of swerve module states.
      Parameters:
      states - the module states to generate a matrix with.
      Returns:
      a matrix of swerve module states based on an array of swerve module states.

    • getChassisTransformVector

      private org.ejml.simple.SimpleMatrix getChassisTransformVector​(org.ejml.simple.SimpleMatrix stateMatrix)
      Get a transformation vector based on a matrix of swerve states.
      Parameters:
      stateMatrix - the swerve state matrix to base the newly created transformation vector on.
      Returns:
      a chassis transformation vector based on a matrix of swerve states - my favorite!

    • getTransform

      public RTransform getTransform​(me.wobblyyyy.edt.StaticArray<SwerveState> states)
      Get a positional transformation based on a set of swerve module states. If the size of the provided state array doesn't match the size of the internally-stored array of wheel positions, an exception will be thrown, indicating that no transformation could be generated as the size of the arrays did not match.
      Parameters:
      states - a StaticArray of swerve module states. This array should be the same size as the internal array of wheel positions or an exception will be thrown on this method's invocation. Not cool.