001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.kinematics;
006
007import edu.wpi.first.math.MathSharedStore;
008import edu.wpi.first.math.MathUsageId;
009import edu.wpi.first.math.geometry.Translation2d;
010import org.ejml.simple.SimpleMatrix;
011
012/**
013 * Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual
014 * wheel speeds.
015 *
016 * <p>The inverse kinematics (converting from a desired chassis velocity to individual wheel speeds)
017 * uses the relative locations of the wheels with respect to the center of rotation. The center of
018 * rotation for inverse kinematics is also variable. This means that you can set your set your
019 * center of rotation in a corner of the robot to perform special evasion maneuvers.
020 *
021 * <p>Forward kinematics (converting an array of wheel speeds into the overall chassis motion) is
022 * performs the exact opposite of what inverse kinematics does. Since this is an overdetermined
023 * system (more equations than variables), we use a least-squares approximation.
024 *
025 * <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds] We take the
026 * Moore-Penrose pseudoinverse of [wheelLocations] and then multiply by [wheelSpeeds] to get our
027 * chassis speeds.
028 *
029 * <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
030 * field using encoders and a gyro.
031 */
032public class MecanumDriveKinematics {
033  private final SimpleMatrix m_inverseKinematics;
034  private final SimpleMatrix m_forwardKinematics;
035
036  private final Translation2d m_frontLeftWheelMeters;
037  private final Translation2d m_frontRightWheelMeters;
038  private final Translation2d m_rearLeftWheelMeters;
039  private final Translation2d m_rearRightWheelMeters;
040
041  private Translation2d m_prevCoR = new Translation2d();
042
043  /**
044   * Constructs a mecanum drive kinematics object.
045   *
046   * @param frontLeftWheelMeters The location of the front-left wheel relative to the physical
047   *     center of the robot.
048   * @param frontRightWheelMeters The location of the front-right wheel relative to the physical
049   *     center of the robot.
050   * @param rearLeftWheelMeters The location of the rear-left wheel relative to the physical center
051   *     of the robot.
052   * @param rearRightWheelMeters The location of the rear-right wheel relative to the physical
053   *     center of the robot.
054   */
055  public MecanumDriveKinematics(
056      Translation2d frontLeftWheelMeters,
057      Translation2d frontRightWheelMeters,
058      Translation2d rearLeftWheelMeters,
059      Translation2d rearRightWheelMeters) {
060    m_frontLeftWheelMeters = frontLeftWheelMeters;
061    m_frontRightWheelMeters = frontRightWheelMeters;
062    m_rearLeftWheelMeters = rearLeftWheelMeters;
063    m_rearRightWheelMeters = rearRightWheelMeters;
064
065    m_inverseKinematics = new SimpleMatrix(4, 3);
066
067    setInverseKinematics(
068        frontLeftWheelMeters, frontRightWheelMeters, rearLeftWheelMeters, rearRightWheelMeters);
069    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
070
071    MathSharedStore.reportUsage(MathUsageId.kKinematics_MecanumDrive, 1);
072  }
073
074  /**
075   * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
076   * method is often used to convert joystick values into wheel speeds.
077   *
078   * <p>This function also supports variable centers of rotation. During normal operations, the
079   * center of rotation is usually the same as the physical center of the robot; therefore, the
080   * argument is defaulted to that use case. However, if you wish to change the center of rotation
081   * for evasive maneuvers, vision alignment, or for any other use case, you can do so.
082   *
083   * @param chassisSpeeds The desired chassis speed.
084   * @param centerOfRotationMeters The center of rotation. For example, if you set the center of
085   *     rotation at one corner of the robot and provide a chassis speed that only has a dtheta
086   *     component, the robot will rotate around that corner.
087   * @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user input
088   *     may cause one of the wheel speeds to go above the attainable max velocity. Use the {@link
089   *     MecanumDriveWheelSpeeds#desaturate(double)} function to rectify this issue.
090   */
091  public MecanumDriveWheelSpeeds toWheelSpeeds(
092      ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
093    // We have a new center of rotation. We need to compute the matrix again.
094    if (!centerOfRotationMeters.equals(m_prevCoR)) {
095      var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
096      var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
097      var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);
098      var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters);
099
100      setInverseKinematics(fl, fr, rl, rr);
101      m_prevCoR = centerOfRotationMeters;
102    }
103
104    var chassisSpeedsVector = new SimpleMatrix(3, 1);
105    chassisSpeedsVector.setColumn(
106        0,
107        0,
108        chassisSpeeds.vxMetersPerSecond,
109        chassisSpeeds.vyMetersPerSecond,
110        chassisSpeeds.omegaRadiansPerSecond);
111
112    var wheelsVector = m_inverseKinematics.mult(chassisSpeedsVector);
113    return new MecanumDriveWheelSpeeds(
114        wheelsVector.get(0, 0),
115        wheelsVector.get(1, 0),
116        wheelsVector.get(2, 0),
117        wheelsVector.get(3, 0));
118  }
119
120  /**
121   * Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
122   * information.
123   *
124   * @param chassisSpeeds The desired chassis speed.
125   * @return The wheel speeds.
126   */
127  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
128    return toWheelSpeeds(chassisSpeeds, new Translation2d());
129  }
130
131  /**
132   * Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
133   * This method is often used for odometry -- determining the robot's position on the field using
134   * data from the real-world speed of each wheel on the robot.
135   *
136   * @param wheelSpeeds The current mecanum drive wheel speeds.
137   * @return The resulting chassis speed.
138   */
139  public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
140    var wheelSpeedsVector = new SimpleMatrix(4, 1);
141    wheelSpeedsVector.setColumn(
142        0,
143        0,
144        wheelSpeeds.frontLeftMetersPerSecond,
145        wheelSpeeds.frontRightMetersPerSecond,
146        wheelSpeeds.rearLeftMetersPerSecond,
147        wheelSpeeds.rearRightMetersPerSecond);
148    var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsVector);
149
150    return new ChassisSpeeds(
151        chassisSpeedsVector.get(0, 0),
152        chassisSpeedsVector.get(1, 0),
153        chassisSpeedsVector.get(2, 0));
154  }
155
156  /**
157   * Construct inverse kinematics matrix from wheel locations.
158   *
159   * @param fl The location of the front-left wheel relative to the physical center of the robot.
160   * @param fr The location of the front-right wheel relative to the physical center of the robot.
161   * @param rl The location of the rear-left wheel relative to the physical center of the robot.
162   * @param rr The location of the rear-right wheel relative to the physical center of the robot.
163   */
164  private void setInverseKinematics(
165      Translation2d fl, Translation2d fr, Translation2d rl, Translation2d rr) {
166    m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
167    m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
168    m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
169    m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
170  }
171}