Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Remove unit suffixes from variable names (#7529)

Browse Source 
* Move units into API docs instead because suffixes make user code verbose and hard to read
* Rename trackWidth to trackwidth
* Make ultrasonic classes use meters instead of a mix of m, cm, mm, ft,
  and inches
This commit is contained in:
Tyler Veness
2025-02-10 07:23:04 -08:00
committed by GitHub
parent 764ada9b66
commit ac1705ae2b
250 changed files with 2953 additions and 3584 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
View File

@@ -37,12 +37,12 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
* @param kg The gravity gain in volts.
* @param kv The velocity gain in V/(rad/s).
* @param ka The acceleration gain in V/(rad/s²).
* @param dtSeconds The period in seconds.
* @param dt The period in seconds.
* @throws IllegalArgumentException for kv < zero.
* @throws IllegalArgumentException for ka < zero.
* @throws IllegalArgumentException for period ≤ zero.
*/
public ArmFeedforward(double ks, double kg, double kv, double ka, double dtSeconds) {
public ArmFeedforward(double ks, double kg, double kv, double ka, double dt) {
this.ks = ks;
this.kg = kg;
this.kv = kv;
@@ -53,11 +53,10 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
if (ka < 0.0) {
throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
}
if (dtSeconds <= 0.0) {
throw new IllegalArgumentException(
"period must be a positive number, got " + dtSeconds + "!");
if (dt <= 0.0) {
throw new IllegalArgumentException("period must be a positive number, got " + dt + "!");
}
m_dt = dtSeconds;
m_dt = dt;
}
/**

12
wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
View File

@@ -54,14 +54,14 @@ public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs e
* @param inputs A {@link Nat} representing the number of inputs.
* @param f A vector-valued function of x, the state, and u, the input, that returns the
* derivative of the state vector. HAS to be control-affine (of the form f(x) + Bu).
* @param dtSeconds The timestep between calls of calculate().
* @param dt The timestep between calls of calculate() in seconds.
*/
public ControlAffinePlantInversionFeedforward(
Nat<States> states,
Nat<Inputs> inputs,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
double dtSeconds) {
this.m_dt = dtSeconds;
double dt) {
this.m_dt = dt;
this.m_f = f;
this.m_inputs = inputs;
@@ -84,15 +84,15 @@ public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs e
* @param f A vector-valued function of x, the state, that returns the derivative of the state
* vector.
* @param B Continuous input matrix of the plant being controlled.
* @param dtSeconds The timestep between calls of calculate().
* @param dt The timestep between calls of calculate() in seconds.
*/
public ControlAffinePlantInversionFeedforward(
Nat<States> states,
Nat<Inputs> inputs,
Function<Matrix<States, N1>, Matrix<States, N1>> f,
Matrix<States, Inputs> B,
double dtSeconds) {
this.m_dt = dtSeconds;
double dt) {
this.m_dt = dt;
this.m_inputs = inputs;
this.m_f = (x, u) -> f.apply(x);

6
wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveFeedforward.java
View File

@@ -71,7 +71,7 @@ public class DifferentialDriveFeedforward implements ProtobufSerializable, Struc
* @param currentRightVelocity The current right velocity of the differential drive in
* meters/second.
* @param nextRightVelocity The next right velocity of the differential drive in meters/second.
* @param dtSeconds Discretization timestep.
* @param dt Discretization timestep in seconds.
* @return A DifferentialDriveWheelVoltages object containing the computed feedforward voltages.
*/
public DifferentialDriveWheelVoltages calculate(
@@ -79,8 +79,8 @@ public class DifferentialDriveFeedforward implements ProtobufSerializable, Struc
double nextLeftVelocity,
double currentRightVelocity,
double nextRightVelocity,
double dtSeconds) {
var feedforward = new LinearPlantInversionFeedforward<>(m_plant, dtSeconds);
double dt) {
var feedforward = new LinearPlantInversionFeedforward<>(m_plant, dt);
var r = VecBuilder.fill(currentLeftVelocity, currentRightVelocity);
var nextR = VecBuilder.fill(nextLeftVelocity, nextRightVelocity);
var u = feedforward.calculate(r, nextR);

11
wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
View File

@@ -36,12 +36,12 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
* @param kg The gravity gain in volts.
* @param kv The velocity gain in V/(m/s).
* @param ka The acceleration gain in V/(m/s²).
* @param dtSeconds The period in seconds.
* @param dt The period in seconds.
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
* @throws IllegalArgumentException for period &le; zero.
*/
public ElevatorFeedforward(double ks, double kg, double kv, double ka, double dtSeconds) {
public ElevatorFeedforward(double ks, double kg, double kv, double ka, double dt) {
this.ks = ks;
this.kg = kg;
this.kv = kv;
@@ -52,11 +52,10 @@ public class ElevatorFeedforward implements ProtobufSerializable, StructSerializ
if (ka < 0.0) {
throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
}
if (dtSeconds <= 0.0) {
throw new IllegalArgumentException(
"period must be a positive number, got " + dtSeconds + "!");
if (dt <= 0.0) {
throw new IllegalArgumentException("period must be a positive number, got " + dt + "!");
}
m_dt = dtSeconds;
m_dt = dt;
}
/**

11
wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
View File

@@ -77,14 +77,14 @@ public class HolonomicDriveController {
*
* @param currentPose The current pose, as measured by odometry or pose estimator.
* @param trajectoryPose The desired trajectory pose, as sampled for the current timestep.
* @param desiredLinearVelocityMetersPerSecond The desired linear velocity.
* @param desiredLinearVelocity The desired linear velocity in m/s.
* @param desiredHeading The desired heading.
* @return The next output of the holonomic drive controller.
*/
public ChassisSpeeds calculate(
Pose2d currentPose,
Pose2d trajectoryPose,
double desiredLinearVelocityMetersPerSecond,
double desiredLinearVelocity,
Rotation2d desiredHeading) {
// If this is the first run, then we need to reset the theta controller to the current pose's
// heading.
@@ -94,8 +94,8 @@ public class HolonomicDriveController {
}
// Calculate feedforward velocities (field-relative).
double xFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getCos();
double yFF = desiredLinearVelocityMetersPerSecond * trajectoryPose.getRotation().getSin();
double xFF = desiredLinearVelocity * trajectoryPose.getRotation().getCos();
double yFF = desiredLinearVelocity * trajectoryPose.getRotation().getSin();
double thetaFF =
m_thetaController.calculate(
currentPose.getRotation().getRadians(), desiredHeading.getRadians());
@@ -126,8 +126,7 @@ public class HolonomicDriveController {
*/
public ChassisSpeeds calculate(
Pose2d currentPose, Trajectory.State desiredState, Rotation2d desiredHeading) {
return calculate(
currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, desiredHeading);
return calculate(currentPose, desiredState.pose, desiredState.velocity, desiredHeading);
}
/**

8
wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
View File

@@ -242,10 +242,8 @@ public class LTVDifferentialDriveController {
currentPose,
leftVelocity,
rightVelocity,
desiredState.poseMeters,
desiredState.velocityMetersPerSecond
* (1 - (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0)),
desiredState.velocityMetersPerSecond
* (1 + (desiredState.curvatureRadPerMeter * m_trackwidth / 2.0)));
desiredState.pose,
desiredState.velocity * (1 - (desiredState.curvature * m_trackwidth / 2.0)),
desiredState.velocity * (1 + (desiredState.curvature * m_trackwidth / 2.0)));
}
}

6
wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
View File

@@ -205,9 +205,9 @@ public class LTVUnicycleController {
public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
return calculate(
currentPose,
desiredState.poseMeters,
desiredState.velocityMetersPerSecond,
desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
desiredState.pose,
desiredState.velocity,
desiredState.velocity * desiredState.curvature);
}
/**

13
wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
View File

@@ -40,11 +40,10 @@ public class LinearPlantInversionFeedforward<
* Constructs a feedforward with the given plant.
*
* @param plant The plant being controlled.
* @param dtSeconds Discretization timestep.
* @param dt Discretization timestep in seconds.
*/
public LinearPlantInversionFeedforward(
LinearSystem<States, Inputs, Outputs> plant, double dtSeconds) {
this(plant.getA(), plant.getB(), dtSeconds);
public LinearPlantInversionFeedforward(LinearSystem<States, Inputs, Outputs> plant, double dt) {
this(plant.getA(), plant.getB(), dt);
}
/**
@@ -52,11 +51,11 @@ public class LinearPlantInversionFeedforward<
*
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param dtSeconds Discretization timestep.
* @param dt Discretization timestep in seconds.
*/
public LinearPlantInversionFeedforward(
Matrix<States, States> A, Matrix<States, Inputs> B, double dtSeconds) {
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
Matrix<States, States> A, Matrix<States, Inputs> B, double dt) {
var discABPair = Discretization.discretizeAB(A, B, dt);
this.m_A = discABPair.getFirst();
this.m_B = discABPair.getSecond();

39
wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
View File

@@ -45,20 +45,20 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
* @param plant The plant being controlled.
* @param qelms The maximum desired error tolerance for each state.
* @param relms The maximum desired control effort for each input.
* @param dtSeconds Discretization timestep.
* @param dt Discretization timestep in seconds.
* @throws IllegalArgumentException If the system is unstabilizable.
*/
public LinearQuadraticRegulator(
LinearSystem<States, Inputs, Outputs> plant,
Vector<States> qelms,
Vector<Inputs> relms,
double dtSeconds) {
double dt) {
this(
plant.getA(),
plant.getB(),
StateSpaceUtil.makeCostMatrix(qelms),
StateSpaceUtil.makeCostMatrix(relms),
dtSeconds);
dt);
}
/**
@@ -72,7 +72,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
* @param B Continuous input matrix of the plant being controlled.
* @param qelms The maximum desired error tolerance for each state.
* @param relms The maximum desired control effort for each input.
* @param dtSeconds Discretization timestep.
* @param dt Discretization timestep in seconds.
* @throws IllegalArgumentException If the system is unstabilizable.
*/
public LinearQuadraticRegulator(
@@ -80,13 +80,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
Matrix<States, Inputs> B,
Vector<States> qelms,
Vector<Inputs> relms,
double dtSeconds) {
this(
A,
B,
StateSpaceUtil.makeCostMatrix(qelms),
StateSpaceUtil.makeCostMatrix(relms),
dtSeconds);
double dt) {
this(A, B, StateSpaceUtil.makeCostMatrix(qelms), StateSpaceUtil.makeCostMatrix(relms), dt);
}
/**
@@ -96,7 +91,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
* @param B Continuous input matrix of the plant being controlled.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param dtSeconds Discretization timestep.
* @param dt Discretization timestep in seconds.
* @throws IllegalArgumentException If the system is unstabilizable.
*/
public LinearQuadraticRegulator(
@@ -104,8 +99,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
double dtSeconds) {
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
double dt) {
var discABPair = Discretization.discretizeAB(A, B, dt);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
@@ -134,7 +129,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param N The state-input cross-term cost matrix.
* @param dtSeconds Discretization timestep.
* @param dt Discretization timestep in seconds.
* @throws IllegalArgumentException If the system is unstabilizable.
*/
public LinearQuadraticRegulator(
@@ -143,8 +138,8 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
Matrix<States, Inputs> N,
double dtSeconds) {
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
double dt) {
var discABPair = Discretization.discretizeAB(A, B, dt);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
@@ -253,15 +248,15 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
* appendix C.4 for a derivation.
*
* @param plant The plant being controlled.
* @param dtSeconds Discretization timestep in seconds.
* @param inputDelaySeconds Input time delay in seconds.
* @param dt Discretization timestep in seconds.
* @param inputDelay Input time delay in seconds.
*/
public void latencyCompensate(
LinearSystem<States, Inputs, Outputs> plant, double dtSeconds, double inputDelaySeconds) {
var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dtSeconds);
LinearSystem<States, Inputs, Outputs> plant, double dt, double inputDelay) {
var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
m_K = m_K.times((discA.minus(discB.times(m_K))).pow(inputDelaySeconds / dtSeconds));
m_K = m_K.times((discA.minus(discB.times(m_K))).pow(inputDelay / dt));
}
}

11
wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
View File

@@ -31,12 +31,12 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
* @param ks The static gain in volts.
* @param kv The velocity gain in V/(units/s).
* @param ka The acceleration gain in V/(units/s²).
* @param dtSeconds The period in seconds.
* @param dt The period in seconds.
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
* @throws IllegalArgumentException for period &le; zero.
*/
public SimpleMotorFeedforward(double ks, double kv, double ka, double dtSeconds) {
public SimpleMotorFeedforward(double ks, double kv, double ka, double dt) {
this.ks = ks;
this.kv = kv;
this.ka = ka;
@@ -46,11 +46,10 @@ public class SimpleMotorFeedforward implements ProtobufSerializable, StructSeria
if (ka < 0.0) {
throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
}
if (dtSeconds <= 0.0) {
throw new IllegalArgumentException(
"period must be a positive number, got " + dtSeconds + "!");
if (dt <= 0.0) {
throw new IllegalArgumentException("period must be a positive number, got " + dt + "!");
}
m_dt = dtSeconds;
m_dt = dt;
}
/**

73
wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
View File

@@ -38,22 +38,22 @@ public class DifferentialDrivePoseEstimator extends PoseEstimator<DifferentialDr
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting pose estimate.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
* @param initialPose The starting pose estimate.
*/
public DifferentialDrivePoseEstimator(
DifferentialDriveKinematics kinematics,
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters) {
double leftDistance,
double rightDistance,
Pose2d initialPose) {
this(
kinematics,
gyroAngle,
leftDistanceMeters,
rightDistanceMeters,
initialPoseMeters,
leftDistance,
rightDistance,
initialPose,
VecBuilder.fill(0.02, 0.02, 0.01),
VecBuilder.fill(0.1, 0.1, 0.1));
}
@@ -63,9 +63,9 @@ public class DifferentialDrivePoseEstimator extends PoseEstimator<DifferentialDr
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The gyro angle of the robot.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The estimated initial pose.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
* @param initialPose The estimated initial pose.
* @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
* in meters, and heading in radians). Increase these numbers to trust your state estimate
* less.
@@ -76,15 +76,14 @@ public class DifferentialDrivePoseEstimator extends PoseEstimator<DifferentialDr
public DifferentialDrivePoseEstimator(
DifferentialDriveKinematics kinematics,
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters,
double leftDistance,
double rightDistance,
Pose2d initialPose,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
super(
kinematics,
new DifferentialDriveOdometry(
gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters),
new DifferentialDriveOdometry(gyroAngle, leftDistance, rightDistance, initialPose),
stateStdDevs,
visionMeasurementStdDevs);
}
@@ -96,19 +95,14 @@ public class DifferentialDrivePoseEstimator extends PoseEstimator<DifferentialDr
* automatically takes care of offsetting the gyro angle.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftPositionMeters The distance traveled by the left encoder.
* @param rightPositionMeters The distance traveled by the right encoder.
* @param poseMeters The position on the field that your robot is at.
* @param leftPosition The distance traveled by the left encoder in meters.
* @param rightPosition The distance traveled by the right encoder in meters.
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(
Rotation2d gyroAngle,
double leftPositionMeters,
double rightPositionMeters,
Pose2d poseMeters) {
Rotation2d gyroAngle, double leftPosition, double rightPosition, Pose2d pose) {
resetPosition(
gyroAngle,
new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters),
poseMeters);
gyroAngle, new DifferentialDriveWheelPositions(leftPosition, rightPosition), pose);
}
/**
@@ -116,34 +110,27 @@ public class DifferentialDrivePoseEstimator extends PoseEstimator<DifferentialDr
* loop.
*
* @param gyroAngle The current gyro angle.
* @param distanceLeftMeters The total distance travelled by the left wheel in meters.
* @param distanceRightMeters The total distance travelled by the right wheel in meters.
* @param distanceLeft The total distance travelled by the left wheel in meters.
* @param distanceRight The total distance travelled by the right wheel in meters.
* @return The estimated pose of the robot in meters.
*/
public Pose2d update(
Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
return update(
gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
public Pose2d update(Rotation2d gyroAngle, double distanceLeft, double distanceRight) {
return update(gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight));
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
* @param distanceLeftMeters The total distance travelled by the left wheel in meters.
* @param distanceRightMeters The total distance travelled by the right wheel in meters.
* @param distanceLeft The total distance travelled by the left wheel in meters.
* @param distanceRight The total distance travelled by the right wheel in meters.
* @return The estimated pose of the robot in meters.
*/
public Pose2d updateWithTime(
double currentTimeSeconds,
Rotation2d gyroAngle,
double distanceLeftMeters,
double distanceRightMeters) {
double currentTime, Rotation2d gyroAngle, double distanceLeft, double distanceRight) {
return updateWithTime(
currentTimeSeconds,
gyroAngle,
new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
currentTime, gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight));
}
}

73
wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java
View File

@@ -48,22 +48,22 @@ public class DifferentialDrivePoseEstimator3d
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting pose estimate.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
* @param initialPose The starting pose estimate.
*/
public DifferentialDrivePoseEstimator3d(
DifferentialDriveKinematics kinematics,
Rotation3d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose3d initialPoseMeters) {
double leftDistance,
double rightDistance,
Pose3d initialPose) {
this(
kinematics,
gyroAngle,
leftDistanceMeters,
rightDistanceMeters,
initialPoseMeters,
leftDistance,
rightDistance,
initialPose,
VecBuilder.fill(0.02, 0.02, 0.02, 0.01),
VecBuilder.fill(0.1, 0.1, 0.1, 0.1));
}
@@ -73,9 +73,9 @@ public class DifferentialDrivePoseEstimator3d
*
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The gyro angle of the robot.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The estimated initial pose.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
* @param initialPose The estimated initial pose.
* @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
* in meters, and heading in radians). Increase these numbers to trust your state estimate
* less.
@@ -86,15 +86,14 @@ public class DifferentialDrivePoseEstimator3d
public DifferentialDrivePoseEstimator3d(
DifferentialDriveKinematics kinematics,
Rotation3d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose3d initialPoseMeters,
double leftDistance,
double rightDistance,
Pose3d initialPose,
Matrix<N4, N1> stateStdDevs,
Matrix<N4, N1> visionMeasurementStdDevs) {
super(
kinematics,
new DifferentialDriveOdometry3d(
gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters),
new DifferentialDriveOdometry3d(gyroAngle, leftDistance, rightDistance, initialPose),
stateStdDevs,
visionMeasurementStdDevs);
}
@@ -106,19 +105,14 @@ public class DifferentialDrivePoseEstimator3d
* automatically takes care of offsetting the gyro angle.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftPositionMeters The distance traveled by the left encoder.
* @param rightPositionMeters The distance traveled by the right encoder.
* @param poseMeters The position on the field that your robot is at.
* @param leftPosition The distance traveled by the left encoder in meters.
* @param rightPosition The distance traveled by the right encoder in meters.
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(
Rotation3d gyroAngle,
double leftPositionMeters,
double rightPositionMeters,
Pose3d poseMeters) {
Rotation3d gyroAngle, double leftPosition, double rightPosition, Pose3d pose) {
resetPosition(
gyroAngle,
new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters),
poseMeters);
gyroAngle, new DifferentialDriveWheelPositions(leftPosition, rightPosition), pose);
}
/**
@@ -126,34 +120,27 @@ public class DifferentialDrivePoseEstimator3d
* loop.
*
* @param gyroAngle The current gyro angle.
* @param distanceLeftMeters The total distance travelled by the left wheel in meters.
* @param distanceRightMeters The total distance travelled by the right wheel in meters.
* @param distanceLeft The total distance travelled by the left wheel in meters.
* @param distanceRight The total distance travelled by the right wheel in meters.
* @return The estimated pose of the robot in meters.
*/
public Pose3d update(
Rotation3d gyroAngle, double distanceLeftMeters, double distanceRightMeters) {
return update(
gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
public Pose3d update(Rotation3d gyroAngle, double distanceLeft, double distanceRight) {
return update(gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight));
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
* @param distanceLeftMeters The total distance travelled by the left wheel in meters.
* @param distanceRightMeters The total distance travelled by the right wheel in meters.
* @param distanceLeft The total distance travelled by the left wheel in meters.
* @param distanceRight The total distance travelled by the right wheel in meters.
* @return The estimated pose of the robot in meters.
*/
public Pose3d updateWithTime(
double currentTimeSeconds,
Rotation3d gyroAngle,
double distanceLeftMeters,
double distanceRightMeters) {
double currentTime, Rotation3d gyroAngle, double distanceLeft, double distanceRight) {
return updateWithTime(
currentTimeSeconds,
gyroAngle,
new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters));
currentTime, gyroAngle, new DifferentialDriveWheelPositions(distanceLeft, distanceRight));
}
}

36
wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
View File

@@ -58,7 +58,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
private Matrix<States, States> m_P;
private double m_dtSeconds;
private double m_dt;
/**
* Constructs an extended Kalman filter.
@@ -74,7 +74,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param dtSeconds Nominal discretization timestep.
* @param dt Nominal discretization timestep in seconds.
*/
public ExtendedKalmanFilter(
Nat<States> states,
@@ -84,7 +84,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double dtSeconds) {
double dt) {
this(
states,
inputs,
@@ -95,7 +95,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
measurementStdDevs,
Matrix::minus,
Matrix::plus,
dtSeconds);
dt);
}
/**
@@ -115,7 +115,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
* @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
* subtracts them.)
* @param addFuncX A function that adds two state vectors.
* @param dtSeconds Nominal discretization timestep.
* @param dt Nominal discretization timestep in seconds.
*/
public ExtendedKalmanFilter(
Nat<States> states,
@@ -127,7 +127,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
Matrix<Outputs, N1> measurementStdDevs,
BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX,
double dtSeconds) {
double dt) {
m_states = states;
m_outputs = outputs;
@@ -139,7 +139,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
m_dtSeconds = dtSeconds;
m_dt = dt;
reset();
@@ -150,11 +150,11 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
NumericalJacobian.numericalJacobianX(
outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1()));
final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds);
final var discPair = Discretization.discretizeAQ(contA, m_contQ, dt);
final var discA = discPair.getFirst();
final var discQ = discPair.getSecond();
final var discR = Discretization.discretizeR(m_contR, dtSeconds);
final var discR = Discretization.discretizeR(m_contR, dt);
if (StateSpaceUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
m_initP = DARE.dare(discA.transpose(), C.transpose(), discQ, discR);
@@ -249,11 +249,11 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
* Project the model into the future with a new control input u.
*
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
* @param dt Timestep for prediction in seconds.
*/
@Override
public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
predict(u, m_f, dtSeconds);
public void predict(Matrix<Inputs, N1> u, double dt) {
predict(u, m_f, dt);
}
/**
@@ -261,26 +261,26 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
*
* @param u New control input from controller.
* @param f The function used to linearize the model.
* @param dtSeconds Timestep for prediction.
* @param dt Timestep for prediction in seconds.
*/
public void predict(
Matrix<Inputs, N1> u,
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
double dtSeconds) {
double dt) {
// Find continuous A
final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u);
// Find discrete A and Q
final var discPair = Discretization.discretizeAQ(contA, m_contQ, dtSeconds);
final var discPair = Discretization.discretizeAQ(contA, m_contQ, dt);
final var discA = discPair.getFirst();
final var discQ = discPair.getSecond();
m_xHat = NumericalIntegration.rk4(f, m_xHat, u, dtSeconds);
m_xHat = NumericalIntegration.rk4(f, m_xHat, u, dt);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
m_dtSeconds = dtSeconds;
m_dt = dt;
}
/**
@@ -356,7 +356,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
BiFunction<Matrix<Rows, N1>, Matrix<Rows, N1>, Matrix<Rows, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
final var discR = Discretization.discretizeR(R, m_dtSeconds);
final var discR = Discretization.discretizeR(R, m_dt);
final var S = C.times(m_P).times(C.transpose()).plus(discR);

24
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
View File

@@ -41,7 +41,7 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
private Matrix<States, States> m_P;
private final Matrix<States, States> m_contQ;
private final Matrix<Outputs, Outputs> m_contR;
private double m_dtSeconds;
private double m_dt;
private final Matrix<States, States> m_initP;
@@ -57,7 +57,7 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
* @param plant The plant used for the prediction step.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param dtSeconds Nominal discretization timestep.
* @param dt Nominal discretization timestep in seconds.
* @throws IllegalArgumentException If the system is undetectable.
*/
public KalmanFilter(
@@ -66,21 +66,21 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
LinearSystem<States, Inputs, Outputs> plant,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double dtSeconds) {
double dt) {
this.m_states = states;
this.m_plant = plant;
m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
m_dtSeconds = dtSeconds;
m_dt = dt;
// Find discrete A and Q
var pair = Discretization.discretizeAQ(plant.getA(), m_contQ, dtSeconds);
var pair = Discretization.discretizeAQ(plant.getA(), m_contQ, dt);
var discA = pair.getFirst();
var discQ = pair.getSecond();
var discR = Discretization.discretizeR(m_contR, dtSeconds);
var discR = Discretization.discretizeR(m_contR, dt);
var C = plant.getC();
@@ -173,21 +173,21 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
* Project the model into the future with a new control input u.
*
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
* @param dt Timestep for prediction in seconds.
*/
@Override
public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
public void predict(Matrix<Inputs, N1> u, double dt) {
// Find discrete A and Q
final var discPair = Discretization.discretizeAQ(m_plant.getA(), m_contQ, dtSeconds);
final var discPair = Discretization.discretizeAQ(m_plant.getA(), m_contQ, dt);
final var discA = discPair.getFirst();
final var discQ = discPair.getSecond();
m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds);
m_xHat = m_plant.calculateX(m_xHat, u, dt);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
m_dtSeconds = dtSeconds;
m_dt = dt;
}
/**
@@ -214,7 +214,7 @@ public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extend
final var C = m_plant.getC();
final var D = m_plant.getD();
final var discR = Discretization.discretizeR(R, m_dtSeconds);
final var discR = Discretization.discretizeR(R, m_dt);
final var S = C.times(m_P).times(C.transpose()).plus(discR);

24
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
View File

@@ -41,15 +41,11 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
* @param observer The observer.
* @param u The input at the timestamp.
* @param localY The local output at the timestamp
* @param timestampSeconds The timestamp of the state.
* @param timestamp The timestamp of the state in seconds.
*/
public void addObserverState(
KalmanTypeFilter<S, I, O> observer,
Matrix<I, N1> u,
Matrix<O, N1> localY,
double timestampSeconds) {
m_pastObserverSnapshots.add(
Map.entry(timestampSeconds, new ObserverSnapshot(observer, u, localY)));
KalmanTypeFilter<S, I, O> observer, Matrix<I, N1> u, Matrix<O, N1> localY, double timestamp) {
m_pastObserverSnapshots.add(Map.entry(timestamp, new ObserverSnapshot(observer, u, localY)));
if (m_pastObserverSnapshots.size() > kMaxPastObserverStates) {
m_pastObserverSnapshots.remove(0);
@@ -62,27 +58,24 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
* @param <R> The rows in the global measurement vector.
* @param rows The rows in the global measurement vector.
* @param observer The observer to apply the past global measurement.
* @param nominalDtSeconds The nominal timestep.
* @param nominalDt The nominal timestep in seconds.
* @param y The measurement.
* @param globalMeasurementCorrect The function take calls correct() on the observer.
* @param timestampSeconds The timestamp of the measurement.
* @param timestamp The timestamp of the measurement in seconds.
*/
public <R extends Num> void applyPastGlobalMeasurement(
Nat<R> rows,
KalmanTypeFilter<S, I, O> observer,
double nominalDtSeconds,
double nominalDt,
Matrix<R, N1> y,
BiConsumer<Matrix<I, N1>, Matrix<R, N1>> globalMeasurementCorrect,
double timestampSeconds) {
double timestamp) {
if (m_pastObserverSnapshots.isEmpty()) {
// State map was empty, which means that we got a past measurement right at startup. The only
// thing we can really do is ignore the measurement.
return;
}
// Use a less verbose name for timestamp
double timestamp = timestampSeconds;
int maxIdx = m_pastObserverSnapshots.size() - 1;
int low = 0;
int high = maxIdx;
@@ -134,8 +127,7 @@ public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O exte
indexOfClosestEntry = prevTimeDiff <= nextTimeDiff ? prevIdx : nextIdx;
}
double lastTimestamp =
m_pastObserverSnapshots.get(indexOfClosestEntry).getKey() - nominalDtSeconds;
double lastTimestamp = m_pastObserverSnapshots.get(indexOfClosestEntry).getKey() - nominalDt;
// We will now go back in time to the state of the system at the time when
// the measurement was captured. We will reset the observer to that state,

4
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
View File

@@ -76,9 +76,9 @@ public interface KalmanTypeFilter<States extends Num, Inputs extends Num, Output
* Project the model into the future with a new control input u.
*
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
* @param dt Timestep for prediction in seconds.
*/
void predict(Matrix<Inputs, N1> u, double dtSeconds);
void predict(Matrix<Inputs, N1> u, double dt);
/**
* Correct the state estimate x-hat using the measurements in y.

12
wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
View File

@@ -37,18 +37,18 @@ public class MecanumDrivePoseEstimator extends PoseEstimator<MecanumDriveWheelPo
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances driven by each wheel.
* @param initialPoseMeters The starting pose estimate.
* @param initialPose The starting pose estimate.
*/
public MecanumDrivePoseEstimator(
MecanumDriveKinematics kinematics,
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose2d initialPoseMeters) {
Pose2d initialPose) {
this(
kinematics,
gyroAngle,
wheelPositions,
initialPoseMeters,
initialPose,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.45, 0.45, 0.45));
}
@@ -59,7 +59,7 @@ public class MecanumDrivePoseEstimator extends PoseEstimator<MecanumDriveWheelPo
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distance measured by each wheel.
* @param initialPoseMeters The starting pose estimate.
* @param initialPose The starting pose estimate.
* @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
* in meters, and heading in radians). Increase these numbers to trust your state estimate
* less.
@@ -71,12 +71,12 @@ public class MecanumDrivePoseEstimator extends PoseEstimator<MecanumDriveWheelPo
MecanumDriveKinematics kinematics,
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose2d initialPoseMeters,
Pose2d initialPose,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
super(
kinematics,
new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters),
new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPose),
stateStdDevs,
visionMeasurementStdDevs);
}

12
wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java
View File

@@ -46,18 +46,18 @@ public class MecanumDrivePoseEstimator3d extends PoseEstimator3d<MecanumDriveWhe
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distances driven by each wheel.
* @param initialPoseMeters The starting pose estimate.
* @param initialPose The starting pose estimate.
*/
public MecanumDrivePoseEstimator3d(
MecanumDriveKinematics kinematics,
Rotation3d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose3d initialPoseMeters) {
Pose3d initialPose) {
this(
kinematics,
gyroAngle,
wheelPositions,
initialPoseMeters,
initialPose,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.45, 0.45, 0.45, 0.45));
}
@@ -68,7 +68,7 @@ public class MecanumDrivePoseEstimator3d extends PoseEstimator3d<MecanumDriveWhe
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The distance measured by each wheel.
* @param initialPoseMeters The starting pose estimate.
* @param initialPose The starting pose estimate.
* @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
* in meters, and heading in radians). Increase these numbers to trust your state estimate
* less.
@@ -80,12 +80,12 @@ public class MecanumDrivePoseEstimator3d extends PoseEstimator3d<MecanumDriveWhe
MecanumDriveKinematics kinematics,
Rotation3d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose3d initialPoseMeters,
Pose3d initialPose,
Matrix<N4, N1> stateStdDevs,
Matrix<N4, N1> visionMeasurementStdDevs) {
super(
kinematics,
new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPoseMeters),
new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPose),
stateStdDevs,
visionMeasurementStdDevs);
}

