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

[wpimath] Replace Speeds with Velocities (#8479)

Browse Source 
I left "free speed" alone since that's the technical term for it. In
general, velocity is a vector quantity, and speed is a magnitude (i.e.,
a strictly positive value).

This PR also replaces the speed verbiage in MotorController with duty
cycle.

Fixes #8423.
This commit is contained in:
Tyler Veness
2026-03-06 14:19:15 -08:00
committed by GitHub
parent 1e39f39128
commit 9bd9656871
594 changed files with 8073 additions and 7875 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
wpimath/src/test/java/org/wpilib/math/estimator/DifferentialDrivePoseEstimator3dTest.java
View File

@@ -21,7 +21,7 @@ import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.DifferentialDriveKinematics;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.trajectory.Trajectory;
@@ -58,7 +58,7 @@ class DifferentialDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -109,7 +109,7 @@ class DifferentialDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -125,7 +125,7 @@ class DifferentialDrivePoseEstimator3dTest {
final DifferentialDriveKinematics kinematics,
final DifferentialDrivePoseEstimator3d estimator,
final Trajectory trajectory,
final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
final Function<Trajectory.State, ChassisVelocities> chassisVelocitiesGenerator,
final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
final Pose2d startingPose,
final Pose2d endingPose,
@@ -171,12 +171,12 @@ class DifferentialDrivePoseEstimator3dTest {
estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey());
}
var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
var chassisVelocities = chassisVelocitiesGenerator.apply(groundTruthState);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
var wheelVelocities = kinematics.toWheelVelocities(chassisVelocities);
leftDistance += wheelSpeeds.left * dt;
rightDistance += wheelSpeeds.right * dt;
leftDistance += wheelVelocities.left * dt;
rightDistance += wheelVelocities.right * dt;
var xHat =
estimator.updateWithTime(

16
wpimath/src/test/java/org/wpilib/math/estimator/DifferentialDrivePoseEstimatorTest.java
View File

@@ -18,7 +18,7 @@ import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.DifferentialDriveKinematics;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.trajectory.Trajectory;
@@ -55,7 +55,7 @@ class DifferentialDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -106,7 +106,7 @@ class DifferentialDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -122,7 +122,7 @@ class DifferentialDrivePoseEstimatorTest {
final DifferentialDriveKinematics kinematics,
final DifferentialDrivePoseEstimator estimator,
final Trajectory trajectory,
final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
final Function<Trajectory.State, ChassisVelocities> chassisVelocitiesGenerator,
final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
final Pose2d startingPose,
final Pose2d endingPose,
@@ -167,12 +167,12 @@ class DifferentialDrivePoseEstimatorTest {
estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
}
var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
var chassisVelocities = chassisVelocitiesGenerator.apply(groundTruthState);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
var wheelVelocities = kinematics.toWheelVelocities(chassisVelocities);
leftDistance += wheelSpeeds.left * dt;
rightDistance += wheelSpeeds.right * dt;
leftDistance += wheelVelocities.left * dt;
rightDistance += wheelVelocities.right * dt;
var xHat =
estimator.updateWithTime(

20
wpimath/src/test/java/org/wpilib/math/estimator/MecanumDrivePoseEstimator3dTest.java
View File

@@ -21,7 +21,7 @@ import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.MecanumDriveKinematics;
import org.wpilib.math.kinematics.MecanumDriveWheelPositions;
import org.wpilib.math.linalg.VecBuilder;
@@ -64,7 +64,7 @@ class MecanumDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -119,7 +119,7 @@ class MecanumDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -135,7 +135,7 @@ class MecanumDrivePoseEstimator3dTest {
final MecanumDriveKinematics kinematics,
final MecanumDrivePoseEstimator3d estimator,
final Trajectory trajectory,
final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
final Function<Trajectory.State, ChassisVelocities> chassisVelocitiesGenerator,
final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
final Pose2d startingPose,
final Pose2d endingPose,
@@ -179,14 +179,14 @@ class MecanumDrivePoseEstimator3dTest {
estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey());
}
var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
var chassisVelocities = chassisVelocitiesGenerator.apply(groundTruthState);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
var wheelVelocities = kinematics.toWheelVelocities(chassisVelocities);
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
var xHat =
estimator.updateWithTime(

20
wpimath/src/test/java/org/wpilib/math/estimator/MecanumDrivePoseEstimatorTest.java
View File

@@ -18,7 +18,7 @@ import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.MecanumDriveKinematics;
import org.wpilib.math.kinematics.MecanumDriveWheelPositions;
import org.wpilib.math.linalg.VecBuilder;
@@ -61,7 +61,7 @@ class MecanumDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -116,7 +116,7 @@ class MecanumDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -132,7 +132,7 @@ class MecanumDrivePoseEstimatorTest {
final MecanumDriveKinematics kinematics,
final MecanumDrivePoseEstimator estimator,
final Trajectory trajectory,
final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
final Function<Trajectory.State, ChassisVelocities> chassisVelocitiesGenerator,
final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
final Pose2d startingPose,
final Pose2d endingPose,
@@ -176,14 +176,14 @@ class MecanumDrivePoseEstimatorTest {
estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
}
var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
var chassisVelocities = chassisVelocitiesGenerator.apply(groundTruthState);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
var wheelVelocities = kinematics.toWheelVelocities(chassisVelocities);
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
var xHat =
estimator.updateWithTime(

19
wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimator3dTest.java
View File

@@ -21,7 +21,7 @@ import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.SwerveDriveKinematics;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.linalg.VecBuilder;
@@ -70,7 +70,7 @@ class SwerveDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -129,7 +129,7 @@ class SwerveDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -145,7 +145,7 @@ class SwerveDrivePoseEstimator3dTest {
final SwerveDriveKinematics kinematics,
final SwerveDrivePoseEstimator3d estimator,
final Trajectory trajectory,
final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
final Function<Trajectory.State, ChassisVelocities> chassisVelocitiesGenerator,
final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
final Pose2d startingPose,
final Pose2d endingPose,
@@ -194,14 +194,15 @@ class SwerveDrivePoseEstimator3dTest {
estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey());
}
var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
var chassisVelocities = chassisVelocitiesGenerator.apply(groundTruthState);
var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds);
var moduleVelocities = kinematics.toSwerveModuleVelocities(chassisVelocities);
for (int i = 0; i < moduleStates.length; i++) {
positions[i].distance += moduleStates[i].speed * (1 - rand.nextGaussian() * 0.05) * dt;
for (int i = 0; i < moduleVelocities.length; i++) {
positions[i].distance +=
moduleVelocities[i].velocity * (1 - rand.nextGaussian() * 0.05) * dt;
positions[i].angle =
moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
moduleVelocities[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
}
var xHat =

19
wpimath/src/test/java/org/wpilib/math/estimator/SwerveDrivePoseEstimatorTest.java
View File

@@ -18,7 +18,7 @@ import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Transform2d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.SwerveDriveKinematics;
import org.wpilib.math.kinematics.SwerveModulePosition;
import org.wpilib.math.linalg.VecBuilder;
@@ -67,7 +67,7 @@ class SwerveDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -126,7 +126,7 @@ class SwerveDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> new ChassisVelocities(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
@@ -142,7 +142,7 @@ class SwerveDrivePoseEstimatorTest {
final SwerveDriveKinematics kinematics,
final SwerveDrivePoseEstimator estimator,
final Trajectory trajectory,
final Function<Trajectory.State, ChassisSpeeds> chassisSpeedsGenerator,
final Function<Trajectory.State, ChassisVelocities> chassisVelocitiesGenerator,
final Function<Trajectory.State, Pose2d> visionMeasurementGenerator,
final Pose2d startingPose,
final Pose2d endingPose,
@@ -191,14 +191,15 @@ class SwerveDrivePoseEstimatorTest {
estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey());
}
var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState);
var chassisVelocities = chassisVelocitiesGenerator.apply(groundTruthState);
var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds);
var moduleVelocities = kinematics.toSwerveModuleVelocities(chassisVelocities);
for (int i = 0; i < moduleStates.length; i++) {
positions[i].distance += moduleStates[i].speed * (1 - rand.nextGaussian() * 0.05) * dt;
for (int i = 0; i < moduleVelocities.length; i++) {
positions[i].distance +=
moduleVelocities[i].velocity * (1 - rand.nextGaussian() * 0.05) * dt;
positions[i].angle =
moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
moduleVelocities[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
}
var xHat =

162
wpimath/src/test/java/org/wpilib/math/kinematics/ChassisSpeedsTest.java
View File

@@ -1,162 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.wpilib.units.Units.InchesPerSecond;
import static org.wpilib.units.Units.RPM;
import org.junit.jupiter.api.Test;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Twist2d;
class ChassisSpeedsTest {
private static final double kEpsilon = 1E-9;
@Test
void testDiscretize() {
final var target = new ChassisSpeeds(1.0, 0.0, 0.5);
final var duration = 1.0;
final var dt = 0.01;
final var speeds = target.discretize(duration);
final var twist = new Twist2d(speeds.vx * dt, speeds.vy * dt, speeds.omega * dt);
var pose = Pose2d.kZero;
for (double time = 0; time < duration; time += dt) {
pose = pose.plus(twist.exp());
}
final var result = pose; // For lambda capture
assertAll(
() -> assertEquals(target.vx * duration, result.getX(), kEpsilon),
() -> assertEquals(target.vy * duration, result.getY(), kEpsilon),
() -> assertEquals(target.omega * duration, result.getRotation().getRadians(), kEpsilon));
}
@Test
void testMeasureConstructor() {
var vx = InchesPerSecond.of(14.52);
var vy = InchesPerSecond.of(0);
var omega = RPM.of(0.02);
var speeds = new ChassisSpeeds(vx, vy, omega);
assertAll(
() -> assertEquals(0.368808, speeds.vx, kEpsilon),
() -> assertEquals(0, speeds.vy, kEpsilon),
() -> assertEquals(0.002094395102, speeds.omega, kEpsilon));
}
@Test
void testToRobotRelative() {
final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5).toRobotRelative(Rotation2d.kCW_Pi_2);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(1.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.5, chassisSpeeds.omega, kEpsilon));
}
@Test
void testToFieldRelative() {
final var chassisSpeeds =
new ChassisSpeeds(1.0, 0.0, 0.5).toFieldRelative(Rotation2d.fromDegrees(45.0));
assertAll(
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vx, kEpsilon),
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.5, chassisSpeeds.omega, kEpsilon));
}
@Test
void testPlus() {
final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
final var right = new ChassisSpeeds(2.0, 1.5, 0.25);
final var chassisSpeeds = left.plus(right);
assertAll(
() -> assertEquals(3.0, chassisSpeeds.vx),
() -> assertEquals(2.0, chassisSpeeds.vy),
() -> assertEquals(1.0, chassisSpeeds.omega));
}
@Test
void testMinus() {
final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
final var right = new ChassisSpeeds(2.0, 0.5, 0.25);
final var chassisSpeeds = left.minus(right);
assertAll(
() -> assertEquals(-1.0, chassisSpeeds.vx),
() -> assertEquals(0.0, chassisSpeeds.vy),
() -> assertEquals(0.5, chassisSpeeds.omega));
}
@Test
void testUnaryMinus() {
final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).unaryMinus();
assertAll(
() -> assertEquals(-1.0, chassisSpeeds.vx),
() -> assertEquals(-0.5, chassisSpeeds.vy),
() -> assertEquals(-0.75, chassisSpeeds.omega));
}
@Test
void testMultiplication() {
final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).times(2.0);
assertAll(
() -> assertEquals(2.0, chassisSpeeds.vx),
() -> assertEquals(1.0, chassisSpeeds.vy),
() -> assertEquals(1.5, chassisSpeeds.omega));
}
@Test
void testDivision() {
final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).div(2.0);
assertAll(
() -> assertEquals(0.5, chassisSpeeds.vx),
() -> assertEquals(0.25, chassisSpeeds.vy),
() -> assertEquals(0.375, chassisSpeeds.omega));
}
@Test
void testInterpolation() {
var start = new ChassisSpeeds(0.0, 0.0, 0.0);
var end = new ChassisSpeeds(10.0, 20.0, 30.0);
var result = start.interpolate(end, 0.5);
assertAll(
() -> assertEquals(5.0, result.vx, kEpsilon),
() -> assertEquals(10.0, result.vy, kEpsilon),
() -> assertEquals(15.0, result.omega, kEpsilon));
}
@Test
void testInterpolationAtBounds() {
var start = new ChassisSpeeds(1.0, 2.0, 3.0);
var end = new ChassisSpeeds(4.0, 5.0, 6.0);
// Test t = 0 (should return start)
var resultStart = start.interpolate(end, 0.0);
assertAll(
() -> assertEquals(1.0, resultStart.vx, kEpsilon),
() -> assertEquals(2.0, resultStart.vy, kEpsilon),
() -> assertEquals(3.0, resultStart.omega, kEpsilon));
// Test t = 1 (should return end)
var resultEnd = start.interpolate(end, 1.0);
assertAll(
() -> assertEquals(4.0, resultEnd.vx, kEpsilon),
() -> assertEquals(5.0, resultEnd.vy, kEpsilon),
() -> assertEquals(6.0, resultEnd.omega, kEpsilon));
}
}

163
wpimath/src/test/java/org/wpilib/math/kinematics/ChassisVelocitiesTest.java Normal file
View File

@@ -0,0 +1,163 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.wpilib.units.Units.InchesPerSecond;
import static org.wpilib.units.Units.RPM;
import org.junit.jupiter.api.Test;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Twist2d;
class ChassisVelocitiesTest {
private static final double kEpsilon = 1E-9;
@Test
void testDiscretize() {
final var target = new ChassisVelocities(1.0, 0.0, 0.5);
final var duration = 1.0;
final var dt = 0.01;
final var velocities = target.discretize(duration);
final var twist = new Twist2d(velocities.vx * dt, velocities.vy * dt, velocities.omega * dt);
var pose = Pose2d.kZero;
for (double time = 0; time < duration; time += dt) {
pose = pose.plus(twist.exp());
}
final var result = pose; // For lambda capture
assertAll(
() -> assertEquals(target.vx * duration, result.getX(), kEpsilon),
() -> assertEquals(target.vy * duration, result.getY(), kEpsilon),
() -> assertEquals(target.omega * duration, result.getRotation().getRadians(), kEpsilon));
}
@Test
void testMeasureConstructor() {
var vx = InchesPerSecond.of(14.52);
var vy = InchesPerSecond.of(0);
var omega = RPM.of(0.02);
var velocities = new ChassisVelocities(vx, vy, omega);
assertAll(
() -> assertEquals(0.368808, velocities.vx, kEpsilon),
() -> assertEquals(0, velocities.vy, kEpsilon),
() -> assertEquals(0.002094395102, velocities.omega, kEpsilon));
}
@Test
void testToRobotRelative() {
final var chassisVelocities =
new ChassisVelocities(1.0, 0.0, 0.5).toRobotRelative(Rotation2d.kCW_Pi_2);
assertAll(
() -> assertEquals(0.0, chassisVelocities.vx, kEpsilon),
() -> assertEquals(1.0, chassisVelocities.vy, kEpsilon),
() -> assertEquals(0.5, chassisVelocities.omega, kEpsilon));
}
@Test
void testToFieldRelative() {
final var chassisVelocities =
new ChassisVelocities(1.0, 0.0, 0.5).toFieldRelative(Rotation2d.fromDegrees(45.0));
assertAll(
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisVelocities.vx, kEpsilon),
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisVelocities.vy, kEpsilon),
() -> assertEquals(0.5, chassisVelocities.omega, kEpsilon));
}
@Test
void testPlus() {
final var left = new ChassisVelocities(1.0, 0.5, 0.75);
final var right = new ChassisVelocities(2.0, 1.5, 0.25);
final var chassisVelocities = left.plus(right);
assertAll(
() -> assertEquals(3.0, chassisVelocities.vx),
() -> assertEquals(2.0, chassisVelocities.vy),
() -> assertEquals(1.0, chassisVelocities.omega));
}
@Test
void testMinus() {
final var left = new ChassisVelocities(1.0, 0.5, 0.75);
final var right = new ChassisVelocities(2.0, 0.5, 0.25);
final var chassisVelocities = left.minus(right);
assertAll(
() -> assertEquals(-1.0, chassisVelocities.vx),
() -> assertEquals(0.0, chassisVelocities.vy),
() -> assertEquals(0.5, chassisVelocities.omega));
}
@Test
void testUnaryMinus() {
final var chassisVelocities = (new ChassisVelocities(1.0, 0.5, 0.75)).unaryMinus();
assertAll(
() -> assertEquals(-1.0, chassisVelocities.vx),
() -> assertEquals(-0.5, chassisVelocities.vy),
() -> assertEquals(-0.75, chassisVelocities.omega));
}
@Test
void testMultiplication() {
final var chassisVelocities = (new ChassisVelocities(1.0, 0.5, 0.75)).times(2.0);
assertAll(
() -> assertEquals(2.0, chassisVelocities.vx),
() -> assertEquals(1.0, chassisVelocities.vy),
() -> assertEquals(1.5, chassisVelocities.omega));
}
@Test
void testDivision() {
final var chassisVelocities = (new ChassisVelocities(1.0, 0.5, 0.75)).div(2.0);
assertAll(
() -> assertEquals(0.5, chassisVelocities.vx),
() -> assertEquals(0.25, chassisVelocities.vy),
() -> assertEquals(0.375, chassisVelocities.omega));
}
@Test
void testInterpolation() {
var start = new ChassisVelocities(0.0, 0.0, 0.0);
var end = new ChassisVelocities(10.0, 20.0, 30.0);
var result = start.interpolate(end, 0.5);
assertAll(
() -> assertEquals(5.0, result.vx, kEpsilon),
() -> assertEquals(10.0, result.vy, kEpsilon),
() -> assertEquals(15.0, result.omega, kEpsilon));
}
@Test
void testInterpolationAtBounds() {
var start = new ChassisVelocities(1.0, 2.0, 3.0);
var end = new ChassisVelocities(4.0, 5.0, 6.0);
// Test t = 0 (should return start)
var resultStart = start.interpolate(end, 0.0);
assertAll(
() -> assertEquals(1.0, resultStart.vx, kEpsilon),
() -> assertEquals(2.0, resultStart.vy, kEpsilon),
() -> assertEquals(3.0, resultStart.omega, kEpsilon));
// Test t = 1 (should return end)
var resultEnd = start.interpolate(end, 1.0);
assertAll(
() -> assertEquals(4.0, resultEnd.vx, kEpsilon),
() -> assertEquals(5.0, resultEnd.vy, kEpsilon),
() -> assertEquals(6.0, resultEnd.omega, kEpsilon));
}
}

