Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Move Java classes to edu.wpi.first.math (#3316)

Browse Source
This commit is contained in:
Noam Zaks
2021-05-01 15:53:30 +00:00
committed by GitHub
parent 6b50323b07
commit c8ff626fe2
212 changed files with 918 additions and 921 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java Normal file
View File

@@ -0,0 +1,78 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.geometry.Rotation2d;
/**
* Represents the speed of a robot chassis. Although this struct contains similar members compared
* to a Twist2d, they do NOT represent the same thing. Whereas a Twist2d represents a change in pose
* w.r.t to the robot frame of reference, this ChassisSpeeds struct represents a velocity w.r.t to
* the robot frame of reference.
*
* <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have a dy
* component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
* will often have all three components.
*/
@SuppressWarnings("MemberName")
public class ChassisSpeeds {
/** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */
public double vxMetersPerSecond;
/** Represents sideways velocity w.r.t the robot frame of reference. (Left is +) */
public double vyMetersPerSecond;
/** Represents the angular velocity of the robot frame. (CCW is +) */
public double omegaRadiansPerSecond;
/** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */
public ChassisSpeeds() {}
/**
* Constructs a ChassisSpeeds object.
*
* @param vxMetersPerSecond Forward velocity.
* @param vyMetersPerSecond Sideways velocity.
* @param omegaRadiansPerSecond Angular velocity.
*/
public ChassisSpeeds(
double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond) {
this.vxMetersPerSecond = vxMetersPerSecond;
this.vyMetersPerSecond = vyMetersPerSecond;
this.omegaRadiansPerSecond = omegaRadiansPerSecond;
}
/**
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
*
* @param vxMetersPerSecond The component of speed in the x direction relative to the field.
* Positive x is away from your alliance wall.
* @param vyMetersPerSecond The component of speed in the y direction relative to the field.
* Positive y is to your left when standing behind the alliance wall.
* @param omegaRadiansPerSecond The angular rate of the robot.
* @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
*/
public static ChassisSpeeds fromFieldRelativeSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
double omegaRadiansPerSecond,
Rotation2d robotAngle) {
return new ChassisSpeeds(
vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
-vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
omegaRadiansPerSecond);
}
@Override
public String toString() {
return String.format(
"ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
}
}

61
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java Normal file
View File

@@ -0,0 +1,61 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
/**
* Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
* velocities for a differential drive.
*
* <p>Inverse kinematics converts a desired chassis speed into left and right velocity components
* whereas forward kinematics converts left and right component velocities into a linear and angular
* chassis speed.
*/
@SuppressWarnings("MemberName")
public class DifferentialDriveKinematics {
public final double trackWidthMeters;
/**
* Constructs a differential drive kinematics object.
*
* @param trackWidthMeters The track width of the drivetrain. Theoretically, this is the distance
* between the left wheels and right wheels. However, the empirical value may be larger than
* the physical measured value due to scrubbing effects.
*/
public DifferentialDriveKinematics(double trackWidthMeters) {
this.trackWidthMeters = trackWidthMeters;
MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1);
}
/**
* Returns a chassis speed from left and right component velocities using forward kinematics.
*
* @param wheelSpeeds The left and right velocities.
* @return The chassis speed.
*/
public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
return new ChassisSpeeds(
(wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
0,
(wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) / trackWidthMeters);
}
/**
* Returns left and right component velocities from a chassis speed using inverse kinematics.
*
* @param chassisSpeeds The linear and angular (dx and dtheta) components that represent the
* chassis' speed.
* @return The left and right velocities.
*/
public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return new DifferentialDriveWheelSpeeds(
chassisSpeeds.vxMetersPerSecond
- trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond,
chassisSpeeds.vxMetersPerSecond
+ trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
}
}

113
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java Normal file
View File