79
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
View File

@@ -73,7 +73,7 @@ public class PoseEstimator<T> {
Matrix<N3, N1> visionMeasurementStdDevs) {
m_odometry = odometry;
m_poseEstimate = m_odometry.getPoseMeters();
m_poseEstimate = m_odometry.getPose();
for (int i = 0; i < 3; ++i) {
m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
@@ -116,14 +116,14 @@ public class PoseEstimator<T> {
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current encoder readings.
* @param poseMeters The position on the field that your robot is at.
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d pose) {
// Reset state estimate and error covariance
m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
m_odometry.resetPosition(gyroAngle, wheelPositions, pose);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
m_poseEstimate = m_odometry.getPose();
}
/**
@@ -135,7 +135,7 @@ public class PoseEstimator<T> {
m_odometry.resetPose(pose);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
m_poseEstimate = m_odometry.getPose();
}
/**
@@ -147,7 +147,7 @@ public class PoseEstimator<T> {
m_odometry.resetTranslation(translation);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
m_poseEstimate = m_odometry.getPose();
}
/**
@@ -159,7 +159,7 @@ public class PoseEstimator<T> {
m_odometry.resetRotation(rotation);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
m_poseEstimate = m_odometry.getPose();
}
/**
@@ -174,10 +174,10 @@ public class PoseEstimator<T> {
/**
* Return the pose at a given timestamp, if the buffer is not empty.
*
* @param timestampSeconds The pose's timestamp in seconds.
* @param timestamp The pose's timestamp in seconds.
* @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
*/
public Optional<Pose2d> sampleAt(double timestampSeconds) {
public Optional<Pose2d> sampleAt(double timestamp) {
// Step 0: If there are no odometry updates to sample, skip.
if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) {
return Optional.empty();
@@ -187,20 +187,19 @@ public class PoseEstimator<T> {
// the buffer will always use a timestamp between the first and last timestamps)
double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey();
timestampSeconds =
MathUtil.clamp(timestampSeconds, oldestOdometryTimestamp, newestOdometryTimestamp);
timestamp = MathUtil.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
// Step 2: If there are no applicable vision updates, use the odometry-only information.
if (m_visionUpdates.isEmpty() || timestampSeconds < m_visionUpdates.firstKey()) {
return m_odometryPoseBuffer.getSample(timestampSeconds);
if (m_visionUpdates.isEmpty() || timestamp < m_visionUpdates.firstKey()) {
return m_odometryPoseBuffer.getSample(timestamp);
}
// Step 3: Get the latest vision update from before or at the timestamp to sample at.
double floorTimestamp = m_visionUpdates.floorKey(timestampSeconds);
double floorTimestamp = m_visionUpdates.floorKey(timestamp);
var visionUpdate = m_visionUpdates.get(floorTimestamp);
// Step 4: Get the pose measured by odometry at the time of the sample.
var odometryEstimate = m_odometryPoseBuffer.getSample(timestampSeconds);
var odometryEstimate = m_odometryPoseBuffer.getSample(timestamp);
// Step 5: Apply the vision compensation to the odometry pose.
return odometryEstimate.map(odometryPose -> visionUpdate.compensate(odometryPose));
@@ -239,19 +238,18 @@ public class PoseEstimator<T> {
* recommend only adding vision measurements that are already within one meter or so of the
* current pose estimate.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
* @param visionRobotPose The pose of the robot as measured by the vision camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use
* your own time source by calling {@link
* PoseEstimator#updateWithTime(double,Rotation2d,Object)} then you must use a timestamp with
* an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as {@link
* edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should use {@link
* edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync the epochs.
*/
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
public void addVisionMeasurement(Pose2d visionRobotPose, double timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()
|| m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration
> timestampSeconds) {
|| m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestamp) {
return;
}
@@ -259,7 +257,7 @@ public class PoseEstimator<T> {
cleanUpVisionUpdates();
// Step 2: Get the pose measured by odometry at the moment the vision measurement was made.
var odometrySample = m_odometryPoseBuffer.getSample(timestampSeconds);
var odometrySample = m_odometryPoseBuffer.getSample(timestamp);
if (odometrySample.isEmpty()) {
return;
@@ -267,14 +265,14 @@ public class PoseEstimator<T> {
// Step 3: Get the vision-compensated pose estimate at the moment the vision measurement was
// made.
var visionSample = sampleAt(timestampSeconds);
var visionSample = sampleAt(timestamp);
if (visionSample.isEmpty()) {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision pose.
var twist = visionSample.get().log(visionRobotPoseMeters);
var twist = visionSample.get().log(visionRobotPose);
// Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
@@ -286,14 +284,14 @@ public class PoseEstimator<T> {
// Step 7: Calculate and record the vision update.
var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get());
m_visionUpdates.put(timestampSeconds, visionUpdate);
m_visionUpdates.put(timestamp, visionUpdate);
// Step 8: Remove later vision measurements. (Matches previous behavior)
m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear();
m_visionUpdates.tailMap(timestamp, false).entrySet().clear();
// Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
// it's guaranteed to be the latest vision update.
m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
m_poseEstimate = visionUpdate.compensate(m_odometry.getPose());
}
/**
@@ -311,23 +309,20 @@ public class PoseEstimator<T> {
* to apply to future measurements until a subsequent call to {@link
* PoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link #updateWithTime}, then you must use a
* timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same
* epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you
* should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in
* this case.
* @param visionRobotPose The pose of the robot as measured by the vision camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use
* your own time source by calling {@link #updateWithTime}, then you must use a timestamp with
* an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as {@link
* edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you should use {@link
* edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
* in meters, y position in meters, and heading in radians). Increase these numbers to trust
* the vision pose measurement less.
*/
public void addVisionMeasurement(
Pose2d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N3, N1> visionMeasurementStdDevs) {
Pose2d visionRobotPose, double timestamp, Matrix<N3, N1> visionMeasurementStdDevs) {
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
addVisionMeasurement(visionRobotPose, timestamp);
}
/**
@@ -346,15 +341,15 @@ public class PoseEstimator<T> {
* Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The current encoder readings.
* @return The estimated pose of the robot in meters.
*/
public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T wheelPositions) {
public Pose2d updateWithTime(double currentTime, Rotation2d gyroAngle, T wheelPositions) {
var odometryEstimate = m_odometry.update(gyroAngle, wheelPositions);
m_odometryPoseBuffer.addSample(currentTimeSeconds, odometryEstimate);
m_odometryPoseBuffer.addSample(currentTime, odometryEstimate);
if (m_visionUpdates.isEmpty()) {
m_poseEstimate = odometryEstimate;

79
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
View File

@@ -81,7 +81,7 @@ public class PoseEstimator3d<T> {
Matrix<N4, N1> visionMeasurementStdDevs) {
m_odometry = odometry;
m_poseEstimate = m_odometry.getPoseMeters();
m_poseEstimate = m_odometry.getPose();
for (int i = 0; i < 4; ++i) {
m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0));
@@ -128,14 +128,14 @@ public class PoseEstimator3d<T> {
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current encoder readings.
* @param poseMeters The position on the field that your robot is at.
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) {
public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d pose) {
// Reset state estimate and error covariance
m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters);
m_odometry.resetPosition(gyroAngle, wheelPositions, pose);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
m_poseEstimate = m_odometry.getPose();
}
/**
@@ -147,7 +147,7 @@ public class PoseEstimator3d<T> {
m_odometry.resetPose(pose);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
m_poseEstimate = m_odometry.getPose();
}
/**
@@ -159,7 +159,7 @@ public class PoseEstimator3d<T> {
m_odometry.resetTranslation(translation);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
m_poseEstimate = m_odometry.getPose();
}
/**
@@ -171,7 +171,7 @@ public class PoseEstimator3d<T> {
m_odometry.resetRotation(rotation);
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();
m_poseEstimate = m_odometry.getPose();
}
/**
@@ -186,10 +186,10 @@ public class PoseEstimator3d<T> {
/**
* Return the pose at a given timestamp, if the buffer is not empty.
*
* @param timestampSeconds The pose's timestamp in seconds.
* @param timestamp The pose's timestamp in seconds.
* @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
*/
public Optional<Pose3d> sampleAt(double timestampSeconds) {
public Optional<Pose3d> sampleAt(double timestamp) {
// Step 0: If there are no odometry updates to sample, skip.
if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) {
return Optional.empty();
@@ -199,20 +199,19 @@ public class PoseEstimator3d<T> {
// the buffer will always use a timestamp between the first and last timestamps)
double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey();
double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey();
timestampSeconds =
MathUtil.clamp(timestampSeconds, oldestOdometryTimestamp, newestOdometryTimestamp);
timestamp = MathUtil.clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
// Step 2: If there are no applicable vision updates, use the odometry-only information.
if (m_visionUpdates.isEmpty() || timestampSeconds < m_visionUpdates.firstKey()) {
return m_odometryPoseBuffer.getSample(timestampSeconds);
if (m_visionUpdates.isEmpty() || timestamp < m_visionUpdates.firstKey()) {
return m_odometryPoseBuffer.getSample(timestamp);
}
// Step 3: Get the latest vision update from before or at the timestamp to sample at.
double floorTimestamp = m_visionUpdates.floorKey(timestampSeconds);
double floorTimestamp = m_visionUpdates.floorKey(timestamp);
var visionUpdate = m_visionUpdates.get(floorTimestamp);
// Step 4: Get the pose measured by odometry at the time of the sample.
var odometryEstimate = m_odometryPoseBuffer.getSample(timestampSeconds);
var odometryEstimate = m_odometryPoseBuffer.getSample(timestamp);
// Step 5: Apply the vision compensation to the odometry pose.
return odometryEstimate.map(odometryPose -> visionUpdate.compensate(odometryPose));
@@ -251,20 +250,19 @@ public class PoseEstimator3d<T> {
* recommend only adding vision measurements that are already within one meter or so of the
* current pose estimate.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link
* @param visionRobotPose The pose of the robot as measured by the vision camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use
* your own time source by calling {@link
* PoseEstimator3d#updateWithTime(double,Rotation3d,Object)} then you must use a timestamp
* with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as
* {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should use
* {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync the
* epochs.
*/
public void addVisionMeasurement(Pose3d visionRobotPoseMeters, double timestampSeconds) {
public void addVisionMeasurement(Pose3d visionRobotPose, double timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()
|| m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration
> timestampSeconds) {
|| m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestamp) {
return;
}
@@ -272,7 +270,7 @@ public class PoseEstimator3d<T> {
cleanUpVisionUpdates();
// Step 2: Get the pose measured by odometry at the moment the vision measurement was made.
var odometrySample = m_odometryPoseBuffer.getSample(timestampSeconds);
var odometrySample = m_odometryPoseBuffer.getSample(timestamp);
if (odometrySample.isEmpty()) {
return;
@@ -280,14 +278,14 @@ public class PoseEstimator3d<T> {
// Step 3: Get the vision-compensated pose estimate at the moment the vision measurement was
// made.
var visionSample = sampleAt(timestampSeconds);
var visionSample = sampleAt(timestamp);
if (visionSample.isEmpty()) {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision pose.
var twist = visionSample.get().log(visionRobotPoseMeters);
var twist = visionSample.get().log(visionRobotPose);
// Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
@@ -307,14 +305,14 @@ public class PoseEstimator3d<T> {
// Step 7: Calculate and record the vision update.
var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get());
m_visionUpdates.put(timestampSeconds, visionUpdate);
m_visionUpdates.put(timestamp, visionUpdate);
// Step 8: Remove later vision measurements. (Matches previous behavior)
m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear();
m_visionUpdates.tailMap(timestamp, false).entrySet().clear();
// Step 9: Update latest pose estimate. Since we cleared all updates after this vision update,
// it's guaranteed to be the latest vision update.
m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
m_poseEstimate = visionUpdate.compensate(m_odometry.getPose());
}
/**
@@ -332,23 +330,20 @@ public class PoseEstimator3d<T> {
* to apply to future measurements until a subsequent call to {@link
* PoseEstimator3d#setVisionMeasurementStdDevs(Matrix)} or this method.
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
* don't use your own time source by calling {@link #updateWithTime}, then you must use a
* timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same
* epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you
* should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in
* this case.
* @param visionRobotPose The pose of the robot as measured by the vision camera.
* @param timestamp The timestamp of the vision measurement in seconds. Note that if you don't use
* your own time source by calling {@link #updateWithTime}, then you must use a timestamp with
* an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as {@link
* edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you should use {@link
* edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in this case.
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position
* in meters, y position in meters, z position in meters, and angle in radians). Increase
* these numbers to trust the vision pose measurement less.
*/
public void addVisionMeasurement(
Pose3d visionRobotPoseMeters,
double timestampSeconds,
Matrix<N4, N1> visionMeasurementStdDevs) {
Pose3d visionRobotPose, double timestamp, Matrix<N4, N1> visionMeasurementStdDevs) {
setVisionMeasurementStdDevs(visionMeasurementStdDevs);
addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
addVisionMeasurement(visionRobotPose, timestamp);
}
/**
@@ -367,15 +362,15 @@ public class PoseEstimator3d<T> {
* Updates the pose estimator with wheel encoder and gyro information. This should be called every
* loop.
*
* @param currentTimeSeconds Time at which this method was called, in seconds.
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
* @param wheelPositions The current encoder readings.
* @return The estimated pose of the robot in meters.
*/
public Pose3d updateWithTime(double currentTimeSeconds, Rotation3d gyroAngle, T wheelPositions) {
public Pose3d updateWithTime(double currentTime, Rotation3d gyroAngle, T wheelPositions) {
var odometryEstimate = m_odometry.update(gyroAngle, wheelPositions);
m_odometryPoseBuffer.addSample(currentTimeSeconds, odometryEstimate);
m_odometryPoseBuffer.addSample(currentTime, odometryEstimate);
if (m_visionUpdates.isEmpty()) {
m_poseEstimate = odometryEstimate;

14
wpimath/src/main/java/edu/wpi/first/math/estimator/SteadyStateKalmanFilter.java
View File

@@ -58,7 +58,7 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
* @param plant The plant used for the prediction step.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param dtSeconds Nominal discretization timestep.
* @param dt Nominal discretization timestep in seconds.
* @throws IllegalArgumentException If the system is undetectable.
*/
public SteadyStateKalmanFilter(
@@ -67,7 +67,7 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
LinearSystem<States, Inputs, Outputs> plant,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double dtSeconds) {
double dt) {
this.m_states = states;
this.m_plant = plant;
@@ -75,11 +75,11 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
var pair = Discretization.discretizeAQ(plant.getA(), contQ, dtSeconds);
var pair = Discretization.discretizeAQ(plant.getA(), contQ, dt);
var discA = pair.getFirst();
var discQ = pair.getSecond();
var discR = Discretization.discretizeR(contR, dtSeconds);
var discR = Discretization.discretizeR(contR, dt);
var C = plant.getC();
@@ -174,10 +174,10 @@ public class SteadyStateKalmanFilter<States extends Num, Inputs extends Num, Out
* Project the model into the future with a new control input u.
*
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
* @param dt Timestep for prediction in seconds.
*/
public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds);
public void predict(Matrix<Inputs, N1> u, double dt) {
this.m_xHat = m_plant.calculateX(m_xHat, u, dt);
}
/**

16
wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
View File

@@ -38,18 +38,18 @@ public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveModulePosition
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param modulePositions The current distance measurements and rotations of the swerve modules.
* @param initialPoseMeters The starting pose estimate.
* @param initialPose The starting pose estimate.
*/
public SwerveDrivePoseEstimator(
SwerveDriveKinematics kinematics,
Rotation2d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose2d initialPoseMeters) {
Pose2d initialPose) {
this(
kinematics,
gyroAngle,
modulePositions,
initialPoseMeters,
initialPose,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.9, 0.9, 0.9));
}
@@ -60,7 +60,7 @@ public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveModulePosition
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param modulePositions The current distance and rotation measurements of the swerve modules.
* @param initialPoseMeters The starting pose estimate.
* @param initialPose The starting pose estimate.
* @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
* in meters, and heading in radians). Increase these numbers to trust your state estimate
* less.
@@ -72,12 +72,12 @@ public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveModulePosition
SwerveDriveKinematics kinematics,
Rotation2d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose2d initialPoseMeters,
Pose2d initialPose,
Matrix<N3, N1> stateStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
super(
kinematics,
new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters),
new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPose),
stateStdDevs,
visionMeasurementStdDevs);
@@ -86,13 +86,13 @@ public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveModulePosition
@Override
public Pose2d updateWithTime(
double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions) {
double currentTime, Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions) {
if (wheelPositions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions);
return super.updateWithTime(currentTime, gyroAngle, wheelPositions);
}
}

16
wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java
View File

@@ -47,18 +47,18 @@ public class SwerveDrivePoseEstimator3d extends PoseEstimator3d<SwerveModulePosi
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param modulePositions The current distance measurements and rotations of the swerve modules.
* @param initialPoseMeters The starting pose estimate.
* @param initialPose The starting pose estimate.
*/
public SwerveDrivePoseEstimator3d(
SwerveDriveKinematics kinematics,
Rotation3d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose3d initialPoseMeters) {
Pose3d initialPose) {
this(
kinematics,
gyroAngle,
modulePositions,
initialPoseMeters,
initialPose,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.9, 0.9, 0.9, 0.9));
}
@@ -69,7 +69,7 @@ public class SwerveDrivePoseEstimator3d extends PoseEstimator3d<SwerveModulePosi
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param gyroAngle The current gyro angle.
* @param modulePositions The current distance and rotation measurements of the swerve modules.
* @param initialPoseMeters The starting pose estimate.
* @param initialPose The starting pose estimate.
* @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position
* in meters, and heading in radians). Increase these numbers to trust your state estimate
* less.
@@ -81,12 +81,12 @@ public class SwerveDrivePoseEstimator3d extends PoseEstimator3d<SwerveModulePosi
SwerveDriveKinematics kinematics,
Rotation3d gyroAngle,
SwerveModulePosition[] modulePositions,
Pose3d initialPoseMeters,
Pose3d initialPose,
Matrix<N4, N1> stateStdDevs,
Matrix<N4, N1> visionMeasurementStdDevs) {
super(
kinematics,
new SwerveDriveOdometry3d(kinematics, gyroAngle, modulePositions, initialPoseMeters),
new SwerveDriveOdometry3d(kinematics, gyroAngle, modulePositions, initialPose),
stateStdDevs,
visionMeasurementStdDevs);
@@ -95,13 +95,13 @@ public class SwerveDrivePoseEstimator3d extends PoseEstimator3d<SwerveModulePosi
@Override
public Pose3d updateWithTime(
double currentTimeSeconds, Rotation3d gyroAngle, SwerveModulePosition[] wheelPositions) {
double currentTime, Rotation3d gyroAngle, SwerveModulePosition[] wheelPositions) {
if (wheelPositions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "
+ "constructor");
}
return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions);
return super.updateWithTime(currentTime, gyroAngle, wheelPositions);
}
}

26
wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
View File

@@ -62,7 +62,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
private final Matrix<States, States> m_contQ;
private final Matrix<Outputs, Outputs> m_contR;
private Matrix<States, ?> m_sigmasF;
private double m_dtSeconds;
private double m_dt;
private final MerweScaledSigmaPoints<States> m_pts;
@@ -79,7 +79,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* @param h A vector-valued function of x and u that returns the measurement vector.
* @param stateStdDevs Standard deviations of model states.
* @param measurementStdDevs Standard deviations of measurements.
* @param nominalDtSeconds Nominal discretization timestep.
* @param nominalDt Nominal discretization timestep in seconds.
*/
public UnscentedKalmanFilter(
Nat<States> states,
@@ -88,7 +88,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> measurementStdDevs,
double nominalDtSeconds) {
double nominalDt) {
this(
states,
outputs,
@@ -101,7 +101,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
Matrix::minus,
Matrix::minus,
Matrix::plus,
nominalDtSeconds);
nominalDt);
}
/**
@@ -128,7 +128,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
* subtracts them.)
* @param addFuncX A function that adds two state vectors.
* @param nominalDtSeconds Nominal discretization timestep.
* @param nominalDt Nominal discretization timestep in seconds.
*/
public UnscentedKalmanFilter(
Nat<States> states,
@@ -142,7 +142,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX,
double nominalDtSeconds) {
double nominalDt) {
this.m_states = states;
this.m_outputs = outputs;
@@ -155,7 +155,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
m_residualFuncY = residualFuncY;
m_addFuncX = addFuncX;
m_dtSeconds = nominalDtSeconds;
m_dt = nominalDt;
m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
@@ -337,14 +337,14 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
* Project the model into the future with a new control input u.
*
* @param u New control input from controller.
* @param dtSeconds Timestep for prediction.
* @param dt Timestep for prediction in seconds.
*/
@Override
public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
public void predict(Matrix<Inputs, N1> u, double dt) {
// Discretize Q before projecting mean and covariance forward
Matrix<States, States> contA =
NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u);
var discQ = Discretization.discretizeAQ(contA, m_contQ, dtSeconds).getSecond();
var discQ = Discretization.discretizeAQ(contA, m_contQ, dt).getSecond();
var squareRootDiscQ = discQ.lltDecompose(true);
var sigmas = m_pts.squareRootSigmaPoints(m_xHat, m_S);
@@ -352,7 +352,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
Matrix<States, N1> x = sigmas.extractColumnVector(i);
m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dtSeconds));
m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dt));
}
var ret =
@@ -368,7 +368,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
m_xHat = ret.getFirst();
m_S = ret.getSecond();
m_dtSeconds = dtSeconds;
m_dt = dt;
}
/**
@@ -456,7 +456,7 @@ public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outpu
BiFunction<Matrix<R, N1>, Matrix<R, N1>, Matrix<R, N1>> residualFuncY,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
final var discR = Discretization.discretizeR(R, m_dtSeconds);
final var discR = Discretization.discretizeR(R, m_dt);
final var squareRootDiscR = discR.lltDecompose(true);
// Transform sigma points into measurement space

10
wpimath/src/main/java/edu/wpi/first/math/filter/Debouncer.java
View File

@@ -21,11 +21,11 @@ public class Debouncer {
kBoth
}
private final double m_debounceTimeSeconds;
private final double m_debounceTime;
private final DebounceType m_debounceType;
private boolean m_baseline;
private double m_prevTimeSeconds;
private double m_prevTime;
/**
* Creates a new Debouncer.
@@ -35,7 +35,7 @@ public class Debouncer {
* @param type Which type of state change the debouncing will be performed on.
*/
public Debouncer(double debounceTime, DebounceType type) {
m_debounceTimeSeconds = debounceTime;
m_debounceTime = debounceTime;
m_debounceType = type;
resetTimer();
@@ -58,11 +58,11 @@ public class Debouncer {
}
private void resetTimer() {
m_prevTimeSeconds = MathSharedStore.getTimestamp();
m_prevTime = MathSharedStore.getTimestamp();
}
private boolean hasElapsed() {
return MathSharedStore.getTimestamp() - m_prevTimeSeconds >= m_debounceTimeSeconds;
return MathSharedStore.getTimestamp() - m_prevTime >= m_debounceTime;
}
/**

50
wpimath/src/main/java/edu/wpi/first/math/interpolation/TimeInterpolatableBuffer.java
View File

@@ -22,8 +22,14 @@ public final class TimeInterpolatableBuffer<T> {
private final Interpolator<T> m_interpolatingFunc;
private final NavigableMap<Double, T> m_pastSnapshots = new TreeMap<>();
private TimeInterpolatableBuffer(Interpolator<T> interpolateFunction, double historySizeSeconds) {
this.m_historySize = historySizeSeconds;
/**
* Constructs a TimeInterpolatableBuffer.
*
* @param interpolateFunction Interpolation function.
* @param historySize The history size of the buffer in seconds.
*/
private TimeInterpolatableBuffer(Interpolator<T> interpolateFunction, double historySize) {
this.m_historySize = historySize;
this.m_interpolatingFunc = interpolateFunction;
}
@@ -31,52 +37,52 @@ public final class TimeInterpolatableBuffer<T> {
* Create a new TimeInterpolatableBuffer.
*
* @param interpolateFunction The function used to interpolate between values.
* @param historySizeSeconds The history size of the buffer.
* @param historySize The history size of the buffer in seconds.
* @param <T> The type of data to store in the buffer.
* @return The new TimeInterpolatableBuffer.
*/
public static <T> TimeInterpolatableBuffer<T> createBuffer(
Interpolator<T> interpolateFunction, double historySizeSeconds) {
return new TimeInterpolatableBuffer<>(interpolateFunction, historySizeSeconds);
Interpolator<T> interpolateFunction, double historySize) {
return new TimeInterpolatableBuffer<>(interpolateFunction, historySize);
}
/**
* Create a new TimeInterpolatableBuffer that stores a given subclass of {@link Interpolatable}.
*
* @param historySizeSeconds The history size of the buffer.
* @param historySize The history size of the buffer in seconds.
* @param <T> The type of {@link Interpolatable} to store in the buffer.
* @return The new TimeInterpolatableBuffer.
*/
public static <T extends Interpolatable<T>> TimeInterpolatableBuffer<T> createBuffer(
double historySizeSeconds) {
return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySizeSeconds);
double historySize) {
return new TimeInterpolatableBuffer<>(Interpolatable::interpolate, historySize);
}
/**
* Create a new TimeInterpolatableBuffer to store Double values.
*
* @param historySizeSeconds The history size of the buffer.
* @param historySize The history size of the buffer in seconds.
* @return The new TimeInterpolatableBuffer.
*/
public static TimeInterpolatableBuffer<Double> createDoubleBuffer(double historySizeSeconds) {
return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySizeSeconds);
public static TimeInterpolatableBuffer<Double> createDoubleBuffer(double historySize) {
return new TimeInterpolatableBuffer<>(MathUtil::interpolate, historySize);
}
/**
* Add a sample to the buffer.
*
* @param timeSeconds The timestamp of the sample.
* @param time The timestamp of the sample in seconds.
* @param sample The sample object.
*/
public void addSample(double timeSeconds, T sample) {
cleanUp(timeSeconds);
m_pastSnapshots.put(timeSeconds, sample);
public void addSample(double time, T sample) {
cleanUp(time);
m_pastSnapshots.put(time, sample);
}
/**
* Removes samples older than our current history size.
*
* @param time The current timestamp.
* @param time The current timestamp in seconds.
*/
private void cleanUp(double time) {
while (!m_pastSnapshots.isEmpty()) {
@@ -97,22 +103,22 @@ public final class TimeInterpolatableBuffer<T> {
/**
* Sample the buffer at the given time. If the buffer is empty, an empty Optional is returned.
*
* @param timeSeconds The time at which to sample.
* @param time The time at which to sample in seconds.
* @return The interpolated value at that timestamp or an empty Optional.
*/
public Optional<T> getSample(double timeSeconds) {
public Optional<T> getSample(double time) {
if (m_pastSnapshots.isEmpty()) {
return Optional.empty();
}
// Special case for when the requested time is the same as a sample
var nowEntry = m_pastSnapshots.get(timeSeconds);
var nowEntry = m_pastSnapshots.get(time);
if (nowEntry != null) {
return Optional.of(nowEntry);
}
var topBound = m_pastSnapshots.ceilingEntry(timeSeconds);
var bottomBound = m_pastSnapshots.floorEntry(timeSeconds);
var topBound = m_pastSnapshots.ceilingEntry(time);
var bottomBound = m_pastSnapshots.floorEntry(time);
// Return null if neither sample exists, and the opposite bound if the other is null
if (topBound == null && bottomBound == null) {
@@ -129,7 +135,7 @@ public final class TimeInterpolatableBuffer<T> {
m_interpolatingFunc.interpolate(
bottomBound.getValue(),
topBound.getValue(),
(timeSeconds - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
(time - bottomBound.getKey()) / (topBound.getKey() - bottomBound.getKey())));
}
}

87
wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
View File

@@ -29,14 +29,14 @@ import java.util.Objects;
* will often have all three components.
*/
public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
/** Velocity along the x-axis. (Fwd is +) */
public double vxMetersPerSecond;
/** Velocity along the x-axis in meters per second. (Fwd is +) */
public double vx;
/** Velocity along the y-axis. (Left is +) */
public double vyMetersPerSecond;
/** Velocity along the y-axis in meters per second. (Left is +) */
public double vy;
/** Represents the angular velocity of the robot frame. (CCW is +) */
public double omegaRadiansPerSecond;
/** Angular velocity of the robot frame in radians per second. (CCW is +) */
public double omega;
/** ChassisSpeeds protobuf for serialization. */
public static final ChassisSpeedsProto proto = new ChassisSpeedsProto();
@@ -50,15 +50,14 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
/**
* Constructs a ChassisSpeeds object.
*
* @param vxMetersPerSecond Forward velocity.
* @param vyMetersPerSecond Sideways velocity.
* @param omegaRadiansPerSecond Angular velocity.
* @param vx Forward velocity in meters per second.
* @param vy Sideways velocity in meters per second.
* @param omega Angular velocity in radians per second.
*/
public ChassisSpeeds(
double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond) {
this.vxMetersPerSecond = vxMetersPerSecond;
this.vyMetersPerSecond = vyMetersPerSecond;
this.omegaRadiansPerSecond = omegaRadiansPerSecond;
public ChassisSpeeds(double vx, double vy, double omega) {
this.vx = vx;
this.vy = vy;
this.omega = omega;
}
/**
@@ -75,14 +74,11 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
/**
* Creates a Twist2d from ChassisSpeeds.
*
* @param dtSeconds The duration of the timestep.
* @param dt The duration of the timestep in seconds.
* @return Twist2d.
*/
public Twist2d toTwist2d(double dtSeconds) {
return new Twist2d(
vxMetersPerSecond * dtSeconds,
vyMetersPerSecond * dtSeconds,
omegaRadiansPerSecond * dtSeconds);
public Twist2d toTwist2d(double dt) {
return new Twist2d(vx * dt, vy * dt, omega * dt);
}
/**
@@ -96,22 +92,20 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* <p>This is useful for compensating for translational skew when translating and rotating a
* swerve drivetrain.
*
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @param dt The duration of the timestep in seconds the speeds should be applied for.
* @return Discretized ChassisSpeeds.
*/
public ChassisSpeeds discretize(double dtSeconds) {
var desiredDeltaPose =
new Pose2d(
vxMetersPerSecond * dtSeconds,
vyMetersPerSecond * dtSeconds,
new Rotation2d(omegaRadiansPerSecond * dtSeconds));
public ChassisSpeeds discretize(double dt) {
// Construct the desired pose after a timestep, relative to the current pose. The desired pose
// has decoupled translation and rotation.
var desiredDeltaPose = new Pose2d(vx * dt, vy * dt, new Rotation2d(omega * dt));
// Find the chassis translation/rotation deltas in the robot frame that move the robot from its
// current pose to the desired pose
var twist = Pose2d.kZero.log(desiredDeltaPose);
// Turn the chassis translation/rotation deltas into average velocities
return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds);
return new ChassisSpeeds(twist.dx / dt, twist.dy / dt, twist.dtheta / dt);
}
/**
@@ -124,9 +118,8 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
*/
public ChassisSpeeds toRobotRelative(Rotation2d robotAngle) {
// CW rotation into chassis frame
var rotated =
new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
var rotated = new Translation2d(vx, vy).rotateBy(robotAngle.unaryMinus());
return new ChassisSpeeds(rotated.getX(), rotated.getY(), omega);
}
/**
@@ -139,8 +132,8 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
*/
public ChassisSpeeds toFieldRelative(Rotation2d robotAngle) {
// CCW rotation out of chassis frame
var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
var rotated = new Translation2d(vx, vy).rotateBy(robotAngle);
return new ChassisSpeeds(rotated.getX(), rotated.getY(), omega);
}
/**
@@ -153,10 +146,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* @return The sum of the ChassisSpeeds.
*/
public ChassisSpeeds plus(ChassisSpeeds other) {
return new ChassisSpeeds(
vxMetersPerSecond + other.vxMetersPerSecond,
vyMetersPerSecond + other.vyMetersPerSecond,
omegaRadiansPerSecond + other.omegaRadiansPerSecond);
return new ChassisSpeeds(vx + other.vx, vy + other.vy, omega + other.omega);
}
/**
@@ -169,10 +159,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* @return The difference between the two ChassisSpeeds.
*/
public ChassisSpeeds minus(ChassisSpeeds other) {
return new ChassisSpeeds(
vxMetersPerSecond - other.vxMetersPerSecond,
vyMetersPerSecond - other.vyMetersPerSecond,
omegaRadiansPerSecond - other.omegaRadiansPerSecond);
return new ChassisSpeeds(vx - other.vx, vy - other.vy, omega - other.omega);
}
/**
@@ -182,7 +169,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* @return The inverse of the current ChassisSpeeds.
*/
public ChassisSpeeds unaryMinus() {
return new ChassisSpeeds(-vxMetersPerSecond, -vyMetersPerSecond, -omegaRadiansPerSecond);
return new ChassisSpeeds(-vx, -vy, -omega);
}
/**
@@ -194,8 +181,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* @return The scaled ChassisSpeeds.
*/
public ChassisSpeeds times(double scalar) {
return new ChassisSpeeds(
vxMetersPerSecond * scalar, vyMetersPerSecond * scalar, omegaRadiansPerSecond * scalar);
return new ChassisSpeeds(vx * scalar, vy * scalar, omega * scalar);
}
/**
@@ -207,28 +193,23 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
* @return The scaled ChassisSpeeds.
*/
public ChassisSpeeds div(double scalar) {
return new ChassisSpeeds(
vxMetersPerSecond / scalar, vyMetersPerSecond / scalar, omegaRadiansPerSecond / scalar);
return new ChassisSpeeds(vx / scalar, vy / scalar, omega / scalar);
}
@Override
public final int hashCode() {
return Objects.hash(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
return Objects.hash(vx, vy, omega);
}
@Override
public boolean equals(Object o) {
return o == this
|| o instanceof ChassisSpeeds c
&& vxMetersPerSecond == c.vxMetersPerSecond
&& vyMetersPerSecond == c.vyMetersPerSecond
&& omegaRadiansPerSecond == c.omegaRadiansPerSecond;
|| o instanceof ChassisSpeeds c && vx == c.vx && vy == c.vy && omega == c.omega;
}
@Override
public String toString() {
return String.format(
"ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
"ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)", vx, vy, omega);
}
}

52
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
View File

@@ -26,8 +26,8 @@ public class DifferentialDriveKinematics
implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions>,
ProtobufSerializable,
StructSerializable {
/** Differential drive trackwidth. */
public final double trackWidthMeters;
/** Differential drive trackwidth in meters. */
public final double trackwidth;
/** DifferentialDriveKinematics protobuf for serialization. */
public static final DifferentialDriveKinematicsProto proto =
@@ -40,24 +40,24 @@ public class DifferentialDriveKinematics
/**
* 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.
* @param trackwidth The trackwidth of the drivetrain in meters. 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;
public DifferentialDriveKinematics(double trackwidth) {
this.trackwidth = trackwidth;
MathSharedStore.reportUsage("DifferentialDriveKinematics", "");
}
/**
* Constructs a differential drive kinematics object.
*
* @param trackWidth 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.
* @param trackwidth The trackwidth of the drivetrain in meters. 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(Distance trackWidth) {
this(trackWidth.in(Meters));
public DifferentialDriveKinematics(Distance trackwidth) {
this(trackwidth.in(Meters));
}
/**
@@ -69,9 +69,9 @@ public class DifferentialDriveKinematics
@Override
public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
return new ChassisSpeeds(
(wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
(wheelSpeeds.left + wheelSpeeds.right) / 2,
0,
(wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) / trackWidthMeters);
(wheelSpeeds.right - wheelSpeeds.left) / trackwidth);
}
/**
@@ -84,16 +84,14 @@ public class DifferentialDriveKinematics
@Override
public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
return new DifferentialDriveWheelSpeeds(
chassisSpeeds.vxMetersPerSecond
- trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond,
chassisSpeeds.vxMetersPerSecond
+ trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
chassisSpeeds.vx - trackwidth / 2 * chassisSpeeds.omega,
chassisSpeeds.vx + trackwidth / 2 * chassisSpeeds.omega);
}
@Override
public Twist2d toTwist2d(
DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) {
return toTwist2d(end.leftMeters - start.leftMeters, end.rightMeters - start.rightMeters);
return toTwist2d(end.left - start.left, end.right - start.right);
}
/**
@@ -101,27 +99,25 @@ public class DifferentialDriveKinematics
* distance deltas. This method is often used for odometry -- determining the robot's position on
* the field using changes in the distance driven by each wheel on the robot.
*
* @param leftDistanceMeters The distance measured by the left side encoder.
* @param rightDistanceMeters The distance measured by the right side encoder.
* @param leftDistance The distance measured by the left side encoder in meters.
* @param rightDistance The distance measured by the right side encoder in meters.
* @return The resulting Twist2d.
*/
public Twist2d toTwist2d(double leftDistanceMeters, double rightDistanceMeters) {
public Twist2d toTwist2d(double leftDistance, double rightDistance) {
return new Twist2d(
(leftDistanceMeters + rightDistanceMeters) / 2,
0,
(rightDistanceMeters - leftDistanceMeters) / trackWidthMeters);
(leftDistance + rightDistance) / 2, 0, (rightDistance - leftDistance) / trackwidth);
}
@Override
public DifferentialDriveWheelPositions copy(DifferentialDriveWheelPositions positions) {
return new DifferentialDriveWheelPositions(positions.leftMeters, positions.rightMeters);
return new DifferentialDriveWheelPositions(positions.left, positions.right);
}
@Override
public void copyInto(
DifferentialDriveWheelPositions positions, DifferentialDriveWheelPositions output) {
output.leftMeters = positions.leftMeters;
output.rightMeters = positions.rightMeters;
output.left = positions.left;
output.right = positions.right;
}
@Override

62
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
View File

@@ -26,20 +26,17 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
* Constructs a DifferentialDriveOdometry object.
*
* @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.
* @param initialPoseMeters The starting position of the robot on the field.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
* @param initialPose The starting position of the robot on the field.
*/
public DifferentialDriveOdometry(
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters) {
Rotation2d gyroAngle, double leftDistance, double rightDistance, Pose2d initialPose) {
super(
new DifferentialDriveKinematics(1),
gyroAngle,
new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
initialPoseMeters);
new DifferentialDriveWheelPositions(leftDistance, rightDistance),
initialPose);
MathSharedStore.reportUsage("DifferentialDriveOdometry", "");
}
@@ -49,26 +46,23 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @param initialPoseMeters The starting position of the robot on the field.
* @param initialPose The starting position of the robot on the field.
*/
public DifferentialDriveOdometry(
Rotation2d gyroAngle,
Distance leftDistance,
Distance rightDistance,
Pose2d initialPoseMeters) {
this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters);
Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance, Pose2d initialPose) {
this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPose);
}
/**
* Constructs a DifferentialDriveOdometry object.
*
* @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.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
*/
public DifferentialDriveOdometry(
Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
this(gyroAngle, leftDistanceMeters, rightDistanceMeters, Pose2d.kZero);
Rotation2d gyroAngle, double leftDistance, double rightDistance) {
this(gyroAngle, leftDistance, rightDistance, Pose2d.kZero);
}
/**
@@ -90,19 +84,14 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
* automatically takes care of offsetting the gyro angle.
*
* @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.
* @param poseMeters The position on the field that your robot is at.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d poseMeters) {
Rotation2d gyroAngle, double leftDistance, double rightDistance, Pose2d pose) {
super.resetPosition(
gyroAngle,
new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
poseMeters);
gyroAngle, new DifferentialDriveWheelPositions(leftDistance, rightDistance), pose);
}
/**
@@ -114,11 +103,11 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @param poseMeters The position on the field that your robot is at.
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(
Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance, Pose2d poseMeters) {
resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters);
Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance, Pose2d pose) {
resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), pose);
}
/**
@@ -127,13 +116,12 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
* 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.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
* @return The new pose of the robot.
*/
public Pose2d update(
Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
public Pose2d update(Rotation2d gyroAngle, double leftDistance, double rightDistance) {
return super.update(
gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters));
gyroAngle, new DifferentialDriveWheelPositions(leftDistance, rightDistance));
}
}

62
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java
View File

@@ -35,20 +35,17 @@ public class DifferentialDriveOdometry3d extends Odometry3d<DifferentialDriveWhe
* Constructs a DifferentialDriveOdometry3d object.
*
* @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.
* @param initialPoseMeters The starting position of the robot on the field.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
* @param initialPose The starting position of the robot on the field.
*/
public DifferentialDriveOdometry3d(
Rotation3d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose3d initialPoseMeters) {
Rotation3d gyroAngle, double leftDistance, double rightDistance, Pose3d initialPose) {
super(
new DifferentialDriveKinematics(1),
gyroAngle,
new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
initialPoseMeters);
new DifferentialDriveWheelPositions(leftDistance, rightDistance),
initialPose);
MathSharedStore.reportUsage("DifferentialDriveOdometry3d", "");
}
@@ -58,26 +55,23 @@ public class DifferentialDriveOdometry3d extends Odometry3d<DifferentialDriveWhe
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @param initialPoseMeters The starting position of the robot on the field.
* @param initialPose The starting position of the robot on the field.
*/
public DifferentialDriveOdometry3d(
Rotation3d gyroAngle,
Distance leftDistance,
Distance rightDistance,
Pose3d initialPoseMeters) {
this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters);
Rotation3d gyroAngle, Distance leftDistance, Distance rightDistance, Pose3d initialPose) {
this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPose);
}
/**
* Constructs a DifferentialDriveOdometry3d object.
*
* @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.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
*/
public DifferentialDriveOdometry3d(
Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
this(gyroAngle, leftDistanceMeters, rightDistanceMeters, Pose3d.kZero);
Rotation3d gyroAngle, double leftDistance, double rightDistance) {
this(gyroAngle, leftDistance, rightDistance, Pose3d.kZero);
}
/**
@@ -99,19 +93,14 @@ public class DifferentialDriveOdometry3d extends Odometry3d<DifferentialDriveWhe
* automatically takes care of offsetting the gyro angle.
*
* @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.
* @param poseMeters The position on the field that your robot is at.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(
Rotation3d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose3d poseMeters) {
Rotation3d gyroAngle, double leftDistance, double rightDistance, Pose3d pose) {
super.resetPosition(
gyroAngle,
new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
poseMeters);
gyroAngle, new DifferentialDriveWheelPositions(leftDistance, rightDistance), pose);
}
/**
@@ -123,11 +112,11 @@ public class DifferentialDriveOdometry3d extends Odometry3d<DifferentialDriveWhe
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @param poseMeters The position on the field that your robot is at.
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(
Rotation3d gyroAngle, Distance leftDistance, Distance rightDistance, Pose3d poseMeters) {
resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters);
Rotation3d gyroAngle, Distance leftDistance, Distance rightDistance, Pose3d pose) {
resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), pose);
}
/**
@@ -136,13 +125,12 @@ public class DifferentialDriveOdometry3d extends Odometry3d<DifferentialDriveWhe
* 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.
* @param leftDistance The distance traveled by the left encoder in meters.
* @param rightDistance The distance traveled by the right encoder in meters.
* @return The new pose of the robot.
*/
public Pose3d update(
Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
public Pose3d update(Rotation3d gyroAngle, double leftDistance, double rightDistance) {
return super.update(
gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters));
gyroAngle, new DifferentialDriveWheelPositions(leftDistance, rightDistance));
}
}

34
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
View File

@@ -20,11 +20,11 @@ public class DifferentialDriveWheelPositions
implements StructSerializable,
ProtobufSerializable,
Interpolatable<DifferentialDriveWheelPositions> {
/** Distance measured by the left side. */
public double leftMeters;
/** Distance measured by the left side in meters. */
public double left;
/** Distance measured by the right side. */
public double rightMeters;
/** Distance measured by the right side in meters. */
public double right;
/** DifferentialDriveWheelPositions struct for serialization. */
public static final DifferentialDriveWheelPositionsStruct struct =
@@ -37,19 +37,19 @@ public class DifferentialDriveWheelPositions
/**
* Constructs a DifferentialDriveWheelPositions.
*
* @param leftMeters Distance measured by the left side.
* @param rightMeters Distance measured by the right side.
* @param left Distance measured by the left side in meters.
* @param right Distance measured by the right side in meters.
*/
public DifferentialDriveWheelPositions(double leftMeters, double rightMeters) {
this.leftMeters = leftMeters;
this.rightMeters = rightMeters;
public DifferentialDriveWheelPositions(double left, double right) {
this.left = left;
this.right = right;
}
/**
* Constructs a DifferentialDriveWheelPositions.
*
* @param left Distance measured by the left side.
* @param right Distance measured by the right side.
* @param left Distance measured by the left side in meters.
* @param right Distance measured by the right side in meters.
*/
public DifferentialDriveWheelPositions(Distance left, Distance right) {
this(left.in(Meters), right.in(Meters));
@@ -58,26 +58,26 @@ public class DifferentialDriveWheelPositions
@Override
public boolean equals(Object obj) {
return obj instanceof DifferentialDriveWheelPositions other
&& Math.abs(other.leftMeters - leftMeters) < 1E-9
&& Math.abs(other.rightMeters - rightMeters) < 1E-9;
&& Math.abs(other.left - left) < 1E-9
&& Math.abs(other.right - right) < 1E-9;
}
@Override
public int hashCode() {
return Objects.hash(leftMeters, rightMeters);
return Objects.hash(left, right);
}
@Override
public String toString() {
return String.format(
"DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters);
"DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", left, right);
}
@Override
public DifferentialDriveWheelPositions interpolate(
DifferentialDriveWheelPositions endValue, double t) {
return new DifferentialDriveWheelPositions(
MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t),
MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t));
MathUtil.interpolate(this.left, endValue.left, t),
MathUtil.interpolate(this.right, endValue.right, t));
}
}

56
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
View File

@@ -14,11 +14,11 @@ import edu.wpi.first.util.struct.StructSerializable;
/** Represents the wheel speeds for a differential drive drivetrain. */
public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, StructSerializable {
/** Speed of the left side of the robot. */
public double leftMetersPerSecond;
/** Speed of the left side of the robot in meters per second. */
public double left;
/** Speed of the right side of the robot. */
public double rightMetersPerSecond;
/** Speed of the right side of the robot in meters per second. */
public double right;
/** DifferentialDriveWheelSpeeds protobuf for serialization. */
public static final DifferentialDriveWheelSpeedsProto proto =
@@ -34,19 +34,19 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
/**
* Constructs a DifferentialDriveWheelSpeeds.
*
* @param leftMetersPerSecond The left speed.
* @param rightMetersPerSecond The right speed.
* @param left The left speed in meters per second.
* @param right The right speed in meters per second.
*/
public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
this.leftMetersPerSecond = leftMetersPerSecond;
this.rightMetersPerSecond = rightMetersPerSecond;
public DifferentialDriveWheelSpeeds(double left, double right) {
this.left = left;
this.right = right;
}
/**
* Constructs a DifferentialDriveWheelSpeeds.
*
* @param left The left speed.
* @param right The right speed.
* @param left The left speed in meters per second.
* @param right The right speed in meters per second.
*/
public DifferentialDriveWheelSpeeds(LinearVelocity left, LinearVelocity right) {
this(left.in(MetersPerSecond), right.in(MetersPerSecond));
@@ -60,15 +60,14 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
* @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach.
*/
public void desaturate(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
public void desaturate(double attainableMaxSpeed) {
double realMaxSpeed = Math.max(Math.abs(left), Math.abs(right));
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
rightMetersPerSecond =
rightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
if (realMaxSpeed > attainableMaxSpeed) {
left = left / realMaxSpeed * attainableMaxSpeed;
right = right / realMaxSpeed * attainableMaxSpeed;
}
}
@@ -80,7 +79,7 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeed The absolute max speed that a wheel can reach.
* @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach.
*/
public void desaturate(LinearVelocity attainableMaxSpeed) {
desaturate(attainableMaxSpeed.in(MetersPerSecond));
@@ -96,9 +95,7 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
* @return The sum of the DifferentialDriveWheelSpeeds.
*/
public DifferentialDriveWheelSpeeds plus(DifferentialDriveWheelSpeeds other) {
return new DifferentialDriveWheelSpeeds(
leftMetersPerSecond + other.leftMetersPerSecond,
rightMetersPerSecond + other.rightMetersPerSecond);
return new DifferentialDriveWheelSpeeds(left + other.left, right + other.right);
}
/**
@@ -112,9 +109,7 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
* @return The difference between the two DifferentialDriveWheelSpeeds.
*/
public DifferentialDriveWheelSpeeds minus(DifferentialDriveWheelSpeeds other) {
return new DifferentialDriveWheelSpeeds(
leftMetersPerSecond - other.leftMetersPerSecond,
rightMetersPerSecond - other.rightMetersPerSecond);
return new DifferentialDriveWheelSpeeds(left - other.left, right - other.right);
}
/**
@@ -124,7 +119,7 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
* @return The inverse of the current DifferentialDriveWheelSpeeds.
*/
public DifferentialDriveWheelSpeeds unaryMinus() {
return new DifferentialDriveWheelSpeeds(-leftMetersPerSecond, -rightMetersPerSecond);
return new DifferentialDriveWheelSpeeds(-left, -right);
}
/**
@@ -138,8 +133,7 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
* @return The scaled DifferentialDriveWheelSpeeds.
*/
public DifferentialDriveWheelSpeeds times(double scalar) {
return new DifferentialDriveWheelSpeeds(
leftMetersPerSecond * scalar, rightMetersPerSecond * scalar);
return new DifferentialDriveWheelSpeeds(left * scalar, right * scalar);
}
/**
@@ -153,14 +147,12 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
* @return The scaled DifferentialDriveWheelSpeeds.
*/
public DifferentialDriveWheelSpeeds div(double scalar) {
return new DifferentialDriveWheelSpeeds(
leftMetersPerSecond / scalar, rightMetersPerSecond / scalar);
return new DifferentialDriveWheelSpeeds(left / scalar, right / scalar);
}
@Override
public String toString() {
return String.format(
"DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
leftMetersPerSecond, rightMetersPerSecond);
"DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)", left, right);
}
}

115
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
View File

@@ -40,10 +40,10 @@ public class MecanumDriveKinematics
private final 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 final Translation2d m_frontLeftWheel;
private final Translation2d m_frontRightWheel;
private final Translation2d m_rearLeftWheel;
private final Translation2d m_rearRightWheel;
private Translation2d m_prevCoR = Translation2d.kZero;
@@ -56,29 +56,28 @@ public class MecanumDriveKinematics
/**
* 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.
* @param frontLeftWheel The location of the front-left wheel relative to the physical center of
* the robot, in meters.
* @param frontRightWheel The location of the front-right wheel relative to the physical center of
* the robot, in meters.
* @param rearLeftWheel The location of the rear-left wheel relative to the physical center of the
* robot, in meters.
* @param rearRightWheel The location of the rear-right wheel relative to the physical center of
* the robot, in meters.
*/
public MecanumDriveKinematics(
Translation2d frontLeftWheelMeters,
Translation2d frontRightWheelMeters,
Translation2d rearLeftWheelMeters,
Translation2d rearRightWheelMeters) {
m_frontLeftWheelMeters = frontLeftWheelMeters;
m_frontRightWheelMeters = frontRightWheelMeters;
m_rearLeftWheelMeters = rearLeftWheelMeters;
m_rearRightWheelMeters = rearRightWheelMeters;
Translation2d frontLeftWheel,
Translation2d frontRightWheel,
Translation2d rearLeftWheel,
Translation2d rearRightWheel) {
m_frontLeftWheel = frontLeftWheel;
m_frontRightWheel = frontRightWheel;
m_rearLeftWheel = rearLeftWheel;
m_rearRightWheel = rearRightWheel;
m_inverseKinematics = new SimpleMatrix(4, 3);
setInverseKinematics(
frontLeftWheelMeters, frontRightWheelMeters, rearLeftWheelMeters, rearRightWheelMeters);
setInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel, rearRightWheel);
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
MathSharedStore.reportUsage("MecanumDriveKinematics", "");
@@ -94,33 +93,28 @@ public class MecanumDriveKinematics
* 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.
* @param centerOfRotation 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#desaturate(double)} function to rectify this issue.
*/
public MecanumDriveWheelSpeeds toWheelSpeeds(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation) {
// 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);
if (!centerOfRotation.equals(m_prevCoR)) {
var fl = m_frontLeftWheel.minus(centerOfRotation);
var fr = m_frontRightWheel.minus(centerOfRotation);
var rl = m_rearLeftWheel.minus(centerOfRotation);
var rr = m_rearRightWheel.minus(centerOfRotation);
setInverseKinematics(fl, fr, rl, rr);
m_prevCoR = centerOfRotationMeters;
m_prevCoR = centerOfRotation;
}
var chassisSpeedsVector = new SimpleMatrix(3, 1);
chassisSpeedsVector.setColumn(
0,
0,
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond,
chassisSpeeds.omegaRadiansPerSecond);
chassisSpeedsVector.setColumn(0, 0, chassisSpeeds.vx, chassisSpeeds.vy, chassisSpeeds.omega);
var wheelsVector = m_inverseKinematics.mult(chassisSpeedsVector);
return new MecanumDriveWheelSpeeds(
@@ -156,10 +150,10 @@ public class MecanumDriveKinematics
wheelSpeedsVector.setColumn(
0,
0,
wheelSpeeds.frontLeftMetersPerSecond,
wheelSpeeds.frontRightMetersPerSecond,
wheelSpeeds.rearLeftMetersPerSecond,
wheelSpeeds.rearRightMetersPerSecond);
wheelSpeeds.frontLeft,
wheelSpeeds.frontRight,
wheelSpeeds.rearLeft,
wheelSpeeds.rearRight);
var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsVector);
return new ChassisSpeeds(
@@ -174,10 +168,10 @@ public class MecanumDriveKinematics
wheelDeltasVector.setColumn(
0,
0,
end.frontLeftMeters - start.frontLeftMeters,
end.frontRightMeters - start.frontRightMeters,
end.rearLeftMeters - start.rearLeftMeters,
end.rearRightMeters - start.rearRightMeters);
end.frontLeft - start.frontLeft,
end.frontRight - start.frontRight,
end.rearLeft - start.rearLeft,
end.rearRight - start.rearRight);
var twist = m_forwardKinematics.mult(wheelDeltasVector);
return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
}
@@ -195,10 +189,10 @@ public class MecanumDriveKinematics
wheelDeltasVector.setColumn(
0,
0,
wheelDeltas.frontLeftMeters,
wheelDeltas.frontRightMeters,
wheelDeltas.rearLeftMeters,
wheelDeltas.rearRightMeters);
wheelDeltas.frontLeft,
wheelDeltas.frontRight,
wheelDeltas.rearLeft,
wheelDeltas.rearRight);
var twist = m_forwardKinematics.mult(wheelDeltasVector);
return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
}
@@ -225,7 +219,7 @@ public class MecanumDriveKinematics
* @return The front-left wheel translation.
*/
public Translation2d getFrontLeft() {
return m_frontLeftWheelMeters;
return m_frontLeftWheel;
}
/**
@@ -234,7 +228,7 @@ public class MecanumDriveKinematics
* @return The front-right wheel translation.
*/
public Translation2d getFrontRight() {
return m_frontRightWheelMeters;
return m_frontRightWheel;
}
/**
@@ -243,7 +237,7 @@ public class MecanumDriveKinematics
* @return The rear-left wheel translation.
*/
public Translation2d getRearLeft() {
return m_rearLeftWheelMeters;
return m_rearLeftWheel;
}
/**
@@ -252,24 +246,21 @@ public class MecanumDriveKinematics
* @return The rear-right wheel translation.
*/
public Translation2d getRearRight() {
return m_rearRightWheelMeters;
return m_rearRightWheel;
}
@Override
public MecanumDriveWheelPositions copy(MecanumDriveWheelPositions positions) {
return new MecanumDriveWheelPositions(
positions.frontLeftMeters,
positions.frontRightMeters,
positions.rearLeftMeters,
positions.rearRightMeters);
positions.frontLeft, positions.frontRight, positions.rearLeft, positions.rearRight);
}
@Override
public void copyInto(MecanumDriveWheelPositions positions, MecanumDriveWheelPositions output) {
output.frontLeftMeters = positions.frontLeftMeters;
output.frontRightMeters = positions.frontRightMeters;
output.rearLeftMeters = positions.rearLeftMeters;
output.rearRightMeters = positions.rearRightMeters;
output.frontLeft = positions.frontLeft;
output.frontRight = positions.frontRight;
output.rearLeft = positions.rearLeft;
output.rearRight = positions.rearRight;
}
@Override

31
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
View File

@@ -8,16 +8,16 @@ package edu.wpi.first.math.kinematics;
@Deprecated(since = "2025", forRemoval = true)
public class MecanumDriveMotorVoltages {
/** Voltage of the front left motor. */
public double frontLeftVoltage;
public double frontLeft;
/** Voltage of the front right motor. */
public double frontRightVoltage;
public double frontRight;
/** Voltage of the rear left motor. */
public double rearLeftVoltage;
public double rearLeft;
/** Voltage of the rear right motor. */
public double rearRightVoltage;
public double rearRight;
/** Constructs a MecanumDriveMotorVoltages with zeros for all member fields. */
public MecanumDriveMotorVoltages() {}
@@ -25,20 +25,17 @@ public class 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.
* @param frontLeft Voltage of the front left motor.
* @param frontRight Voltage of the front right motor.
* @param rearLeft Voltage of the rear left motor.
* @param rearRight 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;
double frontLeft, double frontRight, double rearLeft, double rearRight) {
this.frontLeft = frontLeft;
this.frontRight = frontRight;
this.rearLeft = rearLeft;
this.rearRight = rearRight;
}
@Override
@@ -46,6 +43,6 @@ public class MecanumDriveMotorVoltages {
return String.format(
"MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+ "Rear Left: %.2f V, Rear Right: %.2f V)",
frontLeftVoltage, frontRightVoltage, rearLeftVoltage, rearRightVoltage);
frontLeft, frontRight, rearLeft, rearRight);
}
}

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
View File

@@ -22,14 +22,14 @@ public class MecanumDriveOdometry extends Odometry<MecanumDriveWheelPositions> {
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
* @param initialPoseMeters The starting position of the robot on the field.
* @param initialPose The starting position of the robot on the field.
*/
public MecanumDriveOdometry(
MecanumDriveKinematics kinematics,
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose2d initialPoseMeters) {
super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
Pose2d initialPose) {
super(kinematics, gyroAngle, wheelPositions, initialPose);
MathSharedStore.reportUsage("MecanumDriveOdometry", "");
}

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java
View File

@@ -31,14 +31,14 @@ public class MecanumDriveOdometry3d extends Odometry3d<MecanumDriveWheelPosition
* @param kinematics The mecanum drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
* @param initialPoseMeters The starting position of the robot on the field.
* @param initialPose The starting position of the robot on the field.
*/
public MecanumDriveOdometry3d(
MecanumDriveKinematics kinematics,
Rotation3d gyroAngle,
MecanumDriveWheelPositions wheelPositions,
Pose3d initialPoseMeters) {
super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
Pose3d initialPose) {
super(kinematics, gyroAngle, wheelPositions, initialPose);
MathSharedStore.reportUsage("MecanumDriveOdometry3d", "");
}

65
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
View File

@@ -20,17 +20,17 @@ public class MecanumDriveWheelPositions
implements Interpolatable<MecanumDriveWheelPositions>,
ProtobufSerializable,
StructSerializable {
/** Distance measured by the front left wheel. */
public double frontLeftMeters;
/** Distance measured by the front left wheel in meters. */
public double frontLeft;
/** Distance measured by the front right wheel. */
public double frontRightMeters;
/** Distance measured by the front right wheel in meters. */
public double frontRight;
/** Distance measured by the rear left wheel. */
public double rearLeftMeters;
/** Distance measured by the rear left wheel in meters. */
public double rearLeft;
/** Distance measured by the rear right wheel. */
public double rearRightMeters;
/** Distance measured by the rear right wheel in meters. */
public double rearRight;
/** MecanumDriveWheelPositions protobuf for serialization. */
public static final MecanumDriveWheelPositionsProto proto = new MecanumDriveWheelPositionsProto();
@@ -45,29 +45,26 @@ public class MecanumDriveWheelPositions
/**
* Constructs a MecanumDriveWheelPositions.
*
* @param frontLeftMeters Distance measured by the front left wheel.
* @param frontRightMeters Distance measured by the front right wheel.
* @param rearLeftMeters Distance measured by the rear left wheel.
* @param rearRightMeters Distance measured by the rear right wheel.
* @param frontLeft Distance measured by the front left wheel in meters.
* @param frontRight Distance measured by the front right wheel in meters.
* @param rearLeft Distance measured by the rear left wheel in meters.
* @param rearRight Distance measured by the rear right wheel in meters.
*/
public MecanumDriveWheelPositions(
double frontLeftMeters,
double frontRightMeters,
double rearLeftMeters,
double rearRightMeters) {
this.frontLeftMeters = frontLeftMeters;
this.frontRightMeters = frontRightMeters;
this.rearLeftMeters = rearLeftMeters;
this.rearRightMeters = rearRightMeters;
double frontLeft, double frontRight, double rearLeft, double rearRight) {
this.frontLeft = frontLeft;
this.frontRight = frontRight;
this.rearLeft = rearLeft;
this.rearRight = rearRight;
}
/**
* Constructs a MecanumDriveWheelPositions.
*
* @param frontLeft Distance measured by the front left wheel.
* @param frontRight Distance measured by the front right wheel.
* @param rearLeft Distance measured by the rear left wheel.
* @param rearRight Distance measured by the rear right wheel.
* @param frontLeft Distance measured by the front left wheel in meters.
* @param frontRight Distance measured by the front right wheel in meters.
* @param rearLeft Distance measured by the rear left wheel in meters.
* @param rearRight Distance measured by the rear right wheel in meters.
*/
public MecanumDriveWheelPositions(
Distance frontLeft, Distance frontRight, Distance rearLeft, Distance rearRight) {
@@ -77,15 +74,15 @@ public class MecanumDriveWheelPositions
@Override
public boolean equals(Object obj) {
return obj instanceof MecanumDriveWheelPositions other
&& Math.abs(other.frontLeftMeters - frontLeftMeters) < 1E-9
&& Math.abs(other.frontRightMeters - frontRightMeters) < 1E-9
&& Math.abs(other.rearLeftMeters - rearLeftMeters) < 1E-9
&& Math.abs(other.rearRightMeters - rearRightMeters) < 1E-9;
&& Math.abs(other.frontLeft - frontLeft) < 1E-9
&& Math.abs(other.frontRight - frontRight) < 1E-9
&& Math.abs(other.rearLeft - rearLeft) < 1E-9
&& Math.abs(other.rearRight - rearRight) < 1E-9;
}
@Override
public int hashCode() {
return Objects.hash(frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
return Objects.hash(frontLeft, frontRight, rearLeft, rearRight);
}
@Override
@@ -93,15 +90,15 @@ public class MecanumDriveWheelPositions
return String.format(
"MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, "
+ "Rear Left: %.2f m, Rear Right: %.2f m)",
frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
frontLeft, frontRight, rearLeft, rearRight);
}
@Override
public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
return new MecanumDriveWheelPositions(
MathUtil.interpolate(this.frontLeftMeters, endValue.frontLeftMeters, t),
MathUtil.interpolate(this.frontRightMeters, endValue.frontRightMeters, t),
MathUtil.interpolate(this.rearLeftMeters, endValue.rearLeftMeters, t),
MathUtil.interpolate(this.rearRightMeters, endValue.rearRightMeters, t));
MathUtil.interpolate(this.frontLeft, endValue.frontLeft, t),
MathUtil.interpolate(this.frontRight, endValue.frontRight, t),
MathUtil.interpolate(this.rearLeft, endValue.rearLeft, t),
MathUtil.interpolate(this.rearRight, endValue.rearRight, t));
}
}

109
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
View File

@@ -14,17 +14,17 @@ import edu.wpi.first.util.struct.StructSerializable;
/** Represents the wheel speeds for a mecanum drive drivetrain. */
public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSerializable {
/** Speed of the front left wheel. */
public double frontLeftMetersPerSecond;
/** Speed of the front left wheel in meters per second. */
public double frontLeft;
/** Speed of the front right wheel. */
public double frontRightMetersPerSecond;
/** Speed of the front right wheel in meters per second. */
public double frontRight;
/** Speed of the rear left wheel. */
public double rearLeftMetersPerSecond;
/** Speed of the rear left wheel in meters per second. */
public double rearLeft;
/** Speed of the rear right wheel. */
public double rearRightMetersPerSecond;
/** Speed of the rear right wheel in meters per second. */
public double rearRight;
/** MecanumDriveWheelSpeeds protobuf for serialization. */
public static final MecanumDriveWheelSpeedsProto proto = new MecanumDriveWheelSpeedsProto();
@@ -38,29 +38,26 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
/**
* 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.
* @param frontLeft Speed of the front left wheel in meters per second.
* @param frontRight Speed of the front right wheel in meters per second.
* @param rearLeft Speed of the rear left wheel in meters per second.
* @param rearRight Speed of the rear right wheel in meters per second.
*/
public MecanumDriveWheelSpeeds(
double frontLeftMetersPerSecond,
double frontRightMetersPerSecond,
double rearLeftMetersPerSecond,
double rearRightMetersPerSecond) {
this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
this.frontRightMetersPerSecond = frontRightMetersPerSecond;
this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
this.rearRightMetersPerSecond = rearRightMetersPerSecond;
double frontLeft, double frontRight, double rearLeft, double rearRight) {
this.frontLeft = frontLeft;
this.frontRight = frontRight;
this.rearLeft = rearLeft;
this.rearRight = rearRight;
}
/**
* Constructs a MecanumDriveWheelSpeeds.
*
* @param frontLeft Speed of the front left wheel.
* @param frontRight Speed of the front right wheel.
* @param rearLeft Speed of the rear left wheel.
* @param rearRight Speed of the rear right wheel.
* @param frontLeft Speed of the front left wheel in meters per second.
* @param frontRight Speed of the front right wheel in meters per second.
* @param rearLeft Speed of the rear left wheel in meters per second.
* @param rearRight Speed of the rear right wheel in meters per second.
*/
public MecanumDriveWheelSpeeds(
LinearVelocity frontLeft,
@@ -82,28 +79,23 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
* reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
* absolute threshold, while maintaining the ratio of speeds between wheels.
*
* @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
* @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach.
* @return Desaturated MecanumDriveWheelSpeeds.
*/
public MecanumDriveWheelSpeeds desaturate(double attainableMaxSpeedMetersPerSecond) {
double realMaxSpeed =
Math.max(Math.abs(frontLeftMetersPerSecond), Math.abs(frontRightMetersPerSecond));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeftMetersPerSecond));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRightMetersPerSecond));
public MecanumDriveWheelSpeeds desaturate(double attainableMaxSpeed) {
double realMaxSpeed = Math.max(Math.abs(frontLeft), Math.abs(frontRight));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeft));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRight));
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
if (realMaxSpeed > attainableMaxSpeed) {
return new MecanumDriveWheelSpeeds(
frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond,
frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond,
rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond,
rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond);
frontLeft / realMaxSpeed * attainableMaxSpeed,
frontRight / realMaxSpeed * attainableMaxSpeed,
rearLeft / realMaxSpeed * attainableMaxSpeed,
rearRight / realMaxSpeed * attainableMaxSpeed);
}
return new MecanumDriveWheelSpeeds(
frontLeftMetersPerSecond,
frontRightMetersPerSecond,
rearLeftMetersPerSecond,
rearRightMetersPerSecond);
return new MecanumDriveWheelSpeeds(frontLeft, frontRight, rearLeft, rearRight);
}
/**
@@ -132,10 +124,10 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
*/
public MecanumDriveWheelSpeeds plus(MecanumDriveWheelSpeeds other) {
return new MecanumDriveWheelSpeeds(
frontLeftMetersPerSecond + other.frontLeftMetersPerSecond,
frontRightMetersPerSecond + other.frontRightMetersPerSecond,
rearLeftMetersPerSecond + other.rearLeftMetersPerSecond,
rearRightMetersPerSecond + other.rearRightMetersPerSecond);
frontLeft + other.frontLeft,
frontRight + other.frontRight,
rearLeft + other.rearLeft,
rearRight + other.rearRight);
}
/**
@@ -150,10 +142,10 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
*/
public MecanumDriveWheelSpeeds minus(MecanumDriveWheelSpeeds other) {
return new MecanumDriveWheelSpeeds(
frontLeftMetersPerSecond - other.frontLeftMetersPerSecond,
frontRightMetersPerSecond - other.frontRightMetersPerSecond,
rearLeftMetersPerSecond - other.rearLeftMetersPerSecond,
rearRightMetersPerSecond - other.rearRightMetersPerSecond);
frontLeft - other.frontLeft,
frontRight - other.frontRight,
rearLeft - other.rearLeft,
rearRight - other.rearRight);
}
/**
@@ -163,11 +155,7 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
* @return The inverse of the current MecanumDriveWheelSpeeds.
*/
public MecanumDriveWheelSpeeds unaryMinus() {
return new MecanumDriveWheelSpeeds(
-frontLeftMetersPerSecond,
-frontRightMetersPerSecond,
-rearLeftMetersPerSecond,
-rearRightMetersPerSecond);
return new MecanumDriveWheelSpeeds(-frontLeft, -frontRight, -rearLeft, -rearRight);
}
/**
@@ -181,10 +169,7 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
*/
public MecanumDriveWheelSpeeds times(double scalar) {
return new MecanumDriveWheelSpeeds(
frontLeftMetersPerSecond * scalar,
frontRightMetersPerSecond * scalar,
rearLeftMetersPerSecond * scalar,
rearRightMetersPerSecond * scalar);
frontLeft * scalar, frontRight * scalar, rearLeft * scalar, rearRight * scalar);
}
/**
@@ -198,10 +183,7 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
*/
public MecanumDriveWheelSpeeds div(double scalar) {
return new MecanumDriveWheelSpeeds(
frontLeftMetersPerSecond / scalar,
frontRightMetersPerSecond / scalar,
rearLeftMetersPerSecond / scalar,
rearRightMetersPerSecond / scalar);
frontLeft / scalar, frontRight / scalar, rearLeft / scalar, rearRight / scalar);
}
@Override
@@ -209,9 +191,6 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
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);
frontLeft, frontRight, rearLeft, rearRight);
}
}