54
wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveKinematicsTest.java
View File

@@ -16,65 +16,65 @@ class DifferentialDriveKinematicsTest {
@Test
void testInverseKinematicsForZeros() {
var chassisSpeeds = new ChassisSpeeds();
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
var chassisVelocities = new ChassisVelocities();
var wheelVelocities = m_kinematics.toWheelVelocities(chassisVelocities);
assertAll(
() -> assertEquals(0.0, wheelSpeeds.left, kEpsilon),
() -> assertEquals(0.0, wheelSpeeds.right, kEpsilon));
() -> assertEquals(0.0, wheelVelocities.left, kEpsilon),
() -> assertEquals(0.0, wheelVelocities.right, kEpsilon));
}
@Test
void testForwardKinematicsForZeros() {
var wheelSpeeds = new DifferentialDriveWheelSpeeds();
var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
var wheelVelocities = new DifferentialDriveWheelVelocities();
var chassisVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon));
() -> assertEquals(0.0, chassisVelocities.vx, kEpsilon),
() -> assertEquals(0.0, chassisVelocities.vy, kEpsilon),
() -> assertEquals(0.0, chassisVelocities.omega, kEpsilon));
}
@Test
void testInverseKinematicsForStraightLine() {
var chassisSpeeds = new ChassisSpeeds(3, 0, 0);
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
var chassisVelocities = new ChassisVelocities(3, 0, 0);
var wheelVelocities = m_kinematics.toWheelVelocities(chassisVelocities);
assertAll(
() -> assertEquals(3.0, wheelSpeeds.left, kEpsilon),
() -> assertEquals(3.0, wheelSpeeds.right, kEpsilon));
() -> assertEquals(3.0, wheelVelocities.left, kEpsilon),
() -> assertEquals(3.0, wheelVelocities.right, kEpsilon));
}
@Test
void testForwardKinematicsForStraightLine() {
var wheelSpeeds = new DifferentialDriveWheelSpeeds(3, 3);
var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
var wheelVelocities = new DifferentialDriveWheelVelocities(3, 3);
var chassisVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
assertAll(
() -> assertEquals(3.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon));
() -> assertEquals(3.0, chassisVelocities.vx, kEpsilon),
() -> assertEquals(0.0, chassisVelocities.vy, kEpsilon),
() -> assertEquals(0.0, chassisVelocities.omega, kEpsilon));
}
@Test
void testInverseKinematicsForRotateInPlace() {
var chassisSpeeds = new ChassisSpeeds(0, 0, Math.PI);
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
var chassisVelocities = new ChassisVelocities(0, 0, Math.PI);
var wheelVelocities = m_kinematics.toWheelVelocities(chassisVelocities);
assertAll(
() -> assertEquals(-0.381 * Math.PI, wheelSpeeds.left, kEpsilon),
() -> assertEquals(+0.381 * Math.PI, wheelSpeeds.right, kEpsilon));
() -> assertEquals(-0.381 * Math.PI, wheelVelocities.left, kEpsilon),
() -> assertEquals(+0.381 * Math.PI, wheelVelocities.right, kEpsilon));
}
@Test
void testForwardKinematicsForRotateInPlace() {
var wheelSpeeds = new DifferentialDriveWheelSpeeds(+0.381 * Math.PI, -0.381 * Math.PI);
var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
var wheelVelocities = new DifferentialDriveWheelVelocities(+0.381 * Math.PI, -0.381 * Math.PI);
var chassisVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(-Math.PI, chassisSpeeds.omega, kEpsilon));
() -> assertEquals(0.0, chassisVelocities.vx, kEpsilon),
() -> assertEquals(0.0, chassisVelocities.vy, kEpsilon),
() -> assertEquals(-Math.PI, chassisVelocities.omega, kEpsilon));
}
@Test

39
wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveWheelSpeedsTest.java → wpimath/src/test/java/org/wpilib/math/kinematics/DifferentialDriveWheelVelocitiesTest.java
View File