@@ -0,0 +1,113 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
/**
* Class for differential drive odometry. Odometry allows you to track the robot's position on the
* field over the course of a match using readings from 2 encoders and a gyroscope.
*
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
*
* <p>It is important that you reset your encoders to zero before using this class. Any subsequent
* pose resets also require the encoders to be reset to zero.
*/
public class DifferentialDriveOdometry {
private Pose2d m_poseMeters;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
private double m_prevLeftDistance;
private double m_prevRightDistance;
/**
* Constructs a DifferentialDriveOdometry object.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public DifferentialDriveOdometry(Rotation2d gyroAngle, Pose2d initialPoseMeters) {
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
}
/**
* Constructs a DifferentialDriveOdometry object with the default pose at the origin.
*
* @param gyroAngle The angle reported by the gyroscope.
*/
public DifferentialDriveOdometry(Rotation2d gyroAngle) {
this(gyroAngle, new Pose2d());
}
/**
* Resets the robot's position on the field.
*
* <p>You NEED to reset your encoders (to zero) when calling this method.
*
* <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_prevLeftDistance = 0.0;
m_prevRightDistance = 0.0;
}
/**
* Returns the position of the robot on the field.
*
* @return The pose of the robot (x and y are in meters).
*/
public Pose2d getPoseMeters() {
return m_poseMeters;
}
/**
* Updates the robot position on the field using distance measurements from encoders. This method
* is more numerically accurate than using velocities to integrate the pose and is also
* advantageous for teams that are using lower CPR encoders.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @return The new pose of the robot.
*/
public Pose2d update(
Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
m_prevLeftDistance = leftDistanceMeters;
m_prevRightDistance = rightDistanceMeters;
double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
var angle = gyroAngle.plus(m_gyroOffset);
var newPose =
m_poseMeters.exp(
new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
return m_poseMeters;
}
}

55
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java Normal file
View File

@@ -0,0 +1,55 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
/** Represents the wheel speeds for a differential drive drivetrain. */
@SuppressWarnings("MemberName")
public class DifferentialDriveWheelSpeeds {
/** Speed of the left side of the robot. */
public double leftMetersPerSecond;
/** Speed of the right side of the robot. */
public double rightMetersPerSecond;
/** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */
public DifferentialDriveWheelSpeeds() {}
/**
* Constructs a DifferentialDriveWheelSpeeds.
*
* @param leftMetersPerSecond The left speed.
* @param rightMetersPerSecond The right speed.
*/
public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
this.leftMetersPerSecond = leftMetersPerSecond;
this.rightMetersPerSecond = rightMetersPerSecond;
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
* kinematics, the requested speed from a/several modules may be above the max attainable speed
* for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
* speeds to make sure that all requested module speeds are below the absolute threshold, while
* maintaining the ratio of speeds between modules.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
*/
public void normalize(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
rightMetersPerSecond =
rightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
}
}
@Override
public String toString() {
return String.format(
"DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
leftMetersPerSecond, rightMetersPerSecond);
}
}

172
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java Normal file
View File