53
wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
View File

@@ -21,7 +21,7 @@ import edu.wpi.first.math.geometry.Translation2d;
*/
public class Odometry<T> {
private final Kinematics<?, T> m_kinematics;
private Pose2d m_poseMeters;
private Pose2d m_pose;
private Rotation2d m_gyroOffset;
private Rotation2d m_previousAngle;
@@ -33,17 +33,14 @@ public class Odometry<T> {
* @param kinematics The kinematics of the drivebase.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current encoder readings.
* @param initialPoseMeters The starting position of the robot on the field.
* @param initialPose The starting position of the robot on the field.
*/
public Odometry(
Kinematics<?, T> kinematics,
Rotation2d gyroAngle,
T wheelPositions,
Pose2d initialPoseMeters) {
Kinematics<?, T> kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = m_poseMeters.getRotation();
m_pose = initialPose;
m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
m_previousAngle = m_pose.getRotation();
m_previousWheelPositions = m_kinematics.copy(wheelPositions);
}
@@ -55,24 +52,24 @@ public class Odometry<T> {
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current encoder readings.
* @param poseMeters The position on the field that your robot is at.
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = m_poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d pose) {
m_pose = pose;
m_previousAngle = m_pose.getRotation();
m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
}
/**
* Resets the pose.
*
* @param poseMeters The pose to reset to.
* @param pose The pose to reset to.
*/
public void resetPose(Pose2d poseMeters) {
m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation()));
m_poseMeters = poseMeters;
m_previousAngle = m_poseMeters.getRotation();
public void resetPose(Pose2d pose) {
m_gyroOffset = m_gyroOffset.plus(pose.getRotation().minus(m_pose.getRotation()));
m_pose = pose;
m_previousAngle = m_pose.getRotation();
}
/**
@@ -81,7 +78,7 @@ public class Odometry<T> {
* @param translation The translation to reset to.
*/
public void resetTranslation(Translation2d translation) {
m_poseMeters = new Pose2d(translation, m_poseMeters.getRotation());
m_pose = new Pose2d(translation, m_pose.getRotation());
}
/**
@@ -90,9 +87,9 @@ public class Odometry<T> {
* @param rotation The rotation to reset to.
*/
public void resetRotation(Rotation2d rotation) {
m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation()));
m_poseMeters = new Pose2d(m_poseMeters.getTranslation(), rotation);
m_previousAngle = m_poseMeters.getRotation();
m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_pose.getRotation()));
m_pose = new Pose2d(m_pose.getTranslation(), rotation);
m_previousAngle = m_pose.getRotation();
}
/**
@@ -100,8 +97,8 @@ public class Odometry<T> {
*
* @return The pose of the robot (x and y are in meters).
*/
public Pose2d getPoseMeters() {
return m_poseMeters;
public Pose2d getPose() {
return m_pose;
}
/**
@@ -120,12 +117,12 @@ public class Odometry<T> {
var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
twist.dtheta = angle.minus(m_previousAngle).getRadians();
var newPose = m_poseMeters.exp(twist);
var newPose = m_pose.exp(twist);
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
m_previousAngle = angle;
m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
m_pose = new Pose2d(newPose.getTranslation(), angle);
return m_poseMeters;
return m_pose;
}
}

53
wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java
View File

@@ -30,7 +30,7 @@ import edu.wpi.first.math.geometry.Twist3d;
*/
public class Odometry3d<T> {
private final Kinematics<?, T> m_kinematics;
private Pose3d m_poseMeters;
private Pose3d m_pose;
private Rotation3d m_gyroOffset;
private Rotation3d m_previousAngle;
@@ -42,17 +42,14 @@ public class Odometry3d<T> {
* @param kinematics The kinematics of the drivebase.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current encoder readings.
* @param initialPoseMeters The starting position of the robot on the field.
* @param initialPose The starting position of the robot on the field.
*/
public Odometry3d(
Kinematics<?, T> kinematics,
Rotation3d gyroAngle,
T wheelPositions,
Pose3d initialPoseMeters) {
Kinematics<?, T> kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) {
m_kinematics = kinematics;
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = m_poseMeters.getRotation();
m_pose = initialPose;
m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
m_previousAngle = m_pose.getRotation();
m_previousWheelPositions = m_kinematics.copy(wheelPositions);
}
@@ -64,24 +61,24 @@ public class Odometry3d<T> {
*
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current encoder readings.
* @param poseMeters The position on the field that your robot is at.
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = m_poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d pose) {
m_pose = pose;
m_previousAngle = m_pose.getRotation();
m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
}
/**
* Resets the pose.
*
* @param poseMeters The pose to reset to.
* @param pose The pose to reset to.
*/
public void resetPose(Pose3d poseMeters) {
m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation()));
m_poseMeters = poseMeters;
m_previousAngle = m_poseMeters.getRotation();
public void resetPose(Pose3d pose) {
m_gyroOffset = m_gyroOffset.plus(pose.getRotation().minus(m_pose.getRotation()));
m_pose = pose;
m_previousAngle = m_pose.getRotation();
}
/**
@@ -90,7 +87,7 @@ public class Odometry3d<T> {
* @param translation The translation to reset to.
*/
public void resetTranslation(Translation3d translation) {
m_poseMeters = new Pose3d(translation, m_poseMeters.getRotation());
m_pose = new Pose3d(translation, m_pose.getRotation());
}
/**
@@ -99,9 +96,9 @@ public class Odometry3d<T> {
* @param rotation The rotation to reset to.
*/
public void resetRotation(Rotation3d rotation) {
m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation()));
m_poseMeters = new Pose3d(m_poseMeters.getTranslation(), rotation);
m_previousAngle = m_poseMeters.getRotation();
m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_pose.getRotation()));
m_pose = new Pose3d(m_pose.getTranslation(), rotation);
m_previousAngle = m_pose.getRotation();
}
/**
@@ -109,8 +106,8 @@ public class Odometry3d<T> {
*
* @return The pose of the robot (x, y, and z are in meters).
*/
public Pose3d getPoseMeters() {
return m_poseMeters;
public Pose3d getPose() {
return m_pose;
}
/**
@@ -137,12 +134,12 @@ public class Odometry3d<T> {
angle_difference.get(1),
angle_difference.get(2));
var newPose = m_poseMeters.exp(twist);
var newPose = m_pose.exp(twist);
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
m_previousAngle = angle;
m_poseMeters = new Pose3d(newPose.getTranslation(), angle);
m_pose = new Pose3d(newPose.getTranslation(), angle);
return m_poseMeters;
return m_pose;
}
}

108
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -61,15 +61,15 @@ public class SwerveDriveKinematics
* also expected that you pass in the module states in the same order when calling the forward
* kinematics methods.
*
* @param moduleTranslationsMeters The locations of the modules relative to the physical center of
* the robot.
* @param moduleTranslations The locations of the modules relative to the physical center of the
* robot.
*/
public SwerveDriveKinematics(Translation2d... moduleTranslationsMeters) {
if (moduleTranslationsMeters.length < 2) {
public SwerveDriveKinematics(Translation2d... moduleTranslations) {
if (moduleTranslations.length < 2) {
throw new IllegalArgumentException("A swerve drive requires at least two modules");
}
m_numModules = moduleTranslationsMeters.length;
m_modules = Arrays.copyOf(moduleTranslationsMeters, m_numModules);
m_numModules = moduleTranslations.length;
m_modules = Arrays.copyOf(moduleTranslations, m_numModules);
m_moduleHeadings = new Rotation2d[m_numModules];
Arrays.fill(m_moduleHeadings, Rotation2d.kZero);
m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
@@ -111,21 +111,19 @@ public class SwerveDriveKinematics
* the previously calculated module angle will be maintained.
*
* @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.
* @param centerOfRotation 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 #desaturateWheelSpeeds(SwerveModuleState[], double)
* DesaturateWheelSpeeds} function to rectify this issue.
*/
public SwerveModuleState[] toSwerveModuleStates(
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
ChassisSpeeds chassisSpeeds, Translation2d centerOfRotation) {
var moduleStates = new SwerveModuleState[m_numModules];
if (chassisSpeeds.vxMetersPerSecond == 0.0
&& chassisSpeeds.vyMetersPerSecond == 0.0
&& chassisSpeeds.omegaRadiansPerSecond == 0.0) {
if (chassisSpeeds.vx == 0.0 && chassisSpeeds.vy == 0.0 && chassisSpeeds.omega == 0.0) {
for (int i = 0; i < m_numModules; i++) {
moduleStates[i] = new SwerveModuleState(0.0, m_moduleHeadings[i]);
}
@@ -133,31 +131,18 @@ public class SwerveDriveKinematics
return moduleStates;
}
if (!centerOfRotationMeters.equals(m_prevCoR)) {
if (!centerOfRotation.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());
i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotation.getY());
m_inverseKinematics.setRow(
i * 2 + 1,
0, /* Start Data */
0,
1,
+m_modules[i].getX() - centerOfRotationMeters.getX());
i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX() - centerOfRotation.getX());
}
m_prevCoR = centerOfRotationMeters;
m_prevCoR = centerOfRotation;
}
var chassisSpeedsVector = new SimpleMatrix(3, 1);
chassisSpeedsVector.setColumn(
0,
0,
chassisSpeeds.vxMetersPerSecond,
chassisSpeeds.vyMetersPerSecond,
chassisSpeeds.omegaRadiansPerSecond);
chassisSpeedsVector.setColumn(0, 0, chassisSpeeds.vx, chassisSpeeds.vy, chassisSpeeds.omega);
var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
@@ -212,8 +197,8 @@ public class SwerveDriveKinematics
for (int i = 0; i < m_numModules; i++) {
var module = moduleStates[i];
moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
moduleStatesMatrix.set(i * 2, 0, module.speed * module.angle.getCos());
moduleStatesMatrix.set(i * 2 + 1, module.speed * module.angle.getSin());
}
var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
@@ -243,8 +228,8 @@ public class SwerveDriveKinematics
for (int i = 0; i < m_numModules; i++) {
var module = moduleDeltas[i];
moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos());
moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin());
moduleDeltaMatrix.set(i * 2, 0, module.distance * module.angle.getCos());
moduleDeltaMatrix.set(i * 2 + 1, module.distance * module.angle.getSin());
}
var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix);
@@ -259,8 +244,7 @@ public class SwerveDriveKinematics
}
var newPositions = new SwerveModulePosition[start.length];
for (int i = 0; i < start.length; i++) {
newPositions[i] =
new SwerveModulePosition(end[i].distanceMeters - start[i].distanceMeters, end[i].angle);
newPositions[i] = new SwerveModulePosition(end[i].distance - start[i].distance, end[i].angle);
}
return toTwist2d(newPositions);
}
@@ -275,18 +259,17 @@ public class SwerveDriveKinematics
*
* @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.
* @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach.
*/
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
SwerveModuleState[] moduleStates, double attainableMaxSpeed) {
double realMaxSpeed = 0;
for (SwerveModuleState moduleState : moduleStates) {
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed));
}
if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
if (realMaxSpeed > attainableMaxSpeed) {
for (SwerveModuleState moduleState : moduleStates) {
moduleState.speedMetersPerSecond =
moduleState.speedMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
moduleState.speed = moduleState.speed / realMaxSpeed * attainableMaxSpeed;
}
}
}
@@ -301,7 +284,7 @@ public class SwerveDriveKinematics
*
* @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds!
* @param attainableMaxSpeed The absolute max speed that a module can reach.
* @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach.
*/
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates, LinearVelocity attainableMaxSpeed) {
@@ -320,38 +303,37 @@ public class SwerveDriveKinematics
* @param moduleStates Reference to array of module states. The array will be mutated with the
* normalized speeds!
* @param desiredChassisSpeed The desired speed of the robot
* @param attainableMaxModuleSpeedMetersPerSecond The absolute max speed that a module can reach
* @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot
* can reach while translating
* @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can
* reach while rotating
* @param attainableMaxModuleSpeed The absolute max speed in meters per second that a module can
* reach
* @param attainableMaxTranslationalSpeed The absolute max speed in meters per second that your
* robot can reach while translating
* @param attainableMaxRotationalVelocity The absolute max speed in radians per second the robot
* can reach while rotating
*/
public static void desaturateWheelSpeeds(
SwerveModuleState[] moduleStates,
ChassisSpeeds desiredChassisSpeed,
double attainableMaxModuleSpeedMetersPerSecond,
double attainableMaxTranslationalSpeedMetersPerSecond,
double attainableMaxRotationalVelocityRadiansPerSecond) {
double attainableMaxModuleSpeed,
double attainableMaxTranslationalSpeed,
double attainableMaxRotationalVelocity) {
double realMaxSpeed = 0;
for (SwerveModuleState moduleState : moduleStates) {
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speedMetersPerSecond));
realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed));
}
if (attainableMaxTranslationalSpeedMetersPerSecond == 0
|| attainableMaxRotationalVelocityRadiansPerSecond == 0
if (attainableMaxTranslationalSpeed == 0
|| attainableMaxRotationalVelocity == 0
|| realMaxSpeed == 0) {
return;
}
double translationalK =
Math.hypot(desiredChassisSpeed.vxMetersPerSecond, desiredChassisSpeed.vyMetersPerSecond)
/ attainableMaxTranslationalSpeedMetersPerSecond;
double rotationalK =
Math.abs(desiredChassisSpeed.omegaRadiansPerSecond)
/ attainableMaxRotationalVelocityRadiansPerSecond;
Math.hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy)
/ attainableMaxTranslationalSpeed;
double rotationalK = Math.abs(desiredChassisSpeed.omega) / attainableMaxRotationalVelocity;
double k = Math.max(translationalK, rotationalK);
double scale = Math.min(k * attainableMaxModuleSpeedMetersPerSecond / realMaxSpeed, 1);
double scale = Math.min(k * attainableMaxModuleSpeed / realMaxSpeed, 1);
for (SwerveModuleState moduleState : moduleStates) {
moduleState.speedMetersPerSecond *= scale;
moduleState.speed *= scale;
}
}
@@ -402,7 +384,7 @@ public class SwerveDriveKinematics
throw new IllegalArgumentException("Inconsistent number of modules!");
}
for (int i = 0; i < positions.length; ++i) {
output[i].distanceMeters = positions[i].distanceMeters;
output[i].distance = positions[i].distance;
output[i].angle = positions[i].angle;
}
}

