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