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    implements Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
035  private final SimpleMatrix m_inverseKinematics;
036  private final SimpleMatrix m_forwardKinematics;
037
038  private final Translation2d m_frontLeftWheelMeters;
039  private final Translation2d m_frontRightWheelMeters;
040  private final Translation2d m_rearLeftWheelMeters;
041  private final Translation2d m_rearRightWheelMeters;
042
043  private Translation2d m_prevCoR = new Translation2d();
044
045  /**
046   * Constructs a mecanum drive kinematics object.
047   *
048   * @param frontLeftWheelMeters The location of the front-left wheel relative to the physical
049   *     center of the robot.
050   * @param frontRightWheelMeters The location of the front-right wheel relative to the physical
051   *     center of the robot.
052   * @param rearLeftWheelMeters The location of the rear-left wheel relative to the physical center
053   *     of the robot.
054   * @param rearRightWheelMeters The location of the rear-right wheel relative to the physical
055   *     center of the robot.
056   */
057  public MecanumDriveKinematics(
058      Translation2d frontLeftWheelMeters,
059      Translation2d frontRightWheelMeters,
060      Translation2d rearLeftWheelMeters,
061      Translation2d rearRightWheelMeters) {
062    m_frontLeftWheelMeters = frontLeftWheelMeters;
063    m_frontRightWheelMeters = frontRightWheelMeters;
064    m_rearLeftWheelMeters = rearLeftWheelMeters;
065    m_rearRightWheelMeters = rearRightWheelMeters;
066
067    m_inverseKinematics = new SimpleMatrix(4, 3);
068
069    setInverseKinematics(
070        frontLeftWheelMeters, frontRightWheelMeters, rearLeftWheelMeters, rearRightWheelMeters);
071    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
072
073    MathSharedStore.reportUsage(MathUsageId.kKinematics_MecanumDrive, 1);
074  }
075
076  /**
077   * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
078   * method is often used to convert joystick values into wheel speeds.
079   *
080   * <p>This function also supports variable centers of rotation. During normal operations, the
081   * center of rotation is usually the same as the physical center of the robot; therefore, the
082   * argument is defaulted to that use case. However, if you wish to change the center of rotation
083   * for evasive maneuvers, vision alignment, or for any other use case, you can do so.
084   *
085   * @param chassisSpeeds The desired chassis speed.
086   * @param centerOfRotationMeters The center of rotation. For example, if you set the center of
087   *     rotation at one corner of the robot and provide a chassis speed that only has a dtheta
088   *     component, the robot will rotate around that corner.
089   * @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user input
090   *     may cause one of the wheel speeds to go above the attainable max velocity. Use the {@link
091   *     MecanumDriveWheelSpeeds#desaturate(double)} function to rectify this issue.
092   */
093  public MecanumDriveWheelSpeeds toWheelSpeeds(
094      ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
095    // We have a new center of rotation. We need to compute the matrix again.
096    if (!centerOfRotationMeters.equals(m_prevCoR)) {
097      var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
098      var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
099      var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);
100      var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters);
101
102      setInverseKinematics(fl, fr, rl, rr);
103      m_prevCoR = centerOfRotationMeters;
104    }
105
106    var chassisSpeedsVector = new SimpleMatrix(3, 1);
107    chassisSpeedsVector.setColumn(
108        0,
109        0,
110        chassisSpeeds.vxMetersPerSecond,
111        chassisSpeeds.vyMetersPerSecond,
112        chassisSpeeds.omegaRadiansPerSecond);
113
114    var wheelsVector = m_inverseKinematics.mult(chassisSpeedsVector);
115    return new MecanumDriveWheelSpeeds(
116        wheelsVector.get(0, 0),
117        wheelsVector.get(1, 0),
118        wheelsVector.get(2, 0),
119        wheelsVector.get(3, 0));
120  }
121
122  /**
123   * Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
124   * information.
125   *
126   * @param chassisSpeeds The desired chassis speed.
127   * @return The wheel speeds.
128   */
129  @Override
130  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
131    return toWheelSpeeds(chassisSpeeds, new Translation2d());
132  }
133
134  /**
135   * Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
136   * This method is often used for odometry -- determining the robot's position on the field using
137   * data from the real-world speed of each wheel on the robot.
138   *
139   * @param wheelSpeeds The current mecanum drive wheel speeds.
140   * @return The resulting chassis speed.
141   */
142  @Override
143  public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
144    var wheelSpeedsVector = new SimpleMatrix(4, 1);
145    wheelSpeedsVector.setColumn(
146        0,
147        0,
148        wheelSpeeds.frontLeftMetersPerSecond,
149        wheelSpeeds.frontRightMetersPerSecond,
150        wheelSpeeds.rearLeftMetersPerSecond,
151        wheelSpeeds.rearRightMetersPerSecond);
152    var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsVector);
153
154    return new ChassisSpeeds(
155        chassisSpeedsVector.get(0, 0),
156        chassisSpeedsVector.get(1, 0),
157        chassisSpeedsVector.get(2, 0));
158  }
159
160  @Override
161  public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) {
162    var wheelDeltasVector = new SimpleMatrix(4, 1);
163    wheelDeltasVector.setColumn(
164        0,
165        0,
166        wheelDeltas.frontLeftMeters,
167        wheelDeltas.frontRightMeters,
168        wheelDeltas.rearLeftMeters,
169        wheelDeltas.rearRightMeters);
170    var twist = m_forwardKinematics.mult(wheelDeltasVector);
171
172    return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
173  }
174
175  /**
176   * Construct inverse kinematics matrix from wheel locations.
177   *
178   * @param fl The location of the front-left wheel relative to the physical center of the robot.
179   * @param fr The location of the front-right wheel relative to the physical center of the robot.
180   * @param rl The location of the rear-left wheel relative to the physical center of the robot.
181   * @param rr The location of the rear-right wheel relative to the physical center of the robot.
182   */
183  private void setInverseKinematics(
184      Translation2d fl, Translation2d fr, Translation2d rl, Translation2d rr) {
185    m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
186    m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
187    m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
188    m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
189  }
190}