23
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
View File

@@ -22,8 +22,8 @@ public class SwerveModulePosition
Interpolatable<SwerveModulePosition>,
ProtobufSerializable,
StructSerializable {
/** Distance measured by the wheel of the module. */
public double distanceMeters;
/** Distance measured by the wheel of the module in meters. */
public double distance;
/** Angle of the module. */
public Rotation2d angle = Rotation2d.kZero;
@@ -40,11 +40,11 @@ public class SwerveModulePosition
/**
* Constructs a SwerveModulePosition.
*
* @param distanceMeters The distance measured by the wheel of the module.
* @param distance The distance measured by the wheel of the module in meters.
* @param angle The angle of the module.
*/
public SwerveModulePosition(double distanceMeters, Rotation2d angle) {
this.distanceMeters = distanceMeters;
public SwerveModulePosition(double distance, Rotation2d angle) {
this.distance = distance;
this.angle = angle;
}
@@ -61,13 +61,13 @@ public class SwerveModulePosition
@Override
public boolean equals(Object obj) {
return obj instanceof SwerveModulePosition other
&& Math.abs(other.distanceMeters - distanceMeters) < 1E-9
&& Math.abs(other.distance - distance) < 1E-9
&& angle.equals(other.angle);
}
@Override
public int hashCode() {
return Objects.hash(distanceMeters, angle);
return Objects.hash(distance, angle);
}
/**
@@ -79,13 +79,12 @@ public class SwerveModulePosition
*/
@Override
public int compareTo(SwerveModulePosition other) {
return Double.compare(this.distanceMeters, other.distanceMeters);
return Double.compare(this.distance, other.distance);
}
@Override
public String toString() {
return String.format(
"SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle);
return String.format("SwerveModulePosition(Distance: %.2f m, Angle: %s)", distance, angle);
}
/**
@@ -94,13 +93,13 @@ public class SwerveModulePosition
* @return A copy.
*/
public SwerveModulePosition copy() {
return new SwerveModulePosition(distanceMeters, angle);
return new SwerveModulePosition(distance, angle);
}
@Override
public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) {
return new SwerveModulePosition(
MathUtil.interpolate(this.distanceMeters, endValue.distanceMeters, t),
MathUtil.interpolate(this.distance, endValue.distance, t),
this.angle.interpolate(endValue.angle, t));
}
}

27
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
View File

@@ -17,8 +17,8 @@ import java.util.Objects;
/** Represents the state of one swerve module. */
public class SwerveModuleState
implements Comparable<SwerveModuleState>, ProtobufSerializable, StructSerializable {
/** Speed of the wheel of the module. */
public double speedMetersPerSecond;
/** Speed of the wheel of the module in meters per second. */
public double speed;
/** Angle of the module. */
public Rotation2d angle = Rotation2d.kZero;
@@ -35,11 +35,11 @@ public class SwerveModuleState
/**
* Constructs a SwerveModuleState.
*
* @param speedMetersPerSecond The speed of the wheel of the module.
* @param speed The speed of the wheel of the module in meters per second.
* @param angle The angle of the module.
*/
public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
this.speedMetersPerSecond = speedMetersPerSecond;
public SwerveModuleState(double speed, Rotation2d angle) {
this.speed = speed;
this.angle = angle;
}
@@ -56,13 +56,13 @@ public class SwerveModuleState
@Override
public boolean equals(Object obj) {
return obj instanceof SwerveModuleState other
&& Math.abs(other.speedMetersPerSecond - speedMetersPerSecond) < 1E-9
&& Math.abs(other.speed - speed) < 1E-9
&& angle.equals(other.angle);
}
@Override
public int hashCode() {
return Objects.hash(speedMetersPerSecond, angle);
return Objects.hash(speed, angle);
}
/**
@@ -74,13 +74,12 @@ public class SwerveModuleState
*/
@Override
public int compareTo(SwerveModuleState other) {
return Double.compare(this.speedMetersPerSecond, other.speedMetersPerSecond);
return Double.compare(this.speed, other.speed);
}
@Override
public String toString() {
return String.format(
"SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle);
return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speed, angle);
}
/**
@@ -93,7 +92,7 @@ public class SwerveModuleState
public void optimize(Rotation2d currentAngle) {
var delta = angle.minus(currentAngle);
if (Math.abs(delta.getDegrees()) > 90.0) {
speedMetersPerSecond *= -1;
speed *= -1;
angle = angle.rotateBy(Rotation2d.kPi);
}
}
@@ -114,9 +113,9 @@ public class SwerveModuleState
var delta = desiredState.angle.minus(currentAngle);
if (Math.abs(delta.getDegrees()) > 90.0) {
return new SwerveModuleState(
-desiredState.speedMetersPerSecond, desiredState.angle.rotateBy(Rotation2d.kPi));
-desiredState.speed, desiredState.angle.rotateBy(Rotation2d.kPi));
} else {
return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
return new SwerveModuleState(desiredState.speed, desiredState.angle);
}
}
@@ -128,6 +127,6 @@ public class SwerveModuleState
* @param currentAngle The current module angle.
*/
public void cosineScale(Rotation2d currentAngle) {
speedMetersPerSecond *= angle.minus(currentAngle).getCos();
speed *= angle.minus(currentAngle).getCos();
}
}

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProto.java
View File

@@ -32,8 +32,8 @@ public class ChassisSpeedsProto implements Protobuf<ChassisSpeeds, ProtobufChass
@Override
public void pack(ProtobufChassisSpeeds msg, ChassisSpeeds value) {
msg.setVx(value.vxMetersPerSecond);
msg.setVy(value.vyMetersPerSecond);
msg.setOmega(value.omegaRadiansPerSecond);
msg.setVx(value.vx);
msg.setVy(value.vy);
msg.setOmega(value.omega);
}
}

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProto.java
View File