@@ -9,57 +9,62 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class DifferentialDriveWheelSpeedsTest {
class DifferentialDriveWheelVelocitiesTest {
@Test
void testPlus() {
final var left = new DifferentialDriveWheelSpeeds(1.0, 0.5);
final var right = new DifferentialDriveWheelSpeeds(2.0, 1.5);
final var left = new DifferentialDriveWheelVelocities(1.0, 0.5);
final var right = new DifferentialDriveWheelVelocities(2.0, 1.5);
final var wheelSpeeds = left.plus(right);
final var wheelVelocities = left.plus(right);
assertAll(
() -> assertEquals(3.0, wheelSpeeds.left), () -> assertEquals(2.0, wheelSpeeds.right));
() -> assertEquals(3.0, wheelVelocities.left),
() -> assertEquals(2.0, wheelVelocities.right));
}
@Test
void testMinus() {
final var left = new DifferentialDriveWheelSpeeds(1.0, 0.5);
final var right = new DifferentialDriveWheelSpeeds(2.0, 0.5);
final var left = new DifferentialDriveWheelVelocities(1.0, 0.5);
final var right = new DifferentialDriveWheelVelocities(2.0, 0.5);
final var wheelSpeeds = left.minus(right);
final var wheelVelocities = left.minus(right);
assertAll(
() -> assertEquals(-1.0, wheelSpeeds.left), () -> assertEquals(0.0, wheelSpeeds.right));
() -> assertEquals(-1.0, wheelVelocities.left),
() -> assertEquals(0.0, wheelVelocities.right));
}
@Test
void testUnaryMinus() {
final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).unaryMinus();
final var wheelVelocities = new DifferentialDriveWheelVelocities(1.0, 0.5).unaryMinus();
assertAll(
() -> assertEquals(-1.0, wheelSpeeds.left), () -> assertEquals(-0.5, wheelSpeeds.right));
() -> assertEquals(-1.0, wheelVelocities.left),
() -> assertEquals(-0.5, wheelVelocities.right));
}
@Test
void testMultiplication() {
final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).times(2.0);
final var wheelVelocities = new DifferentialDriveWheelVelocities(1.0, 0.5).times(2.0);
assertAll(
() -> assertEquals(2.0, wheelSpeeds.left), () -> assertEquals(1.0, wheelSpeeds.right));
() -> assertEquals(2.0, wheelVelocities.left),
() -> assertEquals(1.0, wheelVelocities.right));
}
@Test
void testDivision() {
final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).div(2.0);
final var wheelVelocities = new DifferentialDriveWheelVelocities(1.0, 0.5).div(2.0);
assertAll(
() -> assertEquals(0.5, wheelSpeeds.left), () -> assertEquals(0.25, wheelSpeeds.right));
() -> assertEquals(0.5, wheelVelocities.left),
() -> assertEquals(0.25, wheelVelocities.right));
}
@Test
void testInterpolate() {
final var start = new DifferentialDriveWheelSpeeds(1.0, 2.0);
final var end = new DifferentialDriveWheelSpeeds(5.0, 6.0);
final var start = new DifferentialDriveWheelVelocities(1.0, 2.0);
final var end = new DifferentialDriveWheelVelocities(5.0, 6.0);
// Test interpolation at t=0 (should return start)
final var atStart = start.interpolate(end, 0.0);

155
wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveKinematicsTest.java
View File

@@ -23,25 +23,25 @@ class MecanumDriveKinematicsTest {
@Test
void testStraightLineInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
ChassisVelocities velocities = new ChassisVelocities(5, 0, 0);
var moduleVelocities = m_kinematics.toWheelVelocities(velocities);
assertAll(
() -> assertEquals(5.0, moduleStates.frontLeft, 0.1),
() -> assertEquals(5.0, moduleStates.frontRight, 0.1),
() -> assertEquals(5.0, moduleStates.rearLeft, 0.1),
() -> assertEquals(5.0, moduleStates.rearRight, 0.1));
() -> assertEquals(5.0, moduleVelocities.frontLeft, 0.1),
() -> assertEquals(5.0, moduleVelocities.frontRight, 0.1),
() -> assertEquals(5.0, moduleVelocities.rearLeft, 0.1),
() -> assertEquals(5.0, moduleVelocities.rearRight, 0.1));
}
@Test
void testStraightLineForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
var wheelVelocities = new MecanumDriveWheelVelocities(3.536, 3.536, 3.536, 3.536);
var moduleVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
assertAll(
() -> assertEquals(3.536, moduleStates.vx, 0.1),
() -> assertEquals(0, moduleStates.vy, 0.1),
() -> assertEquals(0, moduleStates.omega, 0.1));
() -> assertEquals(3.536, moduleVelocities.vx, 0.1),
() -> assertEquals(0, moduleVelocities.vy, 0.1),
() -> assertEquals(0, moduleVelocities.omega, 0.1));
}
@Test
@@ -57,25 +57,25 @@ class MecanumDriveKinematicsTest {
@Test
void testStrafeInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
ChassisVelocities velocities = new ChassisVelocities(0, 4, 0);
var moduleVelocities = m_kinematics.toWheelVelocities(velocities);
assertAll(
() -> assertEquals(-4.0, moduleStates.frontLeft, 0.1),
() -> assertEquals(4.0, moduleStates.frontRight, 0.1),
() -> assertEquals(4.0, moduleStates.rearLeft, 0.1),
() -> assertEquals(-4.0, moduleStates.rearRight, 0.1));
() -> assertEquals(-4.0, moduleVelocities.frontLeft, 0.1),
() -> assertEquals(4.0, moduleVelocities.frontRight, 0.1),
() -> assertEquals(4.0, moduleVelocities.rearLeft, 0.1),
() -> assertEquals(-4.0, moduleVelocities.rearRight, 0.1));
}
@Test
void testStrafeForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-2.828427, 2.828427, 2.828427, -2.828427);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
var wheelVelocities = new MecanumDriveWheelVelocities(-2.828427, 2.828427, 2.828427, -2.828427);
var moduleVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
assertAll(
() -> assertEquals(0, moduleStates.vx, 0.1),
() -> assertEquals(2.8284, moduleStates.vy, 0.1),
() -> assertEquals(0, moduleStates.omega, 0.1));
() -> assertEquals(0, moduleVelocities.vx, 0.1),
() -> assertEquals(2.8284, moduleVelocities.vy, 0.1),
() -> assertEquals(0, moduleVelocities.omega, 0.1));
}
@Test
@@ -91,25 +91,26 @@ class MecanumDriveKinematicsTest {
@Test
void testRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
ChassisVelocities velocities = new ChassisVelocities(0, 0, 2 * Math.PI);
var moduleVelocities = m_kinematics.toWheelVelocities(velocities);
assertAll(
() -> assertEquals(-150.79645, moduleStates.frontLeft, 0.1),
() -> assertEquals(150.79645, moduleStates.frontRight, 0.1),
() -> assertEquals(-150.79645, moduleStates.rearLeft, 0.1),
() -> assertEquals(150.79645, moduleStates.rearRight, 0.1));
() -> assertEquals(-150.79645, moduleVelocities.frontLeft, 0.1),
() -> assertEquals(150.79645, moduleVelocities.frontRight, 0.1),
() -> assertEquals(-150.79645, moduleVelocities.rearLeft, 0.1),
() -> assertEquals(150.79645, moduleVelocities.rearRight, 0.1));
}
@Test
void testRotationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-150.79645, 150.79645, -150.79645, 150.79645);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
var wheelVelocities =
new MecanumDriveWheelVelocities(-150.79645, 150.79645, -150.79645, 150.79645);
var moduleVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
assertAll(
() -> assertEquals(0, moduleStates.vx, 0.1),
() -> assertEquals(0, moduleStates.vy, 0.1),
() -> assertEquals(2 * Math.PI, moduleStates.omega, 0.1));
() -> assertEquals(0, moduleVelocities.vx, 0.1),
() -> assertEquals(0, moduleVelocities.vy, 0.1),
() -> assertEquals(2 * Math.PI, moduleVelocities.omega, 0.1));
}
@Test
@@ -125,25 +126,25 @@ class MecanumDriveKinematicsTest {
@Test
void testMixedTranslationRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
ChassisVelocities velocities = new ChassisVelocities(2, 3, 1);
var moduleVelocities = m_kinematics.toWheelVelocities(velocities);
assertAll(
() -> assertEquals(-25.0, moduleStates.frontLeft, 0.1),
() -> assertEquals(29.0, moduleStates.frontRight, 0.1),
() -> assertEquals(-19.0, moduleStates.rearLeft, 0.1),
() -> assertEquals(23.0, moduleStates.rearRight, 0.1));
() -> assertEquals(-25.0, moduleVelocities.frontLeft, 0.1),
() -> assertEquals(29.0, moduleVelocities.frontRight, 0.1),
() -> assertEquals(-19.0, moduleVelocities.rearLeft, 0.1),
() -> assertEquals(23.0, moduleVelocities.rearRight, 0.1));
}
@Test
void testMixedTranslationRotationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.677670, 20.51, -13.44, 16.26);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
var wheelVelocities = new MecanumDriveWheelVelocities(-17.677670, 20.51, -13.44, 16.26);
var moduleVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
assertAll(
() -> assertEquals(1.413, moduleStates.vx, 0.1),
() -> assertEquals(2.122, moduleStates.vy, 0.1),
() -> assertEquals(0.707, moduleStates.omega, 0.1));
() -> assertEquals(1.413, moduleVelocities.vx, 0.1),
() -> assertEquals(2.122, moduleVelocities.vy, 0.1),
() -> assertEquals(0.707, moduleVelocities.omega, 0.1));
}
@Test
@@ -159,14 +160,14 @@ class MecanumDriveKinematicsTest {
@Test
void testOffCenterRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
ChassisVelocities velocities = new ChassisVelocities(0, 0, 1);
var moduleVelocities = m_kinematics.toWheelVelocities(velocities, m_fl);
assertAll(
() -> assertEquals(0, moduleStates.frontLeft, 0.1),
() -> assertEquals(24.0, moduleStates.frontRight, 0.1),
() -> assertEquals(-24.0, moduleStates.rearLeft, 0.1),
() -> assertEquals(48.0, moduleStates.rearRight, 0.1));
() -> assertEquals(0, moduleVelocities.frontLeft, 0.1),
() -> assertEquals(24.0, moduleVelocities.frontRight, 0.1),
() -> assertEquals(-24.0, moduleVelocities.rearLeft, 0.1),
() -> assertEquals(48.0, moduleVelocities.rearRight, 0.1));
}
@Test
@@ -277,13 +278,13 @@ class MecanumDriveKinematicsTest {
@Test
void testOffCenterRotationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
var wheelVelocities = new MecanumDriveWheelVelocities(0, 16.971, -16.971, 33.941);
var moduleVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
assertAll(
() -> assertEquals(8.48525, moduleStates.vx, 0.1),
() -> assertEquals(-8.48525, moduleStates.vy, 0.1),
() -> assertEquals(0.707, moduleStates.omega, 0.1));
() -> assertEquals(8.48525, moduleVelocities.vx, 0.1),
() -> assertEquals(-8.48525, moduleVelocities.vy, 0.1),
() -> assertEquals(0.707, moduleVelocities.omega, 0.1));
}
@Test
@@ -299,25 +300,25 @@ class MecanumDriveKinematicsTest {
@Test
void testOffCenterTranslationRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
ChassisVelocities velocities = new ChassisVelocities(5, 2, 1);
var moduleVelocities = m_kinematics.toWheelVelocities(velocities, m_fl);
assertAll(
() -> assertEquals(3.0, moduleStates.frontLeft, 0.1),
() -> assertEquals(31.0, moduleStates.frontRight, 0.1),
() -> assertEquals(-17.0, moduleStates.rearLeft, 0.1),
() -> assertEquals(51.0, moduleStates.rearRight, 0.1));
() -> assertEquals(3.0, moduleVelocities.frontLeft, 0.1),
() -> assertEquals(31.0, moduleVelocities.frontRight, 0.1),
() -> assertEquals(-17.0, moduleVelocities.rearLeft, 0.1),
() -> assertEquals(51.0, moduleVelocities.rearRight, 0.1));
}
@Test
void testOffCenterRotationTranslationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(2.12, 21.92, -12.02, 36.06);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
var wheelVelocities = new MecanumDriveWheelVelocities(2.12, 21.92, -12.02, 36.06);
var moduleVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
assertAll(
() -> assertEquals(12.02, moduleStates.vx, 0.1),
() -> assertEquals(-7.07, moduleStates.vy, 0.1),
() -> assertEquals(0.707, moduleStates.omega, 0.1));
() -> assertEquals(12.02, moduleVelocities.vx, 0.1),
() -> assertEquals(-7.07, moduleVelocities.vy, 0.1),
() -> assertEquals(0.707, moduleVelocities.omega, 0.1));
}
@Test
@@ -333,27 +334,27 @@ class MecanumDriveKinematicsTest {
@Test
void testDesaturate() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7).desaturate(5.5);
var wheelVelocities = new MecanumDriveWheelVelocities(5, 6, 4, 7).desaturate(5.5);
double factor = 5.5 / 7.0;
assertAll(
() -> assertEquals(5.0 * factor, wheelSpeeds.frontLeft, kEpsilon),
() -> assertEquals(6.0 * factor, wheelSpeeds.frontRight, kEpsilon),
() -> assertEquals(4.0 * factor, wheelSpeeds.rearLeft, kEpsilon),
() -> assertEquals(7.0 * factor, wheelSpeeds.rearRight, kEpsilon));
() -> assertEquals(5.0 * factor, wheelVelocities.frontLeft, kEpsilon),
() -> assertEquals(6.0 * factor, wheelVelocities.frontRight, kEpsilon),
() -> assertEquals(4.0 * factor, wheelVelocities.rearLeft, kEpsilon),
() -> assertEquals(7.0 * factor, wheelVelocities.rearRight, kEpsilon));
}
@Test
void testDesaturateNegativeSpeeds() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-5, 6, 4, -7).desaturate(5.5);
void testDesaturateNegativeVelocities() {
var wheelVelocities = new MecanumDriveWheelVelocities(-5, 6, 4, -7).desaturate(5.5);
final double kFactor = 5.5 / 7.0;
assertAll(
() -> assertEquals(-5.0 * kFactor, wheelSpeeds.frontLeft, kEpsilon),
() -> assertEquals(6.0 * kFactor, wheelSpeeds.frontRight, kEpsilon),
() -> assertEquals(4.0 * kFactor, wheelSpeeds.rearLeft, kEpsilon),
() -> assertEquals(-7.0 * kFactor, wheelSpeeds.rearRight, kEpsilon));
() -> assertEquals(-5.0 * kFactor, wheelVelocities.frontLeft, kEpsilon),
() -> assertEquals(6.0 * kFactor, wheelVelocities.frontRight, kEpsilon),
() -> assertEquals(4.0 * kFactor, wheelVelocities.rearLeft, kEpsilon),
() -> assertEquals(-7.0 * kFactor, wheelVelocities.rearRight, kEpsilon));
}
}