@@ -0,0 +1,172 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Translation2d;
import org.ejml.simple.SimpleMatrix;
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual
* wheel speeds.
*
* <p>The inverse kinematics (converting from a desired chassis velocity to individual wheel speeds)
* uses the relative locations of the wheels with respect to the center of rotation. The center of
* rotation for inverse kinematics is also variable. This means that you can set your set your
* center of rotation in a corner of the robot to perform special evasion maneuvers.
*
* <p>Forward kinematics (converting an array of wheel speeds 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.
*
* <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds] We take the
* Moore-Penrose pseudoinverse of [wheelLocations] and then multiply by [wheelSpeeds] to get our
* chassis speeds.
*
* <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
* field using encoders and a gyro.
*/
public class MecanumDriveKinematics {
private SimpleMatrix m_inverseKinematics;
private final SimpleMatrix m_forwardKinematics;
private final Translation2d m_frontLeftWheelMeters;
private final Translation2d m_frontRightWheelMeters;
private final Translation2d m_rearLeftWheelMeters;
private final Translation2d m_rearRightWheelMeters;
private Translation2d m_prevCoR = new Translation2d();
/**
* Constructs a mecanum drive kinematics object.
*
* @param frontLeftWheelMeters The location of the front-left wheel relative to the physical
* center of the robot.
* @param frontRightWheelMeters The location of the front-right wheel relative to the physical
* center of the robot.
* @param rearLeftWheelMeters The location of the rear-left wheel relative to the physical center
* of the robot.
* @param rearRightWheelMeters The location of the rear-right wheel relative to the physical
* center of the robot.
*/
public MecanumDriveKinematics(
Translation2d frontLeftWheelMeters,
Translation2d frontRightWheelMeters,
Translation2d rearLeftWheelMeters,
Translation2d rearRightWheelMeters) {
m_frontLeftWheelMeters = frontLeftWheelMeters;
m_frontRightWheelMeters = frontRightWheelMeters;
m_rearLeftWheelMeters = rearLeftWheelMeters;
m_rearRightWheelMeters = rearRightWheelMeters;
m_inverseKinematics = new SimpleMatrix(4, 3);
setInverseKinematics(
frontLeftWheelMeters, frontRightWheelMeters, rearLeftWheelMeters, rearRightWheelMeters);
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
MathSharedStore.reportUsage(MathUsageId.kKinematics_MecanumDrive, 1);
}
/**
* Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
* method is often used to convert joystick values into wheel speeds.
*
* <p>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.
*
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotationMeters The center of rotation. For example, if you set the center of
* rotation at one corner of the robot and provide a chassis speed that only has a dtheta
* component, the robot will rotate around that corner.
* @return The wheel speeds. Use caution because they are not normalized. Sometimes, a user input
* may cause one of the wheel speeds to go above the attainable max velocity. Use the {@link
* MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue.
*/
public MecanumDriveWheelSpeeds toWheelSpeeds(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
// We have a new center of rotation. We need to compute the matrix again.
if (!centerOfRotationMeters.equals(m_prevCoR)) {
var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);
var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters);
setInverseKinematics(fl, fr, rl, rr);
m_prevCoR = centerOfRotationMeters;
}
var chassisSpeedsVector = new SimpleMatrix(3, 1);
chassisSpeedsVector.setColumn(
0,
0,
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond,
chassisSpeeds.omegaRadiansPerSecond);
var wheelsMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
return new MecanumDriveWheelSpeeds(
wheelsMatrix.get(0, 0),
wheelsMatrix.get(1, 0),
wheelsMatrix.get(2, 0),
wheelsMatrix.get(3, 0));
}
/**
* Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
* information.
*
* @param chassisSpeeds The desired chassis speed.
* @return The wheel speeds.
*/
public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return toWheelSpeeds(chassisSpeeds, new Translation2d());
}
/**
* Performs forward kinematics to return the resulting chassis state from the given wheel speeds.
* This method is often used for odometry -- determining the robot's position on the field using
* data from the real-world speed of each wheel on the robot.
*
* @param wheelSpeeds The current mecanum drive wheel speeds.
* @return The resulting chassis speed.
*/
public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
var wheelSpeedsMatrix = new SimpleMatrix(4, 1);
wheelSpeedsMatrix.setColumn(
0,
0,
wheelSpeeds.frontLeftMetersPerSecond,
wheelSpeeds.frontRightMetersPerSecond,
wheelSpeeds.rearLeftMetersPerSecond,
wheelSpeeds.rearRightMetersPerSecond);
var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix);
return new ChassisSpeeds(
chassisSpeedsVector.get(0, 0),
chassisSpeedsVector.get(1, 0),
chassisSpeedsVector.get(2, 0));
}
/**
* Construct inverse kinematics matrix from wheel locations.
*
* @param fl The location of the front-left wheel relative to the physical center of the robot.
* @param fr The location of the front-right wheel relative to the physical center of the robot.
* @param rl The location of the rear-left wheel relative to the physical center of the robot.
* @param rr The location of the rear-right wheel relative to the physical center of the robot.
*/
private void setInverseKinematics(
Translation2d fl, Translation2d fr, Translation2d rl, Translation2d rr) {
m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
m_inverseKinematics = m_inverseKinematics.scale(1.0 / Math.sqrt(2));
}
}

51
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java Normal file
View File

@@ -0,0 +1,51 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
/** Represents the motor voltages for a mecanum drive drivetrain. */
@SuppressWarnings("MemberName")
public class MecanumDriveMotorVoltages {
/** Voltage of the front left motor. */
public double frontLeftVoltage;
/** Voltage of the front right motor. */
public double frontRightVoltage;
/** Voltage of the rear left motor. */
public double rearLeftVoltage;
/** Voltage of the rear right motor. */
public double rearRightVoltage;
/** Constructs a MecanumDriveMotorVoltages with zeros for all member fields. */
public MecanumDriveMotorVoltages() {}
/**
* Constructs a MecanumDriveMotorVoltages.
*
* @param frontLeftVoltage Voltage of the front left motor.
* @param frontRightVoltage Voltage of the front right motor.
* @param rearLeftVoltage Voltage of the rear left motor.
* @param rearRightVoltage Voltage of the rear right motor.
*/
public MecanumDriveMotorVoltages(
double frontLeftVoltage,
double frontRightVoltage,
double rearLeftVoltage,
double rearRightVoltage) {
this.frontLeftVoltage = frontLeftVoltage;
this.frontRightVoltage = frontRightVoltage;
this.rearLeftVoltage = rearLeftVoltage;
this.rearRightVoltage = rearRightVoltage;
}
@Override
public String toString() {
return String.format(
"MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+ "Rear Left: %.2f V, Rear Right: %.2f V)",
frontLeftVoltage, frontRightVoltage, rearLeftVoltage, rearRightVoltage);
}
}