@@ -28,11 +28,11 @@ public class DifferentialDriveKinematicsProto
@Override
public DifferentialDriveKinematics unpack(ProtobufDifferentialDriveKinematics msg) {
return new DifferentialDriveKinematics(msg.getTrackWidth());
return new DifferentialDriveKinematics(msg.getTrackwidth());
}
@Override
public void pack(ProtobufDifferentialDriveKinematics msg, DifferentialDriveKinematics value) {
msg.setTrackWidth(value.trackWidthMeters);
msg.setTrackwidth(value.trackwidth);
}
}

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelPositionsProto.java
View File

@@ -34,7 +34,7 @@ public class DifferentialDriveWheelPositionsProto
@Override
public void pack(
ProtobufDifferentialDriveWheelPositions msg, DifferentialDriveWheelPositions value) {
msg.setLeft(value.leftMeters);
msg.setRight(value.rightMeters);
msg.setLeft(value.left);
msg.setRight(value.right);
}
}

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProto.java
View File

@@ -33,7 +33,7 @@ public class DifferentialDriveWheelSpeedsProto
@Override
public void pack(ProtobufDifferentialDriveWheelSpeeds msg, DifferentialDriveWheelSpeeds value) {
msg.setLeft(value.leftMetersPerSecond);
msg.setRight(value.rightMetersPerSecond);
msg.setLeft(value.left);
msg.setRight(value.right);
}
}

8
wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProto.java
View File

@@ -34,9 +34,9 @@ public class MecanumDriveWheelPositionsProto
@Override
public void pack(ProtobufMecanumDriveWheelPositions msg, MecanumDriveWheelPositions value) {
msg.setFrontLeft(value.frontLeftMeters);
msg.setFrontRight(value.frontRightMeters);
msg.setRearLeft(value.rearLeftMeters);
msg.setRearRight(value.rearRightMeters);
msg.setFrontLeft(value.frontLeft);
msg.setFrontRight(value.frontRight);
msg.setRearLeft(value.rearLeft);
msg.setRearRight(value.rearRight);
}
}

8
wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProto.java
View File

@@ -34,9 +34,9 @@ public class MecanumDriveWheelSpeedsProto
@Override
public void pack(ProtobufMecanumDriveWheelSpeeds msg, MecanumDriveWheelSpeeds value) {
msg.setFrontLeft(value.frontLeftMetersPerSecond);
msg.setFrontRight(value.frontRightMetersPerSecond);
msg.setRearLeft(value.rearLeftMetersPerSecond);
msg.setRearRight(value.rearRightMetersPerSecond);
msg.setFrontLeft(value.frontLeft);
msg.setFrontRight(value.frontRight);
msg.setRearLeft(value.rearLeft);
msg.setRearRight(value.rearRight);
}
}

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProto.java
View File

@@ -34,7 +34,7 @@ public class SwerveModulePositionProto
@Override
public void pack(ProtobufSwerveModulePosition msg, SwerveModulePosition value) {
msg.setDistance(value.distanceMeters);
msg.setDistance(value.distance);
Rotation2d.proto.pack(msg.getMutableAngle(), value.angle);
}
}

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProto.java
View File

@@ -34,7 +34,7 @@ public class SwerveModuleStateProto
@Override
public void pack(ProtobufSwerveModuleState msg, SwerveModuleState value) {
msg.setSpeed(value.speedMetersPerSecond);
msg.setSpeed(value.speed);
Rotation2d.proto.pack(msg.getMutableAngle(), value.angle);
}
}

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStruct.java
View File

@@ -39,8 +39,8 @@ public class ChassisSpeedsStruct implements Struct<ChassisSpeeds> {
@Override
public void pack(ByteBuffer bb, ChassisSpeeds value) {
bb.putDouble(value.vxMetersPerSecond);
bb.putDouble(value.vyMetersPerSecond);
bb.putDouble(value.omegaRadiansPerSecond);
bb.putDouble(value.vx);
bb.putDouble(value.vy);
bb.putDouble(value.omega);
}
}