48
wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveOdometry3dTest.java
View File

@@ -105,9 +105,9 @@ class MecanumDriveOdometry3dTest {
var fieldAngle = Rotation3d.kZero;
m_odometry.resetPosition(
gyro, new MecanumDriveWheelPositions(), new Pose3d(Translation3d.kZero, fieldAngle));
var speeds = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
var velocities = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
m_odometry.update(gyro, new MecanumDriveWheelPositions());
var pose = m_odometry.update(gyro, speeds);
var pose = m_odometry.update(gyro, velocities);
assertAll(
() -> assertEquals(3.536, pose.getX(), 0.1),
@@ -154,22 +154,22 @@ class MecanumDriveOdometry3dTest {
trajectoryDistanceTravelled +=
groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
var wheelVelocities =
kinematics.toWheelVelocities(
new ChassisVelocities(
groundTruthState.velocity,
0,
groundTruthState.velocity * groundTruthState.curvature));
wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRight += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRight += rand.nextGaussian() * 0.1;
wheelVelocities.frontLeft += rand.nextGaussian() * 0.1;
wheelVelocities.frontRight += rand.nextGaussian() * 0.1;
wheelVelocities.rearLeft += rand.nextGaussian() * 0.1;
wheelVelocities.rearRight += rand.nextGaussian() * 0.1;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
var lastPose = odometry.getPose();
@@ -244,22 +244,22 @@ class MecanumDriveOdometry3dTest {
trajectoryDistanceTravelled +=
groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
var wheelVelocities =
kinematics.toWheelVelocities(
new ChassisVelocities(
groundTruthState.velocity * groundTruthState.pose.getRotation().getCos(),
groundTruthState.velocity * groundTruthState.pose.getRotation().getSin(),
0));
wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRight += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRight += rand.nextGaussian() * 0.1;
wheelVelocities.frontLeft += rand.nextGaussian() * 0.1;
wheelVelocities.frontRight += rand.nextGaussian() * 0.1;
wheelVelocities.rearLeft += rand.nextGaussian() * 0.1;
wheelVelocities.rearRight += rand.nextGaussian() * 0.1;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
var lastPose = odometry.getPose();

48
wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveOdometryTest.java
View File

@@ -82,9 +82,9 @@ class MecanumDriveOdometryTest {
var fieldAngle = Rotation2d.kZero;
m_odometry.resetPosition(
gyro, new MecanumDriveWheelPositions(), new Pose2d(Translation2d.kZero, fieldAngle));
var speeds = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
var velocities = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
m_odometry.update(gyro, new MecanumDriveWheelPositions());
var pose = m_odometry.update(gyro, speeds);
var pose = m_odometry.update(gyro, velocities);
assertAll(
() -> assertEquals(3.536, pose.getX(), 0.1),
@@ -130,22 +130,22 @@ class MecanumDriveOdometryTest {
trajectoryDistanceTravelled +=
groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
var wheelVelocities =
kinematics.toWheelVelocities(
new ChassisVelocities(
groundTruthState.velocity,
0,
groundTruthState.velocity * groundTruthState.curvature));
wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRight += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRight += rand.nextGaussian() * 0.1;
wheelVelocities.frontLeft += rand.nextGaussian() * 0.1;
wheelVelocities.frontRight += rand.nextGaussian() * 0.1;
wheelVelocities.rearLeft += rand.nextGaussian() * 0.1;
wheelVelocities.rearRight += rand.nextGaussian() * 0.1;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
var lastPose = odometry.getPose();
@@ -212,22 +212,22 @@ class MecanumDriveOdometryTest {
trajectoryDistanceTravelled +=
groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
var wheelVelocities =
kinematics.toWheelVelocities(
new ChassisVelocities(
groundTruthState.velocity * groundTruthState.pose.getRotation().getCos(),
groundTruthState.velocity * groundTruthState.pose.getRotation().getSin(),
0));
wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRight += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRight += rand.nextGaussian() * 0.1;
wheelVelocities.frontLeft += rand.nextGaussian() * 0.1;
wheelVelocities.frontRight += rand.nextGaussian() * 0.1;
wheelVelocities.rearLeft += rand.nextGaussian() * 0.1;
wheelVelocities.rearRight += rand.nextGaussian() * 0.1;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
wheelPositions.frontLeft += wheelVelocities.frontLeft * dt;
wheelPositions.frontRight += wheelVelocities.frontRight * dt;
wheelPositions.rearLeft += wheelVelocities.rearLeft * dt;
wheelPositions.rearRight += wheelVelocities.rearRight * dt;
var lastPose = odometry.getPose();

64
wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveWheelSpeedsTest.java → wpimath/src/test/java/org/wpilib/math/kinematics/MecanumDriveWheelVelocitiesTest.java
View File

@@ -9,72 +9,72 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class MecanumDriveWheelSpeedsTest {
class MecanumDriveWheelVelocitiesTest {
@Test
void testPlus() {
final var left = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5);
final var right = new MecanumDriveWheelSpeeds(2.0, 1.5, 0.5, 1.0);
final var left = new MecanumDriveWheelVelocities(1.0, 0.5, 2.0, 1.5);
final var right = new MecanumDriveWheelVelocities(2.0, 1.5, 0.5, 1.0);
final var wheelSpeeds = left.plus(right);
final var wheelVelocities = left.plus(right);
assertAll(
() -> assertEquals(3.0, wheelSpeeds.frontLeft),
() -> assertEquals(2.0, wheelSpeeds.frontRight),
() -> assertEquals(2.5, wheelSpeeds.rearLeft),
() -> assertEquals(2.5, wheelSpeeds.rearRight));
() -> assertEquals(3.0, wheelVelocities.frontLeft),
() -> assertEquals(2.0, wheelVelocities.frontRight),
() -> assertEquals(2.5, wheelVelocities.rearLeft),
() -> assertEquals(2.5, wheelVelocities.rearRight));
}
@Test
void testMinus() {
final var left = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5);
final var right = new MecanumDriveWheelSpeeds(2.0, 0.5, 0.5, 1.0);
final var left = new MecanumDriveWheelVelocities(1.0, 0.5, 2.0, 1.5);
final var right = new MecanumDriveWheelVelocities(2.0, 0.5, 0.5, 1.0);
final var wheelSpeeds = left.minus(right);
final var wheelVelocities = left.minus(right);
assertAll(
() -> assertEquals(-1.0, wheelSpeeds.frontLeft),
() -> assertEquals(0.0, wheelSpeeds.frontRight),
() -> assertEquals(1.5, wheelSpeeds.rearLeft),
() -> assertEquals(0.5, wheelSpeeds.rearRight));
() -> assertEquals(-1.0, wheelVelocities.frontLeft),
() -> assertEquals(0.0, wheelVelocities.frontRight),
() -> assertEquals(1.5, wheelVelocities.rearLeft),
() -> assertEquals(0.5, wheelVelocities.rearRight));
}
@Test
void testUnaryMinus() {
final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).unaryMinus();
final var wheelVelocities = new MecanumDriveWheelVelocities(1.0, 0.5, 2.0, 1.5).unaryMinus();
assertAll(
() -> assertEquals(-1.0, wheelSpeeds.frontLeft),
() -> assertEquals(-0.5, wheelSpeeds.frontRight),
() -> assertEquals(-2.0, wheelSpeeds.rearLeft),
() -> assertEquals(-1.5, wheelSpeeds.rearRight));
() -> assertEquals(-1.0, wheelVelocities.frontLeft),
() -> assertEquals(-0.5, wheelVelocities.frontRight),
() -> assertEquals(-2.0, wheelVelocities.rearLeft),
() -> assertEquals(-1.5, wheelVelocities.rearRight));
}
@Test
void testMultiplication() {
final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).times(2.0);
final var wheelVelocities = new MecanumDriveWheelVelocities(1.0, 0.5, 2.0, 1.5).times(2.0);
assertAll(
() -> assertEquals(2.0, wheelSpeeds.frontLeft),
() -> assertEquals(1.0, wheelSpeeds.frontRight),
() -> assertEquals(4.0, wheelSpeeds.rearLeft),
() -> assertEquals(3.0, wheelSpeeds.rearRight));
() -> assertEquals(2.0, wheelVelocities.frontLeft),
() -> assertEquals(1.0, wheelVelocities.frontRight),
() -> assertEquals(4.0, wheelVelocities.rearLeft),
() -> assertEquals(3.0, wheelVelocities.rearRight));
}
@Test
void testDivision() {
final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).div(2.0);
final var wheelVelocities = new MecanumDriveWheelVelocities(1.0, 0.5, 2.0, 1.5).div(2.0);
assertAll(
() -> assertEquals(0.5, wheelSpeeds.frontLeft),
() -> assertEquals(0.25, wheelSpeeds.frontRight),
() -> assertEquals(1.0, wheelSpeeds.rearLeft),
() -> assertEquals(0.75, wheelSpeeds.rearRight));
() -> assertEquals(0.5, wheelVelocities.frontLeft),
() -> assertEquals(0.25, wheelVelocities.frontRight),
() -> assertEquals(1.0, wheelVelocities.rearLeft),
() -> assertEquals(0.75, wheelVelocities.rearRight));
}
@Test
void testInterpolate() {
final var start = new MecanumDriveWheelSpeeds(1.0, 2.0, 3.0, 4.0);
final var end = new MecanumDriveWheelSpeeds(5.0, 6.0, 7.0, 8.0);
final var start = new MecanumDriveWheelVelocities(1.0, 2.0, 3.0, 4.0);
final var end = new MecanumDriveWheelVelocities(5.0, 6.0, 7.0, 8.0);
// Test interpolation at t=0 (should return start)
final var atStart = start.interpolate(end, 0.0);

288
wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java
View File

@@ -26,29 +26,29 @@ class SwerveDriveKinematicsTest {
@Test
void testStraightLineInverseKinematics() { // test inverse kinematics going in a straight line
ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
ChassisVelocities velocities = new ChassisVelocities(5, 0, 0);
var moduleVelocities = m_kinematics.toSwerveModuleVelocities(velocities);
assertAll(
() -> assertEquals(5.0, moduleStates[0].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[1].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[2].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[3].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[0].angle.getRadians(), kEpsilon),
() -> assertEquals(0.0, moduleStates[1].angle.getRadians(), kEpsilon),
() -> assertEquals(0.0, moduleStates[2].angle.getRadians(), kEpsilon),
() -> assertEquals(0.0, moduleStates[3].angle.getRadians(), kEpsilon));
() -> assertEquals(5.0, moduleVelocities[0].velocity, kEpsilon),
() -> assertEquals(5.0, moduleVelocities[1].velocity, kEpsilon),
() -> assertEquals(5.0, moduleVelocities[2].velocity, kEpsilon),
() -> assertEquals(5.0, moduleVelocities[3].velocity, kEpsilon),
() -> assertEquals(0.0, moduleVelocities[0].angle.getRadians(), kEpsilon),
() -> assertEquals(0.0, moduleVelocities[1].angle.getRadians(), kEpsilon),
() -> assertEquals(0.0, moduleVelocities[2].angle.getRadians(), kEpsilon),
() -> assertEquals(0.0, moduleVelocities[3].angle.getRadians(), kEpsilon));
}
@Test
void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.kZero);
var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
SwerveModuleVelocity state = new SwerveModuleVelocity(5.0, Rotation2d.kZero);
var chassisVelocities = m_kinematics.toChassisVelocities(state, state, state, state);
assertAll(
() -> assertEquals(5.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon));
() -> assertEquals(5.0, chassisVelocities.vx, kEpsilon),
() -> assertEquals(0.0, chassisVelocities.vy, kEpsilon),
() -> assertEquals(0.0, chassisVelocities.omega, kEpsilon));
}
@Test
@@ -65,29 +65,29 @@ class SwerveDriveKinematicsTest {
@Test
void testStraightStrafeInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
ChassisVelocities velocities = new ChassisVelocities(0, 5, 0);
var moduleVelocities = m_kinematics.toSwerveModuleVelocities(velocities);
assertAll(
() -> assertEquals(5.0, moduleStates[0].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[1].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[2].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[3].speed, kEpsilon),
() -> assertEquals(90.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleStates[3].angle.getDegrees(), kEpsilon));
() -> assertEquals(5.0, moduleVelocities[0].velocity, kEpsilon),
() -> assertEquals(5.0, moduleVelocities[1].velocity, kEpsilon),
() -> assertEquals(5.0, moduleVelocities[2].velocity, kEpsilon),
() -> assertEquals(5.0, moduleVelocities[3].velocity, kEpsilon),
() -> assertEquals(90.0, moduleVelocities[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleVelocities[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleVelocities[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleVelocities[3].angle.getDegrees(), kEpsilon));
}
@Test
void testStraightStrafeForwardKinematics() {
SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.kCCW_Pi_2);
var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
SwerveModuleVelocity state = new SwerveModuleVelocity(5.0, Rotation2d.kCCW_Pi_2);
var chassisVelocities = m_kinematics.toChassisVelocities(state, state, state, state);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(5.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon));
() -> assertEquals(0.0, chassisVelocities.vx, kEpsilon),
() -> assertEquals(5.0, chassisVelocities.vy, kEpsilon),
() -> assertEquals(0.0, chassisVelocities.omega, kEpsilon));
}
@Test
@@ -103,21 +103,21 @@ class SwerveDriveKinematicsTest {
@Test
void testConserveWheelAngle() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
m_kinematics.toSwerveModuleStates(speeds);
var moduleStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds());
ChassisVelocities velocities = new ChassisVelocities(0, 0, 2 * Math.PI);
m_kinematics.toSwerveModuleVelocities(velocities);
var moduleVelocities = m_kinematics.toSwerveModuleVelocities(new ChassisVelocities());
// Robot is stationary, but module angles are preserved.
assertAll(
() -> assertEquals(0.0, moduleStates[0].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[1].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[2].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[3].speed, kEpsilon),
() -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
() -> assertEquals(0.0, moduleVelocities[0].velocity, kEpsilon),
() -> assertEquals(0.0, moduleVelocities[1].velocity, kEpsilon),
() -> assertEquals(0.0, moduleVelocities[2].velocity, kEpsilon),
() -> assertEquals(0.0, moduleVelocities[3].velocity, kEpsilon),
() -> assertEquals(135.0, moduleVelocities[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(45.0, moduleVelocities[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(-135.0, moduleVelocities[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(-45.0, moduleVelocities[3].angle.getDegrees(), kEpsilon));
}
@Test
@@ -127,25 +127,25 @@ class SwerveDriveKinematicsTest {
Rotation2d bl = Rotation2d.kPi;
Rotation2d br = new Rotation2d(3 * Math.PI / 2);
m_kinematics.resetHeadings(fl, fr, bl, br);
var moduleStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds());
var moduleVelocities = m_kinematics.toSwerveModuleVelocities(new ChassisVelocities());
// Robot is stationary, but module angles are preserved.
assertAll(
() -> assertEquals(0.0, moduleStates[0].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[1].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[2].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[3].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(180.0, moduleStates[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(-90.0, moduleStates[3].angle.getDegrees(), kEpsilon));
() -> assertEquals(0.0, moduleVelocities[0].velocity, kEpsilon),
() -> assertEquals(0.0, moduleVelocities[1].velocity, kEpsilon),
() -> assertEquals(0.0, moduleVelocities[2].velocity, kEpsilon),
() -> assertEquals(0.0, moduleVelocities[3].velocity, kEpsilon),
() -> assertEquals(0.0, moduleVelocities[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleVelocities[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(180.0, moduleVelocities[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(-90.0, moduleVelocities[3].angle.getDegrees(), kEpsilon));
}
@Test
void testTurnInPlaceInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
ChassisVelocities velocities = new ChassisVelocities(0, 0, 2 * Math.PI);
var moduleVelocities = m_kinematics.toSwerveModuleVelocities(velocities);
// The circumference of the wheels about the COR is π * diameter, or 2π * radius. The radius
// is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels trace out is
@@ -153,29 +153,29 @@ class SwerveDriveKinematicsTest {
// must trace out 1 rotation (or 106.63 inches) per second.
assertAll(
() -> assertEquals(106.63, moduleStates[0].speed, 0.1),
() -> assertEquals(106.63, moduleStates[1].speed, 0.1),
() -> assertEquals(106.63, moduleStates[2].speed, 0.1),
() -> assertEquals(106.63, moduleStates[3].speed, 0.1),
() -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
() -> assertEquals(106.63, moduleVelocities[0].velocity, 0.1),
() -> assertEquals(106.63, moduleVelocities[1].velocity, 0.1),
() -> assertEquals(106.63, moduleVelocities[2].velocity, 0.1),
() -> assertEquals(106.63, moduleVelocities[3].velocity, 0.1),
() -> assertEquals(135.0, moduleVelocities[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(45.0, moduleVelocities[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(-135.0, moduleVelocities[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(-45.0, moduleVelocities[3].angle.getDegrees(), kEpsilon));
}
@Test
void testTurnInPlaceForwardKinematics() {
SwerveModuleState flState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(135));
SwerveModuleState frState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(45));
SwerveModuleState blState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-135));
SwerveModuleState brState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-45));
SwerveModuleVelocity flState = new SwerveModuleVelocity(106.629, Rotation2d.fromDegrees(135));
SwerveModuleVelocity frState = new SwerveModuleVelocity(106.629, Rotation2d.fromDegrees(45));
SwerveModuleVelocity blState = new SwerveModuleVelocity(106.629, Rotation2d.fromDegrees(-135));
SwerveModuleVelocity brState = new SwerveModuleVelocity(106.629, Rotation2d.fromDegrees(-45));
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
var chassisVelocities = m_kinematics.toChassisVelocities(flState, frState, blState, brState);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(2 * Math.PI, chassisSpeeds.omega, 0.1));
() -> assertEquals(0.0, chassisVelocities.vx, kEpsilon),
() -> assertEquals(0.0, chassisVelocities.vy, kEpsilon),
() -> assertEquals(2 * Math.PI, chassisVelocities.omega, 0.1));
}
@Test
@@ -195,35 +195,36 @@ class SwerveDriveKinematicsTest {
@Test
void testOffCenterCORRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl);
ChassisVelocities velocities = new ChassisVelocities(0, 0, 2 * Math.PI);
var moduleVelocities = m_kinematics.toSwerveModuleVelocities(velocities, m_fl);
// This one is a bit trickier. Because we are rotating about the front-left wheel, it should
// be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel an arc
// be parked at 0 degrees and 0 velocity. The front-right and back-left wheels both travel an
// arc
// with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
// radius √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel should
// be pointing straight forward, the back-left wheel should be pointing straight right, and the
// back-right wheel should be at a -45 degree angle.
assertAll(
() -> assertEquals(0.0, moduleStates[0].speed, 0.1),
() -> assertEquals(150.796, moduleStates[1].speed, 0.1),
() -> assertEquals(150.796, moduleStates[2].speed, 0.1),
() -> assertEquals(213.258, moduleStates[3].speed, 0.1),
() -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(0.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(-90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
() -> assertEquals(0.0, moduleVelocities[0].velocity, 0.1),
() -> assertEquals(150.796, moduleVelocities[1].velocity, 0.1),
() -> assertEquals(150.796, moduleVelocities[2].velocity, 0.1),
() -> assertEquals(213.258, moduleVelocities[3].velocity, 0.1),
() -> assertEquals(0.0, moduleVelocities[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(0.0, moduleVelocities[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(-90.0, moduleVelocities[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(-45.0, moduleVelocities[3].angle.getDegrees(), kEpsilon));
}
@Test
void testOffCenterCORRotationForwardKinematics() {
SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.kZero);
SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.kZero);
SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.kCW_Pi_2);
SwerveModuleState brState = new SwerveModuleState(213.258, Rotation2d.fromDegrees(-45));
SwerveModuleVelocity flState = new SwerveModuleVelocity(0.0, Rotation2d.kZero);
SwerveModuleVelocity frState = new SwerveModuleVelocity(150.796, Rotation2d.kZero);
SwerveModuleVelocity blState = new SwerveModuleVelocity(150.796, Rotation2d.kCW_Pi_2);
SwerveModuleVelocity brState = new SwerveModuleVelocity(213.258, Rotation2d.fromDegrees(-45));
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
var chassisVelocities = m_kinematics.toChassisVelocities(flState, frState, blState, brState);
// We already know that our omega should be 2π from the previous test. Next, we need to
// determine the vx and vy of our chassis center. Because our COR is at a 45 degree angle from
@@ -233,9 +234,9 @@ class SwerveDriveKinematicsTest {
// triangle are 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
assertAll(
() -> assertEquals(75.398, chassisSpeeds.vx, 0.1),
() -> assertEquals(-75.398, chassisSpeeds.vy, 0.1),
() -> assertEquals(2 * Math.PI, chassisSpeeds.omega, 0.1));
() -> assertEquals(75.398, chassisVelocities.vx, 0.1),
() -> assertEquals(-75.398, chassisVelocities.vy, 0.1),
() -> assertEquals(2 * Math.PI, chassisVelocities.omega, 0.1));
}
@Test
@@ -260,10 +261,10 @@ class SwerveDriveKinematicsTest {
() -> assertEquals(2 * Math.PI, twist.dtheta, 0.1));
}
private void assertModuleState(
SwerveModuleState expected, SwerveModuleState actual, SwerveModuleState tolerance) {
private void assertModuleVelocity(
SwerveModuleVelocity expected, SwerveModuleVelocity actual, SwerveModuleVelocity tolerance) {
assertAll(
() -> assertEquals(expected.speed, actual.speed, tolerance.speed),
() -> assertEquals(expected.velocity, actual.velocity, tolerance.velocity),
() ->
assertEquals(
expected.angle.getDegrees(),
@@ -277,45 +278,46 @@ class SwerveDriveKinematicsTest {
*/
@Test
void testOffCenterCORRotationAndTranslationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0.0, 3.0, 1.5);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds, new Translation2d(24, 0));
ChassisVelocities velocities = new ChassisVelocities(0.0, 3.0, 1.5);
var moduleVelocities =
m_kinematics.toSwerveModuleVelocities(velocities, new Translation2d(24, 0));
// By equation (13.14) from state-space guide, our wheels/angles will be as follows,
// (+-1 degree or speed):
SwerveModuleState[] expectedStates =
new SwerveModuleState[] {
new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19)),
new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81)),
new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44)),
new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56))
// (+-1 degree or velocity):
SwerveModuleVelocity[] expectedStates =
new SwerveModuleVelocity[] {
new SwerveModuleVelocity(23.43, Rotation2d.fromDegrees(-140.19)),
new SwerveModuleVelocity(23.43, Rotation2d.fromDegrees(-39.81)),
new SwerveModuleVelocity(54.08, Rotation2d.fromDegrees(-109.44)),
new SwerveModuleVelocity(54.08, Rotation2d.fromDegrees(-70.56))
};
var stateTolerance = new SwerveModuleState(0.1, Rotation2d.fromDegrees(0.1));
var stateTolerance = new SwerveModuleVelocity(0.1, Rotation2d.fromDegrees(0.1));
for (int i = 0; i < expectedStates.length; i++) {
assertModuleState(expectedStates[i], moduleStates[i], stateTolerance);
assertModuleVelocity(expectedStates[i], moduleVelocities[i], stateTolerance);
}
}
@Test
void testOffCenterCORRotationAndTranslationForwardKinematics() {
SwerveModuleState flState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
SwerveModuleState frState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
SwerveModuleState blState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
SwerveModuleState brState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));
SwerveModuleVelocity flState = new SwerveModuleVelocity(23.43, Rotation2d.fromDegrees(-140.19));
SwerveModuleVelocity frState = new SwerveModuleVelocity(23.43, Rotation2d.fromDegrees(-39.81));
SwerveModuleVelocity blState = new SwerveModuleVelocity(54.08, Rotation2d.fromDegrees(-109.44));
SwerveModuleVelocity brState = new SwerveModuleVelocity(54.08, Rotation2d.fromDegrees(-70.56));
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
var chassisVelocities = m_kinematics.toChassisVelocities(flState, frState, blState, brState);
// From equation (13.17), we know that chassis motion is th dot product of the
// pseudoinverse of the inverseKinematics matrix (with the center of rotation at
// (0,0) -- we don't want the motion of the center of rotation, we want it of
// the center of the robot). These above SwerveModuleStates are known to be from
// the center of the robot). These above SwerveModuleVelocities are known to be from
// a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
// calculated using Numpy's linalg.pinv function.
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vx, 0.1),
() -> assertEquals(-33.0, chassisSpeeds.vy, 0.1),
() -> assertEquals(1.5, chassisSpeeds.omega, 0.1));
() -> assertEquals(0.0, chassisVelocities.vx, 0.1),
() -> assertEquals(-33.0, chassisVelocities.vy, 0.1),
() -> assertEquals(1.5, chassisVelocities.omega, 0.1));
}
@Test
@@ -330,7 +332,7 @@ class SwerveDriveKinematicsTest {
// From equation (13.17), we know that chassis motion is th dot product of the
// pseudoinverse of the inverseKinematics matrix (with the center of rotation at
// (0,0) -- we don't want the motion of the center of rotation, we want it of
// the center of the robot). These above SwerveModuleStates are known to be from
// the center of the robot). These above SwerveModuleVelocities are known to be from
// a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
// calculated using Numpy's linalg.pinv function.
@@ -342,58 +344,58 @@ class SwerveDriveKinematicsTest {
@Test
void testDesaturate() {
SwerveModuleState fl = new SwerveModuleState(5, Rotation2d.kZero);
SwerveModuleState fr = new SwerveModuleState(6, Rotation2d.kZero);
SwerveModuleState bl = new SwerveModuleState(4, Rotation2d.kZero);
SwerveModuleState br = new SwerveModuleState(7, Rotation2d.kZero);
SwerveModuleVelocity fl = new SwerveModuleVelocity(5, Rotation2d.kZero);
SwerveModuleVelocity fr = new SwerveModuleVelocity(6, Rotation2d.kZero);
SwerveModuleVelocity bl = new SwerveModuleVelocity(4, Rotation2d.kZero);
SwerveModuleVelocity br = new SwerveModuleVelocity(7, Rotation2d.kZero);
SwerveModuleState[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelSpeeds(arr, 5.5);
SwerveModuleVelocity[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelVelocities(arr, 5.5);
double factor = 5.5 / 7.0;
assertAll(
() -> assertEquals(5.0 * factor, arr[0].speed, kEpsilon),
() -> assertEquals(6.0 * factor, arr[1].speed, kEpsilon),
() -> assertEquals(4.0 * factor, arr[2].speed, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].speed, kEpsilon));
() -> assertEquals(5.0 * factor, arr[0].velocity, kEpsilon),
() -> assertEquals(6.0 * factor, arr[1].velocity, kEpsilon),
() -> assertEquals(4.0 * factor, arr[2].velocity, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].velocity, kEpsilon));
}
@Test
void testDesaturateSmooth() {
SwerveModuleState fl = new SwerveModuleState(5, Rotation2d.kZero);
SwerveModuleState fr = new SwerveModuleState(6, Rotation2d.kZero);
SwerveModuleState bl = new SwerveModuleState(4, Rotation2d.kZero);
SwerveModuleState br = new SwerveModuleState(7, Rotation2d.kZero);
SwerveModuleVelocity fl = new SwerveModuleVelocity(5, Rotation2d.kZero);
SwerveModuleVelocity fr = new SwerveModuleVelocity(6, Rotation2d.kZero);
SwerveModuleVelocity bl = new SwerveModuleVelocity(4, Rotation2d.kZero);
SwerveModuleVelocity br = new SwerveModuleVelocity(7, Rotation2d.kZero);
SwerveModuleState[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelSpeeds(
arr, m_kinematics.toChassisSpeeds(arr), 5.5, 5.5, 3.5);
SwerveModuleVelocity[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelVelocities(
arr, m_kinematics.toChassisVelocities(arr), 5.5, 5.5, 3.5);
double factor = 5.5 / 7.0;
assertAll(
() -> assertEquals(5.0 * factor, arr[0].speed, kEpsilon),
() -> assertEquals(6.0 * factor, arr[1].speed, kEpsilon),
() -> assertEquals(4.0 * factor, arr[2].speed, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].speed, kEpsilon));
() -> assertEquals(5.0 * factor, arr[0].velocity, kEpsilon),
() -> assertEquals(6.0 * factor, arr[1].velocity, kEpsilon),
() -> assertEquals(4.0 * factor, arr[2].velocity, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].velocity, kEpsilon));
}
@Test
void testDesaturateNegativeSpeed() {
SwerveModuleState fl = new SwerveModuleState(1, Rotation2d.kZero);
SwerveModuleState fr = new SwerveModuleState(1, Rotation2d.kZero);
SwerveModuleState bl = new SwerveModuleState(-2, Rotation2d.kZero);
SwerveModuleState br = new SwerveModuleState(-2, Rotation2d.kZero);
void testDesaturateNegativeVelocity() {
SwerveModuleVelocity fl = new SwerveModuleVelocity(1, Rotation2d.kZero);
SwerveModuleVelocity fr = new SwerveModuleVelocity(1, Rotation2d.kZero);
SwerveModuleVelocity bl = new SwerveModuleVelocity(-2, Rotation2d.kZero);
SwerveModuleVelocity br = new SwerveModuleVelocity(-2, Rotation2d.kZero);
SwerveModuleState[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelSpeeds(arr, 1);
SwerveModuleVelocity[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelVelocities(arr, 1);
assertAll(
() -> assertEquals(0.5, arr[0].speed, kEpsilon),
() -> assertEquals(0.5, arr[1].speed, kEpsilon),
() -> assertEquals(-1.0, arr[2].speed, kEpsilon),
() -> assertEquals(-1.0, arr[3].speed, kEpsilon));
() -> assertEquals(0.5, arr[0].velocity, kEpsilon),
() -> assertEquals(0.5, arr[1].velocity, kEpsilon),
() -> assertEquals(-1.0, arr[2].velocity, kEpsilon),
() -> assertEquals(-1.0, arr[3].velocity, kEpsilon));
}
@Test

37
wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveOdometry3dTest.java
View File

@@ -81,10 +81,10 @@ class SwerveDriveOdometry3dTest {
@Test
void test90degreeTurn() {
// This is a 90 degree turn about the point between front left and rear left wheels
// Module 0: speed 18.84955592153876 angle 90.0
// Module 1: speed 42.14888838624436 angle 26.565051177077986
// Module 2: speed 18.84955592153876 angle -90.0
// Module 3: speed 42.14888838624436 angle -26.565051177077986
// Module 0: velocity 18.84955592153876 angle 90.0
// Module 1: velocity 42.14888838624436 angle 26.565051177077986
// Module 2: velocity 18.84955592153876 angle -90.0
// Module 3: velocity 42.14888838624436 angle -26.565051177077986
final SwerveModulePosition[] wheelDeltas = {
new SwerveModulePosition(18.85, Rotation2d.kCCW_Pi_2),
@@ -161,26 +161,27 @@ class SwerveDriveOdometry3dTest {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
var moduleStates =
kinematics.toSwerveModuleStates(
new ChassisSpeeds(
var moduleVelocities =
kinematics.toSwerveModuleVelocities(
new ChassisVelocities(
groundTruthState.velocity,
0.0,
groundTruthState.velocity * groundTruthState.curvature));
for (var moduleState : moduleStates) {
moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
moduleState.speed += rand.nextGaussian() * 0.1;
for (var moduleVelocity : moduleVelocities) {
moduleVelocity.angle =
moduleVelocity.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
moduleVelocity.velocity += rand.nextGaussian() * 0.1;
}
fl.distance += moduleStates[0].speed * dt;
fr.distance += moduleStates[1].speed * dt;
bl.distance += moduleStates[2].speed * dt;
br.distance += moduleStates[3].speed * dt;
fl.distance += moduleVelocities[0].velocity * dt;
fr.distance += moduleVelocities[1].velocity * dt;
bl.distance += moduleVelocities[2].velocity * dt;
br.distance += moduleVelocities[3].velocity * dt;
fl.angle = moduleStates[0].angle;
fr.angle = moduleStates[1].angle;
bl.angle = moduleStates[2].angle;
br.angle = moduleStates[3].angle;
fl.angle = moduleVelocities[0].angle;
fr.angle = moduleVelocities[1].angle;
bl.angle = moduleVelocities[2].angle;
br.angle = moduleVelocities[3].angle;
var xHat =
odometry.update(

37
wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveOdometryTest.java
View File

@@ -60,10 +60,10 @@ class SwerveDriveOdometryTest {
@Test
void test90degreeTurn() {
// This is a 90 degree turn about the point between front left and rear left wheels
// Module 0: speed 18.84955592153876 angle 90.0
// Module 1: speed 42.14888838624436 angle 26.565051177077986
// Module 2: speed 18.84955592153876 angle -90.0
// Module 3: speed 42.14888838624436 angle -26.565051177077986
// Module 0: velocity 18.84955592153876 angle 90.0
// Module 1: velocity 42.14888838624436 angle 26.565051177077986
// Module 2: velocity 18.84955592153876 angle -90.0
// Module 3: velocity 42.14888838624436 angle -26.565051177077986
final SwerveModulePosition[] wheelDeltas = {
new SwerveModulePosition(18.85, Rotation2d.kCCW_Pi_2),
@@ -138,26 +138,27 @@ class SwerveDriveOdometryTest {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
var moduleStates =
kinematics.toSwerveModuleStates(
new ChassisSpeeds(
var moduleVelocities =
kinematics.toSwerveModuleVelocities(
new ChassisVelocities(
groundTruthState.velocity,
0.0,
groundTruthState.velocity * groundTruthState.curvature));
for (var moduleState : moduleStates) {
moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
moduleState.speed += rand.nextGaussian() * 0.1;
for (var moduleVelocity : moduleVelocities) {
moduleVelocity.angle =
moduleVelocity.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
moduleVelocity.velocity += rand.nextGaussian() * 0.1;
}
fl.distance += moduleStates[0].speed * dt;
fr.distance += moduleStates[1].speed * dt;
bl.distance += moduleStates[2].speed * dt;
br.distance += moduleStates[3].speed * dt;
fl.distance += moduleVelocities[0].velocity * dt;
fr.distance += moduleVelocities[1].velocity * dt;
bl.distance += moduleVelocities[2].velocity * dt;
br.distance += moduleVelocities[3].velocity * dt;
fl.angle = moduleStates[0].angle;
fr.angle = moduleStates[1].angle;
bl.angle = moduleStates[2].angle;
br.angle = moduleStates[3].angle;
fl.angle = moduleVelocities[0].angle;
fr.angle = moduleVelocities[1].angle;
bl.angle = moduleVelocities[2].angle;
br.angle = moduleVelocities[3].angle;
var xHat =
odometry.update(

88
wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleStateTest.java → wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleVelocityTest.java
View File

@@ -10,194 +10,194 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
import org.wpilib.math.geometry.Rotation2d;
class SwerveModuleStateTest {
class SwerveModuleVelocityTest {
private static final double kEpsilon = 1E-9;
@Test
void testOptimize() {
var angleA = Rotation2d.fromDegrees(45);
var refA = new SwerveModuleState(-2.0, Rotation2d.kPi);
var refA = new SwerveModuleVelocity(-2.0, Rotation2d.kPi);
refA.optimize(angleA);
assertAll(
() -> assertEquals(2.0, refA.speed, kEpsilon),
() -> assertEquals(2.0, refA.velocity, kEpsilon),
() -> assertEquals(0.0, refA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(-50);
var refB = new SwerveModuleState(4.7, Rotation2d.fromDegrees(41));
var refB = new SwerveModuleVelocity(4.7, Rotation2d.fromDegrees(41));
refB.optimize(angleB);
assertAll(
() -> assertEquals(-4.7, refB.speed, kEpsilon),
() -> assertEquals(-4.7, refB.velocity, kEpsilon),
() -> assertEquals(-139.0, refB.angle.getDegrees(), kEpsilon));
}
@Test
void testNoOptimize() {
var angleA = Rotation2d.kZero;
var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(89));
var refA = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(89));
refA.optimize(angleA);
assertAll(
() -> assertEquals(2.0, refA.speed, kEpsilon),
() -> assertEquals(2.0, refA.velocity, kEpsilon),
() -> assertEquals(89.0, refA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.kZero;
var refB = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(-2));
var refB = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(-2));
refB.optimize(angleB);
assertAll(
() -> assertEquals(-2.0, refB.speed, kEpsilon),
() -> assertEquals(-2.0, refB.velocity, kEpsilon),
() -> assertEquals(-2.0, refB.angle.getDegrees(), kEpsilon));
}
@Test
void testCosineScale() {
var angleA = Rotation2d.fromDegrees(0.0);
var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
var refA = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refA.cosineScale(angleA);
assertAll(
() -> assertEquals(Math.sqrt(2.0), refA.speed, kEpsilon),
() -> assertEquals(Math.sqrt(2.0), refA.velocity, kEpsilon),
() -> assertEquals(45.0, refA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(45.0);
var refB = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
var refB = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refB.cosineScale(angleB);
assertAll(
() -> assertEquals(2.0, refB.speed, kEpsilon),
() -> assertEquals(2.0, refB.velocity, kEpsilon),
() -> assertEquals(45.0, refB.angle.getDegrees(), kEpsilon));
var angleC = Rotation2d.fromDegrees(-45.0);
var refC = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
var refC = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refC.cosineScale(angleC);
assertAll(
() -> assertEquals(0.0, refC.speed, kEpsilon),
() -> assertEquals(0.0, refC.velocity, kEpsilon),
() -> assertEquals(45.0, refC.angle.getDegrees(), kEpsilon));
var angleD = Rotation2d.fromDegrees(135.0);
var refD = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
var refD = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refD.cosineScale(angleD);
assertAll(
() -> assertEquals(0.0, refD.speed, kEpsilon),
() -> assertEquals(0.0, refD.velocity, kEpsilon),
() -> assertEquals(45.0, refD.angle.getDegrees(), kEpsilon));
var angleE = Rotation2d.fromDegrees(-135.0);
var refE = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
var refE = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refE.cosineScale(angleE);
assertAll(
() -> assertEquals(-2.0, refE.speed, kEpsilon),
() -> assertEquals(-2.0, refE.velocity, kEpsilon),
() -> assertEquals(45.0, refE.angle.getDegrees(), kEpsilon));
var angleF = Rotation2d.fromDegrees(180.0);
var refF = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
var refF = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refF.cosineScale(angleF);
assertAll(
() -> assertEquals(-Math.sqrt(2.0), refF.speed, kEpsilon),
() -> assertEquals(-Math.sqrt(2.0), refF.velocity, kEpsilon),
() -> assertEquals(45.0, refF.angle.getDegrees(), kEpsilon));
var angleG = Rotation2d.fromDegrees(0.0);
var refG = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
var refG = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refG.cosineScale(angleG);
assertAll(
() -> assertEquals(-Math.sqrt(2.0), refG.speed, kEpsilon),
() -> assertEquals(-Math.sqrt(2.0), refG.velocity, kEpsilon),
() -> assertEquals(45.0, refG.angle.getDegrees(), kEpsilon));
var angleH = Rotation2d.fromDegrees(45.0);
var refH = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
var refH = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refH.cosineScale(angleH);
assertAll(
() -> assertEquals(-2.0, refH.speed, kEpsilon),
() -> assertEquals(-2.0, refH.velocity, kEpsilon),
() -> assertEquals(45.0, refH.angle.getDegrees(), kEpsilon));
var angleI = Rotation2d.fromDegrees(-45.0);
var refI = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
var refI = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refI.cosineScale(angleI);
assertAll(
() -> assertEquals(0.0, refI.speed, kEpsilon),
() -> assertEquals(0.0, refI.velocity, kEpsilon),
() -> assertEquals(45.0, refI.angle.getDegrees(), kEpsilon));
var angleJ = Rotation2d.fromDegrees(135.0);
var refJ = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
var refJ = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refJ.cosineScale(angleJ);
assertAll(
() -> assertEquals(0.0, refJ.speed, kEpsilon),
() -> assertEquals(0.0, refJ.velocity, kEpsilon),
() -> assertEquals(45.0, refJ.angle.getDegrees(), kEpsilon));
var angleK = Rotation2d.fromDegrees(-135.0);
var refK = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
var refK = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refK.cosineScale(angleK);
assertAll(
() -> assertEquals(2.0, refK.speed, kEpsilon),
() -> assertEquals(2.0, refK.velocity, kEpsilon),
() -> assertEquals(45.0, refK.angle.getDegrees(), kEpsilon));
var angleL = Rotation2d.fromDegrees(180.0);
var refL = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
var refL = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refL.cosineScale(angleL);
assertAll(
() -> assertEquals(Math.sqrt(2.0), refL.speed, kEpsilon),
() -> assertEquals(Math.sqrt(2.0), refL.velocity, kEpsilon),
() -> assertEquals(45.0, refL.angle.getDegrees(), kEpsilon));
}
@Test
void testInterpolate() {
// Test basic interpolation with simple angles
final var start = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0.0));
final var end = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
final var start = new SwerveModuleVelocity(1.0, Rotation2d.fromDegrees(0.0));
final var end = new SwerveModuleVelocity(5.0, Rotation2d.fromDegrees(90.0));
// Test interpolation at t=0 (should return start)
final var atStart = start.interpolate(end, 0.0);
assertAll(
() -> assertEquals(1.0, atStart.speed, kEpsilon),
() -> assertEquals(1.0, atStart.velocity, kEpsilon),
() -> assertEquals(0.0, atStart.angle.getDegrees(), kEpsilon));
// Test interpolation at t=1 (should return end)
final var atEnd = start.interpolate(end, 1.0);
assertAll(
() -> assertEquals(5.0, atEnd.speed, kEpsilon),
() -> assertEquals(5.0, atEnd.velocity, kEpsilon),
() -> assertEquals(90.0, atEnd.angle.getDegrees(), kEpsilon));
// Test interpolation at t=0.5 (should return midpoint)
final var atMidpoint = start.interpolate(end, 0.5);
assertAll(
() -> assertEquals(3.0, atMidpoint.speed, kEpsilon),
() -> assertEquals(3.0, atMidpoint.velocity, kEpsilon),
() -> assertEquals(45.0, atMidpoint.angle.getDegrees(), kEpsilon));
// Test interpolation at t=0.25
final var atQuarter = start.interpolate(end, 0.25);
assertAll(
() -> assertEquals(2.0, atQuarter.speed, kEpsilon),
() -> assertEquals(2.0, atQuarter.velocity, kEpsilon),
() -> assertEquals(22.5, atQuarter.angle.getDegrees(), kEpsilon));
// Test clamping: t < 0 should clamp to 0
final var belowRange = start.interpolate(end, -0.5);
assertAll(
() -> assertEquals(1.0, belowRange.speed, kEpsilon),
() -> assertEquals(1.0, belowRange.velocity, kEpsilon),
() -> assertEquals(0.0, belowRange.angle.getDegrees(), kEpsilon));
// Test clamping: t > 1 should clamp to 1
final var aboveRange = start.interpolate(end, 1.5);
assertAll(
() -> assertEquals(5.0, aboveRange.speed, kEpsilon),
() -> assertEquals(5.0, aboveRange.velocity, kEpsilon),
() -> assertEquals(90.0, aboveRange.angle.getDegrees(), kEpsilon));
// Test angle wrapping with crossing 180/-180 boundary
final var startWrap = new SwerveModuleState(2.0, Rotation2d.fromDegrees(170.0));
final var endWrap = new SwerveModuleState(4.0, Rotation2d.fromDegrees(-170.0));
final var startWrap = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(170.0));
final var endWrap = new SwerveModuleVelocity(4.0, Rotation2d.fromDegrees(-170.0));
final var midpointWrap = startWrap.interpolate(endWrap, 0.5);
assertAll(
() -> assertEquals(3.0, midpointWrap.speed, kEpsilon),
() -> assertEquals(3.0, midpointWrap.velocity, kEpsilon),
() -> assertEquals(180.0, Math.abs(midpointWrap.angle.getDegrees()), kEpsilon));
}
}

14
wpimath/src/test/java/org/wpilib/math/kinematics/proto/ChassisSpeedsProtoTest.java → wpimath/src/test/java/org/wpilib/math/kinematics/proto/ChassisVelocitiesProtoTest.java
View File

@@ -7,18 +7,18 @@ package org.wpilib.math.kinematics.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.proto.ProtobufChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.proto.ProtobufChassisVelocities;
class ChassisSpeedsProtoTest {
private static final ChassisSpeeds DATA = new ChassisSpeeds(2.29, 2.2, 0.3504);
class ChassisVelocitiesProtoTest {
private static final ChassisVelocities DATA = new ChassisVelocities(2.29, 2.2, 0.3504);
@Test
void testRoundtrip() {
ProtobufChassisSpeeds proto = ChassisSpeeds.proto.createMessage();
ChassisSpeeds.proto.pack(proto, DATA);
ProtobufChassisVelocities proto = ChassisVelocities.proto.createMessage();
ChassisVelocities.proto.pack(proto, DATA);
ChassisSpeeds data = ChassisSpeeds.proto.unpack(proto);
ChassisVelocities data = ChassisVelocities.proto.unpack(proto);
assertEquals(DATA.vx, data.vx);
assertEquals(DATA.vy, data.vy);
assertEquals(DATA.omega, data.omega);

26
wpimath/src/test/java/org/wpilib/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java
View File

@@ -1,26 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
import org.wpilib.math.kinematics.DifferentialDriveWheelSpeeds;
import org.wpilib.math.proto.ProtobufDifferentialDriveWheelSpeeds;
class DifferentialDriveWheelSpeedsProtoTest {
private static final DifferentialDriveWheelSpeeds DATA =
new DifferentialDriveWheelSpeeds(1.74, 35.04);
@Test
void testRoundtrip() {
ProtobufDifferentialDriveWheelSpeeds proto = DifferentialDriveWheelSpeeds.proto.createMessage();
DifferentialDriveWheelSpeeds.proto.pack(proto, DATA);
DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.proto.unpack(proto);
assertEquals(DATA.left, data.left);
assertEquals(DATA.right, data.right);
}
}

27
wpimath/src/test/java/org/wpilib/math/kinematics/proto/DifferentialDriveWheelVelocitiesProtoTest.java Normal file
View File

@@ -0,0 +1,27 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
import org.wpilib.math.kinematics.DifferentialDriveWheelVelocities;
import org.wpilib.math.proto.ProtobufDifferentialDriveWheelVelocities;
class DifferentialDriveWheelVelocitiesProtoTest {
private static final DifferentialDriveWheelVelocities DATA =
new DifferentialDriveWheelVelocities(1.74, 35.04);
@Test
void testRoundtrip() {
ProtobufDifferentialDriveWheelVelocities proto =
DifferentialDriveWheelVelocities.proto.createMessage();
DifferentialDriveWheelVelocities.proto.pack(proto, DATA);
DifferentialDriveWheelVelocities data = DifferentialDriveWheelVelocities.proto.unpack(proto);
assertEquals(DATA.left, data.left);
assertEquals(DATA.right, data.right);
}
}

16
wpimath/src/test/java/org/wpilib/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java → wpimath/src/test/java/org/wpilib/math/kinematics/proto/MecanumDriveWheelVelocitiesProtoTest.java
View File

@@ -7,19 +7,19 @@ package org.wpilib.math.kinematics.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
import org.wpilib.math.kinematics.MecanumDriveWheelSpeeds;
import org.wpilib.math.proto.ProtobufMecanumDriveWheelSpeeds;
import org.wpilib.math.kinematics.MecanumDriveWheelVelocities;
import org.wpilib.math.proto.ProtobufMecanumDriveWheelVelocities;
class MecanumDriveWheelSpeedsProtoTest {
private static final MecanumDriveWheelSpeeds DATA =
new MecanumDriveWheelSpeeds(2.29, 17.4, 4.4, 0.229);
class MecanumDriveWheelVelocitiesProtoTest {
private static final MecanumDriveWheelVelocities DATA =
new MecanumDriveWheelVelocities(2.29, 17.4, 4.4, 0.229);
@Test
void testRoundtrip() {
ProtobufMecanumDriveWheelSpeeds proto = MecanumDriveWheelSpeeds.proto.createMessage();
MecanumDriveWheelSpeeds.proto.pack(proto, DATA);
ProtobufMecanumDriveWheelVelocities proto = MecanumDriveWheelVelocities.proto.createMessage();
MecanumDriveWheelVelocities.proto.pack(proto, DATA);
MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.proto.unpack(proto);
MecanumDriveWheelVelocities data = MecanumDriveWheelVelocities.proto.unpack(proto);
assertEquals(DATA.frontLeft, data.frontLeft);
assertEquals(DATA.frontRight, data.frontRight);
assertEquals(DATA.rearLeft, data.rearLeft);

26
wpimath/src/test/java/org/wpilib/math/kinematics/proto/SwerveModuleStateProtoTest.java
View File

@@ -1,26 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.proto.ProtobufSwerveModuleState;
class SwerveModuleStateProtoTest {
private static final SwerveModuleState DATA = new SwerveModuleState(22.9, new Rotation2d(3.3));
@Test
void testRoundtrip() {
ProtobufSwerveModuleState proto = SwerveModuleState.proto.createMessage();
SwerveModuleState.proto.pack(proto, DATA);
SwerveModuleState data = SwerveModuleState.proto.unpack(proto);
assertEquals(DATA.speed, data.speed);
assertEquals(DATA.angle, data.angle);
}
}

27
wpimath/src/test/java/org/wpilib/math/kinematics/proto/SwerveModuleVelocityProtoTest.java Normal file
View File

@@ -0,0 +1,27 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.math.kinematics.proto;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.SwerveModuleVelocity;
import org.wpilib.math.proto.ProtobufSwerveModuleVelocity;
class SwerveModuleVelocityProtoTest {
private static final SwerveModuleVelocity DATA =
new SwerveModuleVelocity(22.9, new Rotation2d(3.3));
@Test
void testRoundtrip() {
ProtobufSwerveModuleVelocity proto = SwerveModuleVelocity.proto.createMessage();
SwerveModuleVelocity.proto.pack(proto, DATA);
SwerveModuleVelocity data = SwerveModuleVelocity.proto.unpack(proto);
assertEquals(DATA.velocity, data.velocity);
assertEquals(DATA.angle, data.angle);
}
}

12
wpimath/src/test/java/org/wpilib/math/kinematics/struct/ChassisSpeedsStructTest.java → wpimath/src/test/java/org/wpilib/math/kinematics/struct/ChassisVelocitiesStructTest.java
View File

@@ -9,19 +9,19 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
class ChassisSpeedsStructTest {
private static final ChassisSpeeds DATA = new ChassisSpeeds(2.29, 2.2, 0.3504);
class ChassisVelocitiesStructTest {
private static final ChassisVelocities DATA = new ChassisVelocities(2.29, 2.2, 0.3504);
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(ChassisSpeeds.struct.getSize());
ByteBuffer buffer = ByteBuffer.allocate(ChassisVelocities.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
ChassisSpeeds.struct.pack(buffer, DATA);
ChassisVelocities.struct.pack(buffer, DATA);
buffer.rewind();
ChassisSpeeds data = ChassisSpeeds.struct.unpack(buffer);
ChassisVelocities data = ChassisVelocities.struct.unpack(buffer);
assertEquals(DATA.vx, data.vx);
assertEquals(DATA.vy, data.vy);
assertEquals(DATA.omega, data.omega);

14
wpimath/src/test/java/org/wpilib/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java → wpimath/src/test/java/org/wpilib/math/kinematics/struct/DifferentialDriveWheelVelocitiesStructTest.java
View File

@@ -9,20 +9,20 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
import org.wpilib.math.kinematics.DifferentialDriveWheelSpeeds;
import org.wpilib.math.kinematics.DifferentialDriveWheelVelocities;
class DifferentialDriveWheelSpeedsStructTest {
private static final DifferentialDriveWheelSpeeds DATA =
new DifferentialDriveWheelSpeeds(1.74, 35.04);
class DifferentialDriveWheelVelocitiesStructTest {
private static final DifferentialDriveWheelVelocities DATA =
new DifferentialDriveWheelVelocities(1.74, 35.04);
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelSpeeds.struct.getSize());
ByteBuffer buffer = ByteBuffer.allocate(DifferentialDriveWheelVelocities.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
DifferentialDriveWheelSpeeds.struct.pack(buffer, DATA);
DifferentialDriveWheelVelocities.struct.pack(buffer, DATA);
buffer.rewind();
DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.struct.unpack(buffer);
DifferentialDriveWheelVelocities data = DifferentialDriveWheelVelocities.struct.unpack(buffer);
assertEquals(DATA.left, data.left);
assertEquals(DATA.right, data.right);
}

14
wpimath/src/test/java/org/wpilib/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java → wpimath/src/test/java/org/wpilib/math/kinematics/struct/MecanumDriveWheelVelocitiesStructTest.java
View File

@@ -9,20 +9,20 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
import org.wpilib.math.kinematics.MecanumDriveWheelSpeeds;
import org.wpilib.math.kinematics.MecanumDriveWheelVelocities;
class MecanumDriveWheelSpeedsStructTest {
private static final MecanumDriveWheelSpeeds DATA =
new MecanumDriveWheelSpeeds(2.29, 17.4, 4.4, 0.229);
class MecanumDriveWheelVelocitiesStructTest {
private static final MecanumDriveWheelVelocities DATA =
new MecanumDriveWheelVelocities(2.29, 17.4, 4.4, 0.229);
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveWheelSpeeds.struct.getSize());
ByteBuffer buffer = ByteBuffer.allocate(MecanumDriveWheelVelocities.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
MecanumDriveWheelSpeeds.struct.pack(buffer, DATA);
MecanumDriveWheelVelocities.struct.pack(buffer, DATA);
buffer.rewind();
MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.struct.unpack(buffer);
MecanumDriveWheelVelocities data = MecanumDriveWheelVelocities.struct.unpack(buffer);
assertEquals(DATA.frontLeft, data.frontLeft);
assertEquals(DATA.frontRight, data.frontRight);
assertEquals(DATA.rearLeft, data.rearLeft);

15
wpimath/src/test/java/org/wpilib/math/kinematics/struct/SwerveModuleStateStructTest.java → wpimath/src/test/java/org/wpilib/math/kinematics/struct/SwerveModuleVelocityStructTest.java
View File

@@ -10,20 +10,21 @@ import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import org.junit.jupiter.api.Test;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.SwerveModuleState;
import org.wpilib.math.kinematics.SwerveModuleVelocity;
class SwerveModuleStateStructTest {
private static final SwerveModuleState DATA = new SwerveModuleState(22.9, new Rotation2d(3.3));
class SwerveModuleVelocityStructTest {
private static final SwerveModuleVelocity DATA =
new SwerveModuleVelocity(22.9, new Rotation2d(3.3));
@Test
void testRoundtrip() {
ByteBuffer buffer = ByteBuffer.allocate(SwerveModuleState.struct.getSize());
ByteBuffer buffer = ByteBuffer.allocate(SwerveModuleVelocity.struct.getSize());
buffer.order(ByteOrder.LITTLE_ENDIAN);
SwerveModuleState.struct.pack(buffer, DATA);
SwerveModuleVelocity.struct.pack(buffer, DATA);
buffer.rewind();
SwerveModuleState data = SwerveModuleState.struct.unpack(buffer);
assertEquals(DATA.speed, data.speed);
SwerveModuleVelocity data = SwerveModuleVelocity.struct.unpack(buffer);
assertEquals(DATA.velocity, data.velocity);
assertEquals(DATA.angle, data.angle);
}
}

11
wpimath/src/test/java/org/wpilib/math/trajectory/DifferentialDriveKinematicsConstraintTest.java
View File

@@ -9,7 +9,7 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
import java.util.Collections;
import org.junit.jupiter.api.Test;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.DifferentialDriveKinematics;
import org.wpilib.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
import org.wpilib.math.util.Units;
@@ -30,14 +30,15 @@ class DifferentialDriveKinematicsConstraintTest {
while (t < duration) {
var point = trajectory.sample(t);
var chassisSpeeds = new ChassisSpeeds(point.velocity, 0, point.velocity * point.curvature);
var chassisVelocities =
new ChassisVelocities(point.velocity, 0, point.velocity * point.curvature);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
var wheelVelocities = kinematics.toWheelVelocities(chassisVelocities);
t += dt;
assertAll(
() -> assertTrue(wheelSpeeds.left <= maxVelocity + 0.05),
() -> assertTrue(wheelSpeeds.right <= maxVelocity + 0.05));
() -> assertTrue(wheelVelocities.left <= maxVelocity + 0.05),
() -> assertTrue(wheelVelocities.right <= maxVelocity + 0.05));
}
}
}

19
wpimath/src/test/java/org/wpilib/math/trajectory/DifferentialDriveVoltageConstraintTest.java
View File

@@ -14,7 +14,7 @@ import org.junit.jupiter.api.Test;
import org.wpilib.math.controller.SimpleMotorFeedforward;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.kinematics.ChassisSpeeds;
import org.wpilib.math.kinematics.ChassisVelocities;
import org.wpilib.math.kinematics.DifferentialDriveKinematics;
import org.wpilib.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
@@ -36,8 +36,9 @@ class DifferentialDriveVoltageConstraintTest {
while (t < duration) {
var point = trajectory.sample(t);
var chassisSpeeds = new ChassisSpeeds(point.velocity, 0, point.velocity * point.curvature);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
var chassisVelocities =
new ChassisVelocities(point.velocity, 0, point.velocity * point.curvature);
var wheelVelocities = kinematics.toWheelVelocities(chassisVelocities);
t += dt;
var acceleration = point.acceleration;
@@ -47,19 +48,23 @@ class DifferentialDriveVoltageConstraintTest {
assertAll(
() ->
assertTrue(
feedforward.calculate(wheelSpeeds.left, wheelSpeeds.left + dt * acceleration)
feedforward.calculate(
wheelVelocities.left, wheelVelocities.left + dt * acceleration)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(wheelSpeeds.left, wheelSpeeds.left + dt * acceleration)
feedforward.calculate(
wheelVelocities.left, wheelVelocities.left + dt * acceleration)
>= -maxVoltage - 0.05),
() ->
assertTrue(
feedforward.calculate(wheelSpeeds.right, wheelSpeeds.right + dt * acceleration)
feedforward.calculate(
wheelVelocities.right, wheelVelocities.right + dt * acceleration)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(wheelSpeeds.right, wheelSpeeds.right + dt * acceleration)
feedforward.calculate(
wheelVelocities.right, wheelVelocities.right + dt * acceleration)
>= -maxVoltage - 0.05));
}
}

18
wpimath/src/test/java/org/wpilib/math/trajectory/ExponentialProfileTest.java
View File

@@ -137,36 +137,36 @@ class ExponentialProfileTest {
assertEquals(state, goal);
}
// Checks to make sure that it hits top speed
// Checks to make sure that it hits top velocity
@Test
void topSpeed() {
void topVelocity() {
ExponentialProfile.State goal = new ExponentialProfile.State(40, 0);
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
ExponentialProfile profile = new ExponentialProfile(constraints);
double maxSpeed = 0;
double maxVelocity = 0;
for (int i = 0; i < 900; ++i) {
state = checkDynamics(profile, state, goal);
maxSpeed = Math.max(maxSpeed, state.velocity);
maxVelocity = Math.max(maxVelocity, state.velocity);
}
assertNear(constraints.maxVelocity(), maxSpeed, 10e-5);
assertNear(constraints.maxVelocity(), maxVelocity, 10e-5);
assertEquals(state, goal);
}
@Test
void topSpeedBackward() {
void topVelocityBackward() {
ExponentialProfile.State goal = new ExponentialProfile.State(-40, 0);
ExponentialProfile.State state = new ExponentialProfile.State(0, 0);
ExponentialProfile profile = new ExponentialProfile(constraints);
double maxSpeed = 0;
double maxVelocity = 0;
for (int i = 0; i < 900; ++i) {
state = checkDynamics(profile, state, goal);
maxSpeed = Math.min(maxSpeed, state.velocity);
maxVelocity = Math.min(maxVelocity, state.velocity);
}
assertNear(-constraints.maxVelocity(), maxSpeed, 10e-5);
assertNear(-constraints.maxVelocity(), maxVelocity, 10e-5);
assertEquals(state, goal);
}

4
wpimath/src/test/java/org/wpilib/math/trajectory/TrapezoidProfileTest.java
View File

@@ -132,9 +132,9 @@ class TrapezoidProfileTest {
assertEquals(state, goal);
}
// Checks to make sure that it hits top speed
// Checks to make sure that it hits top velocity
@Test
void topSpeed() {
void topVelocity() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();