125
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java Normal file
View File

@@ -0,0 +1,125 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.wpiutil.WPIUtilJNI;
/**
* Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field
* over a course of a match using readings from your mecanum wheel encoders.
*
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
*/
public class MecanumDriveOdometry {
private final MecanumDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
private double m_prevTimeSeconds = -1;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
/**
* Constructs a MecanumDriveOdometry object.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public MecanumDriveOdometry(
MecanumDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
}
/**
* Constructs a MecanumDriveOdometry object with the default pose at the origin.
*
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
*/
public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
this(kinematics, gyroAngle, new Pose2d());
}
/**
* Resets the robot's position on the field.
*
* <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
}
/**
* Returns the position of the robot on the field.
*
* @return The pose of the robot (x and y are in meters).
*/
public Pose2d getPoseMeters() {
return m_poseMeters;
}
/**
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method takes in the current time as a parameter to calculate period (difference
* between two timestamps). The period is used to calculate the change in distance from a
* velocity. This also takes in an angle parameter which is used instead of the angular rate that
* is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
* @return The new pose of the robot.
*/
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
var angle = gyroAngle.plus(m_gyroOffset);
var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
var newPose =
m_poseMeters.exp(
new Twist2d(
chassisState.vxMetersPerSecond * period,
chassisState.vyMetersPerSecond * period,
angle.minus(m_previousAngle).getRadians()));
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
return m_poseMeters;
}
/**
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method automatically calculates the current time to calculate period
* (difference between two timestamps). The period is used to calculate the change in distance
* from a velocity. This also takes in an angle parameter which is used instead of the angular
* rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelSpeeds The current wheel speeds.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
}
}

86
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java Normal file
View File

@@ -0,0 +1,86 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import java.util.stream.DoubleStream;
@SuppressWarnings("MemberName")
public class MecanumDriveWheelSpeeds {
/** Speed of the front left wheel. */
public double frontLeftMetersPerSecond;
/** Speed of the front right wheel. */
public double frontRightMetersPerSecond;
/** Speed of the rear left wheel. */
public double rearLeftMetersPerSecond;
/** Speed of the rear right wheel. */
public double rearRightMetersPerSecond;
/** Constructs a MecanumDriveWheelSpeeds with zeros for all member fields. */
public MecanumDriveWheelSpeeds() {}
/**
* Constructs a MecanumDriveWheelSpeeds.
*
* @param frontLeftMetersPerSecond Speed of the front left wheel.
* @param frontRightMetersPerSecond Speed of the front right wheel.
* @param rearLeftMetersPerSecond Speed of the rear left wheel.
* @param rearRightMetersPerSecond Speed of the rear right wheel.
*/
public MecanumDriveWheelSpeeds(
double frontLeftMetersPerSecond,
double frontRightMetersPerSecond,
double rearLeftMetersPerSecond,
double rearRightMetersPerSecond) {
this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
this.frontRightMetersPerSecond = frontRightMetersPerSecond;
this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
this.rearRightMetersPerSecond = rearRightMetersPerSecond;
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
* kinematics, the requested speed from a/several modules may be above the max attainable speed
* for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
* speeds to make sure that all requested module speeds are below the absolute threshold, while
* maintaining the ratio of speeds between modules.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
*/
public void normalize(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed =
DoubleStream.of(
frontLeftMetersPerSecond,
frontRightMetersPerSecond,
rearLeftMetersPerSecond,
rearRightMetersPerSecond)
.max()
.getAsDouble();
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
frontLeftMetersPerSecond =
frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
frontRightMetersPerSecond =
frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
rearLeftMetersPerSecond =
rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
rearRightMetersPerSecond =
rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
}
}
@Override
public String toString() {
return String.format(
"MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+ "Rear Left: %.2f m/s, Rear Right: %.2f m/s)",
frontLeftMetersPerSecond,
frontRightMetersPerSecond,
rearLeftMetersPerSecond,
rearRightMetersPerSecond);
}
}

194
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java Normal file
View File