8
wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStruct.java
View File

@@ -26,17 +26,17 @@ public class DifferentialDriveKinematicsStruct implements Struct<DifferentialDri
@Override
public String getSchema() {
return "double track_width";
return "double trackwidth";
}
@Override
public DifferentialDriveKinematics unpack(ByteBuffer bb) {
double trackWidth = bb.getDouble();
return new DifferentialDriveKinematics(trackWidth);
double trackwidth = bb.getDouble();
return new DifferentialDriveKinematics(trackwidth);
}
@Override
public void pack(ByteBuffer bb, DifferentialDriveKinematics value) {
bb.putDouble(value.trackWidthMeters);
bb.putDouble(value.trackwidth);
}
}

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStruct.java
View File

@@ -39,7 +39,7 @@ public class DifferentialDriveWheelPositionsStruct
@Override
public void pack(ByteBuffer bb, DifferentialDriveWheelPositions value) {
bb.putDouble(value.leftMeters);
bb.putDouble(value.rightMeters);
bb.putDouble(value.left);
bb.putDouble(value.right);
}
}

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStruct.java
View File

@@ -38,7 +38,7 @@ public class DifferentialDriveWheelSpeedsStruct implements Struct<DifferentialDr
@Override
public void pack(ByteBuffer bb, DifferentialDriveWheelSpeeds value) {
bb.putDouble(value.leftMetersPerSecond);
bb.putDouble(value.rightMetersPerSecond);
bb.putDouble(value.left);
bb.putDouble(value.right);
}
}

8
wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStruct.java
View File

@@ -40,9 +40,9 @@ public class MecanumDriveWheelPositionsStruct implements Struct<MecanumDriveWhee
@Override
public void pack(ByteBuffer bb, MecanumDriveWheelPositions value) {
bb.putDouble(value.frontLeftMeters);
bb.putDouble(value.frontRightMeters);
bb.putDouble(value.rearLeftMeters);
bb.putDouble(value.rearRightMeters);
bb.putDouble(value.frontLeft);
bb.putDouble(value.frontRight);
bb.putDouble(value.rearLeft);
bb.putDouble(value.rearRight);
}
}

8
wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStruct.java
View File

@@ -40,9 +40,9 @@ public class MecanumDriveWheelSpeedsStruct implements Struct<MecanumDriveWheelSp
@Override
public void pack(ByteBuffer bb, MecanumDriveWheelSpeeds value) {
bb.putDouble(value.frontLeftMetersPerSecond);
bb.putDouble(value.frontRightMetersPerSecond);
bb.putDouble(value.rearLeftMetersPerSecond);
bb.putDouble(value.rearRightMetersPerSecond);
bb.putDouble(value.frontLeft);
bb.putDouble(value.frontRight);
bb.putDouble(value.rearLeft);
bb.putDouble(value.rearRight);
}
}

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStruct.java
View File

@@ -44,7 +44,7 @@ public class SwerveModulePositionStruct implements Struct<SwerveModulePosition>
@Override
public void pack(ByteBuffer bb, SwerveModulePosition value) {
bb.putDouble(value.distanceMeters);
bb.putDouble(value.distance);
Rotation2d.struct.pack(bb, value.angle);
}
}

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStruct.java
View File

@@ -44,7 +44,7 @@ public class SwerveModuleStateStruct implements Struct<SwerveModuleState> {
@Override
public void pack(ByteBuffer bb, SwerveModuleState value) {
bb.putDouble(value.speedMetersPerSecond);
bb.putDouble(value.speed);
Rotation2d.struct.pack(bb, value.angle);
}
}

18
wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
View File

@@ -9,24 +9,24 @@ import edu.wpi.first.math.geometry.Pose2d;
/** Represents a pair of a pose and a curvature. */
public class PoseWithCurvature {
/** Represents the pose. */
public Pose2d poseMeters;
public Pose2d pose;
/** Represents the curvature. */
public double curvatureRadPerMeter;
/** Represents the curvature in radians per meter. */
public double curvature;
/**
* Constructs a PoseWithCurvature.
*
* @param poseMeters The pose.
* @param curvatureRadPerMeter The curvature.
* @param pose The pose.
* @param curvature The curvature in radians per meter.
*/
public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) {
this.poseMeters = poseMeters;
this.curvatureRadPerMeter = curvatureRadPerMeter;
public PoseWithCurvature(Pose2d pose, double curvature) {
this.pose = pose;
this.curvature = curvature;
}
/** Constructs a PoseWithCurvature with default values. */
public PoseWithCurvature() {
poseMeters = Pose2d.kZero;
pose = Pose2d.kZero;
}
}

2
wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
View File

