Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-06 03:31:43 +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();

20
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp
View File

@@ -26,8 +26,8 @@ void testFollowTrajectory(
const wpi::math::DifferentialDriveKinematics& kinematics,
wpi::math::DifferentialDrivePoseEstimator3d& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -86,12 +86,12 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
leftDistance += wheelSpeeds.left * dt;
rightDistance += wheelSpeeds.right * dt;
leftDistance += wheelVelocities.left * dt;
rightDistance += wheelVelocities.right * dt;
auto xhat = estimator.UpdateWithTime(
t,
@@ -181,8 +181,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -226,8 +226,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

20
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -26,8 +26,8 @@ void testFollowTrajectory(
const wpi::math::DifferentialDriveKinematics& kinematics,
wpi::math::DifferentialDrivePoseEstimator& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -85,12 +85,12 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
leftDistance += wheelSpeeds.left * dt;
rightDistance += wheelSpeeds.right * dt;
leftDistance += wheelVelocities.left * dt;
rightDistance += wheelVelocities.right * dt;
auto xhat = estimator.UpdateWithTime(
t,
@@ -174,8 +174,8 @@ TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -218,8 +218,8 @@ TEST(DifferentialDrivePoseEstimatorTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

24
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp
View File

@@ -21,8 +21,8 @@ void testFollowTrajectory(
const wpi::math::MecanumDriveKinematics& kinematics,
wpi::math::MecanumDrivePoseEstimator3d& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -79,14 +79,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto 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;
auto xhat = estimator.UpdateWithTime(
t,
@@ -175,8 +175,8 @@ TEST(MecanumDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -220,8 +220,8 @@ TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

24
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -21,8 +21,8 @@ void testFollowTrajectory(
const wpi::math::MecanumDriveKinematics& kinematics,
wpi::math::MecanumDrivePoseEstimator& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -78,14 +78,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
auto 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;
auto xhat = estimator.UpdateWithTime(
t,
@@ -169,8 +169,8 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
trajectory.InitialPose(), {0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -214,8 +214,8 @@ TEST(MecanumDrivePoseEstimatorTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

21
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
View File

@@ -22,8 +22,8 @@ void testFollowTrajectory(
const wpi::math::SwerveDriveKinematics<4>& kinematics,
wpi::math::SwerveDrivePoseEstimator3d<4>& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -81,13 +81,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds);
auto moduleVelocities =
kinematics.ToSwerveModuleVelocities(chassisVelocities);
for (size_t i = 0; i < 4; i++) {
positions[i].distance += moduleStates[i].speed * dt;
positions[i].angle = moduleStates[i].angle;
positions[i].distance += moduleVelocities[i].velocity * dt;
positions[i].angle = moduleVelocities[i].angle;
}
auto xhat = estimator.UpdateWithTime(
@@ -180,8 +181,8 @@ TEST(SwerveDrivePoseEstimator3dTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -229,8 +230,8 @@ TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

21
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -22,8 +22,8 @@ void testFollowTrajectory(
const wpi::math::SwerveDriveKinematics<4>& kinematics,
wpi::math::SwerveDrivePoseEstimator<4>& estimator,
const wpi::math::Trajectory& trajectory,
std::function<wpi::math::ChassisSpeeds(wpi::math::Trajectory::State&)>
chassisSpeedsGenerator,
std::function<wpi::math::ChassisVelocities(wpi::math::Trajectory::State&)>
chassisVelocitiesGenerator,
std::function<wpi::math::Pose2d(wpi::math::Trajectory::State&)>
visionMeasurementGenerator,
const wpi::math::Pose2d& startingPose, const wpi::math::Pose2d& endingPose,
@@ -79,13 +79,14 @@ void testFollowTrajectory(
visionLog.push_back({t, visionEntry.first, visionEntry.second});
}
auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState);
auto chassisVelocities = chassisVelocitiesGenerator(groundTruthState);
auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds);
auto moduleVelocities =
kinematics.ToSwerveModuleVelocities(chassisVelocities);
for (size_t i = 0; i < 4; i++) {
positions[i].distance += moduleStates[i].speed * dt;
positions[i].angle = moduleStates[i].angle;
positions[i].distance += moduleVelocities[i].velocity * dt;
positions[i].angle = moduleVelocities[i].angle;
}
auto xhat = estimator.UpdateWithTime(
@@ -173,8 +174,8 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{state.velocity, 0_mps,
state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
{0_m, 0_m, wpi::math::Rotation2d{45_deg}},
@@ -222,8 +223,8 @@ TEST(SwerveDrivePoseEstimatorTest, BadInitialPose) {
testFollowTrajectory(
kinematics, estimator, trajectory,
[&](wpi::math::Trajectory::State& state) {
return wpi::math::ChassisSpeeds{state.velocity, 0_mps,
state.velocity * state.curvature};
return wpi::math::ChassisVelocities{
state.velocity, 0_mps, state.velocity * state.curvature};
},
[&](wpi::math::Trajectory::State& state) { return state.pose; },
initial_pose, {0_m, 0_m, wpi::math::Rotation2d{45_deg}}, 20_ms,

103
wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
View File

@@ -1,103 +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.
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include <cmath>
#include <gtest/gtest.h>
static constexpr double kEpsilon = 1E-9;
TEST(ChassisSpeedsTest, Discretize) {
constexpr wpi::math::ChassisSpeeds target{1_mps, 0_mps, 0.5_rad_per_s};
constexpr wpi::units::second_t duration = 1_s;
constexpr wpi::units::second_t dt = 10_ms;
const auto speeds = target.Discretize(duration);
const wpi::math::Twist2d twist{speeds.vx * dt, speeds.vy * dt,
speeds.omega * dt};
wpi::math::Pose2d pose;
for (wpi::units::second_t time = 0_s; time < duration; time += dt) {
pose = pose + twist.Exp();
}
EXPECT_NEAR((target.vx * duration).value(), pose.X().value(), kEpsilon);
EXPECT_NEAR((target.vy * duration).value(), pose.Y().value(), kEpsilon);
EXPECT_NEAR((target.omega * duration).value(),
pose.Rotation().Radians().value(), kEpsilon);
}
TEST(ChassisSpeedsTest, ToRobotRelative) {
const auto chassisSpeeds =
wpi::math::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToRobotRelative(
-90.0_deg);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, ToFieldRelative) {
const auto chassisSpeeds =
wpi::math::ChassisSpeeds{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(
45.0_deg);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, Plus) {
const wpi::math::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisSpeeds result = left + right;
EXPECT_NEAR(3.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(2.0, result.vy.value(), kEpsilon);
EXPECT_NEAR(1.0, result.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, Minus) {
const wpi::math::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds right{2.0_mps, 0.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisSpeeds result = left - right;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(0, result.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, result.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, UnaryMinus) {
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds result = -speeds;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(-0.5, result.vy.value(), kEpsilon);
EXPECT_NEAR(-0.75, result.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, Multiplication) {
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds result = speeds * 2;
EXPECT_NEAR(2.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, result.vy.value(), kEpsilon);
EXPECT_NEAR(1.5, result.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, Division) {
const wpi::math::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisSpeeds result = speeds / 2;
EXPECT_NEAR(0.5, result.vx.value(), kEpsilon);
EXPECT_NEAR(0.25, result.vy.value(), kEpsilon);
EXPECT_NEAR(0.375, result.omega.value(), kEpsilon);
}

106
wpimath/src/test/native/cpp/kinematics/ChassisVelocitiesTest.cpp Normal file
View File

@@ -0,0 +1,106 @@
// 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.
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include <cmath>
#include <gtest/gtest.h>
static constexpr double kEpsilon = 1E-9;
TEST(ChassisVelocitiesTest, Discretize) {
constexpr wpi::math::ChassisVelocities target{1_mps, 0_mps, 0.5_rad_per_s};
constexpr wpi::units::second_t duration = 1_s;
constexpr wpi::units::second_t dt = 10_ms;
const auto velocities = target.Discretize(duration);
const wpi::math::Twist2d twist{velocities.vx * dt, velocities.vy * dt,
velocities.omega * dt};
wpi::math::Pose2d pose;
for (wpi::units::second_t time = 0_s; time < duration; time += dt) {
pose = pose + twist.Exp();
}
EXPECT_NEAR((target.vx * duration).value(), pose.X().value(), kEpsilon);
EXPECT_NEAR((target.vy * duration).value(), pose.Y().value(), kEpsilon);
EXPECT_NEAR((target.omega * duration).value(),
pose.Rotation().Radians().value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, ToRobotRelative) {
const auto chassisVelocities =
wpi::math::ChassisVelocities{1_mps, 0_mps, 0.5_rad_per_s}.ToRobotRelative(
-90.0_deg);
EXPECT_NEAR(0.0, chassisVelocities.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, chassisVelocities.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisVelocities.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, ToFieldRelative) {
const auto chassisVelocities =
wpi::math::ChassisVelocities{1_mps, 0_mps, 0.5_rad_per_s}.ToFieldRelative(
45.0_deg);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisVelocities.vx.value(), kEpsilon);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisVelocities.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisVelocities.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, Plus) {
const wpi::math::ChassisVelocities left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisVelocities right{2.0_mps, 1.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisVelocities result = left + right;
EXPECT_NEAR(3.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(2.0, result.vy.value(), kEpsilon);
EXPECT_NEAR(1.0, result.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, Minus) {
const wpi::math::ChassisVelocities left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const wpi::math::ChassisVelocities right{2.0_mps, 0.5_mps, 0.25_rad_per_s};
const wpi::math::ChassisVelocities result = left - right;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(0, result.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, result.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, UnaryMinus) {
const wpi::math::ChassisVelocities velocities{1.0_mps, 0.5_mps,
0.75_rad_per_s};
const wpi::math::ChassisVelocities result = -velocities;
EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(-0.5, result.vy.value(), kEpsilon);
EXPECT_NEAR(-0.75, result.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, Multiplication) {
const wpi::math::ChassisVelocities velocities{1.0_mps, 0.5_mps,
0.75_rad_per_s};
const wpi::math::ChassisVelocities result = velocities * 2;
EXPECT_NEAR(2.0, result.vx.value(), kEpsilon);
EXPECT_NEAR(1.0, result.vy.value(), kEpsilon);
EXPECT_NEAR(1.5, result.omega.value(), kEpsilon);
}
TEST(ChassisVelocitiesTest, Division) {
const wpi::math::ChassisVelocities velocities{1.0_mps, 0.5_mps,
0.75_rad_per_s};
const wpi::math::ChassisVelocities result = velocities / 2;
EXPECT_NEAR(0.5, result.vx.value(), kEpsilon);
EXPECT_NEAR(0.25, result.vy.value(), kEpsilon);
EXPECT_NEAR(0.375, result.omega.value(), kEpsilon);
}

61
wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
View File

@@ -9,7 +9,7 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/ChassisAccelerations.hpp"
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/units/angular_velocity.hpp"
#include "wpi/units/length.hpp"
#include "wpi/units/velocity.hpp"
@@ -20,62 +20,67 @@ static constexpr double kEpsilon = 1E-9;
TEST(DifferentialDriveKinematicsTest, InverseKinematicsFromZero) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds;
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
const ChassisVelocities chassisVelocities;
const auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
EXPECT_NEAR(wheelSpeeds.left.value(), 0, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), 0, kEpsilon);
EXPECT_NEAR(wheelVelocities.left.value(), 0, kEpsilon);
EXPECT_NEAR(wheelVelocities.right.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsFromZero) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds;
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
const DifferentialDriveWheelVelocities wheelVelocities;
const auto chassisVelocities =
kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForStraightLine) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
const ChassisVelocities chassisVelocities{3.0_mps, 0_mps, 0_rad_per_s};
const auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
EXPECT_NEAR(wheelSpeeds.left.value(), 3, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), 3, kEpsilon);
EXPECT_NEAR(wheelVelocities.left.value(), 3, kEpsilon);
EXPECT_NEAR(wheelVelocities.right.value(), 3, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
const DifferentialDriveWheelVelocities wheelVelocities{3.0_mps, 3.0_mps};
const auto chassisVelocities =
kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(chassisSpeeds.vx.value(), 3, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 3, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 0, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const ChassisSpeeds chassisSpeeds{
const ChassisVelocities chassisVelocities{
0.0_mps, 0.0_mps, wpi::units::radians_per_second_t{std::numbers::pi}};
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
const auto wheelVelocities = kinematics.ToWheelVelocities(chassisVelocities);
EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * std::numbers::pi, kEpsilon);
EXPECT_NEAR(wheelSpeeds.right.value(), +0.381 * std::numbers::pi, kEpsilon);
EXPECT_NEAR(wheelVelocities.left.value(), -0.381 * std::numbers::pi,
kEpsilon);
EXPECT_NEAR(wheelVelocities.right.value(), +0.381 * std::numbers::pi,
kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
const DifferentialDriveKinematics kinematics{0.381_m * 2};
const DifferentialDriveWheelSpeeds wheelSpeeds{
const DifferentialDriveWheelVelocities wheelVelocities{
wpi::units::meters_per_second_t{+0.381 * std::numbers::pi},
wpi::units::meters_per_second_t{-0.381 * std::numbers::pi}};
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
const auto chassisVelocities =
kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), -std::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), -std::numbers::pi, kEpsilon);
}
TEST(DifferentialDriveKinematicsTest, InverseAccelerationsForZeros) {

54
wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelSpeedsTest.cpp
View File

@@ -1,54 +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.
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include <gtest/gtest.h>
TEST(DifferentialDriveWheelSpeedsTest, Plus) {
const wpi::math::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds right{2.0_mps, 1.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds result = left + right;
EXPECT_EQ(3.0, result.left.value());
EXPECT_EQ(2.0, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, Minus) {
const wpi::math::DifferentialDriveWheelSpeeds left{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds right{2.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds result = left - right;
EXPECT_EQ(-1.0, result.left.value());
EXPECT_EQ(0, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, UnaryMinus) {
const wpi::math::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds result = -speeds;
EXPECT_EQ(-1.0, result.left.value());
EXPECT_EQ(-0.5, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, Multiplication) {
const wpi::math::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds result = speeds * 2;
EXPECT_EQ(2.0, result.left.value());
EXPECT_EQ(1.0, result.right.value());
}
TEST(DifferentialDriveWheelSpeedsTest, Division) {
const wpi::math::DifferentialDriveWheelSpeeds speeds{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelSpeeds result = speeds / 2;
EXPECT_EQ(0.5, result.left.value());
EXPECT_EQ(0.25, result.right.value());
}

57
wpimath/src/test/native/cpp/kinematics/DifferentialDriveWheelVelocitiesTest.cpp Normal file
View File

@@ -0,0 +1,57 @@
// 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.
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include <gtest/gtest.h>
TEST(DifferentialDriveWheelVelocitiesTest, Plus) {
const wpi::math::DifferentialDriveWheelVelocities left{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities right{2.0_mps, 1.5_mps};
const wpi::math::DifferentialDriveWheelVelocities result = left + right;
EXPECT_EQ(3.0, result.left.value());
EXPECT_EQ(2.0, result.right.value());
}
TEST(DifferentialDriveWheelVelocitiesTest, Minus) {
const wpi::math::DifferentialDriveWheelVelocities left{1.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities right{2.0_mps, 0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities result = left - right;
EXPECT_EQ(-1.0, result.left.value());
EXPECT_EQ(0, result.right.value());
}
TEST(DifferentialDriveWheelVelocitiesTest, UnaryMinus) {
const wpi::math::DifferentialDriveWheelVelocities velocities{1.0_mps,
0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities result = -velocities;
EXPECT_EQ(-1.0, result.left.value());
EXPECT_EQ(-0.5, result.right.value());
}
TEST(DifferentialDriveWheelVelocitiesTest, Multiplication) {
const wpi::math::DifferentialDriveWheelVelocities velocities{1.0_mps,
0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities result = velocities * 2;
EXPECT_EQ(2.0, result.left.value());
EXPECT_EQ(1.0, result.right.value());
}
TEST(DifferentialDriveWheelVelocitiesTest, Division) {
const wpi::math::DifferentialDriveWheelVelocities velocities{1.0_mps,
0.5_mps};
const wpi::math::DifferentialDriveWheelVelocities result = velocities / 2;
EXPECT_EQ(0.5, result.left.value());
EXPECT_EQ(0.25, result.right.value());
}

170
wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
View File

@@ -24,22 +24,22 @@ class MecanumDriveKinematicsTest : public ::testing::Test {
};
TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
ChassisVelocities velocities{5_mps, 0_mps, 0_rad_per_s};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities);
EXPECT_NEAR(5.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(5.0, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(5.0, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(5.0, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 5_mps, 5_mps, 5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
MecanumDriveWheelVelocities wheelVelocities{5_mps, 5_mps, 5_mps, 5_mps};
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(5.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(5.0, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
@@ -52,22 +52,22 @@ TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
}
TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
ChassisVelocities velocities{0_mps, 4_mps, 0_rad_per_s};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities);
EXPECT_NEAR(-4.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(4.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(4.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(-4.0, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(-4.0, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(4.0, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(4.0, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(-4.0, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 5_mps, 5_mps, -5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
MecanumDriveWheelVelocities wheelVelocities{-5_mps, 5_mps, 5_mps, -5_mps};
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(5.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(5.0, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematicsWithDeltas) {
@@ -80,24 +80,24 @@ TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematicsWithDeltas) {
}
TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
ChassisVelocities velocities{
0_mps, 0_mps, wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities);
EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(-150.79644737, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(-150.79644737, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(150.79644737, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-150.79644737_mps, 150.79644737_mps,
-150.79644737_mps, 150.79644737_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
MecanumDriveWheelVelocities wheelVelocities{
-150.79644737_mps, 150.79644737_mps, -150.79644737_mps, 150.79644737_mps};
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(2 * std::numbers::pi, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(0.0, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(2 * std::numbers::pi, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematicsWithDeltas) {
@@ -111,24 +111,24 @@ TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematicsWithDeltas) {
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
ChassisVelocities velocities{2_mps, 3_mps, 1_rad_per_s};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities);
EXPECT_NEAR(-25.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(29.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-19.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(23.0, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(-25.0, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(29.0, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(-19.0, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(23.0, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-17.677670_mps, 20.506097_mps,
-13.435_mps, 16.26_mps};
MecanumDriveWheelVelocities wheelVelocities{-17.677670_mps, 20.506097_mps,
-13.435_mps, 16.26_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(1.41335, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(2.1221, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(1.41335, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(2.1221, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -144,23 +144,23 @@ TEST_F(MecanumDriveKinematicsTest,
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
ChassisVelocities velocities{0_mps, 0_mps, 1_rad_per_s};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities, m_fl);
EXPECT_NEAR(0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(24.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-24.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(48.0, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(0, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(24.0, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(-24.0, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(48.0, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{0_mps, 16.971_mps, -16.971_mps,
33.941_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
MecanumDriveWheelVelocities wheelVelocities{0_mps, 16.971_mps, -16.971_mps,
33.941_mps};
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(8.48525, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(-8.48525, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(8.48525, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(-8.48525, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -175,24 +175,24 @@ TEST_F(MecanumDriveKinematicsTest,
TEST_F(MecanumDriveKinematicsTest,
OffCenterTranslationRotationInverseKinematics) {
ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
ChassisVelocities velocities{5_mps, 2_mps, 1_rad_per_s};
auto moduleVelocities = kinematics.ToWheelVelocities(velocities, m_fl);
EXPECT_NEAR(3.0, moduleStates.frontLeft.value(), 0.1);
EXPECT_NEAR(31.0, moduleStates.frontRight.value(), 0.1);
EXPECT_NEAR(-17.0, moduleStates.rearLeft.value(), 0.1);
EXPECT_NEAR(51.0, moduleStates.rearRight.value(), 0.1);
EXPECT_NEAR(3.0, moduleVelocities.frontLeft.value(), 0.1);
EXPECT_NEAR(31.0, moduleVelocities.frontRight.value(), 0.1);
EXPECT_NEAR(-17.0, moduleVelocities.rearLeft.value(), 0.1);
EXPECT_NEAR(51.0, moduleVelocities.rearRight.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
OffCenterTranslationRotationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{2.12_mps, 21.92_mps, -12.02_mps,
36.06_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
MecanumDriveWheelVelocities wheelVelocities{2.12_mps, 21.92_mps, -12.02_mps,
36.06_mps};
auto chassisVelocities = kinematics.ToChassisVelocities(wheelVelocities);
EXPECT_NEAR(12.02, chassisSpeeds.vx.value(), 0.1);
EXPECT_NEAR(-7.07, chassisSpeeds.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
EXPECT_NEAR(12.02, chassisVelocities.vx.value(), 0.1);
EXPECT_NEAR(-7.07, chassisVelocities.vy.value(), 0.1);
EXPECT_NEAR(0.707, chassisVelocities.omega.value(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -206,27 +206,29 @@ TEST_F(MecanumDriveKinematicsTest,
}
TEST_F(MecanumDriveKinematicsTest, Desaturate) {
auto wheelSpeeds =
MecanumDriveWheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps}.Desaturate(5.5_mps);
auto wheelVelocities =
MecanumDriveWheelVelocities{5_mps, 6_mps, 4_mps, 7_mps}.Desaturate(
5.5_mps);
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(wheelSpeeds.frontLeft.value(), 5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearRight.value(), 7.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.frontLeft.value(), 5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.frontRight.value(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.rearRight.value(), 7.0 * kFactor, 1E-9);
}
TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeSpeeds) {
auto wheelSpeeds =
MecanumDriveWheelSpeeds{-5_mps, 6_mps, 4_mps, -7_mps}.Desaturate(5.5_mps);
TEST_F(MecanumDriveKinematicsTest, DesaturateNegativeVelocities) {
auto wheelVelocities =
MecanumDriveWheelVelocities{-5_mps, 6_mps, 4_mps, -7_mps}.Desaturate(
5.5_mps);
constexpr double kFactor = 5.5 / 7.0;
EXPECT_NEAR(wheelSpeeds.frontLeft.value(), -5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelSpeeds.rearRight.value(), -7.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.frontLeft.value(), -5.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.frontRight.value(), 6.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.rearLeft.value(), 4.0 * kFactor, 1E-9);
EXPECT_NEAR(wheelVelocities.rearRight.value(), -7.0 * kFactor, 1E-9);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineInverseAccelerations) {

36
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp
View File

@@ -131,19 +131,19 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) {
while (t < trajectory.TotalTime()) {
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
auto wheelVelocities = kinematics.ToWheelVelocities(
{groundTruthState.velocity, 0_mps,
groundTruthState.velocity * groundTruthState.curvature});
wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
wheelVelocities.frontLeft += distribution(generator) * 0.1_mps;
wheelVelocities.frontRight += distribution(generator) * 0.1_mps;
wheelVelocities.rearLeft += distribution(generator) * 0.1_mps;
wheelVelocities.rearRight += distribution(generator) * 0.1_mps;
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;
auto xhat = odometry.Update(
wpi::math::Rotation3d{
@@ -198,20 +198,20 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) {
while (t < trajectory.TotalTime()) {
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
auto wheelVelocities = kinematics.ToWheelVelocities(
{groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
0_rad_per_s});
wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
wheelVelocities.frontLeft += distribution(generator) * 0.1_mps;
wheelVelocities.frontRight += distribution(generator) * 0.1_mps;
wheelVelocities.rearLeft += distribution(generator) * 0.1_mps;
wheelVelocities.rearRight += distribution(generator) * 0.1_mps;
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;
auto xhat = odometry.Update(
wpi::math::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad},

36
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View File

@@ -110,19 +110,19 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) {
while (t < trajectory.TotalTime()) {
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
auto wheelVelocities = kinematics.ToWheelVelocities(
{groundTruthState.velocity, 0_mps,
groundTruthState.velocity * groundTruthState.curvature});
wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
wheelVelocities.frontLeft += distribution(generator) * 0.1_mps;
wheelVelocities.frontRight += distribution(generator) * 0.1_mps;
wheelVelocities.rearLeft += distribution(generator) * 0.1_mps;
wheelVelocities.rearRight += distribution(generator) * 0.1_mps;
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;
auto xhat = odometry.Update(
groundTruthState.pose.Rotation() +
@@ -176,20 +176,20 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) {
while (t < trajectory.TotalTime()) {
wpi::math::Trajectory::State groundTruthState = trajectory.Sample(t);
auto wheelSpeeds = kinematics.ToWheelSpeeds(
auto wheelVelocities = kinematics.ToWheelVelocities(
{groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(),
groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(),
0_rad_per_s});
wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.frontRight += distribution(generator) * 0.1_mps;
wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps;
wheelSpeeds.rearRight += distribution(generator) * 0.1_mps;
wheelVelocities.frontLeft += distribution(generator) * 0.1_mps;
wheelVelocities.frontRight += distribution(generator) * 0.1_mps;
wheelVelocities.rearLeft += distribution(generator) * 0.1_mps;
wheelVelocities.rearRight += distribution(generator) * 0.1_mps;
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;
auto xhat = odometry.Update(
wpi::math::Rotation2d{distribution(generator) * 0.05_rad},

71
wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelSpeedsTest.cpp
View File

@@ -1,71 +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.
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include <gtest/gtest.h>
TEST(MecanumDriveWheelSpeedsTest, Plus) {
const wpi::math::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps,
1.0_mps};
const wpi::math::MecanumDriveWheelSpeeds result = left + right;
EXPECT_EQ(3.0, result.frontLeft.value());
EXPECT_EQ(2.0, result.frontRight.value());
EXPECT_EQ(2.5, result.rearLeft.value());
EXPECT_EQ(2.5, result.rearRight.value());
}
TEST(MecanumDriveWheelSpeedsTest, Minus) {
const wpi::math::MecanumDriveWheelSpeeds left{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds right{2.0_mps, 1.5_mps, 0.5_mps,
1.0_mps};
const wpi::math::MecanumDriveWheelSpeeds result = left - right;
EXPECT_EQ(-1.0, result.frontLeft.value());
EXPECT_EQ(-1.0, result.frontRight.value());
EXPECT_EQ(1.5, result.rearLeft.value());
EXPECT_EQ(0.5, result.rearRight.value());
}
TEST(MecanumDriveWheelSpeedsTest, UnaryMinus) {
const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds result = -speeds;
EXPECT_EQ(-1.0, result.frontLeft.value());
EXPECT_EQ(-0.5, result.frontRight.value());
EXPECT_EQ(-2.0, result.rearLeft.value());
EXPECT_EQ(-1.5, result.rearRight.value());
}
TEST(MecanumDriveWheelSpeedsTest, Multiplication) {
const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds result = speeds * 2;
EXPECT_EQ(2.0, result.frontLeft.value());
EXPECT_EQ(1.0, result.frontRight.value());
EXPECT_EQ(4.0, result.rearLeft.value());
EXPECT_EQ(3.0, result.rearRight.value());
}
TEST(MecanumDriveWheelSpeedsTest, Division) {
const wpi::math::MecanumDriveWheelSpeeds speeds{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelSpeeds result = speeds / 2;
EXPECT_EQ(0.5, result.frontLeft.value());
EXPECT_EQ(0.25, result.frontRight.value());
EXPECT_EQ(1.0, result.rearLeft.value());
EXPECT_EQ(0.75, result.rearRight.value());
}

71
wpimath/src/test/native/cpp/kinematics/MecanumDriveWheelVelocitiesTest.cpp Normal file
View File

@@ -0,0 +1,71 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include <gtest/gtest.h>
TEST(MecanumDriveWheelVelocitiesTest, Plus) {
const wpi::math::MecanumDriveWheelVelocities left{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelVelocities right{2.0_mps, 1.5_mps, 0.5_mps,
1.0_mps};
const wpi::math::MecanumDriveWheelVelocities result = left + right;
EXPECT_EQ(3.0, result.frontLeft.value());
EXPECT_EQ(2.0, result.frontRight.value());
EXPECT_EQ(2.5, result.rearLeft.value());
EXPECT_EQ(2.5, result.rearRight.value());
}
TEST(MecanumDriveWheelVelocitiesTest, Minus) {
const wpi::math::MecanumDriveWheelVelocities left{1.0_mps, 0.5_mps, 2.0_mps,
1.5_mps};
const wpi::math::MecanumDriveWheelVelocities right{2.0_mps, 1.5_mps, 0.5_mps,
1.0_mps};
const wpi::math::MecanumDriveWheelVelocities result = left - right;
EXPECT_EQ(-1.0, result.frontLeft.value());
EXPECT_EQ(-1.0, result.frontRight.value());
EXPECT_EQ(1.5, result.rearLeft.value());
EXPECT_EQ(0.5, result.rearRight.value());
}
TEST(MecanumDriveWheelVelocitiesTest, UnaryMinus) {
const wpi::math::MecanumDriveWheelVelocities velocities{1.0_mps, 0.5_mps,
2.0_mps, 1.5_mps};
const wpi::math::MecanumDriveWheelVelocities result = -velocities;
EXPECT_EQ(-1.0, result.frontLeft.value());
EXPECT_EQ(-0.5, result.frontRight.value());
EXPECT_EQ(-2.0, result.rearLeft.value());
EXPECT_EQ(-1.5, result.rearRight.value());
}
TEST(MecanumDriveWheelVelocitiesTest, Multiplication) {
const wpi::math::MecanumDriveWheelVelocities velocities{1.0_mps, 0.5_mps,
2.0_mps, 1.5_mps};
const wpi::math::MecanumDriveWheelVelocities result = velocities * 2;
EXPECT_EQ(2.0, result.frontLeft.value());
EXPECT_EQ(1.0, result.frontRight.value());
EXPECT_EQ(4.0, result.rearLeft.value());
EXPECT_EQ(3.0, result.rearRight.value());
}
TEST(MecanumDriveWheelVelocitiesTest, Division) {
const wpi::math::MecanumDriveWheelVelocities velocities{1.0_mps, 0.5_mps,
2.0_mps, 1.5_mps};
const wpi::math::MecanumDriveWheelVelocities result = velocities / 2;
EXPECT_EQ(0.5, result.frontLeft.value());
EXPECT_EQ(0.25, result.frontRight.value());
EXPECT_EQ(1.0, result.rearLeft.value());
EXPECT_EQ(0.75, result.rearRight.value());
}

229
wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
View File

@@ -26,14 +26,14 @@ class SwerveDriveKinematicsTest : public ::testing::Test {
};
TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
ChassisSpeeds speeds{5.0_mps, 0.0_mps, 0.0_rad_per_s};
ChassisVelocities velocities{5.0_mps, 0.0_mps, 0.0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleVelocities(velocities);
EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.angle.Radians().value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Radians().value(), 0.0, kEpsilon);
@@ -42,13 +42,14 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
}
TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
SwerveModuleState state{5.0_mps, 0_deg};
SwerveModuleVelocity state{5.0_mps, 0_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
auto chassisVelocities =
m_kinematics.ToChassisVelocities(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
@@ -62,13 +63,13 @@ TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematicsWithDeltas) {
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
ChassisVelocities velocities{0_mps, 5_mps, 0_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleVelocities(velocities);
EXPECT_NEAR(fl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.speed.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 5.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 90.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 90.0, kEpsilon);
@@ -77,12 +78,13 @@ TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
SwerveModuleState state{5_mps, 90_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
SwerveModuleVelocity state{5_mps, 90_deg};
auto chassisVelocities =
m_kinematics.ToChassisVelocities(state, state, state, state);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 5.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 0.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematicsWithDeltas) {
@@ -96,14 +98,14 @@ TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematicsWithDeltas) {
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
ChassisVelocities velocities{
0_mps, 0_mps, wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleVelocities(velocities);
EXPECT_NEAR(fl.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(br.speed.value(), 106.63, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 106.63, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 106.63, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 106.63, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 106.63, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
@@ -112,15 +114,16 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
}
TEST_F(SwerveDriveKinematicsTest, ConserveWheelAngle) {
ChassisSpeeds speeds{0_mps, 0_mps,
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
m_kinematics.ToSwerveModuleStates(speeds);
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
ChassisVelocities velocities{
0_mps, 0_mps, wpi::units::radians_per_second_t{2 * std::numbers::pi}};
m_kinematics.ToSwerveModuleVelocities(velocities);
auto [fl, fr, bl, br] =
m_kinematics.ToSwerveModuleVelocities(ChassisVelocities{});
EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(br.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
@@ -134,12 +137,12 @@ TEST_F(SwerveDriveKinematicsTest, ResetWheelAngle) {
Rotation2d br = {270_deg};
m_kinematics.ResetHeadings(fl, fr, bl, br);
auto [flMod, frMod, blMod, brMod] =
m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
m_kinematics.ToSwerveModuleVelocities(ChassisVelocities{});
EXPECT_NEAR(flMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(frMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(blMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(brMod.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(flMod.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(frMod.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(blMod.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(brMod.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(flMod.angle.Degrees().value(), 0.0, kEpsilon);
EXPECT_NEAR(frMod.angle.Degrees().value(), 90.0, kEpsilon);
@@ -148,16 +151,16 @@ TEST_F(SwerveDriveKinematicsTest, ResetWheelAngle) {
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
SwerveModuleState fl{106.629_mps, 135_deg};
SwerveModuleState fr{106.629_mps, 45_deg};
SwerveModuleState bl{106.629_mps, -135_deg};
SwerveModuleState br{106.629_mps, -45_deg};
SwerveModuleVelocity fl{106.629_mps, 135_deg};
SwerveModuleVelocity fr{106.629_mps, 45_deg};
SwerveModuleVelocity bl{106.629_mps, -135_deg};
SwerveModuleVelocity br{106.629_mps, -45_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
auto chassisVelocities = m_kinematics.ToChassisVelocities(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 2 * std::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematicsWithDeltas) {
@@ -174,14 +177,15 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematicsWithDeltas) {
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps,
wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
ChassisVelocities velocities{
0_mps, 0_mps, wpi::units::radians_per_second_t{2 * std::numbers::pi}};
auto [fl, fr, bl, br] =
m_kinematics.ToSwerveModuleVelocities(velocities, m_fl);
EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 150.796, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 150.796, kEpsilon);
EXPECT_NEAR(br.speed.value(), 213.258, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 150.796, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 150.796, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 213.258, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), 0.0, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), 0.0, kEpsilon);
@@ -190,16 +194,16 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
}
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
SwerveModuleState fl{0.0_mps, 0_deg};
SwerveModuleState fr{150.796_mps, 0_deg};
SwerveModuleState bl{150.796_mps, -90_deg};
SwerveModuleState br{213.258_mps, -45_deg};
SwerveModuleVelocity fl{0.0_mps, 0_deg};
SwerveModuleVelocity fr{150.796_mps, 0_deg};
SwerveModuleVelocity bl{150.796_mps, -90_deg};
SwerveModuleVelocity br{213.258_mps, -45_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
auto chassisVelocities = m_kinematics.ToChassisVelocities(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.value(), 75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), -75.398, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * std::numbers::pi, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 75.398, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), -75.398, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 2 * std::numbers::pi, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
@@ -218,14 +222,14 @@ TEST_F(SwerveDriveKinematicsTest,
TEST_F(SwerveDriveKinematicsTest,
OffCenterCORRotationAndTranslationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
auto [fl, fr, bl, br] =
m_kinematics.ToSwerveModuleStates(speeds, Translation2d{24_m, 0_m});
ChassisVelocities velocities{0_mps, 3.0_mps, 1.5_rad_per_s};
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleVelocities(
velocities, Translation2d{24_m, 0_m});
EXPECT_NEAR(fl.speed.value(), 23.43, kEpsilon);
EXPECT_NEAR(fr.speed.value(), 23.43, kEpsilon);
EXPECT_NEAR(bl.speed.value(), 54.08, kEpsilon);
EXPECT_NEAR(br.speed.value(), 54.08, kEpsilon);
EXPECT_NEAR(fl.velocity.value(), 23.43, kEpsilon);
EXPECT_NEAR(fr.velocity.value(), 23.43, kEpsilon);
EXPECT_NEAR(bl.velocity.value(), 54.08, kEpsilon);
EXPECT_NEAR(br.velocity.value(), 54.08, kEpsilon);
EXPECT_NEAR(fl.angle.Degrees().value(), -140.19, kEpsilon);
EXPECT_NEAR(fr.angle.Degrees().value(), -39.81, kEpsilon);
@@ -235,16 +239,16 @@ TEST_F(SwerveDriveKinematicsTest,
TEST_F(SwerveDriveKinematicsTest,
OffCenterCORRotationAndTranslationForwardKinematics) {
SwerveModuleState fl{23.43_mps, -140.19_deg};
SwerveModuleState fr{23.43_mps, -39.81_deg};
SwerveModuleState bl{54.08_mps, -109.44_deg};
SwerveModuleState br{54.08_mps, -70.56_deg};
SwerveModuleVelocity fl{23.43_mps, -140.19_deg};
SwerveModuleVelocity fr{23.43_mps, -39.81_deg};
SwerveModuleVelocity bl{54.08_mps, -109.44_deg};
SwerveModuleVelocity br{54.08_mps, -70.56_deg};
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
auto chassisVelocities = m_kinematics.ToChassisVelocities(fl, fr, bl, br);
EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.vy.value(), -33.0, kEpsilon);
EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon);
EXPECT_NEAR(chassisVelocities.vx.value(), 0.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.vy.value(), -33.0, kEpsilon);
EXPECT_NEAR(chassisVelocities.omega.value(), 1.5, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest,
@@ -262,53 +266,54 @@ TEST_F(SwerveDriveKinematicsTest,
}
TEST_F(SwerveDriveKinematicsTest, Desaturate) {
SwerveModuleState state1{5.0_mps, 0_deg};
SwerveModuleState state2{6.0_mps, 0_deg};
SwerveModuleState state3{4.0_mps, 0_deg};
SwerveModuleState state4{7.0_mps, 0_deg};
SwerveModuleVelocity state1{5.0_mps, 0_deg};
SwerveModuleVelocity state2{6.0_mps, 0_deg};
SwerveModuleVelocity state3{4.0_mps, 0_deg};
SwerveModuleVelocity state4{7.0_mps, 0_deg};
wpi::util::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 5.5_mps);
wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelVelocities(&arr, 5.5_mps);
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[0].velocity.value(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].velocity.value(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].velocity.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].velocity.value(), 7.0 * kFactor, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, DesaturateSmooth) {
SwerveModuleState state1{5.0_mps, 0_deg};
SwerveModuleState state2{6.0_mps, 0_deg};
SwerveModuleState state3{4.0_mps, 0_deg};
SwerveModuleState state4{7.0_mps, 0_deg};
SwerveModuleVelocity state1{5.0_mps, 0_deg};
SwerveModuleVelocity state2{6.0_mps, 0_deg};
SwerveModuleVelocity state3{4.0_mps, 0_deg};
SwerveModuleVelocity state4{7.0_mps, 0_deg};
wpi::util::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
&arr, m_kinematics.ToChassisSpeeds(arr), 5.5_mps, 5.5_mps, 3.5_rad_per_s);
wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelVelocities(
&arr, m_kinematics.ToChassisVelocities(arr), 5.5_mps, 5.5_mps,
3.5_rad_per_s);
double kFactor = 5.5 / 7.0;
EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[0].velocity.value(), 5.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[1].velocity.value(), 6.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[2].velocity.value(), 4.0 * kFactor, kEpsilon);
EXPECT_NEAR(arr[3].velocity.value(), 7.0 * kFactor, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) {
SwerveModuleState state1{1.0_mps, 0_deg};
SwerveModuleState state2{1.0_mps, 0_deg};
SwerveModuleState state3{-2.0_mps, 0_deg};
SwerveModuleState state4{-2.0_mps, 0_deg};
TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeVelocity) {
SwerveModuleVelocity state1{1.0_mps, 0_deg};
SwerveModuleVelocity state2{1.0_mps, 0_deg};
SwerveModuleVelocity state3{-2.0_mps, 0_deg};
SwerveModuleVelocity state4{-2.0_mps, 0_deg};
wpi::util::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 1.0_mps);
wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelVelocities(&arr, 1.0_mps);
EXPECT_NEAR(arr[0].speed.value(), 0.5, kEpsilon);
EXPECT_NEAR(arr[1].speed.value(), 0.5, kEpsilon);
EXPECT_NEAR(arr[2].speed.value(), -1.0, kEpsilon);
EXPECT_NEAR(arr[3].speed.value(), -1.0, kEpsilon);
EXPECT_NEAR(arr[0].velocity.value(), 0.5, kEpsilon);
EXPECT_NEAR(arr[1].velocity.value(), 0.5, kEpsilon);
EXPECT_NEAR(arr[2].velocity.value(), -1.0, kEpsilon);
EXPECT_NEAR(arr[3].velocity.value(), -1.0, kEpsilon);
}
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseAccelerations) {

18
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp
View File

@@ -127,19 +127,19 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) {
while (t < trajectory.TotalTime()) {
Trajectory::State groundTruthState = trajectory.Sample(t);
auto moduleStates = kinematics.ToSwerveModuleStates(
auto moduleVelocities = kinematics.ToSwerveModuleVelocities(
{groundTruthState.velocity, 0_mps,
groundTruthState.velocity * groundTruthState.curvature});
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;
auto xhat = odometry.Update(
wpi::math::Rotation3d{

18
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
View File

@@ -104,19 +104,19 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
while (t < trajectory.TotalTime()) {
Trajectory::State groundTruthState = trajectory.Sample(t);
auto moduleStates = kinematics.ToSwerveModuleStates(
auto moduleVelocities = kinematics.ToSwerveModuleVelocities(
{groundTruthState.velocity, 0_mps,
groundTruthState.velocity * groundTruthState.curvature});
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;
auto xhat = odometry.Update(
groundTruthState.pose.Rotation() +

86
wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp → wpimath/src/test/native/cpp/kinematics/SwerveModuleVelocityTest.cpp
View File

@@ -2,7 +2,7 @@
// 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.
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include <gtest/gtest.h>
@@ -10,135 +10,135 @@
static constexpr double kEpsilon = 1E-9;
TEST(SwerveModuleStateTest, Optimize) {
TEST(SwerveModuleVelocityTest, Optimize) {
wpi::math::Rotation2d angleA{45_deg};
wpi::math::SwerveModuleState refA{-2_mps, 180_deg};
wpi::math::SwerveModuleVelocity refA{-2_mps, 180_deg};
refA.Optimize(angleA);
EXPECT_NEAR(refA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 0.0, kEpsilon);
wpi::math::Rotation2d angleB{-50_deg};
wpi::math::SwerveModuleState refB{4.7_mps, 41_deg};
wpi::math::SwerveModuleVelocity refB{4.7_mps, 41_deg};
refB.Optimize(angleB);
EXPECT_NEAR(refB.speed.value(), -4.7, kEpsilon);
EXPECT_NEAR(refB.velocity.value(), -4.7, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), -139.0, kEpsilon);
}
TEST(SwerveModuleStateTest, NoOptimize) {
TEST(SwerveModuleVelocityTest, NoOptimize) {
wpi::math::Rotation2d angleA{0_deg};
wpi::math::SwerveModuleState refA{2_mps, 89_deg};
wpi::math::SwerveModuleVelocity refA{2_mps, 89_deg};
refA.Optimize(angleA);
EXPECT_NEAR(refA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 89.0, kEpsilon);
wpi::math::Rotation2d angleB{0_deg};
wpi::math::SwerveModuleState refB{-2_mps, -2_deg};
wpi::math::SwerveModuleVelocity refB{-2_mps, -2_deg};
refB.Optimize(angleB);
EXPECT_NEAR(refB.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(refB.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), -2.0, kEpsilon);
}
TEST(SwerveModuleStateTest, CosineScaling) {
TEST(SwerveModuleVelocityTest, CosineScaling) {
wpi::math::Rotation2d angleA{0_deg};
wpi::math::SwerveModuleState refA{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refA{2_mps, 45_deg};
refA.CosineScale(angleA);
EXPECT_NEAR(refA.speed.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refA.velocity.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleB{45_deg};
wpi::math::SwerveModuleState refB{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refB{2_mps, 45_deg};
refB.CosineScale(angleB);
EXPECT_NEAR(refB.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refB.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleC{-45_deg};
wpi::math::SwerveModuleState refC{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refC{2_mps, 45_deg};
refC.CosineScale(angleC);
EXPECT_NEAR(refC.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refC.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refC.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleD{135_deg};
wpi::math::SwerveModuleState refD{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refD{2_mps, 45_deg};
refD.CosineScale(angleD);
EXPECT_NEAR(refD.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refD.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refD.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleE{-135_deg};
wpi::math::SwerveModuleState refE{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refE{2_mps, 45_deg};
refE.CosineScale(angleE);
EXPECT_NEAR(refE.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(refE.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(refE.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleF{180_deg};
wpi::math::SwerveModuleState refF{2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refF{2_mps, 45_deg};
refF.CosineScale(angleF);
EXPECT_NEAR(refF.speed.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refF.velocity.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refF.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleG{0_deg};
wpi::math::SwerveModuleState refG{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refG{-2_mps, 45_deg};
refG.CosineScale(angleG);
EXPECT_NEAR(refG.speed.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refG.velocity.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refG.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleH{45_deg};
wpi::math::SwerveModuleState refH{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refH{-2_mps, 45_deg};
refH.CosineScale(angleH);
EXPECT_NEAR(refH.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(refH.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(refH.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleI{-45_deg};
wpi::math::SwerveModuleState refI{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refI{-2_mps, 45_deg};
refI.CosineScale(angleI);
EXPECT_NEAR(refI.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refI.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refI.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleJ{135_deg};
wpi::math::SwerveModuleState refJ{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refJ{-2_mps, 45_deg};
refJ.CosineScale(angleJ);
EXPECT_NEAR(refJ.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refJ.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refJ.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleK{-135_deg};
wpi::math::SwerveModuleState refK{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refK{-2_mps, 45_deg};
refK.CosineScale(angleK);
EXPECT_NEAR(refK.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refK.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refK.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleL{180_deg};
wpi::math::SwerveModuleState refL{-2_mps, 45_deg};
wpi::math::SwerveModuleVelocity refL{-2_mps, 45_deg};
refL.CosineScale(angleL);
EXPECT_NEAR(refL.speed.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refL.velocity.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refL.angle.Degrees().value(), 45.0, kEpsilon);
}
TEST(SwerveModuleStateTest, Equality) {
wpi::math::SwerveModuleState state1{2_mps, 90_deg};
wpi::math::SwerveModuleState state2{2_mps, 90_deg};
TEST(SwerveModuleVelocityTest, Equality) {
wpi::math::SwerveModuleVelocity state1{2_mps, 90_deg};
wpi::math::SwerveModuleVelocity state2{2_mps, 90_deg};
EXPECT_EQ(state1, state2);
}
TEST(SwerveModuleStateTest, Inequality) {
wpi::math::SwerveModuleState state1{1_mps, 90_deg};
wpi::math::SwerveModuleState state2{2_mps, 90_deg};
wpi::math::SwerveModuleState state3{1_mps, 89_deg};
TEST(SwerveModuleVelocityTest, Inequality) {
wpi::math::SwerveModuleVelocity state1{1_mps, 90_deg};
wpi::math::SwerveModuleVelocity state2{2_mps, 90_deg};
wpi::math::SwerveModuleVelocity state3{1_mps, 89_deg};
EXPECT_NE(state1, state2);
EXPECT_NE(state1, state3);

8
wpimath/src/test/native/cpp/kinematics/proto/ChassisSpeedsProtoTest.cpp → wpimath/src/test/native/cpp/kinematics/proto/ChassisVelocitiesProtoTest.cpp
View File

@@ -4,18 +4,18 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace wpi::math;
namespace {
const ChassisSpeeds kExpectedData =
ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s};
const ChassisVelocities kExpectedData =
ChassisVelocities{2.29_mps, 2.2_mps, 0.3504_rad_per_s};
} // namespace
TEST(ChassisSpeedsProtoTest, Roundtrip) {
TEST(ChassisVelocitiesProtoTest, Roundtrip) {
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;

8
wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.cpp → wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveWheelVelocitiesProtoTest.cpp
View File

@@ -4,18 +4,18 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace wpi::math;
namespace {
const DifferentialDriveWheelSpeeds kExpectedData =
DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps};
const DifferentialDriveWheelVelocities kExpectedData =
DifferentialDriveWheelVelocities{1.74_mps, 35.04_mps};
} // namespace
TEST(DifferentialDriveWheelSpeedsProtoTest, Roundtrip) {
TEST(DifferentialDriveWheelVelocitiesProtoTest, Roundtrip) {
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;

8
wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.cpp → wpimath/src/test/native/cpp/kinematics/proto/MecanumDriveWheelVelocitiesProtoTest.cpp
View File

@@ -4,18 +4,18 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace wpi::math;
namespace {
const MecanumDriveWheelSpeeds kExpectedData =
MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps};
const MecanumDriveWheelVelocities kExpectedData =
MecanumDriveWheelVelocities{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps};
} // namespace
TEST(MecanumDriveWheelSpeedsProtoTest, Roundtrip) {
TEST(MecanumDriveWheelVelocitiesProtoTest, Roundtrip) {
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;

10
wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleStateProtoTest.cpp → wpimath/src/test/native/cpp/kinematics/proto/SwerveModuleVelocityProtoTest.cpp
View File

@@ -4,18 +4,18 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
#include "wpi/util/SmallVector.hpp"
using namespace wpi::math;
namespace {
const SwerveModuleState kExpectedData =
SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}};
const SwerveModuleVelocity kExpectedData =
SwerveModuleVelocity{22.9_mps, Rotation2d{3.3_rad}};
} // namespace
TEST(SwerveModuleStateProtoTest, Roundtrip) {
TEST(SwerveModuleVelocityProtoTest, Roundtrip) {
wpi::util::ProtobufMessage<decltype(kExpectedData)> message;
wpi::util::SmallVector<uint8_t, 64> buf;
@@ -23,6 +23,6 @@ TEST(SwerveModuleStateProtoTest, Roundtrip) {
auto unpacked_data = message.Unpack(buf);
ASSERT_TRUE(unpacked_data.has_value());
EXPECT_EQ(kExpectedData.speed.value(), unpacked_data->speed.value());
EXPECT_EQ(kExpectedData.velocity.value(), unpacked_data->velocity.value());
EXPECT_EQ(kExpectedData.angle, unpacked_data->angle);
}

12
wpimath/src/test/native/cpp/kinematics/struct/ChassisSpeedsStructTest.cpp → wpimath/src/test/native/cpp/kinematics/struct/ChassisVelocitiesStructTest.cpp
View File

@@ -4,23 +4,23 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/ChassisSpeeds.hpp"
#include "wpi/math/kinematics/ChassisVelocities.hpp"
using namespace wpi::math;
namespace {
using StructType = wpi::util::Struct<wpi::math::ChassisSpeeds>;
const ChassisSpeeds kExpectedData{
ChassisSpeeds{2.29_mps, 2.2_mps, 0.3504_rad_per_s}};
using StructType = wpi::util::Struct<wpi::math::ChassisVelocities>;
const ChassisVelocities kExpectedData{
ChassisVelocities{2.29_mps, 2.2_mps, 0.3504_rad_per_s}};
} // namespace
TEST(ChassisSpeedsStructTest, Roundtrip) {
TEST(ChassisVelocitiesStructTest, Roundtrip) {
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
StructType::Pack(buffer, kExpectedData);
ChassisSpeeds unpacked_data = StructType::Unpack(buffer);
ChassisVelocities unpacked_data = StructType::Unpack(buffer);
EXPECT_EQ(kExpectedData.vx.value(), unpacked_data.vx.value());
EXPECT_EQ(kExpectedData.vy.value(), unpacked_data.vy.value());

13
wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.cpp → wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveWheelVelocitiesStructTest.cpp
View File

@@ -4,23 +4,24 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp"
using namespace wpi::math;
namespace {
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeeds>;
const DifferentialDriveWheelSpeeds kExpectedData{
DifferentialDriveWheelSpeeds{1.74_mps, 35.04_mps}};
using StructType =
wpi::util::Struct<wpi::math::DifferentialDriveWheelVelocities>;
const DifferentialDriveWheelVelocities kExpectedData{
DifferentialDriveWheelVelocities{1.74_mps, 35.04_mps}};
} // namespace
TEST(DifferentialDriveWheelSpeedsStructTest, Roundtrip) {
TEST(DifferentialDriveWheelVelocitiesStructTest, Roundtrip) {
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
StructType::Pack(buffer, kExpectedData);
DifferentialDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer);
DifferentialDriveWheelVelocities unpacked_data = StructType::Unpack(buffer);
EXPECT_EQ(kExpectedData.left.value(), unpacked_data.left.value());
EXPECT_EQ(kExpectedData.right.value(), unpacked_data.right.value());

12
wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStructTest.cpp → wpimath/src/test/native/cpp/kinematics/struct/MecanumDriveWheelVelocitiesStructTest.cpp
View File

@@ -4,23 +4,23 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
#include "wpi/math/kinematics/MecanumDriveWheelVelocities.hpp"
using namespace wpi::math;
namespace {
using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelSpeeds>;
const MecanumDriveWheelSpeeds kExpectedData{
MecanumDriveWheelSpeeds{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps}};
using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelVelocities>;
const MecanumDriveWheelVelocities kExpectedData{
MecanumDriveWheelVelocities{2.29_mps, 17.4_mps, 4.4_mps, 0.229_mps}};
} // namespace
TEST(MecanumDriveWheelSpeedsStructTest, Roundtrip) {
TEST(MecanumDriveWheelVelocitiesStructTest, Roundtrip) {
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
StructType::Pack(buffer, kExpectedData);
MecanumDriveWheelSpeeds unpacked_data = StructType::Unpack(buffer);
MecanumDriveWheelVelocities unpacked_data = StructType::Unpack(buffer);
EXPECT_EQ(kExpectedData.frontLeft.value(), unpacked_data.frontLeft.value());
EXPECT_EQ(kExpectedData.frontRight.value(), unpacked_data.frontRight.value());

14
wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleStateStructTest.cpp → wpimath/src/test/native/cpp/kinematics/struct/SwerveModuleVelocityStructTest.cpp
View File

@@ -4,24 +4,24 @@
#include <gtest/gtest.h>
#include "wpi/math/kinematics/SwerveModuleState.hpp"
#include "wpi/math/kinematics/SwerveModuleVelocity.hpp"
using namespace wpi::math;
namespace {
using StructType = wpi::util::Struct<wpi::math::SwerveModuleState>;
const SwerveModuleState kExpectedData{
SwerveModuleState{22.9_mps, Rotation2d{3.3_rad}}};
using StructType = wpi::util::Struct<wpi::math::SwerveModuleVelocity>;
const SwerveModuleVelocity kExpectedData{
SwerveModuleVelocity{22.9_mps, Rotation2d{3.3_rad}}};
} // namespace
TEST(SwerveModuleStateStructTest, Roundtrip) {
TEST(SwerveModuleVelocityStructTest, Roundtrip) {
uint8_t buffer[StructType::GetSize()];
std::memset(buffer, 0, StructType::GetSize());
StructType::Pack(buffer, kExpectedData);
SwerveModuleState unpacked_data = StructType::Unpack(buffer);
SwerveModuleVelocity unpacked_data = StructType::Unpack(buffer);
EXPECT_EQ(kExpectedData.speed.value(), unpacked_data.speed.value());
EXPECT_EQ(kExpectedData.velocity.value(), unpacked_data.velocity.value());
EXPECT_EQ(kExpectedData.angle, unpacked_data.angle);
}

6
wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
View File

@@ -30,10 +30,10 @@ TEST(DifferentialDriveKinematicsConstraintTest, Constraint) {
const Trajectory::State point = trajectory.Sample(time);
time += dt;
const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
point.velocity * point.curvature};
const ChassisVelocities chassisVelocities{point.velocity, 0_mps,
point.velocity * point.curvature};
auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
auto [left, right] = kinematics.ToWheelVelocities(chassisVelocities);
EXPECT_TRUE(left < maxVelocity + 0.05_mps);
EXPECT_TRUE(right < maxVelocity + 0.05_mps);

6
wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
View File

@@ -40,10 +40,10 @@ TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
const Trajectory::State point = trajectory.Sample(time);
time += dt;
const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
point.velocity * point.curvature};
const ChassisVelocities chassisVelocities{point.velocity, 0_mps,
point.velocity * point.curvature};
auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
auto [left, right] = kinematics.ToWheelVelocities(chassisVelocities);
auto acceleration = point.acceleration;
// Not really a strictly-correct test as we're using the chassis accel
// instead of the wheel accel, but much easier than doing it "properly" and

28
wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp
View File

@@ -161,8 +161,8 @@ TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
EXPECT_EQ(state, goal);
}
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, TopSpeed) {
// Checks to make sure that it hits top velocity on long trajectories
TEST(ExponentialProfileTest, TopVelocity) {
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
@@ -175,19 +175,19 @@ TEST(ExponentialProfileTest, TopSpeed) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state;
wpi::units::meters_per_second_t maxSpeed = 0_mps;
wpi::units::meters_per_second_t maxVelocity = 0_mps;
for (int i = 0; i < 900; ++i) {
state = CheckDynamics(profile, constraints, feedforward, state, goal);
maxSpeed = wpi::units::math::max(state.velocity, maxSpeed);
maxVelocity = wpi::units::math::max(state.velocity, maxVelocity);
}
EXPECT_NEAR_UNITS(constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
EXPECT_NEAR_UNITS(constraints.MaxVelocity(), maxVelocity, 1e-5_mps);
EXPECT_EQ(state, goal);
}
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, TopSpeedBackward) {
// Checks to make sure that it hits top velocity on long trajectories
TEST(ExponentialProfileTest, TopVelocityBackward) {
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
@@ -200,19 +200,19 @@ TEST(ExponentialProfileTest, TopSpeedBackward) {
wpi::math::ExponentialProfile<wpi::units::meter, wpi::units::volts>::State
state;
wpi::units::meters_per_second_t maxSpeed = 0_mps;
wpi::units::meters_per_second_t maxVelocity = 0_mps;
for (int i = 0; i < 900; ++i) {
state = CheckDynamics(profile, constraints, feedforward, state, goal);
maxSpeed = wpi::units::math::min(state.velocity, maxSpeed);
maxVelocity = wpi::units::math::min(state.velocity, maxVelocity);
}
EXPECT_NEAR_UNITS(-constraints.MaxVelocity(), maxSpeed, 1e-5_mps);
EXPECT_NEAR_UNITS(-constraints.MaxVelocity(), maxVelocity, 1e-5_mps);
EXPECT_EQ(state, goal);
}
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, HighInitialSpeed) {
// Checks to make sure that it hits top velocity on long trajectories
TEST(ExponentialProfileTest, HighInitialVelocity) {
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
@@ -232,8 +232,8 @@ TEST(ExponentialProfileTest, HighInitialSpeed) {
EXPECT_EQ(state, goal);
}
// Checks to make sure that it hits top speed on long trajectories
TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
// Checks to make sure that it hits top velocity on long trajectories
TEST(ExponentialProfileTest, HighInitialVelocityBackward) {
wpi::math::ExponentialProfile<wpi::units::meter,
wpi::units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};

4
wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
View File

@@ -105,8 +105,8 @@ TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
EXPECT_EQ(state, goal);
}
// Checks to make sure that it hits top speed
TEST(TrapezoidProfileTest, TopSpeed) {
// Checks to make sure that it hits top velocity
TEST(TrapezoidProfileTest, TopVelocity) {
wpi::math::TrapezoidProfile<wpi::units::meter>::Constraints constraints{
0.75_mps, 0.75_mps_sq};
wpi::math::TrapezoidProfile<wpi::units::meter>::State goal{4_m, 0_mps};

52
wpimath/src/test/python/kinematics/test_chassis_speeds.py → wpimath/src/test/python/kinematics/test_chassis_velocities.py
View File

@@ -2,16 +2,16 @@ import pytest
import math
import numpy as np
from wpimath import ChassisSpeeds, Twist2d, Pose2d, Rotation2d
from wpimath import ChassisVelocities, Twist2d, Pose2d, Rotation2d
def test_discretize():
target = ChassisSpeeds(vx=1, vy=0, omega=0.5)
target = ChassisVelocities(vx=1, vy=0, omega=0.5)
duration = 1
dt = 0.01
speeds = target.discretize(duration)
twist = Twist2d(speeds.vx * dt, speeds.vy * dt, speeds.omega * dt)
velocities = target.discretize(duration)
twist = Twist2d(velocities.vx * dt, velocities.vy * dt, velocities.omega * dt)
pose = Pose2d()
time = 0
@@ -27,28 +27,28 @@ def test_discretize():
def test_to_robot_relative():
chassis_speeds = ChassisSpeeds(vx=1, vy=0, omega=0.5).toRobotRelative(
chassis_velocities = ChassisVelocities(vx=1, vy=0, omega=0.5).toRobotRelative(
Rotation2d.fromDegrees(-90)
)
assert chassis_speeds.vx == pytest.approx(0.0, abs=1e-9)
assert chassis_speeds.vy == pytest.approx(1.0, abs=1e-9)
assert chassis_speeds.omega == pytest.approx(0.5, abs=1e-9)
assert chassis_velocities.vx == pytest.approx(0.0, abs=1e-9)
assert chassis_velocities.vy == pytest.approx(1.0, abs=1e-9)
assert chassis_velocities.omega == pytest.approx(0.5, abs=1e-9)
def test_to_field_relative():
chassis_speeds = ChassisSpeeds(vx=1, vy=0, omega=0.5).toFieldRelative(
chassis_velocities = ChassisVelocities(vx=1, vy=0, omega=0.5).toFieldRelative(
Rotation2d.fromDegrees(45)
)
assert chassis_speeds.vx == pytest.approx(1.0 / math.sqrt(2.0), abs=1e-9)
assert chassis_speeds.vy == pytest.approx(1.0 / math.sqrt(2.0), abs=1e-9)
assert chassis_speeds.omega == pytest.approx(0.5, abs=1e-9)
assert chassis_velocities.vx == pytest.approx(1.0 / math.sqrt(2.0), abs=1e-9)
assert chassis_velocities.vy == pytest.approx(1.0 / math.sqrt(2.0), abs=1e-9)
assert chassis_velocities.omega == pytest.approx(0.5, abs=1e-9)
def test_plus() -> None:
left = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
right = ChassisSpeeds(vx=2.0, vy=1.5, omega=0.25)
left = ChassisVelocities(vx=1.0, vy=0.5, omega=0.75)
right = ChassisVelocities(vx=2.0, vy=1.5, omega=0.25)
result = left + right
@@ -58,8 +58,8 @@ def test_plus() -> None:
def test_minus() -> None:
left = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
right = ChassisSpeeds(vx=2.0, vy=0.5, omega=0.25)
left = ChassisVelocities(vx=1.0, vy=0.5, omega=0.75)
right = ChassisVelocities(vx=2.0, vy=0.5, omega=0.25)
result = left - right
@@ -69,9 +69,9 @@ def test_minus() -> None:
def test_neg() -> None:
speeds = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
velocities = ChassisVelocities(vx=1.0, vy=0.5, omega=0.75)
result = -speeds
result = -velocities
assert math.isclose(-1.0, result.vx)
assert math.isclose(-0.5, result.vy)
@@ -79,9 +79,9 @@ def test_neg() -> None:
def test_multiplication() -> None:
speeds = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
velocities = ChassisVelocities(vx=1.0, vy=0.5, omega=0.75)
result = speeds * 2
result = velocities * 2
assert math.isclose(2.0, result.vx)
assert math.isclose(1.0, result.vy)
@@ -89,9 +89,9 @@ def test_multiplication() -> None:
def test_division() -> None:
speeds = ChassisSpeeds(vx=1.0, vy=0.5, omega=0.75)
velocities = ChassisVelocities(vx=1.0, vy=0.5, omega=0.75)
result = speeds / 2
result = velocities / 2
assert math.isclose(0.5, result.vx)
assert math.isclose(0.25, result.vy)
@@ -99,9 +99,9 @@ def test_division() -> None:
def test_unpack() -> None:
speeds = ChassisSpeeds(1, 1, 0.5)
velocities = ChassisVelocities(1, 1, 0.5)
vx, vy, omega = speeds
vx, vy, omega = velocities
assert math.isclose(1, vx)
assert math.isclose(1, vy)
@@ -109,6 +109,6 @@ def test_unpack() -> None:
def test_repr() -> None:
speeds = ChassisSpeeds(2.0, 1.0, 0.0)
velocities = ChassisVelocities(2.0, 1.0, 0.0)
assert repr(speeds) == "ChassisSpeeds(vx=2.000000, vy=1.000000, omega=0.000000)"
assert repr(velocities) == "ChassisVelocities(vx=2.000000, vy=1.000000, omega=0.000000)"

58
wpimath/src/test/python/kinematics/test_differential_drive_kinematics.py
View File

@@ -3,67 +3,67 @@ import math
import numpy as np
from wpimath import (
ChassisSpeeds,
ChassisVelocities,
DifferentialDriveKinematics,
DifferentialDriveWheelSpeeds,
DifferentialDriveWheelVelocities,
Rotation2d,
)
def test_inverse_kinematics_from_zero():
kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
chassis_speeds = ChassisSpeeds()
wheel_speeds = kinematics.toWheelSpeeds(chassis_speeds)
chassis_velocities = ChassisVelocities()
wheel_velocities = kinematics.toWheelVelocities(chassis_velocities)
assert wheel_speeds.left == pytest.approx(0, abs=1e-9)
assert wheel_speeds.right == pytest.approx(0, abs=1e-9)
assert wheel_velocities.left == pytest.approx(0, abs=1e-9)
assert wheel_velocities.right == pytest.approx(0, abs=1e-9)
def test_forward_kinematics_from_zero():
kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
wheel_speeds = DifferentialDriveWheelSpeeds()
chassis_speeds = kinematics.toChassisSpeeds(wheel_speeds)
wheel_velocities = DifferentialDriveWheelVelocities()
chassis_velocities = kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(0, abs=1e-9)
assert chassis_speeds.vy == pytest.approx(0, abs=1e-9)
assert chassis_speeds.omega == pytest.approx(0, abs=1e-9)
assert chassis_velocities.vx == pytest.approx(0, abs=1e-9)
assert chassis_velocities.vy == pytest.approx(0, abs=1e-9)
assert chassis_velocities.omega == pytest.approx(0, abs=1e-9)
def test_inverse_kinematics_for_straight_line():
kinematics = DifferentialDriveKinematics(trackwidth=(0.381 * 2))
chassis_speeds = ChassisSpeeds(vx=3.0, vy=0, omega=0)
wheel_speeds = kinematics.toWheelSpeeds(chassis_speeds)
chassis_velocities = ChassisVelocities(vx=3.0, vy=0, omega=0)
wheel_velocities = kinematics.toWheelVelocities(chassis_velocities)
assert wheel_speeds.left == pytest.approx(3, abs=1e-9)
assert wheel_speeds.right == pytest.approx(3, abs=1e-9)
assert wheel_velocities.left == pytest.approx(3, abs=1e-9)
assert wheel_velocities.right == pytest.approx(3, abs=1e-9)
def test_forward_kinematics_for_straight_line():
kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
wheel_speeds = DifferentialDriveWheelSpeeds(left=3.0, right=3.0)
chassis_speeds = kinematics.toChassisSpeeds(wheel_speeds)
wheel_velocities = DifferentialDriveWheelVelocities(left=3.0, right=3.0)
chassis_velocities = kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(3, abs=1e-9)
assert chassis_speeds.vy == pytest.approx(0, abs=1e-9)
assert chassis_speeds.omega == pytest.approx(0, abs=1e-9)
assert chassis_velocities.vx == pytest.approx(3, abs=1e-9)
assert chassis_velocities.vy == pytest.approx(0, abs=1e-9)
assert chassis_velocities.omega == pytest.approx(0, abs=1e-9)
def test_inverse_kinematics_for_rotate_in_place():
kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
chassis_speeds = ChassisSpeeds(vx=0.0, vy=0.0, omega=math.pi)
wheel_speeds = kinematics.toWheelSpeeds(chassis_speeds)
chassis_velocities = ChassisVelocities(vx=0.0, vy=0.0, omega=math.pi)
wheel_velocities = kinematics.toWheelVelocities(chassis_velocities)
assert wheel_speeds.left == pytest.approx(-0.381 * math.pi, abs=1e-9)
assert wheel_speeds.right == pytest.approx(0.381 * math.pi, abs=1e-9)
assert wheel_velocities.left == pytest.approx(-0.381 * math.pi, abs=1e-9)
assert wheel_velocities.right == pytest.approx(0.381 * math.pi, abs=1e-9)
def test_forward_kinematics_for_rotate_in_place():
kinematics = DifferentialDriveKinematics(trackwidth=0.381 * 2)
wheel_speeds = DifferentialDriveWheelSpeeds(
wheel_velocities = DifferentialDriveWheelVelocities(
left=0.381 * math.pi, right=-0.381 * math.pi
)
chassis_speeds = kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(0, abs=1e-9)
assert chassis_speeds.vy == pytest.approx(0, abs=1e-9)
assert chassis_speeds.omega == pytest.approx(-math.pi, abs=1e-9)
assert chassis_velocities.vx == pytest.approx(0, abs=1e-9)
assert chassis_velocities.vy == pytest.approx(0, abs=1e-9)
assert chassis_velocities.omega == pytest.approx(-math.pi, abs=1e-9)

50
wpimath/src/test/python/kinematics/test_differential_drive_wheel_speeds.py
View File

@@ -1,50 +0,0 @@
import pytest
from wpimath import DifferentialDriveWheelSpeeds
def test_plus():
left = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
right = DifferentialDriveWheelSpeeds(left=2.0, right=1.5)
result = left + right
assert result.left == 3.0
assert result.right == 2.0
def test_minus():
left = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
right = DifferentialDriveWheelSpeeds(left=2.0, right=0.5)
result = left - right
assert result.left == -1.0
assert result.right == 0.0
def test_unary_minus():
speeds = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
result = -speeds
assert result.left == -1.0
assert result.right == -0.5
def test_multiplication():
speeds = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
result = speeds * 2
assert result.left == 2.0
assert result.right == 1.0
def test_division():
speeds = DifferentialDriveWheelSpeeds(left=1.0, right=0.5)
result = speeds / 2
assert result.left == 0.5
assert result.right == 0.25

50
wpimath/src/test/python/kinematics/test_differential_drive_wheel_velocities.py Normal file
View File

@@ -0,0 +1,50 @@
import pytest
from wpimath import DifferentialDriveWheelVelocities
def test_plus():
left = DifferentialDriveWheelVelocities(left=1.0, right=0.5)
right = DifferentialDriveWheelVelocities(left=2.0, right=1.5)
result = left + right
assert result.left == 3.0
assert result.right == 2.0
def test_minus():
left = DifferentialDriveWheelVelocities(left=1.0, right=0.5)
right = DifferentialDriveWheelVelocities(left=2.0, right=0.5)
result = left - right
assert result.left == -1.0
assert result.right == 0.0
def test_unary_minus():
velocities = DifferentialDriveWheelVelocities(left=1.0, right=0.5)
result = -velocities
assert result.left == -1.0
assert result.right == -0.5
def test_multiplication():
velocities = DifferentialDriveWheelVelocities(left=1.0, right=0.5)
result = velocities * 2
assert result.left == 2.0
assert result.right == 1.0
def test_division():
velocities = DifferentialDriveWheelVelocities(left=1.0, right=0.5)
result = velocities / 2
assert result.left == 0.5
assert result.right == 0.25

162
wpimath/src/test/python/kinematics/test_mecanum_drive_kinematics.py
View File

@@ -3,8 +3,8 @@ import math
from wpimath import (
MecanumDriveKinematics,
ChassisSpeeds,
MecanumDriveWheelSpeeds,
ChassisVelocities,
MecanumDriveWheelVelocities,
MecanumDriveWheelPositions,
Rotation2d,
Translation2d,
@@ -27,24 +27,24 @@ def kinematics_test():
def test_straight_line_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=5, vy=0, omega=0)
module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
velocities = ChassisVelocities(vx=5, vy=0, omega=0)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(velocities)
assert module_states.frontLeft == pytest.approx(5.0, abs=0.1)
assert module_states.frontRight == pytest.approx(5.0, abs=0.1)
assert module_states.rearLeft == pytest.approx(5.0, abs=0.1)
assert module_states.rearRight == pytest.approx(5.0, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(5.0, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(5.0, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(5.0, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(5.0, abs=0.1)
def test_straight_line_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=5, frontRight=5, rearLeft=5, rearRight=5
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(5.0, abs=0.1)
assert chassis_speeds.vy == pytest.approx(0.0, abs=0.1)
assert chassis_speeds.omega == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.vx == pytest.approx(5.0, abs=0.1)
assert chassis_velocities.vy == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.omega == pytest.approx(0.0, abs=0.1)
def test_straight_line_forward_kinematics_with_deltas(kinematics_test):
@@ -59,24 +59,24 @@ def test_straight_line_forward_kinematics_with_deltas(kinematics_test):
def test_strafe_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=4, omega=0)
module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
velocities = ChassisVelocities(vx=0, vy=4, omega=0)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(velocities)
assert module_states.frontLeft == pytest.approx(-4.0, abs=0.1)
assert module_states.frontRight == pytest.approx(4.0, abs=0.1)
assert module_states.rearLeft == pytest.approx(4.0, abs=0.1)
assert module_states.rearRight == pytest.approx(-4.0, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(-4.0, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(4.0, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(4.0, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(-4.0, abs=0.1)
def test_strafe_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=-5, frontRight=5, rearLeft=5, rearRight=-5
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(0.0, abs=0.1)
assert chassis_speeds.vy == pytest.approx(5.0, abs=0.1)
assert chassis_speeds.omega == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.vx == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.vy == pytest.approx(5.0, abs=0.1)
assert chassis_velocities.omega == pytest.approx(0.0, abs=0.1)
def test_strafe_forward_kinematics_with_deltas(kinematics_test):
@@ -91,27 +91,27 @@ def test_strafe_forward_kinematics_with_deltas(kinematics_test):
def test_rotation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=0, omega=2 * math.pi)
module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
velocities = ChassisVelocities(vx=0, vy=0, omega=2 * math.pi)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(velocities)
assert module_states.frontLeft == pytest.approx(-150.79644737, abs=0.1)
assert module_states.frontRight == pytest.approx(150.79644737, abs=0.1)
assert module_states.rearLeft == pytest.approx(-150.79644737, abs=0.1)
assert module_states.rearRight == pytest.approx(150.79644737, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(-150.79644737, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(150.79644737, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(-150.79644737, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(150.79644737, abs=0.1)
def test_rotation_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=-150.79644737,
frontRight=150.79644737,
rearLeft=-150.79644737,
rearRight=150.79644737,
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(0.0, abs=0.1)
assert chassis_speeds.vy == pytest.approx(0.0, abs=0.1)
assert chassis_speeds.omega == pytest.approx(2 * math.pi, abs=0.1)
assert chassis_velocities.vx == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.vy == pytest.approx(0.0, abs=0.1)
assert chassis_velocities.omega == pytest.approx(2 * math.pi, abs=0.1)
def test_rotation_forward_kinematics_with_deltas(kinematics_test):
@@ -129,28 +129,28 @@ def test_rotation_forward_kinematics_with_deltas(kinematics_test):
def test_mixed_rotation_translation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=2, vy=3, omega=1)
module_states = kinematics_test.kinematics.toWheelSpeeds(speeds)
velocities = ChassisVelocities(vx=2, vy=3, omega=1)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(velocities)
assert module_states.frontLeft == pytest.approx(-25.0, abs=0.1)
assert module_states.frontRight == pytest.approx(29.0, abs=0.1)
assert module_states.rearLeft == pytest.approx(-19.0, abs=0.1)
assert module_states.rearRight == pytest.approx(23.0, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(-25.0, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(29.0, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(-19.0, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(23.0, abs=0.1)
def test_mixed_rotation_translation_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=-17.677670,
frontRight=20.506097,
rearLeft=-13.435,
rearRight=16.26,
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(1.41335, abs=0.1)
assert chassis_speeds.vy == pytest.approx(2.1221, abs=0.1)
assert chassis_speeds.omega == pytest.approx(0.707, abs=0.1)
assert chassis_velocities.vx == pytest.approx(1.41335, abs=0.1)
assert chassis_velocities.vy == pytest.approx(2.1221, abs=0.1)
assert chassis_velocities.omega == pytest.approx(0.707, abs=0.1)
def test_mixed_rotation_translation_forward_kinematics_with_deltas(kinematics_test):
@@ -166,29 +166,29 @@ def test_mixed_rotation_translation_forward_kinematics_with_deltas(kinematics_te
def test_off_center_rotation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=0, omega=1)
module_states = kinematics_test.kinematics.toWheelSpeeds(
speeds, kinematics_test.m_fl
velocities = ChassisVelocities(vx=0, vy=0, omega=1)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(
velocities, kinematics_test.m_fl
)
assert module_states.frontLeft == pytest.approx(0, abs=0.1)
assert module_states.frontRight == pytest.approx(24.0, abs=0.1)
assert module_states.rearLeft == pytest.approx(-24.0, abs=0.1)
assert module_states.rearRight == pytest.approx(48.0, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(0, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(24.0, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(-24.0, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(48.0, abs=0.1)
def test_off_center_rotation_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=0,
frontRight=16.971,
rearLeft=-16.971,
rearRight=33.941,
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(8.48525, abs=0.1)
assert chassis_speeds.vy == pytest.approx(-8.48525, abs=0.1)
assert chassis_speeds.omega == pytest.approx(0.707, abs=0.1)
assert chassis_velocities.vx == pytest.approx(8.48525, abs=0.1)
assert chassis_velocities.vy == pytest.approx(-8.48525, abs=0.1)
assert chassis_velocities.omega == pytest.approx(0.707, abs=0.1)
def test_off_center_rotation_forward_kinematics_with_deltas(kinematics_test):
@@ -203,29 +203,29 @@ def test_off_center_rotation_forward_kinematics_with_deltas(kinematics_test):
def test_off_center_translation_rotation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=5, vy=2, omega=1)
module_states = kinematics_test.kinematics.toWheelSpeeds(
speeds, kinematics_test.m_fl
velocities = ChassisVelocities(vx=5, vy=2, omega=1)
wheel_velocities = kinematics_test.kinematics.toWheelVelocities(
velocities, kinematics_test.m_fl
)
assert module_states.frontLeft == pytest.approx(3.0, abs=0.1)
assert module_states.frontRight == pytest.approx(31.0, abs=0.1)
assert module_states.rearLeft == pytest.approx(-17.0, abs=0.1)
assert module_states.rearRight == pytest.approx(51.0, abs=0.1)
assert wheel_velocities.frontLeft == pytest.approx(3.0, abs=0.1)
assert wheel_velocities.frontRight == pytest.approx(31.0, abs=0.1)
assert wheel_velocities.rearLeft == pytest.approx(-17.0, abs=0.1)
assert wheel_velocities.rearRight == pytest.approx(51.0, abs=0.1)
def test_off_center_translation_rotation_forward_kinematics(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=2.12,
frontRight=21.92,
rearLeft=-12.02,
rearRight=36.06,
)
chassis_speeds = kinematics_test.kinematics.toChassisSpeeds(wheel_speeds)
chassis_velocities = kinematics_test.kinematics.toChassisVelocities(wheel_velocities)
assert chassis_speeds.vx == pytest.approx(12.02, abs=0.1)
assert chassis_speeds.vy == pytest.approx(-7.07, abs=0.1)
assert chassis_speeds.omega == pytest.approx(0.707, abs=0.1)
assert chassis_velocities.vx == pytest.approx(12.02, abs=0.1)
assert chassis_velocities.vy == pytest.approx(-7.07, abs=0.1)
assert chassis_velocities.omega == pytest.approx(0.707, abs=0.1)
def test_off_center_translation_rotation_forward_kinematics_with_deltas(
@@ -242,20 +242,20 @@ def test_off_center_translation_rotation_forward_kinematics_with_deltas(
def test_desaturate(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=5, frontRight=6, rearLeft=4, rearRight=7
).desaturate(5.5)
k_factor = 5.5 / 7.0
assert wheel_speeds.frontLeft == pytest.approx(5.0 * k_factor, abs=1e-9)
assert wheel_speeds.frontRight == pytest.approx(6.0 * k_factor, abs=1e-9)
assert wheel_speeds.rearLeft == pytest.approx(4.0 * k_factor, abs=1e-9)
assert wheel_speeds.rearRight == pytest.approx(7.0 * k_factor, abs=1e-9)
assert wheel_velocities.frontLeft == pytest.approx(5.0 * k_factor, abs=1e-9)
assert wheel_velocities.frontRight == pytest.approx(6.0 * k_factor, abs=1e-9)
assert wheel_velocities.rearLeft == pytest.approx(4.0 * k_factor, abs=1e-9)
assert wheel_velocities.rearRight == pytest.approx(7.0 * k_factor, abs=1e-9)
def test_desaturate_negative_speeds(kinematics_test):
wheel_speeds = MecanumDriveWheelSpeeds(
def test_desaturate_negative_velocities(kinematics_test):
wheel_velocities = MecanumDriveWheelVelocities(
frontLeft=-5,
frontRight=6,
rearLeft=4,
@@ -264,7 +264,7 @@ def test_desaturate_negative_speeds(kinematics_test):
k_factor = 5.5 / 7.0
assert wheel_speeds.frontLeft == pytest.approx(-5.0 * k_factor, abs=1e-9)
assert wheel_speeds.frontRight == pytest.approx(6.0 * k_factor, abs=1e-9)
assert wheel_speeds.rearLeft == pytest.approx(4.0 * k_factor, abs=1e-9)
assert wheel_speeds.rearRight == pytest.approx(-7.0 * k_factor, abs=1e-9)
assert wheel_velocities.frontLeft == pytest.approx(-5.0 * k_factor, abs=1e-9)
assert wheel_velocities.frontRight == pytest.approx(6.0 * k_factor, abs=1e-9)
assert wheel_velocities.rearLeft == pytest.approx(4.0 * k_factor, abs=1e-9)
assert wheel_velocities.rearRight == pytest.approx(-7.0 * k_factor, abs=1e-9)

42
wpimath/src/test/python/kinematics/test_mecanum_drive_odometry.py
View File

@@ -3,7 +3,7 @@ import math
import random
from wpimath import (
ChassisSpeeds,
ChassisVelocities,
MecanumDriveKinematics,
MecanumDriveOdometry,
MecanumDriveWheelPositions,
@@ -130,23 +130,23 @@ def test_accuracy_facing_trajectory():
while t < trajectory.totalTime():
ground_truth_state = trajectory.sample(t)
wheel_speeds = kinematics.toWheelSpeeds(
ChassisSpeeds(
wheel_velocities = kinematics.toWheelVelocities(
ChassisVelocities(
ground_truth_state.velocity,
0,
ground_truth_state.velocity * ground_truth_state.curvature,
)
)
wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
wheel_positions.frontRight += wheel_speeds.frontRight * dt
wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
wheel_positions.rearRight += wheel_speeds.rearRight * dt
wheel_positions.frontLeft += wheel_velocities.frontLeft * dt
wheel_positions.frontRight += wheel_velocities.frontRight * dt
wheel_positions.rearLeft += wheel_velocities.rearLeft * dt
wheel_positions.rearRight += wheel_velocities.rearRight * dt
xhat = odometry.update(
ground_truth_state.pose.rotation()
@@ -199,23 +199,23 @@ def test_accuracy_facing_x_axis():
while t < trajectory.totalTime():
ground_truth_state = trajectory.sample(t)
wheel_speeds = kinematics.toWheelSpeeds(
ChassisSpeeds(
wheel_velocities = kinematics.toWheelVelocities(
ChassisVelocities(
ground_truth_state.velocity * ground_truth_state.pose.rotation().cos(),
ground_truth_state.velocity * ground_truth_state.pose.rotation().sin(),
0,
)
)
wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
wheel_positions.frontRight += wheel_speeds.frontRight * dt
wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
wheel_positions.rearRight += wheel_speeds.rearRight * dt
wheel_positions.frontLeft += wheel_velocities.frontLeft * dt
wheel_positions.frontRight += wheel_velocities.frontRight * dt
wheel_positions.rearLeft += wheel_velocities.rearLeft * dt
wheel_positions.rearRight += wheel_velocities.rearRight * dt
xhat = odometry.update(
Rotation2d(random.gauss(0.0, 1.0) * 0.05), wheel_positions

42
wpimath/src/test/python/kinematics/test_mecanum_drive_odometry3d.py
View File

@@ -3,7 +3,7 @@ import math
import random
from wpimath import (
ChassisSpeeds,
ChassisVelocities,
MecanumDriveKinematics,
MecanumDriveOdometry3d,
MecanumDriveWheelPositions,
@@ -155,23 +155,23 @@ def test_accuracy_facing_trajectory():
while t < trajectory.totalTime():
ground_truth_state = trajectory.sample(t)
wheel_speeds = kinematics.toWheelSpeeds(
ChassisSpeeds(
wheel_velocities = kinematics.toWheelVelocities(
ChassisVelocities(
ground_truth_state.velocity,
0,
ground_truth_state.velocity * ground_truth_state.curvature,
)
)
wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
wheel_positions.frontRight += wheel_speeds.frontRight * dt
wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
wheel_positions.rearRight += wheel_speeds.rearRight * dt
wheel_positions.frontLeft += wheel_velocities.frontLeft * dt
wheel_positions.frontRight += wheel_velocities.frontRight * dt
wheel_positions.rearLeft += wheel_velocities.rearLeft * dt
wheel_positions.rearRight += wheel_velocities.rearRight * dt
xhat = odometry.update(
Rotation3d(
@@ -228,23 +228,23 @@ def test_accuracy_facing_x_axis():
while t < trajectory.totalTime():
ground_truth_state = trajectory.sample(t)
wheel_speeds = kinematics.toWheelSpeeds(
ChassisSpeeds(
wheel_velocities = kinematics.toWheelVelocities(
ChassisVelocities(
ground_truth_state.velocity * ground_truth_state.pose.rotation().cos(),
ground_truth_state.velocity * ground_truth_state.pose.rotation().sin(),
0,
)
)
wheel_speeds.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_speeds.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.frontRight += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearLeft += random.gauss(0.0, 1.0) * 0.1
wheel_velocities.rearRight += random.gauss(0.0, 1.0) * 0.1
wheel_positions.frontLeft += wheel_speeds.frontLeft * dt
wheel_positions.frontRight += wheel_speeds.frontRight * dt
wheel_positions.rearLeft += wheel_speeds.rearLeft * dt
wheel_positions.rearRight += wheel_speeds.rearRight * dt
wheel_positions.frontLeft += wheel_velocities.frontLeft * dt
wheel_positions.frontRight += wheel_velocities.frontRight * dt
wheel_positions.rearLeft += wheel_velocities.rearLeft * dt
wheel_positions.rearRight += wheel_velocities.rearRight * dt
xhat = odometry.update(
Rotation3d(0, 0, random.gauss(0.0, 1.0) * 0.05),

22
wpimath/src/test/python/kinematics/test_mecanum_drive_wheel_speeds.py → wpimath/src/test/python/kinematics/test_mecanum_drive_wheel_velocities.py
View File

@@ -1,16 +1,16 @@
import pytest
from wpimath import MecanumDriveWheelSpeeds
from wpimath import MecanumDriveWheelVelocities
def test_plus():
left = MecanumDriveWheelSpeeds(
left = MecanumDriveWheelVelocities(
frontLeft=1.0,
frontRight=0.5,
rearLeft=2.0,
rearRight=1.5,
)
right = MecanumDriveWheelSpeeds(
right = MecanumDriveWheelVelocities(
frontLeft=2.0,
frontRight=1.5,
rearLeft=0.5,
@@ -26,13 +26,13 @@ def test_plus():
def test_minus():
left = MecanumDriveWheelSpeeds(
left = MecanumDriveWheelVelocities(
frontLeft=1.0,
frontRight=0.5,
rearLeft=2.0,
rearRight=1.5,
)
right = MecanumDriveWheelSpeeds(
right = MecanumDriveWheelVelocities(
frontLeft=2.0,
frontRight=1.5,
rearLeft=0.5,
@@ -48,14 +48,14 @@ def test_minus():
def test_unary_minus():
speeds = MecanumDriveWheelSpeeds(
velocities = MecanumDriveWheelVelocities(
frontLeft=1.0,
frontRight=0.5,
rearLeft=2.0,
rearRight=1.5,
)
result = -speeds
result = -velocities
assert result.frontLeft == -1.0
assert result.frontRight == -0.5
@@ -64,14 +64,14 @@ def test_unary_minus():
def test_multiplication():
speeds = MecanumDriveWheelSpeeds(
velocities = MecanumDriveWheelVelocities(
frontLeft=1.0,
frontRight=0.5,
rearLeft=2.0,
rearRight=1.5,
)
result = speeds * 2
result = velocities * 2
assert result.frontLeft == 2.0
assert result.frontRight == 1.0
@@ -80,14 +80,14 @@ def test_multiplication():
def test_division():
speeds = MecanumDriveWheelSpeeds(
velocities = MecanumDriveWheelVelocities(
frontLeft=1.0,
frontRight=0.5,
rearLeft=2.0,
rearRight=1.5,
)
result = speeds / 2
result = velocities / 2
assert result.frontLeft == 0.5
assert result.frontRight == 0.25

34
wpimath/src/test/python/kinematics/test_swerve.py
View File

@@ -4,11 +4,11 @@ from wpimath import (
Pose2d,
Rotation2d,
Translation2d,
ChassisSpeeds,
ChassisVelocities,
SwerveDrive4Kinematics,
SwerveDrive4Odometry,
SwerveModulePosition,
SwerveModuleState,
SwerveModuleVelocity,
)
@@ -22,13 +22,13 @@ def s4():
def test_swerve4_straightline(s4: SwerveDrive4Kinematics):
chassisSpeeds = ChassisSpeeds(5, 0, 0)
chassisVelocities = ChassisVelocities(5, 0, 0)
fl, fr, bl, br = s4.toSwerveModuleStates(chassisSpeeds)
assert fl.speed == pytest.approx(5.0)
assert fr.speed == pytest.approx(5.0)
assert bl.speed == pytest.approx(5.0)
assert br.speed == pytest.approx(5.0)
fl, fr, bl, br = s4.toSwerveModuleVelocities(chassisVelocities)
assert fl.velocity == pytest.approx(5.0)
assert fr.velocity == pytest.approx(5.0)
assert bl.velocity == pytest.approx(5.0)
assert br.velocity == pytest.approx(5.0)
assert fl.angle.radians() == pytest.approx(0.0)
assert fr.angle.radians() == pytest.approx(0.0)
@@ -37,19 +37,19 @@ def test_swerve4_straightline(s4: SwerveDrive4Kinematics):
def test_swerve4_normalize():
s1 = SwerveModuleState(5)
s2 = SwerveModuleState(6)
s3 = SwerveModuleState(4)
s4 = SwerveModuleState(7)
s1 = SwerveModuleVelocity(5)
s2 = SwerveModuleVelocity(6)
s3 = SwerveModuleVelocity(4)
s4 = SwerveModuleVelocity(7)
states = SwerveDrive4Kinematics.desaturateWheelSpeeds((s1, s2, s3, s4), 5.5)
states = SwerveDrive4Kinematics.desaturateWheelVelocities((s1, s2, s3, s4), 5.5)
kFactor = 5.5 / 7.0
assert states[0].speed == pytest.approx(5.0 * kFactor)
assert states[1].speed == pytest.approx(6.0 * kFactor)
assert states[2].speed == pytest.approx(4.0 * kFactor)
assert states[3].speed == pytest.approx(7.0 * kFactor)
assert states[0].velocity == pytest.approx(5.0 * kFactor)
assert states[1].velocity == pytest.approx(6.0 * kFactor)
assert states[2].velocity == pytest.approx(4.0 * kFactor)
assert states[3].velocity == pytest.approx(7.0 * kFactor)
def test_swerve4_odometry(s4: SwerveDrive4Kinematics):

220
wpimath/src/test/python/kinematics/test_swerve_drive_kinematics.py
View File

@@ -3,9 +3,9 @@ import math
from wpimath import (
SwerveDrive4Kinematics,
ChassisSpeeds,
ChassisVelocities,
Rotation2d,
SwerveModuleState,
SwerveModuleVelocity,
SwerveModulePosition,
Translation2d,
)
@@ -29,15 +29,15 @@ def kinematics_test():
def test_straight_line_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=5.0, vy=0.0, omega=0.0)
states = kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
velocities = ChassisVelocities(vx=5.0, vy=0.0, omega=0.0)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(velocities)
fl, fr, bl, br = states
assert fl.speed == pytest.approx(5.0, abs=kEpsilon)
assert fr.speed == pytest.approx(5.0, abs=kEpsilon)
assert bl.speed == pytest.approx(5.0, abs=kEpsilon)
assert br.speed == pytest.approx(5.0, abs=kEpsilon)
assert fl.velocity == pytest.approx(5.0, abs=kEpsilon)
assert fr.velocity == pytest.approx(5.0, abs=kEpsilon)
assert bl.velocity == pytest.approx(5.0, abs=kEpsilon)
assert br.velocity == pytest.approx(5.0, abs=kEpsilon)
assert fl.angle.radians() == pytest.approx(0.0, abs=kEpsilon)
assert fr.angle.radians() == pytest.approx(0.0, abs=kEpsilon)
@@ -46,14 +46,14 @@ def test_straight_line_inverse_kinematics(kinematics_test):
def test_straight_line_forward_kinematics(kinematics_test):
state = SwerveModuleState(speed=5.0, angle=Rotation2d.fromDegrees(0))
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds(
state = SwerveModuleVelocity(velocity=5.0, angle=Rotation2d.fromDegrees(0))
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities(
(state, state, state, state)
)
assert chassis_speeds.vx == pytest.approx(5.0, abs=kEpsilon)
assert chassis_speeds.vy == pytest.approx(0.0, abs=kEpsilon)
assert chassis_speeds.omega == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.vx == pytest.approx(5.0, abs=kEpsilon)
assert chassis_velocities.vy == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.omega == pytest.approx(0.0, abs=kEpsilon)
def test_straight_line_forward_kinematics_with_deltas(kinematics_test):
@@ -66,15 +66,15 @@ def test_straight_line_forward_kinematics_with_deltas(kinematics_test):
def test_straight_strafe_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=5, omega=0)
states = kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
velocities = ChassisVelocities(vx=0, vy=5, omega=0)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(velocities)
fl, fr, bl, br = states
assert fl.speed == pytest.approx(5.0, abs=kEpsilon)
assert fr.speed == pytest.approx(5.0, abs=kEpsilon)
assert bl.speed == pytest.approx(5.0, abs=kEpsilon)
assert br.speed == pytest.approx(5.0, abs=kEpsilon)
assert fl.velocity == pytest.approx(5.0, abs=kEpsilon)
assert fr.velocity == pytest.approx(5.0, abs=kEpsilon)
assert bl.velocity == pytest.approx(5.0, abs=kEpsilon)
assert br.velocity == pytest.approx(5.0, abs=kEpsilon)
assert fl.angle.degrees() == pytest.approx(90.0, abs=kEpsilon)
assert fr.angle.degrees() == pytest.approx(90.0, abs=kEpsilon)
@@ -83,14 +83,14 @@ def test_straight_strafe_inverse_kinematics(kinematics_test):
def test_straight_strafe_forward_kinematics(kinematics_test):
state = SwerveModuleState(speed=5, angle=Rotation2d.fromDegrees(90))
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds(
state = SwerveModuleVelocity(velocity=5, angle=Rotation2d.fromDegrees(90))
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities(
(state, state, state, state)
)
assert chassis_speeds.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_speeds.vy == pytest.approx(5.0, abs=kEpsilon)
assert chassis_speeds.omega == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.vy == pytest.approx(5.0, abs=kEpsilon)
assert chassis_velocities.omega == pytest.approx(0.0, abs=kEpsilon)
def test_straight_strafe_forward_kinematics_with_deltas(kinematics_test):
@@ -103,15 +103,15 @@ def test_straight_strafe_forward_kinematics_with_deltas(kinematics_test):
def test_turn_in_place_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=0, omega=2 * math.pi)
states = kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
velocities = ChassisVelocities(vx=0, vy=0, omega=2 * math.pi)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(velocities)
fl, fr, bl, br = states
assert fl.speed == pytest.approx(106.63, abs=kEpsilon)
assert fr.speed == pytest.approx(106.63, abs=kEpsilon)
assert bl.speed == pytest.approx(106.63, abs=kEpsilon)
assert br.speed == pytest.approx(106.63, abs=kEpsilon)
assert fl.velocity == pytest.approx(106.63, abs=kEpsilon)
assert fr.velocity == pytest.approx(106.63, abs=kEpsilon)
assert bl.velocity == pytest.approx(106.63, abs=kEpsilon)
assert br.velocity == pytest.approx(106.63, abs=kEpsilon)
assert fl.angle.degrees() == pytest.approx(135.0, abs=kEpsilon)
assert fr.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
@@ -120,16 +120,16 @@ def test_turn_in_place_inverse_kinematics(kinematics_test):
def test_conserve_wheel_angle(kinematics_test):
speeds = ChassisSpeeds(vx=0, vy=0, omega=2 * math.pi)
kinematics_test.m_kinematics.toSwerveModuleStates(speeds)
states = kinematics_test.m_kinematics.toSwerveModuleStates(ChassisSpeeds())
velocities = ChassisVelocities(vx=0, vy=0, omega=2 * math.pi)
kinematics_test.m_kinematics.toSwerveModuleVelocities(velocities)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(ChassisVelocities())
fl, fr, bl, br = states
assert fl.speed == pytest.approx(0.0, abs=kEpsilon)
assert fr.speed == pytest.approx(0.0, abs=kEpsilon)
assert bl.speed == pytest.approx(0.0, abs=kEpsilon)
assert br.speed == pytest.approx(0.0, abs=kEpsilon)
assert fl.velocity == pytest.approx(0.0, abs=kEpsilon)
assert fr.velocity == pytest.approx(0.0, abs=kEpsilon)
assert bl.velocity == pytest.approx(0.0, abs=kEpsilon)
assert br.velocity == pytest.approx(0.0, abs=kEpsilon)
assert fl.angle.degrees() == pytest.approx(135.0, abs=kEpsilon)
assert fr.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
@@ -143,14 +143,14 @@ def test_reset_wheel_angle(kinematics_test):
bl_angle = Rotation2d.fromDegrees(180)
br_angle = Rotation2d.fromDegrees(270)
kinematics_test.m_kinematics.resetHeadings((fl_angle, fr_angle, bl_angle, br_angle))
states = kinematics_test.m_kinematics.toSwerveModuleStates(ChassisSpeeds())
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(ChassisVelocities())
fl_mod, fr_mod, bl_mod, br_mod = states
assert fl_mod.speed == pytest.approx(0.0, abs=kEpsilon)
assert fr_mod.speed == pytest.approx(0.0, abs=kEpsilon)
assert bl_mod.speed == pytest.approx(0.0, abs=kEpsilon)
assert br_mod.speed == pytest.approx(0.0, abs=kEpsilon)
assert fl_mod.velocity == pytest.approx(0.0, abs=kEpsilon)
assert fr_mod.velocity == pytest.approx(0.0, abs=kEpsilon)
assert bl_mod.velocity == pytest.approx(0.0, abs=kEpsilon)
assert br_mod.velocity == pytest.approx(0.0, abs=kEpsilon)
assert fl_mod.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
assert fr_mod.angle.degrees() == pytest.approx(90.0, abs=kEpsilon)
@@ -159,16 +159,16 @@ def test_reset_wheel_angle(kinematics_test):
def test_turn_in_place_forward_kinematics(kinematics_test):
fl = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(135))
fr = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(45))
bl = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(-135))
br = SwerveModuleState(speed=106.629, angle=Rotation2d.fromDegrees(-45))
fl = SwerveModuleVelocity(velocity=106.629, angle=Rotation2d.fromDegrees(135))
fr = SwerveModuleVelocity(velocity=106.629, angle=Rotation2d.fromDegrees(45))
bl = SwerveModuleVelocity(velocity=106.629, angle=Rotation2d.fromDegrees(-135))
br = SwerveModuleVelocity(velocity=106.629, angle=Rotation2d.fromDegrees(-45))
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds((fl, fr, bl, br))
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities((fl, fr, bl, br))
assert chassis_speeds.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_speeds.vy == pytest.approx(0.0, abs=kEpsilon)
assert chassis_speeds.omega == pytest.approx(2 * math.pi, abs=kEpsilon)
assert chassis_velocities.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.vy == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.omega == pytest.approx(2 * math.pi, abs=kEpsilon)
def test_turn_in_place_forward_kinematics_with_deltas(kinematics_test):
@@ -185,17 +185,17 @@ def test_turn_in_place_forward_kinematics_with_deltas(kinematics_test):
def test_off_center_cor_rotation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(0, 0, 2 * math.pi)
states = kinematics_test.m_kinematics.toSwerveModuleStates(
speeds, kinematics_test.m_fl
velocities = ChassisVelocities(0, 0, 2 * math.pi)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(
velocities, kinematics_test.m_fl
)
fl, fr, bl, br = states
assert fl.speed == pytest.approx(0.0, abs=kEpsilon)
assert fr.speed == pytest.approx(150.796, abs=kEpsilon)
assert bl.speed == pytest.approx(150.796, abs=kEpsilon)
assert br.speed == pytest.approx(213.258, abs=kEpsilon)
assert fl.velocity == pytest.approx(0.0, abs=kEpsilon)
assert fr.velocity == pytest.approx(150.796, abs=kEpsilon)
assert bl.velocity == pytest.approx(150.796, abs=kEpsilon)
assert br.velocity == pytest.approx(213.258, abs=kEpsilon)
assert fl.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
assert fr.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
@@ -204,16 +204,16 @@ def test_off_center_cor_rotation_inverse_kinematics(kinematics_test):
def test_off_center_cor_rotation_forward_kinematics(kinematics_test):
fl = SwerveModuleState(speed=0.0, angle=Rotation2d.fromDegrees(0))
fr = SwerveModuleState(speed=150.796, angle=Rotation2d.fromDegrees(0))
bl = SwerveModuleState(speed=150.796, angle=Rotation2d.fromDegrees(-90))
br = SwerveModuleState(speed=213.258, angle=Rotation2d.fromDegrees(-45))
fl = SwerveModuleVelocity(velocity=0.0, angle=Rotation2d.fromDegrees(0))
fr = SwerveModuleVelocity(velocity=150.796, angle=Rotation2d.fromDegrees(0))
bl = SwerveModuleVelocity(velocity=150.796, angle=Rotation2d.fromDegrees(-90))
br = SwerveModuleVelocity(velocity=213.258, angle=Rotation2d.fromDegrees(-45))
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds((fl, fr, bl, br))
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities((fl, fr, bl, br))
assert chassis_speeds.vx == pytest.approx(75.398, abs=kEpsilon)
assert chassis_speeds.vy == pytest.approx(-75.398, abs=kEpsilon)
assert chassis_speeds.omega == pytest.approx(2 * math.pi, abs=kEpsilon)
assert chassis_velocities.vx == pytest.approx(75.398, abs=kEpsilon)
assert chassis_velocities.vy == pytest.approx(-75.398, abs=kEpsilon)
assert chassis_velocities.omega == pytest.approx(2 * math.pi, abs=kEpsilon)
def test_off_center_cor_rotation_forward_kinematics_with_deltas(kinematics_test):
@@ -230,17 +230,17 @@ def test_off_center_cor_rotation_forward_kinematics_with_deltas(kinematics_test)
def test_off_center_cor_rotation_and_translation_inverse_kinematics(kinematics_test):
speeds = ChassisSpeeds(0, 3.0, 1.5)
states = kinematics_test.m_kinematics.toSwerveModuleStates(
speeds, Translation2d(x=24, y=0)
velocities = ChassisVelocities(0, 3.0, 1.5)
states = kinematics_test.m_kinematics.toSwerveModuleVelocities(
velocities, Translation2d(x=24, y=0)
)
fl, fr, bl, br = states
assert fl.speed == pytest.approx(23.43, abs=kEpsilon)
assert fr.speed == pytest.approx(23.43, abs=kEpsilon)
assert bl.speed == pytest.approx(54.08, abs=kEpsilon)
assert br.speed == pytest.approx(54.08, abs=kEpsilon)
assert fl.velocity == pytest.approx(23.43, abs=kEpsilon)
assert fr.velocity == pytest.approx(23.43, abs=kEpsilon)
assert bl.velocity == pytest.approx(54.08, abs=kEpsilon)
assert br.velocity == pytest.approx(54.08, abs=kEpsilon)
assert fl.angle.degrees() == pytest.approx(-140.19, abs=kEpsilon)
assert fr.angle.degrees() == pytest.approx(-39.81, abs=kEpsilon)
@@ -249,16 +249,16 @@ def test_off_center_cor_rotation_and_translation_inverse_kinematics(kinematics_t
def test_off_center_cor_rotation_and_translation_forward_kinematics(kinematics_test):
fl = SwerveModuleState(speed=23.43, angle=Rotation2d.fromDegrees(-140.19))
fr = SwerveModuleState(speed=23.43, angle=Rotation2d.fromDegrees(-39.81))
bl = SwerveModuleState(speed=54.08, angle=Rotation2d.fromDegrees(-109.44))
br = SwerveModuleState(speed=54.08, angle=Rotation2d.fromDegrees(-70.56))
fl = SwerveModuleVelocity(velocity=23.43, angle=Rotation2d.fromDegrees(-140.19))
fr = SwerveModuleVelocity(velocity=23.43, angle=Rotation2d.fromDegrees(-39.81))
bl = SwerveModuleVelocity(velocity=54.08, angle=Rotation2d.fromDegrees(-109.44))
br = SwerveModuleVelocity(velocity=54.08, angle=Rotation2d.fromDegrees(-70.56))
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds((fl, fr, bl, br))
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities((fl, fr, bl, br))
assert chassis_speeds.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_speeds.vy == pytest.approx(-33.0, abs=kEpsilon)
assert chassis_speeds.omega == pytest.approx(1.5, abs=kEpsilon)
assert chassis_velocities.vx == pytest.approx(0.0, abs=kEpsilon)
assert chassis_velocities.vy == pytest.approx(-33.0, abs=kEpsilon)
assert chassis_velocities.omega == pytest.approx(1.5, abs=kEpsilon)
def test_off_center_cor_rotation_and_translation_forward_kinematics_with_deltas(
@@ -277,54 +277,54 @@ def test_off_center_cor_rotation_and_translation_forward_kinematics_with_deltas(
def test_desaturate(kinematics_test):
state1 = SwerveModuleState(speed=5.0, angle=Rotation2d.fromDegrees(0))
state2 = SwerveModuleState(speed=6.0, angle=Rotation2d.fromDegrees(0))
state3 = SwerveModuleState(speed=4.0, angle=Rotation2d.fromDegrees(0))
state4 = SwerveModuleState(speed=7.0, angle=Rotation2d.fromDegrees(0))
state1 = SwerveModuleVelocity(velocity=5.0, angle=Rotation2d.fromDegrees(0))
state2 = SwerveModuleVelocity(velocity=6.0, angle=Rotation2d.fromDegrees(0))
state3 = SwerveModuleVelocity(velocity=4.0, angle=Rotation2d.fromDegrees(0))
state4 = SwerveModuleVelocity(velocity=7.0, angle=Rotation2d.fromDegrees(0))
arr = [state1, state2, state3, state4]
arr = kinematics_test.m_kinematics.desaturateWheelSpeeds(arr, 5.5)
arr = kinematics_test.m_kinematics.desaturateWheelVelocities(arr, 5.5)
k_factor = 5.5 / 7.0
assert arr[0].speed == pytest.approx(5.0 * k_factor, abs=kEpsilon)
assert arr[1].speed == pytest.approx(6.0 * k_factor, abs=kEpsilon)
assert arr[2].speed == pytest.approx(4.0 * k_factor, abs=kEpsilon)
assert arr[3].speed == pytest.approx(7.0 * k_factor, abs=kEpsilon)
assert arr[0].velocity == pytest.approx(5.0 * k_factor, abs=kEpsilon)
assert arr[1].velocity == pytest.approx(6.0 * k_factor, abs=kEpsilon)
assert arr[2].velocity == pytest.approx(4.0 * k_factor, abs=kEpsilon)
assert arr[3].velocity == pytest.approx(7.0 * k_factor, abs=kEpsilon)
def test_desaturate_smooth(kinematics_test):
state1 = SwerveModuleState(speed=5.0, angle=Rotation2d(0))
state2 = SwerveModuleState(speed=6.0, angle=Rotation2d(0))
state3 = SwerveModuleState(speed=4.0, angle=Rotation2d(0))
state4 = SwerveModuleState(speed=7.0, angle=Rotation2d(0))
state1 = SwerveModuleVelocity(velocity=5.0, angle=Rotation2d(0))
state2 = SwerveModuleVelocity(velocity=6.0, angle=Rotation2d(0))
state3 = SwerveModuleVelocity(velocity=4.0, angle=Rotation2d(0))
state4 = SwerveModuleVelocity(velocity=7.0, angle=Rotation2d(0))
arr = [state1, state2, state3, state4]
chassis_speeds = kinematics_test.m_kinematics.toChassisSpeeds(
chassis_velocities = kinematics_test.m_kinematics.toChassisVelocities(
(arr[0], arr[1], arr[2], arr[3])
)
arr = kinematics_test.m_kinematics.desaturateWheelSpeeds(
arr, chassis_speeds, 5.5, 5.5, 3.5
arr = kinematics_test.m_kinematics.desaturateWheelVelocities(
arr, chassis_velocities, 5.5, 5.5, 3.5
)
k_factor = 5.5 / 7.0
assert arr[0].speed == pytest.approx(5.0 * k_factor, abs=kEpsilon)
assert arr[1].speed == pytest.approx(6.0 * k_factor, abs=kEpsilon)
assert arr[2].speed == pytest.approx(4.0 * k_factor, abs=kEpsilon)
assert arr[3].speed == pytest.approx(7.0 * k_factor, abs=kEpsilon)
assert arr[0].velocity == pytest.approx(5.0 * k_factor, abs=kEpsilon)
assert arr[1].velocity == pytest.approx(6.0 * k_factor, abs=kEpsilon)
assert arr[2].velocity == pytest.approx(4.0 * k_factor, abs=kEpsilon)
assert arr[3].velocity == pytest.approx(7.0 * k_factor, abs=kEpsilon)
def test_desaturate_negative_speed(kinematics_test):
state1 = SwerveModuleState(speed=1.0, angle=Rotation2d(0))
state2 = SwerveModuleState(speed=1.0, angle=Rotation2d(0))
state3 = SwerveModuleState(speed=-2.0, angle=Rotation2d(0))
state4 = SwerveModuleState(speed=-2.0, angle=Rotation2d(0))
def test_desaturate_negative_velocity(kinematics_test):
state1 = SwerveModuleVelocity(velocity=1.0, angle=Rotation2d(0))
state2 = SwerveModuleVelocity(velocity=1.0, angle=Rotation2d(0))
state3 = SwerveModuleVelocity(velocity=-2.0, angle=Rotation2d(0))
state4 = SwerveModuleVelocity(velocity=-2.0, angle=Rotation2d(0))
arr = [state1, state2, state3, state4]
arr = kinematics_test.m_kinematics.desaturateWheelSpeeds(arr, 1.0)
arr = kinematics_test.m_kinematics.desaturateWheelVelocities(arr, 1.0)
assert arr[0].speed == pytest.approx(0.5, abs=kEpsilon)
assert arr[1].speed == pytest.approx(0.5, abs=kEpsilon)
assert arr[2].speed == pytest.approx(-1.0, abs=kEpsilon)
assert arr[3].speed == pytest.approx(-1.0, abs=kEpsilon)
assert arr[0].velocity == pytest.approx(0.5, abs=kEpsilon)
assert arr[1].velocity == pytest.approx(0.5, abs=kEpsilon)
assert arr[2].velocity == pytest.approx(-1.0, abs=kEpsilon)
assert arr[3].velocity == pytest.approx(-1.0, abs=kEpsilon)

24
wpimath/src/test/python/kinematics/test_swerve_drive_odometry.py
View File

@@ -3,12 +3,12 @@ import math
import random
from wpimath import (
ChassisSpeeds,
ChassisVelocities,
Pose2d,
Rotation2d,
SwerveDrive4Kinematics,
SwerveDrive4Odometry,
SwerveModuleState,
SwerveModuleVelocity,
SwerveModulePosition,
TrajectoryGenerator,
TrajectoryConfig,
@@ -154,23 +154,23 @@ def test_accuracy_facing_trajectory():
while t < trajectory.totalTime():
ground_truth_state = trajectory.sample(t)
module_states = kinematics.toSwerveModuleStates(
ChassisSpeeds(
module_velocities = kinematics.toSwerveModuleVelocities(
ChassisVelocities(
vx=ground_truth_state.velocity,
vy=0,
omega=ground_truth_state.velocity * ground_truth_state.curvature,
)
)
fl.distance += module_states[0].speed * dt
fr.distance += module_states[1].speed * dt
bl.distance += module_states[2].speed * dt
br.distance += module_states[3].speed * dt
fl.distance += module_velocities[0].velocity * dt
fr.distance += module_velocities[1].velocity * dt
bl.distance += module_velocities[2].velocity * dt
br.distance += module_velocities[3].velocity * dt
fl.angle = module_states[0].angle
fr.angle = module_states[1].angle
bl.angle = module_states[2].angle
br.angle = module_states[3].angle
fl.angle = module_velocities[0].angle
fr.angle = module_velocities[1].angle
bl.angle = module_velocities[2].angle
br.angle = module_velocities[3].angle
xhat = odometry.update(
ground_truth_state.pose.rotation()

2
wpimath/src/test/python/kinematics/test_swerve_drive_odometry3d.py
View File

@@ -3,7 +3,7 @@ import math
import random
from wpimath import (
ChassisSpeeds,
ChassisVelocities,
Pose3d,
Pose2d,
Rotation2d,

0
wpimath/src/test/python/kinematics/test_swerve_module_accelerations.py → wpimath/src/test/python/kinematics/test_swerve_module_acceleration.py
View File

76
wpimath/src/test/python/kinematics/test_swerve_module_state.py → wpimath/src/test/python/kinematics/test_swerve_module_velocity.py
View File

@@ -1,140 +1,140 @@
import pytest
import math
from wpimath import Rotation2d, SwerveModuleState
from wpimath import Rotation2d, SwerveModuleVelocity
kEpsilon = 1e-9
def test_optimize():
angle_a = Rotation2d.fromDegrees(45)
ref_a = SwerveModuleState(-2, Rotation2d.fromDegrees(180))
ref_a = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(180))
ref_a.optimize(angle_a)
assert ref_a.speed == pytest.approx(2.0, abs=kEpsilon)
assert ref_a.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_a.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
angle_b = Rotation2d.fromDegrees(-50)
ref_b = SwerveModuleState(4.7, Rotation2d.fromDegrees(41))
ref_b = SwerveModuleVelocity(4.7, Rotation2d.fromDegrees(41))
ref_b.optimize(angle_b)
assert ref_b.speed == pytest.approx(-4.7, abs=kEpsilon)
assert ref_b.velocity == pytest.approx(-4.7, abs=kEpsilon)
assert ref_b.angle.degrees() == pytest.approx(-139.0, abs=kEpsilon)
def test_no_optimize():
angle_a = Rotation2d.fromDegrees(0)
ref_a = SwerveModuleState(2, Rotation2d.fromDegrees(89))
ref_a = SwerveModuleVelocity(2, Rotation2d.fromDegrees(89))
ref_a.optimize(angle_a)
assert ref_a.speed == pytest.approx(2.0, abs=kEpsilon)
assert ref_a.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_a.angle.degrees() == pytest.approx(89.0, abs=kEpsilon)
angle_b = Rotation2d.fromDegrees(0)
ref_b = SwerveModuleState(-2, Rotation2d.fromDegrees(-2))
ref_b = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(-2))
ref_b.optimize(angle_b)
assert ref_b.speed == pytest.approx(-2.0, abs=kEpsilon)
assert ref_b.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert ref_b.angle.degrees() == pytest.approx(-2.0, abs=kEpsilon)
def test_cosine_scaling():
angle_a = Rotation2d.fromDegrees(0)
ref_a = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_a = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_a.cosineScale(angle_a)
assert ref_a.speed == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert ref_a.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert ref_a.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_b = Rotation2d.fromDegrees(45)
ref_b = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_b = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_b.cosineScale(angle_b)
assert ref_b.speed == pytest.approx(2.0, abs=kEpsilon)
assert ref_b.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_b.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_c = Rotation2d.fromDegrees(-45)
ref_c = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_c = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_c.cosineScale(angle_c)
assert ref_c.speed == pytest.approx(0.0, abs=kEpsilon)
assert ref_c.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_c.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_d = Rotation2d.fromDegrees(135)
ref_d = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_d = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_d.cosineScale(angle_d)
assert ref_d.speed == pytest.approx(0.0, abs=kEpsilon)
assert ref_d.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_d.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_e = Rotation2d.fromDegrees(-135)
ref_e = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_e = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_e.cosineScale(angle_e)
assert ref_e.speed == pytest.approx(-2.0, abs=kEpsilon)
assert ref_e.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert ref_e.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_f = Rotation2d.fromDegrees(180)
ref_f = SwerveModuleState(2, Rotation2d.fromDegrees(45))
ref_f = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_f.cosineScale(angle_f)
assert ref_f.speed == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert ref_f.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert ref_f.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_g = Rotation2d.fromDegrees(0)
ref_g = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_g = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_g.cosineScale(angle_g)
assert ref_g.speed == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert ref_g.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert ref_g.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_h = Rotation2d.fromDegrees(45)
ref_h = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_h = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_h.cosineScale(angle_h)
assert ref_h.speed == pytest.approx(-2.0, abs=kEpsilon)
assert ref_h.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert ref_h.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_i = Rotation2d.fromDegrees(-45)
ref_i = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_i = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_i.cosineScale(angle_i)
assert ref_i.speed == pytest.approx(0.0, abs=kEpsilon)
assert ref_i.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_i.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_j = Rotation2d.fromDegrees(135)
ref_j = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_j = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_j.cosineScale(angle_j)
assert ref_j.speed == pytest.approx(0.0, abs=kEpsilon)
assert ref_j.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_j.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_k = Rotation2d.fromDegrees(-135)
ref_k = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_k = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_k.cosineScale(angle_k)
assert ref_k.speed == pytest.approx(2.0, abs=kEpsilon)
assert ref_k.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_k.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_l = Rotation2d.fromDegrees(180)
ref_l = SwerveModuleState(-2, Rotation2d.fromDegrees(45))
ref_l = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_l.cosineScale(angle_l)
assert ref_l.speed == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert ref_l.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert ref_l.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
def test_equality():
state1 = SwerveModuleState(2, Rotation2d.fromDegrees(90))
state2 = SwerveModuleState(2, Rotation2d.fromDegrees(90))
state1 = SwerveModuleVelocity(2, Rotation2d.fromDegrees(90))
state2 = SwerveModuleVelocity(2, Rotation2d.fromDegrees(90))
assert state1 == state2
def test_inequality():
state1 = SwerveModuleState(1, Rotation2d.fromDegrees(90))
state2 = SwerveModuleState(2, Rotation2d.fromDegrees(90))
state3 = SwerveModuleState(1, Rotation2d.fromDegrees(89))
state1 = SwerveModuleVelocity(1, Rotation2d.fromDegrees(90))
state2 = SwerveModuleVelocity(2, Rotation2d.fromDegrees(90))
state3 = SwerveModuleVelocity(1, Rotation2d.fromDegrees(89))
assert state1 != state2
assert state1 != state3

16
wpimath/src/test/python/test_trajectory_exponential_profile.py
View File

@@ -113,29 +113,29 @@ def test_switch_goal_in_middle(profile):
assert state == goal
def test_top_speed(profile):
def test_top_velocity(profile):
goal = ExponentialProfileMeterVolts.State(40, 0)
state = ExponentialProfileMeterVolts.State(0, 0)
max_speed = 0
max_velocity = 0
for _ in range(900):
state = check_dynamics(profile, state, goal)
max_speed = max(max_speed, state.velocity)
max_velocity = max(max_velocity, state.velocity)
assert_near(constraints.maxVelocity(), max_speed, 10e-5)
assert_near(constraints.maxVelocity(), max_velocity, 10e-5)
assert state == goal
def test_top_speed_backward(profile):
def test_top_velocity_backward(profile):
goal = ExponentialProfileMeterVolts.State(-40, 0)
state = ExponentialProfileMeterVolts.State(0, 0)
max_speed = 0
max_velocity = 0
for _ in range(900):
state = check_dynamics(profile, state, goal)
max_speed = min(max_speed, state.velocity)
max_velocity = min(max_velocity, state.velocity)
assert_near(-constraints.maxVelocity(), max_speed, 10e-5)
assert_near(-constraints.maxVelocity(), max_velocity, 10e-5)
assert state == goal

2
wpimath/src/test/python/trajectory/test_trapezoidal_profile.py
View File

@@ -78,7 +78,7 @@ def test_switch_goal_in_middle():
assert state == goal
def test_top_speed():
def test_top_velocity():
constraints = TrapezoidProfile.Constraints(0.75, 0.75)
goal = TrapezoidProfile.State(4.0, 0.0)
state = TrapezoidProfile.State()