@@ -0,0 +1,194 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import java.util.Arrays;
import java.util.Collections;
import org.ejml.simple.SimpleMatrix;
/**
* Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual
* module states (speed and angle).
*
* <p>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 set
* your center of rotation in a corner of the robot to perform special evasion maneuvers.
*
* <p>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.
*
* <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds] We take the
* Moore-Penrose pseudoinverse of [moduleLocations] and then multiply by [moduleStates] to get our
* chassis speeds.
*
* <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
* field using encoders and a gyro.
*/
public class SwerveDriveKinematics {
private final SimpleMatrix m_inverseKinematics;
private final SimpleMatrix m_forwardKinematics;
private final int m_numModules;
private final Translation2d[] m_modules;
private Translation2d m_prevCoR = new Translation2d();
/**
* Constructs a swerve drive kinematics object. This takes in a variable number of wheel locations
* as Translation2ds. The order in which you pass in the wheel 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.
*
* @param wheelsMeters The locations of the wheels relative to the physical center of the robot.
*/
public SwerveDriveKinematics(Translation2d... wheelsMeters) {
if (wheelsMeters.length < 2) {
throw new IllegalArgumentException("A swerve drive requires at least two modules");
}
m_numModules = wheelsMeters.length;
m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
for (int i = 0; i < m_numModules; i++) {
m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
}
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
MathSharedStore.reportUsage(MathUsageId.kKinematics_SwerveDrive, 1);
}
/**
* Performs inverse kinematics to return the module states from a desired chassis velocity. This
* method is often used to convert joystick values into module speeds and angles.
*
* <p>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.
*
* @param chassisSpeeds The desired chassis speed.
* @param centerOfRotationMeters The center of rotation. For example, if you set the center of
* rotation at one corner of the robot and provide a chassis speed that only has a dtheta
* component, the robot will rotate around that corner.
* @return 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 speeds to go above the
* attainable max velocity. Use the {@link #normalizeWheelSpeeds(SwerveModuleState[], double)
* normalizeWheelSpeeds} function to rectify this issue.
*/
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
public SwerveModuleState[] toSwerveModuleStates(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
if (!centerOfRotationMeters.equals(m_prevCoR)) {
for (int i = 0; i < m_numModules; i++) {
m_inverseKinematics.setRow(
i * 2 + 0,
0, /* Start Data */
1,
0,
-m_modules[i].getY() + centerOfRotationMeters.getY());
m_inverseKinematics.setRow(
i * 2 + 1,
0, /* Start Data */
0,
1,
+m_modules[i].getX() - centerOfRotationMeters.getX());
}
m_prevCoR = centerOfRotationMeters;
}
var chassisSpeedsVector = new SimpleMatrix(3, 1);
chassisSpeedsVector.setColumn(
0,
0,
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond,
chassisSpeeds.omegaRadiansPerSecond);
var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules];
for (int i = 0; i < m_numModules; i++) {
double x = moduleStatesMatrix.get(i * 2, 0);
double y = moduleStatesMatrix.get(i * 2 + 1, 0);
double speed = Math.hypot(x, y);
Rotation2d angle = new Rotation2d(x, y);
moduleStates[i] = new SwerveModuleState(speed, angle);
}
return moduleStates;
}
/**
* Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)}
* toSwerveModuleStates for more information.
*
* @param chassisSpeeds The desired chassis speed.
* @return An array containing the module states.
*/
public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
return toSwerveModuleStates(chassisSpeeds, new Translation2d());
}
/**
* 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 speed and angle of each module on the robot.
*
* @param wheelStates The state of the modules (as a SwerveModuleState 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.
* @return The resulting chassis speed.
*/
public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
if (wheelStates.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
for (int i = 0; i < m_numModules; i++) {
var module = wheelStates[i];
moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
}
var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
return new ChassisSpeeds(
chassisSpeedsVector.get(0, 0),
chassisSpeedsVector.get(1, 0),
chassisSpeedsVector.get(2, 0));
}
/**
* Normalizes the wheel speeds using some max attainable speed. Sometimes, after inverse
* kinematics, the requested speed from a/several modules may be above the max attainable speed
* for the driving motor on that module. To fix this issue, one can "normalize" all the wheel
* speeds to make sure that all requested module speeds are below the absolute threshold, while
* maintaining the ratio of speeds between modules.
*
* @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds!
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach.
*/
public static void normalizeWheelSpeeds(
SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
for (SwerveModuleState moduleState : moduleStates) {
moduleState.speedMetersPerSecond =
moduleState.speedMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
}
}
}
}