@@ -126,7 +126,7 @@ public final class SplineParameterizer {
throw new MalformedSplineException(kMalformedSplineExceptionMsg);
}
final var twist = start.get().poseMeters.log(end.get().poseMeters);
final var twist = start.get().pose.log(end.get().pose);
if (Math.abs(twist.dy) > kMaxDy
|| Math.abs(twist.dx) > kMaxDx
|| Math.abs(twist.dtheta) > kMaxDtheta) {

24
wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
View File

@@ -20,13 +20,13 @@ public final class Discretization {
*
* @param <States> Num representing the number of states.
* @param contA Continuous system matrix.
* @param dtSeconds Discretization timestep.
* @param dt Discretization timestep in seconds.
* @return the discrete matrix system.
*/
public static <States extends Num> Matrix<States, States> discretizeA(
Matrix<States, States> contA, double dtSeconds) {
Matrix<States, States> contA, double dt) {
// A_d = eᴬᵀ
return contA.times(dtSeconds).exp();
return contA.times(dt).exp();
}
/**
@@ -36,12 +36,12 @@ public final class Discretization {
* @param <Inputs> Nat representing the inputs to the system.
* @param contA Continuous system matrix.
* @param contB Continuous input matrix.
* @param dtSeconds Discretization timestep.
* @param dt Discretization timestep in seconds.
* @return a Pair representing discA and diskB.
*/
public static <States extends Num, Inputs extends Num>
Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB(
Matrix<States, States> contA, Matrix<States, Inputs> contB, double dtSeconds) {
Matrix<States, States> contA, Matrix<States, Inputs> contB, double dt) {
int states = contA.getNumRows();
int inputs = contB.getNumCols();
@@ -53,7 +53,7 @@ public final class Discretization {
// ϕ = eᴹᵀ = [A_d B_d]
// [ 0 I ]
var phi = M.times(dtSeconds).exp();
var phi = M.times(dt).exp();
var discA = new Matrix<States, States>(new SimpleMatrix(states, states));
discA.extractFrom(0, 0, phi);
@@ -70,12 +70,12 @@ public final class Discretization {
* @param <States> Nat representing the number of states.
* @param contA Continuous system matrix.
* @param contQ Continuous process noise covariance matrix.
* @param dtSeconds Discretization timestep.
* @param dt Discretization timestep in seconds.
* @return a pair representing the discrete system matrix and process noise covariance matrix.
*/
public static <States extends Num>
Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQ(
Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
Matrix<States, States> contA, Matrix<States, States> contQ, double dt) {
int states = contA.getNumRows();
// Make continuous Q symmetric if it isn't already
@@ -91,7 +91,7 @@ public final class Discretization {
// ϕ = eᴹᵀ = [−A_d A_d⁻¹Q_d]
// [ 0 A_dᵀ ]
final var phi = M.times(dtSeconds).exp();
final var phi = M.times(dt).exp();
// ϕ₁₂ = A_d⁻¹Q_d
final Matrix<States, States> phi12 = phi.block(states, states, 0, states);
@@ -115,11 +115,11 @@ public final class Discretization {
*
* @param <O> Nat representing the number of outputs.
* @param contR Continuous measurement noise covariance matrix.
* @param dtSeconds Discretization timestep.
* @param dt Discretization timestep in seconds.
* @return Discretized version of the provided continuous measurement noise covariance matrix.
*/
public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> contR, double dtSeconds) {
public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> contR, double dt) {
// R_d = 1/T R
return contR.div(dtSeconds);
return contR.div(dt);
}
}

6
wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
View File

@@ -205,12 +205,12 @@ public class LinearSystem<States extends Num, Inputs extends Num, Outputs extend
*
* @param x The current state.
* @param clampedU The control input.
* @param dtSeconds Timestep for model update.
* @param dt Timestep for model update in seconds.
* @return the updated x.
*/
public Matrix<States, N1> calculateX(
Matrix<States, N1> x, Matrix<Inputs, N1> clampedU, double dtSeconds) {
var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds);
Matrix<States, N1> x, Matrix<Inputs, N1> clampedU, double dt) {
var discABpair = Discretization.discretizeAB(m_A, m_B, dt);
return discABpair.getFirst().times(x).plus(discABpair.getSecond().times(clampedU));
}

22
wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
View File

@@ -48,17 +48,17 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs ex
* @param controller State-space controller.
* @param observer State-space observer.
* @param maxVoltageVolts The maximum voltage that can be applied. Commonly 12.
* @param dtSeconds The nominal timestep.
* @param dt The nominal timestep in seconds.
*/
public LinearSystemLoop(
LinearSystem<States, Inputs, Outputs> plant,
LinearQuadraticRegulator<States, Inputs, Outputs> controller,
KalmanFilter<States, Inputs, Outputs> observer,
double maxVoltageVolts,
double dtSeconds) {
double dt) {
this(
controller,
new LinearPlantInversionFeedforward<>(plant, dtSeconds),
new LinearPlantInversionFeedforward<>(plant, dt),
observer,
u -> StateSpaceUtil.desaturateInputVector(u, maxVoltageVolts));
}
@@ -72,19 +72,15 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs ex
* @param controller State-space controller.
* @param observer State-space observer.
* @param clampFunction The function used to clamp the input U.
* @param dtSeconds The nominal timestep.
* @param dt The nominal timestep in seconds.
*/
public LinearSystemLoop(
LinearSystem<States, Inputs, Outputs> plant,
LinearQuadraticRegulator<States, Inputs, Outputs> controller,
KalmanFilter<States, Inputs, Outputs> observer,
Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction,
double dtSeconds) {
this(
controller,
new LinearPlantInversionFeedforward<>(plant, dtSeconds),
observer,
clampFunction);
double dt) {
this(controller, new LinearPlantInversionFeedforward<>(plant, dt), observer, clampFunction);
}
/**
@@ -327,15 +323,15 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs ex
*
* <p>After calling this, the user should send the elements of u to the actuators.
*
* @param dtSeconds Timestep for model update.
* @param dt Timestep for model update in seconds.
*/
public void predict(double dtSeconds) {
public void predict(double dt) {
var u =
clampInput(
m_controller
.calculate(getObserver().getXhat(), m_nextR)
.plus(m_feedforward.calculate(m_nextR)));
getObserver().predict(u, dtSeconds);
getObserver().predict(u, dt);
}
/**

66
wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
View File

@@ -23,11 +23,11 @@ public final class NumericalIntegration {
*
* @param f The function to integrate, which takes one argument x.
* @param x The initial value of x.
* @param dtSeconds The time over which to integrate.
* @param dt The time over which to integrate in seconds.
* @return the integration of dx/dt = f(x) for dt.
*/
public static double rk4(DoubleUnaryOperator f, double x, double dtSeconds) {
final var h = dtSeconds;
public static double rk4(DoubleUnaryOperator f, double x, double dt) {
final var h = dt;
final var k1 = f.applyAsDouble(x);
final var k2 = f.applyAsDouble(x + h * k1 * 0.5);
final var k3 = f.applyAsDouble(x + h * k2 * 0.5);
@@ -42,11 +42,11 @@ public final class NumericalIntegration {
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dtSeconds The time over which to integrate.
* @param dt The time over which to integrate in seconds.
* @return The result of Runge Kutta integration (4th order).
*/
public static double rk4(DoubleBinaryOperator f, double x, double u, double dtSeconds) {
final var h = dtSeconds;
public static double rk4(DoubleBinaryOperator f, double x, double u, double dt) {
final var h = dt;
final var k1 = f.applyAsDouble(x, u);
final var k2 = f.applyAsDouble(x + h * k1 * 0.5, u);
@@ -64,15 +64,15 @@ public final class NumericalIntegration {
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dtSeconds The time over which to integrate.
* @param dt The time over which to integrate in seconds.
* @return the integration of dx/dt = f(x, u) for dt.
*/
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rk4(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
Matrix<Inputs, N1> u,
double dtSeconds) {
final var h = dtSeconds;
double dt) {
final var h = dt;
Matrix<States, N1> k1 = f.apply(x, u);
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(h * 0.5)), u);
@@ -88,12 +88,12 @@ public final class NumericalIntegration {
* @param <States> A Num representing the states of the system.
* @param f The function to integrate. It must take one argument x.
* @param x The initial value of x.
* @param dtSeconds The time over which to integrate.
* @param dt The time over which to integrate in seconds.
* @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
*/
public static <States extends Num> Matrix<States, N1> rk4(
UnaryOperator<Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
final var h = dtSeconds;
UnaryOperator<Matrix<States, N1>> f, Matrix<States, N1> x, double dt) {
final var h = dt;
Matrix<States, N1> k1 = f.apply(x);
Matrix<States, N1> k2 = f.apply(x.plus(k1.times(h * 0.5)));
@@ -111,20 +111,20 @@ public final class NumericalIntegration {
* @param f The function to integrate. It must take two arguments t and y.
* @param t The initial value of t.
* @param y The initial value of y.
* @param dtSeconds The time over which to integrate.
* @param dt The time over which to integrate in seconds.
* @return the integration of dx/dt = f(x) for dt.
*/
public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rk4(
BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
double t,
Matrix<Rows, Cols> y,
double dtSeconds) {
final var h = dtSeconds;
double dt) {
final var h = dt;
Matrix<Rows, Cols> k1 = f.apply(t, y);
Matrix<Rows, Cols> k2 = f.apply(t + dtSeconds * 0.5, y.plus(k1.times(h * 0.5)));
Matrix<Rows, Cols> k3 = f.apply(t + dtSeconds * 0.5, y.plus(k2.times(h * 0.5)));
Matrix<Rows, Cols> k4 = f.apply(t + dtSeconds, y.plus(k3.times(h)));
Matrix<Rows, Cols> k2 = f.apply(t + dt * 0.5, y.plus(k1.times(h * 0.5)));
Matrix<Rows, Cols> k3 = f.apply(t + dt * 0.5, y.plus(k2.times(h * 0.5)));
Matrix<Rows, Cols> k4 = f.apply(t + dt, y.plus(k3.times(h)));
return y.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
}
@@ -138,15 +138,15 @@ public final class NumericalIntegration {
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dtSeconds The time over which to integrate.
* @param dt The time over which to integrate in seconds.
* @return the integration of dx/dt = f(x, u) for dt.
*/
public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
Matrix<Inputs, N1> u,
double dtSeconds) {
return rkdp(f, x, u, dtSeconds, 1e-6);
double dt) {
return rkdp(f, x, u, dt, 1e-6);
}
/**
@@ -157,7 +157,7 @@ public final class NumericalIntegration {
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param dtSeconds The time over which to integrate.
* @param dt The time over which to integrate in seconds.
* @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
* @return the integration of dx/dt = f(x, u) for dt.
*/
@@ -165,7 +165,7 @@ public final class NumericalIntegration {
BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
Matrix<States, N1> x,
Matrix<Inputs, N1> u,
double dtSeconds,
double dt,
double maxError) {
// See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the
// Butcher tableau the following arrays came from.
@@ -200,13 +200,13 @@ public final class NumericalIntegration {
double truncationError;
double dtElapsed = 0.0;
double h = dtSeconds;
double h = dt;
// Loop until we've gotten to our desired dt
while (dtElapsed < dtSeconds) {
while (dtElapsed < dt) {
do {
// Only allow us to advance up to the dt remaining
h = Math.min(h, dtSeconds - dtElapsed);
h = Math.min(h, dt - dtElapsed);
var k1 = f.apply(x, u);
var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u);
@@ -260,7 +260,7 @@ public final class NumericalIntegration {
.normF();
if (truncationError == 0.0) {
h = dtSeconds - dtElapsed;
h = dt - dtElapsed;
} else {
h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
}
@@ -281,7 +281,7 @@ public final class NumericalIntegration {
* @param f The function to integrate. It must take two arguments t and y.
* @param t The initial value of t.
* @param y The initial value of y.
* @param dtSeconds The time over which to integrate.
* @param dt The time over which to integrate in seconds.
* @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
* @return the integration of dx/dt = f(x, u) for dt.
*/
@@ -289,7 +289,7 @@ public final class NumericalIntegration {
BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
double t,
Matrix<Rows, Cols> y,
double dtSeconds,
double dt,
double maxError) {
// See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the
// Butcher tableau the following arrays came from.
@@ -327,13 +327,13 @@ public final class NumericalIntegration {
double truncationError;
double dtElapsed = 0.0;
double h = dtSeconds;
double h = dt;
// Loop until we've gotten to our desired dt
while (dtElapsed < dtSeconds) {
while (dtElapsed < dt) {
do {
// Only allow us to advance up to the dt remaining
h = Math.min(h, dtSeconds - dtElapsed);
h = Math.min(h, dt - dtElapsed);
var k1 = f.apply(t, y);
var k2 = f.apply(t + h * c[0], y.plus(k1.times(A[0][0]).times(h)));
@@ -387,7 +387,7 @@ public final class NumericalIntegration {
.normF();
if (truncationError == 0.0) {
h = dtSeconds - dtElapsed;
h = dt - dtElapsed;
} else {
h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
}

116
wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
View File

@@ -13,28 +13,28 @@ import edu.wpi.first.util.struct.StructSerializable;
/** Holds the constants for a DC motor. */
public class DCMotor implements ProtobufSerializable, StructSerializable {
/** Voltage at which the motor constants were measured. */
public final double nominalVoltageVolts;
public final double nominalVoltage;
/** Torque when stalled. */
public final double stallTorqueNewtonMeters;
/** Torque when stalled in Newton-meters. */
public final double stallTorque;
/** Current draw when stalled. */
public final double stallCurrentAmps;
/** Current draw when stalled in amps. */
public final double stallCurrent;
/** Current draw under no load. */
public final double freeCurrentAmps;
/** Current draw under no load in amps. */
public final double freeCurrent;
/** Angular velocity under no load. */
public final double freeSpeedRadPerSec;
/** Angular velocity under no load in radians per second. */
public final double freeSpeed;
/** Motor internal resistance. */
public final double rOhms;
/** Motor internal resistance in Ohms. */
public final double R;
/** Motor velocity constant. */
public final double KvRadPerSecPerVolt;
/** Motor velocity constant in (rad/s)/V. */
public final double Kv;
/** Motor torque constant. */
public final double KtNMPerAmp;
/** Motor torque constant in Newton-meters per amp. */
public final double Kt;
/** DCMotor protobuf for serialization. */
public static final DCMotorProto proto = new DCMotorProto();
@@ -45,84 +45,82 @@ public class DCMotor implements ProtobufSerializable, StructSerializable {
/**
* Constructs a DC motor.
*
* @param nominalVoltageVolts Voltage at which the motor constants were measured.
* @param stallTorqueNewtonMeters Torque when stalled.
* @param stallCurrentAmps Current draw when stalled.
* @param freeCurrentAmps Current draw under no load.
* @param freeSpeedRadPerSec Angular velocity under no load.
* @param nominalVoltage Voltage at which the motor constants were measured.
* @param stallTorque Torque when stalled.
* @param stallCurrent Current draw when stalled.
* @param freeCurrent Current draw under no load.
* @param freeSpeed Angular velocity under no load.
* @param numMotors Number of motors in a gearbox.
*/
public DCMotor(
double nominalVoltageVolts,
double stallTorqueNewtonMeters,
double stallCurrentAmps,
double freeCurrentAmps,
double freeSpeedRadPerSec,
double nominalVoltage,
double stallTorque,
double stallCurrent,
double freeCurrent,
double freeSpeed,
int numMotors) {
this.nominalVoltageVolts = nominalVoltageVolts;
this.stallTorqueNewtonMeters = stallTorqueNewtonMeters * numMotors;
this.stallCurrentAmps = stallCurrentAmps * numMotors;
this.freeCurrentAmps = freeCurrentAmps * numMotors;
this.freeSpeedRadPerSec = freeSpeedRadPerSec;
this.nominalVoltage = nominalVoltage;
this.stallTorque = stallTorque * numMotors;
this.stallCurrent = stallCurrent * numMotors;
this.freeCurrent = freeCurrent * numMotors;
this.freeSpeed = freeSpeed;
this.rOhms = nominalVoltageVolts / this.stallCurrentAmps;
this.KvRadPerSecPerVolt =
freeSpeedRadPerSec / (nominalVoltageVolts - rOhms * this.freeCurrentAmps);
this.KtNMPerAmp = this.stallTorqueNewtonMeters / this.stallCurrentAmps;
this.R = nominalVoltage / this.stallCurrent;
this.Kv = freeSpeed / (nominalVoltage - R * this.freeCurrent);
this.Kt = this.stallTorque / this.stallCurrent;
}
/**
* Calculate current drawn by motor with given speed and input voltage.
*
* @param speedRadiansPerSec The current angular velocity of the motor.
* @param voltageInputVolts The voltage being applied to the motor.
* @param speed The current angular velocity of the motor.
* @param voltageInput The voltage being applied to the motor.
* @return The estimated current.
*/
public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
return -1.0 / KvRadPerSecPerVolt / rOhms * speedRadiansPerSec + 1.0 / rOhms * voltageInputVolts;
public double getCurrent(double speed, double voltageInput) {
return -1.0 / Kv / R * speed + 1.0 / R * voltageInput;
}
/**
* Calculate current drawn by motor for a given torque.
*
* @param torqueNm The torque produced by the motor.
* @param torque The torque produced by the motor in Newton-meters.
* @return The current drawn by the motor.
*/
public double getCurrent(double torqueNm) {
return torqueNm / KtNMPerAmp;
public double getCurrent(double torque) {
return torque / Kt;
}
/**
* Calculate torque produced by the motor with a given current.
*
* @param currentAmpere The current drawn by the motor.
* @return The torque output.
* @param current The current drawn by the motor in amps.
* @return The torque output in Newton-meters.
*/
public double getTorque(double currentAmpere) {
return currentAmpere * KtNMPerAmp;
public double getTorque(double current) {
return current * Kt;
}
/**
* Calculate the voltage provided to the motor for a given torque and angular velocity.
*
* @param torqueNm The torque produced by the motor.
* @param speedRadiansPerSec The current angular velocity of the motor.
* @param torque The torque produced by the motor in Newton-meters.
* @param speed The current angular velocity of the motor in radians per second.
* @return The voltage of the motor.
*/
public double getVoltage(double torqueNm, double speedRadiansPerSec) {
return 1.0 / KvRadPerSecPerVolt * speedRadiansPerSec + 1.0 / KtNMPerAmp * rOhms * torqueNm;
public double getVoltage(double torque, double speed) {
return 1.0 / Kv * speed + 1.0 / Kt * R * torque;
}
/**
* Calculates the angular speed produced by the motor at a given torque and input voltage.
*
* @param torqueNm The torque produced by the motor.
* @param voltageInputVolts The voltage applied to the motor.
* @param torque The torque produced by the motor in Newton-meters.
* @param voltageInput The voltage applied to the motor.
* @return The angular speed of the motor.
*/
public double getSpeed(double torqueNm, double voltageInputVolts) {
return voltageInputVolts * KvRadPerSecPerVolt
- 1.0 / KtNMPerAmp * torqueNm * rOhms * KvRadPerSecPerVolt;
public double getSpeed(double torque, double voltageInput) {
return voltageInput * Kv - 1.0 / Kt * torque * R * Kv;
}
/**
@@ -133,11 +131,11 @@ public class DCMotor implements ProtobufSerializable, StructSerializable {
*/
public DCMotor withReduction(double gearboxReduction) {
return new DCMotor(
nominalVoltageVolts,
stallTorqueNewtonMeters * gearboxReduction,
stallCurrentAmps,
freeCurrentAmps,
freeSpeedRadPerSec / gearboxReduction,
nominalVoltage,
stallTorque * gearboxReduction,
stallCurrent,
freeCurrent,
freeSpeed / gearboxReduction,
1);
}

114
wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
View File

@@ -23,19 +23,19 @@ public final class LinearSystemId {
* velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the carriage.
* @param massKg The mass of the elevator carriage, in kilograms.
* @param radiusMeters The radius of the elevator's driving drum, in meters.
* @param mass The mass of the elevator carriage, in kilograms.
* @param radius The radius of the elevator's driving drum, in meters.
* @param gearing The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if massKg &lt;= 0, radiusMeters &lt;= 0, or gearing &lt;= 0.
* @throws IllegalArgumentException if mass &lt;= 0, radius &lt;= 0, or gearing &lt;= 0.
*/
public static LinearSystem<N2, N1, N2> createElevatorSystem(
DCMotor motor, double massKg, double radiusMeters, double gearing) {
if (massKg <= 0.0) {
throw new IllegalArgumentException("massKg must be greater than zero.");
DCMotor motor, double mass, double radius, double gearing) {
if (mass <= 0.0) {
throw new IllegalArgumentException("mass must be greater than zero.");
}
if (radiusMeters <= 0.0) {
throw new IllegalArgumentException("radiusMeters must be greater than zero.");
if (radius <= 0.0) {
throw new IllegalArgumentException("radius must be greater than zero.");
}
if (gearing <= 0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
@@ -48,10 +48,8 @@ public final class LinearSystemId {
0,
1,
0,
-Math.pow(gearing, 2)
* motor.KtNMPerAmp
/ (motor.rOhms * radiusMeters * radiusMeters * massKg * motor.KvRadPerSecPerVolt)),
VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)),
-Math.pow(gearing, 2) * motor.Kt / (motor.R * radius * radius * mass * motor.Kv)),
VecBuilder.fill(0, gearing * motor.Kt / (motor.R * radius * mass)),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
@@ -61,14 +59,14 @@ public final class LinearSystemId {
* velocity], inputs are [voltage], and outputs are [angular velocity].
*
* @param motor The motor (or gearbox) attached to the flywheel.
* @param JKgMetersSquared The moment of inertia J of the flywheel.
* @param J The moment of inertia J of the flywheel in kg-m².
* @param gearing The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if JKgMetersSquared &lt;= 0 or gearing &lt;= 0.
* @throws IllegalArgumentException if J &lt;= 0 or gearing &lt;= 0.
*/
public static LinearSystem<N1, N1, N1> createFlywheelSystem(
DCMotor motor, double JKgMetersSquared, double gearing) {
if (JKgMetersSquared <= 0.0) {
DCMotor motor, double J, double gearing) {
if (J <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (gearing <= 0.0) {
@@ -76,12 +74,8 @@ public final class LinearSystemId {
}
return new LinearSystem<>(
VecBuilder.fill(
-gearing
* gearing
* motor.KtNMPerAmp
/ (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)),
VecBuilder.fill(gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)),
VecBuilder.fill(-gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)),
VecBuilder.fill(gearing * motor.Kt / (motor.R * J)),
Matrix.eye(Nat.N1()),
new Matrix<>(Nat.N1(), Nat.N1()));
}
@@ -92,14 +86,14 @@ public final class LinearSystemId {
* velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to system.
* @param JKgMetersSquared The moment of inertia J of the DC motor.
* @param J The moment of inertia J of the DC motor in kg-m².
* @param gearing The reduction between motor and drum, as a ratio of output to input.
* @return A LinearSystem representing the given characterized constants.
* @throws IllegalArgumentException if JKgMetersSquared &lt;= 0 or gearing &lt;= 0.
* @throws IllegalArgumentException if J &lt;= 0 or gearing &lt;= 0.
*/
public static LinearSystem<N2, N1, N2> createDCMotorSystem(
DCMotor motor, double JKgMetersSquared, double gearing) {
if (JKgMetersSquared <= 0.0) {
DCMotor motor, double J, double gearing) {
if (J <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (gearing <= 0.0) {
@@ -108,16 +102,8 @@ public final class LinearSystemId {
return new LinearSystem<>(
MatBuilder.fill(
Nat.N2(),
Nat.N2(),
0,
1,
0,
-gearing
* gearing
* motor.KtNMPerAmp
/ (motor.KvRadPerSecPerVolt * motor.rOhms * JKgMetersSquared)),
VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgMetersSquared)),
Nat.N2(), Nat.N2(), 0, 1, 0, -gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)),
VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}
@@ -161,46 +147,38 @@ public final class LinearSystemId {
* [left velocity, right velocity]ᵀ.
*
* @param motor The motor (or gearbox) driving the drivetrain.
* @param massKg The mass of the robot in kilograms.
* @param rMeters The radius of the wheels in meters.
* @param rbMeters The radius of the base (half the track width) in meters.
* @param JKgMetersSquared The moment of inertia of the robot.
* @param mass The mass of the robot in kilograms.
* @param r The radius of the wheels in meters.
* @param rb The radius of the base (half the trackwidth) in meters.
* @param J The moment of inertia of the robot in kg-m².
* @param gearing The gearing reduction as output over input.
* @return A LinearSystem representing a differential drivetrain.
* @throws IllegalArgumentException if m &lt;= 0, r &lt;= 0, rb &lt;= 0, J &lt;= 0, or gearing
* &lt;= 0.
*/
public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
DCMotor motor,
double massKg,
double rMeters,
double rbMeters,
double JKgMetersSquared,
double gearing) {
if (massKg <= 0.0) {
throw new IllegalArgumentException("massKg must be greater than zero.");
DCMotor motor, double mass, double r, double rb, double J, double gearing) {
if (mass <= 0.0) {
throw new IllegalArgumentException("mass must be greater than zero.");
}
if (rMeters <= 0.0) {
throw new IllegalArgumentException("rMeters must be greater than zero.");
if (r <= 0.0) {
throw new IllegalArgumentException("r must be greater than zero.");
}
if (rbMeters <= 0.0) {
throw new IllegalArgumentException("rbMeters must be greater than zero.");
if (rb <= 0.0) {
throw new IllegalArgumentException("rb must be greater than zero.");
}
if (JKgMetersSquared <= 0.0) {
throw new IllegalArgumentException("JKgMetersSquared must be greater than zero.");
if (J <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
}
var C1 =
-(gearing * gearing)
* motor.KtNMPerAmp
/ (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters);
var C2 = gearing * motor.KtNMPerAmp / (motor.rOhms * rMeters);
var C1 = -(gearing * gearing) * motor.Kt / (motor.Kv * motor.R * r * r);
var C2 = gearing * motor.Kt / (motor.R * r);
final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared;
final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared;
final double C3 = 1 / mass + rb * rb / J;
final double C4 = 1 / mass - rb * rb / J;
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1);
var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2);
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
@@ -214,15 +192,15 @@ public final class LinearSystemId {
* angular velocity]ᵀ, inputs are [voltage], and outputs are [angle, angular velocity]ᵀ.
*
* @param motor The motor (or gearbox) attached to the arm.
* @param JKgSquaredMeters The moment of inertia J of the arm.
* @param J The moment of inertia J of the arm in kg-m².
* @param gearing The gearing between the motor and arm, in output over input. Most of the time
* this will be greater than 1.
* @return A LinearSystem representing the given characterized constants.
*/
public static LinearSystem<N2, N1, N2> createSingleJointedArmSystem(
DCMotor motor, double JKgSquaredMeters, double gearing) {
if (JKgSquaredMeters <= 0.0) {
throw new IllegalArgumentException("JKgSquaredMeters must be greater than zero.");
DCMotor motor, double J, double gearing) {
if (J <= 0.0) {
throw new IllegalArgumentException("J must be greater than zero.");
}
if (gearing <= 0.0) {
throw new IllegalArgumentException("gearing must be greater than zero.");
@@ -235,10 +213,8 @@ public final class LinearSystemId {
0,
1,
0,
-Math.pow(gearing, 2)
* motor.KtNMPerAmp
/ (motor.KvRadPerSecPerVolt * motor.rOhms * JKgSquaredMeters)),
VecBuilder.fill(0, gearing * motor.KtNMPerAmp / (motor.rOhms * JKgSquaredMeters)),
-Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)),
VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)),
Matrix.eye(Nat.N2()),
new Matrix<>(Nat.N2(), Nat.N1()));
}

10
wpimath/src/main/java/edu/wpi/first/math/system/plant/proto/DCMotorProto.java
View File

@@ -38,10 +38,10 @@ public class DCMotorProto implements Protobuf<DCMotor, ProtobufDCMotor> {
@Override
public void pack(ProtobufDCMotor msg, DCMotor value) {
msg.setNominalVoltage(value.nominalVoltageVolts);
msg.setStallTorque(value.stallTorqueNewtonMeters);
msg.setStallCurrent(value.stallCurrentAmps);
msg.setFreeCurrent(value.freeCurrentAmps);
msg.setFreeSpeed(value.freeSpeedRadPerSec);
msg.setNominalVoltage(value.nominalVoltage);
msg.setStallTorque(value.stallTorque);
msg.setStallCurrent(value.stallCurrent);
msg.setFreeCurrent(value.freeCurrent);
msg.setFreeSpeed(value.freeSpeed);
}
}

10
wpimath/src/main/java/edu/wpi/first/math/system/plant/struct/DCMotorStruct.java
View File

@@ -42,10 +42,10 @@ public class DCMotorStruct implements Struct<DCMotor> {
@Override
public void pack(ByteBuffer bb, DCMotor value) {
bb.putDouble(value.nominalVoltageVolts);
bb.putDouble(value.stallTorqueNewtonMeters);
bb.putDouble(value.stallCurrentAmps);
bb.putDouble(value.freeCurrentAmps);
bb.putDouble(value.freeSpeedRadPerSec);
bb.putDouble(value.nominalVoltage);
bb.putDouble(value.stallTorque);
bb.putDouble(value.stallCurrent);
bb.putDouble(value.freeCurrent);
bb.putDouble(value.freeSpeed);
}
}

171
wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
View File

@@ -23,13 +23,13 @@ public class Trajectory implements ProtobufSerializable {
/** Trajectory protobuf for serialization. */
public static final TrajectoryProto proto = new TrajectoryProto();
private final double m_totalTimeSeconds;
private final double m_totalTime;
private final List<State> m_states;
/** Constructs an empty trajectory. */
public Trajectory() {
m_states = new ArrayList<>();
m_totalTimeSeconds = 0.0;
m_totalTime = 0.0;
}
/**
@@ -45,7 +45,7 @@ public class Trajectory implements ProtobufSerializable {
throw new IllegalArgumentException("Trajectory manually created with no states.");
}
m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
m_totalTime = m_states.get(m_states.size() - 1).time;
}
/**
@@ -78,16 +78,16 @@ public class Trajectory implements ProtobufSerializable {
* @return The initial pose of the trajectory.
*/
public Pose2d getInitialPose() {
return sample(0).poseMeters;
return sample(0).pose;
}
/**
* Returns the overall duration of the trajectory.
*
* @return The duration of the trajectory.
* @return The duration of the trajectory in seconds.
*/
public double getTotalTimeSeconds() {
return m_totalTimeSeconds;
public double getTotalTime() {
return m_totalTime;
}
/**
@@ -102,19 +102,19 @@ public class Trajectory implements ProtobufSerializable {
/**
* Sample the trajectory at a point in time.
*
* @param timeSeconds The point in time since the beginning of the trajectory to sample.
* @param time The point in time since the beginning of the trajectory to sample in seconds.
* @return The state at that point in time.
* @throws IllegalStateException if the trajectory has no states.
*/
public State sample(double timeSeconds) {
public State sample(double time) {
if (m_states.isEmpty()) {
throw new IllegalStateException("Trajectory cannot be sampled if it has no states.");
}
if (timeSeconds <= m_states.get(0).timeSeconds) {
if (time <= m_states.get(0).time) {
return m_states.get(0);
}
if (timeSeconds >= m_totalTimeSeconds) {
if (time >= m_totalTime) {
return m_states.get(m_states.size() - 1);
}
@@ -129,7 +129,7 @@ public class Trajectory implements ProtobufSerializable {
while (low != high) {
int mid = (low + high) / 2;
if (m_states.get(mid).timeSeconds < timeSeconds) {
if (m_states.get(mid).time < time) {
// This index and everything under it are less than the requested
// timestamp. Therefore, we can discard them.
low = mid + 1;
@@ -150,13 +150,12 @@ public class Trajectory implements ProtobufSerializable {
final State prevSample = m_states.get(low - 1);
// If the difference in states is negligible, then we are spot on!
if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) {
if (Math.abs(sample.time - prevSample.time) < 1E-9) {
return sample;
}
// Interpolate between the two states for the state that we want.
return prevSample.interpolate(
sample,
(timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds));
sample, (time - prevSample.time) / (sample.time - prevSample.time));
}
/**
@@ -169,7 +168,7 @@ public class Trajectory implements ProtobufSerializable {
*/
public Trajectory transformBy(Transform2d transform) {
var firstState = m_states.get(0);
var firstPose = firstState.poseMeters;
var firstPose = firstState.pose;
// Calculate the transformed first pose.
var newFirstPose = firstPose.plus(transform);
@@ -177,22 +176,22 @@ public class Trajectory implements ProtobufSerializable {
newStates.add(
new State(
firstState.timeSeconds,
firstState.velocityMetersPerSecond,
firstState.accelerationMetersPerSecondSq,
firstState.time,
firstState.velocity,
firstState.acceleration,
newFirstPose,
firstState.curvatureRadPerMeter));
firstState.curvature));
for (int i = 1; i < m_states.size(); i++) {
var state = m_states.get(i);
// We are transforming relative to the coordinate frame of the new initial pose.
newStates.add(
new State(
state.timeSeconds,
state.velocityMetersPerSecond,
state.accelerationMetersPerSecondSq,
newFirstPose.plus(state.poseMeters.minus(firstPose)),
state.curvatureRadPerMeter));
state.time,
state.velocity,
state.acceleration,
newFirstPose.plus(state.pose.minus(firstPose)),
state.curvature));
}
return new Trajectory(newStates);
@@ -212,11 +211,11 @@ public class Trajectory implements ProtobufSerializable {
.map(
state ->
new State(
state.timeSeconds,
state.velocityMetersPerSecond,
state.accelerationMetersPerSecondSq,
state.poseMeters.relativeTo(pose),
state.curvatureRadPerMeter))
state.time,
state.velocity,
state.acceleration,
state.pose.relativeTo(pose),
state.curvature))
.collect(Collectors.toList()));
}
@@ -241,11 +240,11 @@ public class Trajectory implements ProtobufSerializable {
.map(
state ->
new State(
state.timeSeconds,
state.velocityMetersPerSecond,
state.accelerationMetersPerSecondSq,
state.poseMeters,
state.curvatureRadPerMeter))
state.time,
state.velocity,
state.acceleration,
state.pose,
state.curvature))
.collect(Collectors.toList());
// Here we omit the first state of the other trajectory because we don't want
@@ -254,13 +253,7 @@ public class Trajectory implements ProtobufSerializable {
// other trajectory.
for (int i = 1; i < other.getStates().size(); ++i) {
var s = other.getStates().get(i);
states.add(
new State(
s.timeSeconds + m_totalTimeSeconds,
s.velocityMetersPerSecond,
s.accelerationMetersPerSecondSq,
s.poseMeters,
s.curvatureRadPerMeter));
states.add(new State(s.time + m_totalTime, s.velocity, s.acceleration, s.pose, s.curvature));
}
return new Trajectory(states);
}
@@ -273,51 +266,46 @@ public class Trajectory implements ProtobufSerializable {
/** Trajectory.State protobuf for serialization. */
public static final TrajectoryStateProto proto = new TrajectoryStateProto();
/** The time elapsed since the beginning of the trajectory. */
/** The time elapsed since the beginning of the trajectory in seconds. */
@JsonProperty("time")
public double timeSeconds;
public double time;
/** The speed at that point of the trajectory. */
/** The speed at that point of the trajectory in meters per second. */
@JsonProperty("velocity")
public double velocityMetersPerSecond;
public double velocity;
/** The acceleration at that point of the trajectory. */
/** The acceleration at that point of the trajectory in m/s². */
@JsonProperty("acceleration")
public double accelerationMetersPerSecondSq;
public double acceleration;
/** The pose at that point of the trajectory. */
@JsonProperty("pose")
public Pose2d poseMeters;
public Pose2d pose;
/** The curvature at that point of the trajectory. */
/** The curvature at that point of the trajectory in rad/m. */
@JsonProperty("curvature")
public double curvatureRadPerMeter;
public double curvature;
/** Default constructor. */
public State() {
poseMeters = Pose2d.kZero;
pose = Pose2d.kZero;
}
/**
* Constructs a State with the specified parameters.
*
* @param timeSeconds The time elapsed since the beginning of the trajectory.
* @param velocityMetersPerSecond The speed at that point of the trajectory.
* @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory.
* @param poseMeters The pose at that point of the trajectory.
* @param curvatureRadPerMeter The curvature at that point of the trajectory.
* @param time The time elapsed since the beginning of the trajectory in seconds.
* @param velocity The speed at that point of the trajectory in m/s.
* @param acceleration The acceleration at that point of the trajectory in m/s².
* @param pose The pose at that point of the trajectory.
* @param curvature The curvature at that point of the trajectory in rad/m.
*/
public State(
double timeSeconds,
double velocityMetersPerSecond,
double accelerationMetersPerSecondSq,
Pose2d poseMeters,
double curvatureRadPerMeter) {
this.timeSeconds = timeSeconds;
this.velocityMetersPerSecond = velocityMetersPerSecond;
this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq;
this.poseMeters = poseMeters;
this.curvatureRadPerMeter = curvatureRadPerMeter;
public State(double time, double velocity, double acceleration, Pose2d pose, double curvature) {
this.time = time;
this.velocity = velocity;
this.acceleration = acceleration;
this.pose = pose;
this.curvature = curvature;
}
/**
@@ -329,10 +317,10 @@ public class Trajectory implements ProtobufSerializable {
*/
State interpolate(State endValue, double i) {
// Find the new t value.
final double newT = lerp(timeSeconds, endValue.timeSeconds, i);
final double newT = lerp(time, endValue.time, i);
// Find the delta time between the current state and the interpolated state.
final double deltaT = newT - timeSeconds;
final double deltaT = newT - time;
// If delta time is negative, flip the order of interpolation.
if (deltaT < 0) {
@@ -340,72 +328,59 @@ public class Trajectory implements ProtobufSerializable {
}
// Check whether the robot is reversing at this stage.
final boolean reversing =
velocityMetersPerSecond < 0
|| Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0;
final boolean reversing = velocity < 0 || Math.abs(velocity) < 1E-9 && acceleration < 0;
// Calculate the new velocity
// v_f = v_0 + at
final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
final double newV = velocity + (acceleration * deltaT);
// Calculate the change in position.
// delta_s = v_0 t + 0.5at²
final double newS =
(velocityMetersPerSecond * deltaT
+ 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2))
* (reversing ? -1.0 : 1.0);
(velocity * deltaT + 0.5 * acceleration * Math.pow(deltaT, 2)) * (reversing ? -1.0 : 1.0);
// Return the new state. To find the new position for the new state, we need
// to interpolate between the two endpoint poses. The fraction for
// interpolation is the change in position (delta s) divided by the total
// distance between the two endpoints.
final double interpolationFrac =
newS / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation());
newS / endValue.pose.getTranslation().getDistance(pose.getTranslation());
return new State(
newT,
newV,
accelerationMetersPerSecondSq,
lerp(poseMeters, endValue.poseMeters, interpolationFrac),
lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac));
acceleration,
lerp(pose, endValue.pose, interpolationFrac),
lerp(curvature, endValue.curvature, interpolationFrac));
}
@Override
public String toString() {
return String.format(
"State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
timeSeconds,
velocityMetersPerSecond,
accelerationMetersPerSecondSq,
poseMeters,
curvatureRadPerMeter);
time, velocity, acceleration, pose, curvature);
}
@Override
public boolean equals(Object obj) {
return obj instanceof State state
&& Double.compare(state.timeSeconds, timeSeconds) == 0
&& Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0
&& Double.compare(state.accelerationMetersPerSecondSq, accelerationMetersPerSecondSq) == 0
&& Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0
&& Objects.equals(poseMeters, state.poseMeters);
&& Double.compare(state.time, time) == 0
&& Double.compare(state.velocity, velocity) == 0
&& Double.compare(state.acceleration, acceleration) == 0
&& Double.compare(state.curvature, curvature) == 0
&& Objects.equals(pose, state.pose);
}
@Override
public int hashCode() {
return Objects.hash(
timeSeconds,
velocityMetersPerSecond,
accelerationMetersPerSecondSq,
poseMeters,
curvatureRadPerMeter);
return Objects.hash(time, velocity, acceleration, pose, curvature);
}
}
@Override
public String toString() {
String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n"));
return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList);
return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTime, stateList);
}
@Override

35
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java
View File

@@ -38,21 +38,20 @@ public class TrajectoryConfig {
/**
* Constructs the trajectory configuration class.
*
* @param maxVelocityMetersPerSecond The max velocity for the trajectory.
* @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
* @param maxVelocity The max velocity for the trajectory in m/s.
* @param maxAcceleration The max acceleration for the trajectory in m/s².
*/
public TrajectoryConfig(
double maxVelocityMetersPerSecond, double maxAccelerationMetersPerSecondSq) {
m_maxVelocity = maxVelocityMetersPerSecond;
m_maxAcceleration = maxAccelerationMetersPerSecondSq;
public TrajectoryConfig(double maxVelocity, double maxAcceleration) {
m_maxVelocity = maxVelocity;
m_maxAcceleration = maxAcceleration;
m_constraints = new ArrayList<>();
}
/**
* Constructs the trajectory configuration class.
*
* @param maxVelocity The max velocity for the trajectory.
* @param maxAcceleration The max acceleration for the trajectory.
* @param maxVelocity The max velocity for the trajectory in m/s.
* @param maxAcceleration The max acceleration for the trajectory in m/s².
*/
public TrajectoryConfig(LinearVelocity maxVelocity, LinearAcceleration maxAcceleration) {
this(maxVelocity.in(MetersPerSecond), maxAcceleration.in(MetersPerSecondPerSecond));
@@ -119,7 +118,7 @@ public class TrajectoryConfig {
/**
* Returns the starting velocity of the trajectory.
*
* @return The starting velocity of the trajectory.
* @return The starting velocity of the trajectory in m/s.
*/
public double getStartVelocity() {
return m_startVelocity;
@@ -128,11 +127,11 @@ public class TrajectoryConfig {
/**
* Sets the start velocity of the trajectory.
*
* @param startVelocityMetersPerSecond The start velocity of the trajectory.
* @param startVelocity The start velocity of the trajectory in m/s.
* @return Instance of the current config object.
*/
public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) {
m_startVelocity = startVelocityMetersPerSecond;
public TrajectoryConfig setStartVelocity(double startVelocity) {
m_startVelocity = startVelocity;
return this;
}
@@ -149,7 +148,7 @@ public class TrajectoryConfig {
/**
* Returns the starting velocity of the trajectory.
*
* @return The starting velocity of the trajectory.
* @return The starting velocity of the trajectory in m/s.
*/
public double getEndVelocity() {
return m_endVelocity;
@@ -158,11 +157,11 @@ public class TrajectoryConfig {
/**
* Sets the end velocity of the trajectory.
*
* @param endVelocityMetersPerSecond The end velocity of the trajectory.
* @param endVelocity The end velocity of the trajectory in m/s.
* @return Instance of the current config object.
*/
public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) {
m_endVelocity = endVelocityMetersPerSecond;
public TrajectoryConfig setEndVelocity(double endVelocity) {
m_endVelocity = endVelocity;
return this;
}
@@ -179,7 +178,7 @@ public class TrajectoryConfig {
/**
* Returns the maximum velocity of the trajectory.
*
* @return The maximum velocity of the trajectory.
* @return The maximum velocity of the trajectory in m/s.
*/
public double getMaxVelocity() {
return m_maxVelocity;
@@ -188,7 +187,7 @@ public class TrajectoryConfig {
/**
* Returns the maximum acceleration of the trajectory.
*
* @return The maximum acceleration of the trajectory.
* @return The maximum acceleration of the trajectory in m/s².
*/
public double getMaxAcceleration() {
return m_maxAcceleration;

12
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
View File

@@ -91,8 +91,8 @@ public final class TrajectoryGenerator {
// Change the points back to their original orientation.
if (config.isReversed()) {
for (var point : points) {
point.poseMeters = point.poseMeters.plus(kFlip);
point.curvatureRadPerMeter *= -1;
point.pose = point.pose.plus(kFlip);
point.curvature *= -1;
}
}
@@ -167,8 +167,8 @@ public final class TrajectoryGenerator {
// Change the points back to their original orientation.
if (config.isReversed()) {
for (var point : points) {
point.poseMeters = point.poseMeters.plus(kFlip);
point.curvatureRadPerMeter *= -1;
point.pose = point.pose.plus(kFlip);
point.curvature *= -1;
}
}
@@ -217,8 +217,8 @@ public final class TrajectoryGenerator {
// Change the points back to their original orientation.
if (config.isReversed()) {
for (var point : points) {
point.poseMeters = point.poseMeters.plus(kFlip);
point.curvatureRadPerMeter *= -1;
point.pose = point.pose.plus(kFlip);
point.curvature *= -1;
}
}

182
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
View File

@@ -46,10 +46,10 @@ public final class TrajectoryParameterizer {
*
* @param points Reference to the spline points.
* @param constraints A vector of various velocity and acceleration. constraints.
* @param startVelocityMetersPerSecond The start velocity for the trajectory.
* @param endVelocityMetersPerSecond The end velocity for the trajectory.
* @param maxVelocityMetersPerSecond The max velocity for the trajectory.
* @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
* @param startVelocity The start velocity for the trajectory in m/s.
* @param endVelocity The end velocity for the trajectory in m/s.
* @param maxVelocity The max velocity for the trajectory in m/s.
* @param maxAcceleration The max acceleration for the trajectory in m/s².
* @param reversed Whether the robot should move backwards. Note that the robot will still move
* from a -&gt; b -&gt; ... -&gt; z as defined in the waypoints.
* @return The trajectory.
@@ -57,19 +57,14 @@ public final class TrajectoryParameterizer {
public static Trajectory timeParameterizeTrajectory(
List<PoseWithCurvature> points,
List<TrajectoryConstraint> constraints,
double startVelocityMetersPerSecond,
double endVelocityMetersPerSecond,
double maxVelocityMetersPerSecond,
double maxAccelerationMetersPerSecondSq,
double startVelocity,
double endVelocity,
double maxVelocity,
double maxAcceleration,
boolean reversed) {
var constrainedStates = new ArrayList<ConstrainedState>(points.size());
var predecessor =
new ConstrainedState(
points.get(0),
0,
startVelocityMetersPerSecond,
-maxAccelerationMetersPerSecondSq,
maxAccelerationMetersPerSecondSq);
new ConstrainedState(points.get(0), 0, startVelocity, -maxAcceleration, maxAcceleration);
// Forward pass
for (int i = 0; i < points.size(); i++) {
@@ -81,36 +76,36 @@ public final class TrajectoryParameterizer {
double ds =
constrainedState
.pose
.poseMeters
.pose
.getTranslation()
.getDistance(predecessor.pose.poseMeters.getTranslation());
constrainedState.distanceMeters = predecessor.distanceMeters + ds;
.getDistance(predecessor.pose.pose.getTranslation());
constrainedState.distance = predecessor.distance + ds;
// We may need to iterate to find the maximum end velocity and common
// acceleration, since acceleration limits may be a function of velocity.
while (true) {
// Enforce global max velocity and max reachable velocity by global
// acceleration limit. v_f = √(v_i² + 2ad).
constrainedState.maxVelocityMetersPerSecond =
constrainedState.maxVelocity =
Math.min(
maxVelocityMetersPerSecond,
maxVelocity,
Math.sqrt(
predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond
+ predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0));
predecessor.maxVelocity * predecessor.maxVelocity
+ predecessor.maxAcceleration * ds * 2.0));
constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq;
constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
constrainedState.minAcceleration = -maxAcceleration;
constrainedState.maxAcceleration = maxAcceleration;
// At this point, the constrained state is fully constructed apart from
// all the custom-defined user constraints.
for (final var constraint : constraints) {
constrainedState.maxVelocityMetersPerSecond =
constrainedState.maxVelocity =
Math.min(
constrainedState.maxVelocityMetersPerSecond,
constraint.getMaxVelocityMetersPerSecond(
constrainedState.pose.poseMeters,
constrainedState.pose.curvatureRadPerMeter,
constrainedState.maxVelocityMetersPerSecond));
constrainedState.maxVelocity,
constraint.getMaxVelocity(
constrainedState.pose.pose,
constrainedState.pose.curvature,
constrainedState.maxVelocity));
}
// Now enforce all acceleration limits.
@@ -124,22 +119,19 @@ public final class TrajectoryParameterizer {
// acceleration that we applied, then we need to reduce the max
// acceleration of the predecessor and try again.
double actualAcceleration =
(constrainedState.maxVelocityMetersPerSecond
* constrainedState.maxVelocityMetersPerSecond
- predecessor.maxVelocityMetersPerSecond
* predecessor.maxVelocityMetersPerSecond)
(constrainedState.maxVelocity * constrainedState.maxVelocity
- predecessor.maxVelocity * predecessor.maxVelocity)
/ (ds * 2.0);
// If we violate the max acceleration constraint, let's modify the
// predecessor.
if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) {
predecessor.maxAccelerationMetersPerSecondSq =
constrainedState.maxAccelerationMetersPerSecondSq;
if (constrainedState.maxAcceleration < actualAcceleration - 1E-6) {
predecessor.maxAcceleration = constrainedState.maxAcceleration;
} else {
// Constrain the predecessor's max acceleration to the current
// acceleration.
if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) {
predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration;
if (actualAcceleration > predecessor.minAcceleration) {
predecessor.maxAcceleration = actualAcceleration;
}
// If the actual acceleration is less than the predecessor's min
// acceleration, it will be repaired in the backward pass.
@@ -152,30 +144,30 @@ public final class TrajectoryParameterizer {
var successor =
new ConstrainedState(
points.get(points.size() - 1),
constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
endVelocityMetersPerSecond,
-maxAccelerationMetersPerSecondSq,
maxAccelerationMetersPerSecondSq);
constrainedStates.get(constrainedStates.size() - 1).distance,
endVelocity,
-maxAcceleration,
maxAcceleration);
// Backward pass
for (int i = points.size() - 1; i >= 0; i--) {
var constrainedState = constrainedStates.get(i);
double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative
double ds = constrainedState.distance - successor.distance; // negative
while (true) {
// Enforce max velocity limit (reverse)
// v_f = √(v_i² + 2ad), where v_i = successor.
double newMaxVelocity =
Math.sqrt(
successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
+ successor.minAccelerationMetersPerSecondSq * ds * 2.0);
successor.maxVelocity * successor.maxVelocity
+ successor.minAcceleration * ds * 2.0);
// No more limits to impose! This state can be finalized.
if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) {
if (newMaxVelocity >= constrainedState.maxVelocity) {
break;
}
constrainedState.maxVelocityMetersPerSecond = newMaxVelocity;
constrainedState.maxVelocity = newMaxVelocity;
// Check all acceleration constraints with the new max velocity.
enforceAccelerationLimits(reversed, constraints, constrainedState);
@@ -188,16 +180,14 @@ public final class TrajectoryParameterizer {
// acceleration, then we need to lower the min acceleration of the
// successor and try again.
double actualAcceleration =
(constrainedState.maxVelocityMetersPerSecond
* constrainedState.maxVelocityMetersPerSecond
- successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
(constrainedState.maxVelocity * constrainedState.maxVelocity
- successor.maxVelocity * successor.maxVelocity)
/ (ds * 2.0);
if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) {
successor.minAccelerationMetersPerSecondSq =
constrainedState.minAccelerationMetersPerSecondSq;
if (constrainedState.minAcceleration > actualAcceleration + 1E-6) {
successor.minAcceleration = constrainedState.minAcceleration;
} else {
successor.minAccelerationMetersPerSecondSq = actualAcceleration;
successor.minAcceleration = actualAcceleration;
break;
}
}
@@ -207,52 +197,49 @@ public final class TrajectoryParameterizer {
// Now we can integrate the constrained states forward in time to obtain our
// trajectory states.
var states = new ArrayList<Trajectory.State>(points.size());
double timeSeconds = 0.0;
double distanceMeters = 0.0;
double velocityMetersPerSecond = 0.0;
double time = 0.0;
double distance = 0.0;
double velocity = 0.0;
for (int i = 0; i < constrainedStates.size(); i++) {
final var state = constrainedStates.get(i);
// Calculate the change in position between the current state and the previous
// state.
double ds = state.distanceMeters - distanceMeters;
double ds = state.distance - distance;
// Calculate the acceleration between the current state and the previous
// state.
double accel =
(state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
- velocityMetersPerSecond * velocityMetersPerSecond)
/ (ds * 2);
double accel = (state.maxVelocity * state.maxVelocity - velocity * velocity) / (ds * 2);
// Calculate dt
double dt = 0.0;
if (i > 0) {
states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel;
states.get(i - 1).acceleration = reversed ? -accel : accel;
if (Math.abs(accel) > 1E-6) {
// v_f = v_0 + a * t
dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel;
} else if (Math.abs(velocityMetersPerSecond) > 1E-6) {
dt = (state.maxVelocity - velocity) / accel;
} else if (Math.abs(velocity) > 1E-6) {
// delta_x = v * t
dt = ds / velocityMetersPerSecond;
dt = ds / velocity;
} else {
throw new TrajectoryGenerationException(
"Something went wrong at iteration " + i + " of time parameterization.");
}
}
velocityMetersPerSecond = state.maxVelocityMetersPerSecond;
distanceMeters = state.distanceMeters;
velocity = state.maxVelocity;
distance = state.distance;
timeSeconds += dt;
time += dt;
states.add(
new Trajectory.State(
timeSeconds,
reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
time,
reversed ? -velocity : velocity,
reversed ? -accel : accel,
state.pose.poseMeters,
state.pose.curvatureRadPerMeter));
state.pose.pose,
state.pose.curvature));
}
return new Trajectory(states);
@@ -263,51 +250,44 @@ public final class TrajectoryParameterizer {
for (final var constraint : constraints) {
double factor = reverse ? -1.0 : 1.0;
final var minMaxAccel =
constraint.getMinMaxAccelerationMetersPerSecondSq(
state.pose.poseMeters,
state.pose.curvatureRadPerMeter,
state.maxVelocityMetersPerSecond * factor);
constraint.getMinMaxAcceleration(
state.pose.pose, state.pose.curvature, state.maxVelocity * factor);
if (minMaxAccel.minAccelerationMetersPerSecondSq
> minMaxAccel.maxAccelerationMetersPerSecondSq) {
if (minMaxAccel.minAcceleration > minMaxAccel.maxAcceleration) {
throw new TrajectoryGenerationException(
"Infeasible trajectory constraint: " + constraint.getClass().getName() + "\n");
}
state.minAccelerationMetersPerSecondSq =
state.minAcceleration =
Math.max(
state.minAccelerationMetersPerSecondSq,
reverse
? -minMaxAccel.maxAccelerationMetersPerSecondSq
: minMaxAccel.minAccelerationMetersPerSecondSq);
state.minAcceleration,
reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration);
state.maxAccelerationMetersPerSecondSq =
state.maxAcceleration =
Math.min(
state.maxAccelerationMetersPerSecondSq,
reverse
? -minMaxAccel.minAccelerationMetersPerSecondSq
: minMaxAccel.maxAccelerationMetersPerSecondSq);
state.maxAcceleration,
reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration);
}
}
private static class ConstrainedState {
PoseWithCurvature pose;
double distanceMeters;
double maxVelocityMetersPerSecond;
double minAccelerationMetersPerSecondSq;
double maxAccelerationMetersPerSecondSq;
double distance;
double maxVelocity;
double minAcceleration;
double maxAcceleration;
ConstrainedState(
PoseWithCurvature pose,
double distanceMeters,
double maxVelocityMetersPerSecond,
double minAccelerationMetersPerSecondSq,
double maxAccelerationMetersPerSecondSq) {
double distance,
double maxVelocity,
double minAcceleration,
double maxAcceleration) {
this.pose = pose;
this.distanceMeters = distanceMeters;
this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond;
this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
this.distance = distance;
this.maxVelocity = maxVelocity;
this.minAcceleration = minAcceleration;
this.maxAcceleration = maxAcceleration;
}
ConstrainedState() {

14
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
View File

@@ -58,13 +58,13 @@ public final class TrajectoryUtil {
for (int i = 0; i < trajectory.getStates().size() * 7; i += 7) {
var state = trajectory.getStates().get(i / 7);
elements[i] = state.timeSeconds;
elements[i + 1] = state.velocityMetersPerSecond;
elements[i + 2] = state.accelerationMetersPerSecondSq;
elements[i + 3] = state.poseMeters.getX();
elements[i + 4] = state.poseMeters.getY();
elements[i + 5] = state.poseMeters.getRotation().getRadians();
elements[i + 6] = state.curvatureRadPerMeter;
elements[i] = state.time;
elements[i + 1] = state.velocity;
elements[i + 2] = state.acceleration;
elements[i + 3] = state.pose.getX();
elements[i + 4] = state.pose.getY();
elements[i + 5] = state.pose.getRotation().getRadians();
elements[i + 6] = state.curvature;
}
return elements;
}

31
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java
View File

@@ -15,29 +15,28 @@ import edu.wpi.first.math.geometry.Pose2d;
* around tight turns, making it easier to track trajectories with sharp turns.
*/
public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
private final double m_maxCentripetalAccelerationMetersPerSecondSq;
private final double m_maxCentripetalAcceleration;
/**
* Constructs a centripetal acceleration constraint.
*
* @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration.
* @param maxCentripetalAcceleration The max centripetal acceleration in m/s².
*/
public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) {
m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq;
public CentripetalAccelerationConstraint(double maxCentripetalAcceleration) {
m_maxCentripetalAcceleration = maxCentripetalAcceleration;
}
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The velocity at the current point in the trajectory before constraints are
* applied in m/s.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
// ac = v²/r
// k (curvature) = 1/r
@@ -45,22 +44,20 @@ public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
// ac/k = v²
// v = √(ac/k)
return Math.sqrt(
m_maxCentripetalAccelerationMetersPerSecondSq / Math.abs(curvatureRadPerMeter));
return Math.sqrt(m_maxCentripetalAcceleration / Math.abs(curvature));
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The speed at the current point in the trajectory in m/s.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
// The acceleration of the robot has no impact on the centripetal acceleration
// of the robot.
return new MinMax();

36
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
View File

@@ -14,58 +14,54 @@ import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
* drivetrain stay below a certain limit.
*/
public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeedMetersPerSecond;
private final double m_maxSpeed;
private final DifferentialDriveKinematics m_kinematics;
/**
* Constructs a differential drive dynamics constraint.
*
* @param kinematics A kinematics component describing the drive geometry.
* @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
* @param maxSpeed The max speed that a side of the robot can travel at in m/s.
*/
public DifferentialDriveKinematicsConstraint(
final DifferentialDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
final DifferentialDriveKinematics kinematics, double maxSpeed) {
m_maxSpeed = maxSpeed;
m_kinematics = kinematics;
}
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The velocity at the current point in the trajectory before constraints are
* applied in m/s.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
// Create an object to represent the current chassis speeds.
var chassisSpeeds =
new ChassisSpeeds(
velocityMetersPerSecond, 0, velocityMetersPerSecond * curvatureRadPerMeter);
var chassisSpeeds = new ChassisSpeeds(velocity, 0, velocity * curvature);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
wheelSpeeds.desaturate(m_maxSpeedMetersPerSecond);
wheelSpeeds.desaturate(m_maxSpeed);
// Return the new linear chassis speed.
return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond;
return m_kinematics.toChassisSpeeds(wheelSpeeds).vx;
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The speed at the current point in the trajectory in m/s.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
return new MinMax();
}
}

42
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java
View File

@@ -43,23 +43,17 @@ public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint
}
@Override
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
return Double.POSITIVE_INFINITY;
}
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
var wheelSpeeds =
m_kinematics.toWheelSpeeds(
new ChassisSpeeds(
velocityMetersPerSecond, 0, velocityMetersPerSecond * curvatureRadPerMeter));
m_kinematics.toWheelSpeeds(new ChassisSpeeds(velocity, 0, velocity * curvature));
double maxWheelSpeed =
Math.max(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
double minWheelSpeed =
Math.min(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
double maxWheelSpeed = Math.max(wheelSpeeds.left, wheelSpeeds.right);
double minWheelSpeed = Math.min(wheelSpeeds.left, wheelSpeeds.right);
// Calculate maximum/minimum possible accelerations from motor dynamics
// and max/min wheel speeds
@@ -87,28 +81,18 @@ public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint
double maxChassisAcceleration;
double minChassisAcceleration;
if (velocityMetersPerSecond == 0) {
if (velocity == 0) {
maxChassisAcceleration =
maxWheelAcceleration
/ (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
maxWheelAcceleration / (1 + m_kinematics.trackwidth * Math.abs(curvature) / 2);
minChassisAcceleration =
minWheelAcceleration
/ (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
minWheelAcceleration / (1 + m_kinematics.trackwidth * Math.abs(curvature) / 2);
} else {
maxChassisAcceleration =
maxWheelAcceleration
/ (1
+ m_kinematics.trackWidthMeters
* Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond)
/ 2);
/ (1 + m_kinematics.trackwidth * Math.abs(curvature) * Math.signum(velocity) / 2);
minChassisAcceleration =
minWheelAcceleration
/ (1
- m_kinematics.trackWidthMeters
* Math.abs(curvatureRadPerMeter)
* Math.signum(velocityMetersPerSecond)
/ 2);
/ (1 - m_kinematics.trackwidth * Math.abs(curvature) * Math.signum(velocity) / 2);
}
// When turning about a point inside the wheelbase (i.e. radius less than half
@@ -116,10 +100,10 @@ public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint
// the same. The formula above changes sign for the inner wheel when this happens.
// We can accurately account for this by simply negating the inner wheel.
if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) {
if (velocityMetersPerSecond > 0) {
if ((m_kinematics.trackwidth / 2) > (1 / Math.abs(curvature))) {
if (velocity > 0) {
minChassisAcceleration = -minChassisAcceleration;
} else if (velocityMetersPerSecond < 0) {
} else if (velocity < 0) {
maxChassisAcceleration = -maxChassisAcceleration;
}
}

20
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java
View File

@@ -18,8 +18,8 @@ public class EllipticalRegionConstraint implements TrajectoryConstraint {
* Constructs a new EllipticalRegionConstraint.
*
* @param center The center of the ellipse in which to enforce the constraint.
* @param xWidth The width of the ellipse in which to enforce the constraint.
* @param yWidth The height of the ellipse in which to enforce the constraint.
* @param xWidth The width of the ellipse in which to enforce the constraint in meters.
* @param yWidth The height of the ellipse in which to enforce the constraint in meters.
* @param rotation The rotation to apply to all radii around the origin.
* @param constraint The constraint to enforce when the robot is within the region.
* @deprecated Use constructor taking Ellipse2d instead.
@@ -46,22 +46,18 @@ public class EllipticalRegionConstraint implements TrajectoryConstraint {
}
@Override
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
if (m_ellipse.contains(poseMeters.getTranslation())) {
return m_constraint.getMaxVelocityMetersPerSecond(
poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
if (m_ellipse.contains(pose.getTranslation())) {
return m_constraint.getMaxVelocity(pose, curvature, velocity);
} else {
return Double.POSITIVE_INFINITY;
}
}
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
if (m_ellipse.contains(poseMeters.getTranslation())) {
return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
if (m_ellipse.contains(pose.getTranslation())) {
return m_constraint.getMinMaxAcceleration(pose, curvature, velocity);
} else {
return new MinMax();
}

13
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MaxVelocityConstraint.java
View File

@@ -17,21 +17,20 @@ public class MaxVelocityConstraint implements TrajectoryConstraint {
/**
* Constructs a new MaxVelocityConstraint.
*
* @param maxVelocityMetersPerSecond The max velocity.
* @param maxVelocity The max velocity in m/s.
*/
public MaxVelocityConstraint(double maxVelocityMetersPerSecond) {
m_maxVelocity = maxVelocityMetersPerSecond;
public MaxVelocityConstraint(double maxVelocity) {
m_maxVelocity = maxVelocity;
}
@Override
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
return m_maxVelocity;
}
@Override
public TrajectoryConstraint.MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public TrajectoryConstraint.MinMax getMinMaxAcceleration(
Pose2d pose, double curvature, double velocity) {
return new MinMax();
}
}

40
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java
View File

@@ -14,66 +14,62 @@ import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
* drivetrain stay below a certain limit.
*/
public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeedMetersPerSecond;
private final double m_maxSpeed;
private final MecanumDriveKinematics m_kinematics;
/**
* Constructs a mecanum drive kinematics constraint.
*
* @param kinematics Mecanum drive kinematics.
* @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
* @param maxSpeed The max speed that a side of the robot can travel at in m/s.
*/
public MecanumDriveKinematicsConstraint(
final MecanumDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
final MecanumDriveKinematics kinematics, double maxSpeed) {
m_maxSpeed = maxSpeed;
m_kinematics = kinematics;
}
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The velocity at the current point in the trajectory before constraints are
* applied in m/s.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
// Represents the velocity of the chassis in the x direction
var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
var xdVelocity = velocity * pose.getRotation().getCos();
// Represents the velocity of the chassis in the y direction
var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
var ydVelocity = velocity * pose.getRotation().getSin();
// Create an object to represent the current chassis speeds.
var chassisSpeeds =
new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
var chassisSpeeds = new ChassisSpeeds(xdVelocity, ydVelocity, velocity * curvature);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds =
m_kinematics.toWheelSpeeds(chassisSpeeds).desaturate(m_maxSpeedMetersPerSecond);
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds).desaturate(m_maxSpeed);
// Convert normalized wheel speeds back to chassis speeds
var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
// Return the new linear chassis speed.
return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
return Math.hypot(normSpeeds.vx, normSpeeds.vy);
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The speed at the current point in the trajectory in m/s.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
return new MinMax();
}
}

16
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java
View File

@@ -41,22 +41,18 @@ public class RectangularRegionConstraint implements TrajectoryConstraint {
}
@Override
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
if (m_rectangle.contains(poseMeters.getTranslation())) {
return m_constraint.getMaxVelocityMetersPerSecond(
poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
if (m_rectangle.contains(pose.getTranslation())) {
return m_constraint.getMaxVelocity(pose, curvature, velocity);
} else {
return Double.POSITIVE_INFINITY;
}
}
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
if (m_rectangle.contains(poseMeters.getTranslation())) {
return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
if (m_rectangle.contains(pose.getTranslation())) {
return m_constraint.getMinMaxAcceleration(pose, curvature, velocity);
} else {
return new MinMax();
}

40
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
View File

@@ -14,66 +14,62 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
* stay below a certain limit.
*/
public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
private final double m_maxSpeedMetersPerSecond;
private final double m_maxSpeed;
private final SwerveDriveKinematics m_kinematics;
/**
* Constructs a swerve drive kinematics constraint.
*
* @param kinematics Swerve drive kinematics.
* @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
* @param maxSpeed The max speed that a side of the robot can travel at in m/s.
*/
public SwerveDriveKinematicsConstraint(
final SwerveDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics, double maxSpeed) {
m_maxSpeed = maxSpeed;
m_kinematics = kinematics;
}
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The velocity at the current point in the trajectory before constraints are
* applied in m/s.
* @return The absolute maximum velocity.
*/
@Override
public double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
// Represents the velocity of the chassis in the x direction
var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
var xdVelocity = velocity * pose.getRotation().getCos();
// Represents the velocity of the chassis in the y direction
var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
var ydVelocity = velocity * pose.getRotation().getSin();
// Create an object to represent the current chassis speeds.
var chassisSpeeds =
new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
var chassisSpeeds = new ChassisSpeeds(xdVelocity, ydVelocity, velocity * curvature);
// Get the wheel speeds and normalize them to within the max velocity.
var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
SwerveDriveKinematics.desaturateWheelSpeeds(wheelSpeeds, m_maxSpeed);
// Convert normalized wheel speeds back to chassis speeds
var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
// Return the new linear chassis speed.
return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
return Math.hypot(normSpeeds.vx, normSpeeds.vy);
}
/**
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory in rad/m.
* @param velocity The speed at the current point in the trajectory in m/s.
* @return The min and max acceleration bounds.
*/
@Override
public MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
return new MinMax();
}
}

35
wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
View File

@@ -14,45 +14,42 @@ public interface TrajectoryConstraint {
/**
* Returns the max velocity given the current pose and curvature.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The velocity at the current point in the trajectory before
* constraints are applied.
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory rad/m.
* @param velocity The velocity at the current point in the trajectory before constraints are
* applied in m/s.
* @return The absolute maximum velocity.
*/
double getMaxVelocityMetersPerSecond(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond);
double getMaxVelocity(Pose2d pose, double curvature, double velocity);
/**
* Returns the minimum and maximum allowable acceleration for the trajectory given pose,
* curvature, and speed.
*
* @param poseMeters The pose at the current point in the trajectory.
* @param curvatureRadPerMeter The curvature at the current point in the trajectory.
* @param velocityMetersPerSecond The speed at the current point in the trajectory.
* @param pose The pose at the current point in the trajectory.
* @param curvature The curvature at the current point in the trajectory rad/m.
* @param velocity The speed at the current point in the trajectory in m/s.
* @return The min and max acceleration bounds.
*/
MinMax getMinMaxAccelerationMetersPerSecondSq(
Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond);
MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity);
/** Represents a minimum and maximum acceleration. */
class MinMax {
/** The minimum acceleration. */
public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
public double minAcceleration = -Double.MAX_VALUE;
/** The maximum acceleration. */
public double maxAccelerationMetersPerSecondSq = Double.MAX_VALUE;
public double maxAcceleration = Double.MAX_VALUE;
/**
* Constructs a MinMax.
*
* @param minAccelerationMetersPerSecondSq The minimum acceleration.
* @param maxAccelerationMetersPerSecondSq The maximum acceleration.
* @param minAcceleration The minimum acceleration in m/s².
* @param maxAcceleration The maximum acceleration in m/s².
*/
public MinMax(
double minAccelerationMetersPerSecondSq, double maxAccelerationMetersPerSecondSq) {
this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
public MinMax(double minAcceleration, double maxAcceleration) {
this.minAcceleration = minAcceleration;
this.maxAcceleration = maxAcceleration;
}
/** Constructs a MinMax with default values. */

10
wpimath/src/main/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProto.java
View File

@@ -38,10 +38,10 @@ public class TrajectoryStateProto implements Protobuf<Trajectory.State, Protobuf
@Override
public void pack(ProtobufTrajectoryState msg, Trajectory.State value) {
msg.setTime(value.timeSeconds);
msg.setVelocity(value.velocityMetersPerSecond);
msg.setAcceleration(value.accelerationMetersPerSecondSq);
Pose2d.proto.pack(msg.getMutablePose(), value.poseMeters);
msg.setCurvature(value.curvatureRadPerMeter);
msg.setTime(value.time);
msg.setVelocity(value.velocity);
msg.setAcceleration(value.acceleration);
Pose2d.proto.pack(msg.getMutablePose(), value.pose);
msg.setCurvature(value.curvature);
}
}