129
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java Normal file
View File

@@ -0,0 +1,129 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.MathUsageId;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.wpiutil.WPIUtilJNI;
/**
* Class for swerve drive odometry. Odometry allows you to track the robot's position on the field
* over a course of a match using readings from your swerve drive encoders and swerve azimuth
* encoders.
*
* <p>Teams can use odometry during the autonomous period for complex tasks like path following.
* Furthermore, odometry can be used for latency compensation when using computer-vision systems.
*/
public class SwerveDriveOdometry {
private final SwerveDriveKinematics m_kinematics;
private Pose2d m_poseMeters;
private double m_prevTimeSeconds = -1;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
/**
* Constructs a SwerveDriveOdometry object.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param initialPose The starting position of the robot on the field.
*/
public SwerveDriveOdometry(
SwerveDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPose) {
m_kinematics = kinematics;
m_poseMeters = initialPose;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPose.getRotation();
MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
}
/**
* Constructs a SwerveDriveOdometry object with the default pose at the origin.
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
*/
public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
this(kinematics, gyroAngle, new Pose2d());
}
/**
* Resets the robot's position on the field.
*
* <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
* automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
*/
public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
m_poseMeters = pose;
m_previousAngle = pose.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
}
/**
* Returns the position of the robot on the field.
*
* @return The pose of the robot (x and y are in meters).
*/
public Pose2d getPoseMeters() {
return m_poseMeters;
}
/**
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method takes in the current time as a parameter to calculate period (difference
* between two timestamps). The period is used to calculate the change in distance from a
* velocity. This also takes in an angle parameter which is used instead of the angular rate that
* is calculated from forward kinematics.
*
* @param currentTimeSeconds The current time in seconds.
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide the states in the
* same order in which you instantiated your SwerveDriveKinematics.
* @return The new pose of the robot.
*/
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
m_prevTimeSeconds = currentTimeSeconds;
var angle = gyroAngle.plus(m_gyroOffset);
var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
var newPose =
m_poseMeters.exp(
new Twist2d(
chassisState.vxMetersPerSecond * period,
chassisState.vyMetersPerSecond * period,
angle.minus(m_previousAngle).getRadians()));
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
return m_poseMeters;
}
/**
* Updates the robot's position on the field using forward kinematics and integration of the pose
* over time. This method automatically calculates the current time to calculate period
* (difference between two timestamps). The period is used to calculate the change in distance
* from a velocity. This also takes in an angle parameter which is used instead of the angular
* rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide the states in the
* same order in which you instantiated your SwerveDriveKinematics.
* @return The new pose of the robot.
*/
public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
}
}

71
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java Normal file
View File

@@ -0,0 +1,71 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import edu.wpi.first.math.geometry.Rotation2d;
/** Represents the state of one swerve module. */
@SuppressWarnings("MemberName")
public class SwerveModuleState implements Comparable<SwerveModuleState> {
/** Speed of the wheel of the module. */
public double speedMetersPerSecond;
/** Angle of the module. */
public Rotation2d angle = Rotation2d.fromDegrees(0);
/** Constructs a SwerveModuleState with zeros for speed and angle. */
public SwerveModuleState() {}
/**
* Constructs a SwerveModuleState.
*
* @param speedMetersPerSecond The speed of the wheel of the module.
* @param angle The angle of the module.
*/
public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
this.speedMetersPerSecond = speedMetersPerSecond;
this.angle = angle;
}
/**
* Compares two swerve module states. One swerve module is "greater" than the other if its speed
* is higher than the other.
*
* @param o The other swerve module.
* @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
*/
@Override
@SuppressWarnings("ParameterName")
public int compareTo(SwerveModuleState o) {
return Double.compare(this.speedMetersPerSecond, o.speedMetersPerSecond);
}
@Override
public String toString() {
return String.format(
"SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle);
}
/**
* Minimize the change in heading the desired swerve module state would require by potentially
* reversing the direction the wheel spins. If this is used with the PIDController class's
* continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
*
* @param desiredState The desired state.
* @param currentAngle The current module angle.
*/
public static SwerveModuleState optimize(
SwerveModuleState desiredState, Rotation2d currentAngle) {
var delta = desiredState.angle.minus(currentAngle);
if (Math.abs(delta.getDegrees()) > 90.0) {
return new SwerveModuleState(
-desiredState.speedMetersPerSecond,
desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0)));
} else {
return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
}
}
}