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

[wpimath] Move math functionality into new wpimath library (#2629)

Browse Source 
The wpimath library is a new library designed to separate the reusable math functionality
from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j).

Package names / include file names were NOT changed to minimize breakage.  In a future year
it would be good to revamp these for a more uniform user experience and to reduce the risk
of accidental naming conflicts.

While theoretically all of this functionality could be placed into wpiutil, several pieces
of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this
expense for users who only want cscore or ntcore.  It also allows for easy future separation
of build tasks vs number of workers on memory-constrained machines.

This moves the following functionality from wpiutil into wpimath:
- Eigen
- ejml
- Drake
- DARE
- wpiutil.math package (Matrix etc)
- units

And the following functionality from wpilibc/j into wpimath:
- Geometry
- Kinematics
- Spline
- Trajectory
- LinearFilter
- MedianFilter
- Feed-forward controllers
This commit is contained in:
Peter Johnson
2020-08-06 23:57:39 -07:00
committed by GitHub
parent ad817d4f23
commit 42993b15c6
463 changed files with 1006 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files

116
wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java Normal file
View File

@@ -0,0 +1,116 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj;
import java.util.Random;
import java.util.function.DoubleFunction;
import java.util.stream.Stream;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.Arguments;
import org.junit.jupiter.params.provider.MethodSource;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.params.provider.Arguments.arguments;
class LinearFilterTest {
private static final double kFilterStep = 0.005;
private static final double kFilterTime = 2.0;
private static final double kSinglePoleIIRTimeConstant = 0.015915;
private static final double kHighPassTimeConstant = 0.006631;
private static final int kMovAvgTaps = 6;
private static final double kSinglePoleIIRExpectedOutput = -3.2172003;
private static final double kHighPassExpectedOutput = 10.074717;
private static final double kMovAvgExpectedOutput = -10.191644;
@SuppressWarnings("ParameterName")
private static double getData(double t) {
return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t);
}
@SuppressWarnings("ParameterName")
private static double getPulseData(double t) {
if (Math.abs(t - 1.0) < 0.001) {
return 1.0;
} else {
return 0.0;
}
}
@Test
void illegalTapNumberTest() {
assertThrows(IllegalArgumentException.class, () -> LinearFilter.movingAverage(0));
}
/**
* Test if the filter reduces the noise produced by a signal generator.
*/
@ParameterizedTest
@MethodSource("noiseFilterProvider")
void noiseReduceTest(final LinearFilter filter) {
double noiseGenError = 0.0;
double filterError = 0.0;
final Random gen = new Random();
final double kStdDev = 10.0;
for (double t = 0; t < kFilterTime; t += kFilterStep) {
final double theory = getData(t);
final double noise = gen.nextGaussian() * kStdDev;
filterError += Math.abs(filter.calculate(theory + noise) - theory);
noiseGenError += Math.abs(noise - theory);
}
assertTrue(noiseGenError > filterError,
"Filter should have reduced noise accumulation from " + noiseGenError
+ " but failed. The filter error was " + filterError);
}
static Stream<LinearFilter> noiseFilterProvider() {
return Stream.of(
LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
LinearFilter.movingAverage(kMovAvgTaps)
);
}
/**
* Test if the linear filters produce consistent output for a given data set.
*/
@ParameterizedTest
@MethodSource("outputFilterProvider")
void outputTest(final LinearFilter filter, final DoubleFunction<Double> data,
final double expectedOutput) {
double filterOutput = 0.0;
for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
filterOutput = filter.calculate(data.apply(t));
}
assertEquals(expectedOutput, filterOutput, 5e-5, "Filter output was incorrect.");
}
static Stream<Arguments> outputFilterProvider() {
return Stream.of(
arguments(LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
(DoubleFunction<Double>) LinearFilterTest::getData,
kSinglePoleIIRExpectedOutput),
arguments(LinearFilter.highPass(kHighPassTimeConstant, kFilterStep),
(DoubleFunction<Double>) LinearFilterTest::getData,
kHighPassExpectedOutput),
arguments(LinearFilter.movingAverage(kMovAvgTaps),
(DoubleFunction<Double>) LinearFilterTest::getData,
kMovAvgExpectedOutput),
arguments(LinearFilter.movingAverage(kMovAvgTaps),
(DoubleFunction<Double>) LinearFilterTest::getPulseData,
0.0)
);
}
}

64
wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java Normal file
View File

@@ -0,0 +1,64 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
public class MedianFilterTest {
@Test
void medianFilterNotFullTestEven() {
MedianFilter filter = new MedianFilter(10);
filter.calculate(3);
filter.calculate(0);
filter.calculate(4);
assertEquals(3.5, filter.calculate(1000));
}
@Test
void medianFilterNotFullTestOdd() {
MedianFilter filter = new MedianFilter(10);
filter.calculate(3);
filter.calculate(0);
filter.calculate(4);
filter.calculate(7);
assertEquals(4, filter.calculate(1000));
}
@Test
void medianFilterFullTestEven() {
MedianFilter filter = new MedianFilter(6);
filter.calculate(3);
filter.calculate(0);
filter.calculate(0);
filter.calculate(5);
filter.calculate(4);
filter.calculate(1000);
assertEquals(4.5, filter.calculate(99));
}
@Test
void medianFilterFullTestOdd() {
MedianFilter filter = new MedianFilter(5);
filter.calculate(3);
filter.calculate(0);
filter.calculate(5);
filter.calculate(4);
filter.calculate(1000);
assertEquals(5, filter.calculate(99));
}
}

61
wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java Normal file
View File

@@ -0,0 +1,61 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj;
import java.lang.reflect.Constructor;
import java.lang.reflect.InvocationTargetException;
import java.lang.reflect.Modifier;
import java.util.Arrays;
import java.util.stream.Stream;
import org.junit.jupiter.api.DynamicTest;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.api.TestFactory;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.DynamicTest.dynamicTest;
@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
public abstract class UtilityClassTest<T> {
private final Class<T> m_clazz;
protected UtilityClassTest(Class<T> clazz) {
m_clazz = clazz;
}
@Test
public void singleConstructorTest() {
assertEquals(1, m_clazz.getDeclaredConstructors().length,
"More than one constructor defined");
}
@Test
public void constructorPrivateTest() {
Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
assertFalse(constructor.canAccess(null), "Constructor is not private");
}
@Test
public void constructorReflectionTest() {
Constructor<?> constructor = m_clazz.getDeclaredConstructors()[0];
constructor.setAccessible(true);
assertThrows(InvocationTargetException.class, constructor::newInstance);
}
@TestFactory
Stream<DynamicTest> publicMethodsStaticTestFactory() {
return Arrays.stream(m_clazz.getDeclaredMethods())
.filter(method -> Modifier.isPublic(method.getModifiers()))
.map(method -> dynamicTest(method.getName(),
() -> assertTrue(Modifier.isStatic(method.getModifiers()))));
}
}

75
wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java Normal file
View File

@@ -0,0 +1,75 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.geometry;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
class Pose2dTest {
private static final double kEpsilon = 1E-9;
@Test
void testTransformBy() {
var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
var transformation = new Transform2d(new Translation2d(5.0, 0.0),
Rotation2d.fromDegrees(5.0));
var transformed = initial.plus(transformation);
assertAll(
() -> assertEquals(transformed.getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
() -> assertEquals(transformed.getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
() -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon)
);
}
@Test
void testRelativeTo() {
var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
var finalRelativeToInitial = last.relativeTo(initial);
assertAll(
() -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0),
kEpsilon),
() -> assertEquals(finalRelativeToInitial.getY(), 0.0, kEpsilon),
() -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon)
);
}
@Test
void testEquality() {
var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
var two = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
assertEquals(one, two);
}
@Test
void testInequality() {
var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
var two = new Pose2d(0.0, 1.524, Rotation2d.fromDegrees(43.0));
assertNotEquals(one, two);
}
void testMinus() {
var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
final var transform = last.minus(initial);
assertAll(
() -> assertEquals(transform.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
() -> assertEquals(transform.getY(), 0.0, kEpsilon),
() -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon)
);
}
}

81
wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java Normal file
View File

@@ -0,0 +1,81 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.geometry;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
class Rotation2dTest {
private static final double kEpsilon = 1E-9;
@Test
void testRadiansToDegrees() {
var one = new Rotation2d(Math.PI / 3);
var two = new Rotation2d(Math.PI / 4);
assertAll(
() -> assertEquals(one.getDegrees(), 60.0, kEpsilon),
() -> assertEquals(two.getDegrees(), 45.0, kEpsilon)
);
}
@Test
void testRadiansAndDegrees() {
var one = Rotation2d.fromDegrees(45.0);
var two = Rotation2d.fromDegrees(30.0);
assertAll(
() -> assertEquals(one.getRadians(), Math.PI / 4, kEpsilon),
() -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon)
);
}
@Test
void testRotateByFromZero() {
var zero = new Rotation2d();
var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
assertAll(
() -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
() -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon)
);
}
@Test
void testRotateByNonZero() {
var rot = Rotation2d.fromDegrees(90.0);
rot = rot.plus(Rotation2d.fromDegrees(30.0));
assertEquals(rot.getDegrees(), 120.0, kEpsilon);
}
@Test
void testMinus() {
var one = Rotation2d.fromDegrees(70.0);
var two = Rotation2d.fromDegrees(30.0);
assertEquals(one.minus(two).getDegrees(), 40.0, kEpsilon);
}
@Test
void testEquality() {
var one = Rotation2d.fromDegrees(43.0);
var two = Rotation2d.fromDegrees(43.0);
assertEquals(one, two);
}
@Test
void testInequality() {
var one = Rotation2d.fromDegrees(43.0);
var two = Rotation2d.fromDegrees(43.5);
assertNotEquals(one, two);
}
}

36
wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java Normal file
View File

@@ -0,0 +1,36 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.geometry;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
class Transform2dTest {
private static final double kEpsilon = 1E-9;
@Test
void testInverse() {
var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
var transformation = new Transform2d(new Translation2d(5.0, 0.0),
Rotation2d.fromDegrees(5.0));
var transformed = initial.plus(transformation);
var untransformed = transformed.plus(transformation.inverse());
assertAll(
() -> assertEquals(initial.getX(), untransformed.getX(),
kEpsilon),
() -> assertEquals(initial.getY(), untransformed.getY(),
kEpsilon),
() -> assertEquals(initial.getRotation().getDegrees(),
untransformed.getRotation().getDegrees(), kEpsilon)
);
}
}

115
wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java Normal file
View File

@@ -0,0 +1,115 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.geometry;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
class Translation2dTest {
private static final double kEpsilon = 1E-9;
@Test
void testSum() {
var one = new Translation2d(1.0, 3.0);
var two = new Translation2d(2.0, 5.0);
var sum = one.plus(two);
assertAll(
() -> assertEquals(sum.getX(), 3.0, kEpsilon),
() -> assertEquals(sum.getY(), 8.0, kEpsilon)
);
}
@Test
void testDifference() {
var one = new Translation2d(1.0, 3.0);
var two = new Translation2d(2.0, 5.0);
var difference = one.minus(two);
assertAll(
() -> assertEquals(difference.getX(), -1.0, kEpsilon),
() -> assertEquals(difference.getY(), -2.0, kEpsilon)
);
}
@Test
void testRotateBy() {
var another = new Translation2d(3.0, 0.0);
var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
assertAll(
() -> assertEquals(rotated.getX(), 0.0, kEpsilon),
() -> assertEquals(rotated.getY(), 3.0, kEpsilon)
);
}
@Test
void testMultiplication() {
var original = new Translation2d(3.0, 5.0);
var mult = original.times(3);
assertAll(
() -> assertEquals(mult.getX(), 9.0, kEpsilon),
() -> assertEquals(mult.getY(), 15.0, kEpsilon)
);
}
@Test
void testDivision() {
var original = new Translation2d(3.0, 5.0);
var div = original.div(2);
assertAll(
() -> assertEquals(div.getX(), 1.5, kEpsilon),
() -> assertEquals(div.getY(), 2.5, kEpsilon)
);
}
@Test
void testNorm() {
var one = new Translation2d(3.0, 5.0);
assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
}
@Test
void testDistance() {
var one = new Translation2d(1, 1);
var two = new Translation2d(6, 6);
assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
}
@Test
void testUnaryMinus() {
var original = new Translation2d(-4.5, 7);
var inverted = original.unaryMinus();
assertAll(
() -> assertEquals(inverted.getX(), 4.5, kEpsilon),
() -> assertEquals(inverted.getY(), -7, kEpsilon)
);
}
@Test
void testEquality() {
var one = new Translation2d(9, 5.5);
var two = new Translation2d(9, 5.5);
assertEquals(one, two);
}
@Test
void testInequality() {
var one = new Translation2d(9, 5.5);
var two = new Translation2d(9, 5.7);
assertNotEquals(one, two);
}
}

81
wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java Normal file
View File

@@ -0,0 +1,81 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.geometry;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
class Twist2dTest {
private static final double kEpsilon = 1E-9;
@Test
void testStraightLineTwist() {
var straight = new Twist2d(5.0, 0.0, 0.0);
var straightPose = new Pose2d().exp(straight);
assertAll(
() -> assertEquals(straightPose.getX(), 5.0, kEpsilon),
() -> assertEquals(straightPose.getY(), 0.0, kEpsilon),
() -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon)
);
}
@Test
void testQuarterCirleTwist() {
var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
var quarterCirclePose = new Pose2d().exp(quarterCircle);
assertAll(
() -> assertEquals(quarterCirclePose.getX(), 5.0, kEpsilon),
() -> assertEquals(quarterCirclePose.getY(), 5.0, kEpsilon),
() -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon)
);
}
@Test
void testDiagonalNoDtheta() {
var diagonal = new Twist2d(2.0, 2.0, 0.0);
var diagonalPose = new Pose2d().exp(diagonal);
assertAll(
() -> assertEquals(diagonalPose.getX(), 2.0, kEpsilon),
() -> assertEquals(diagonalPose.getY(), 2.0, kEpsilon),
() -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon)
);
}
@Test
void testEquality() {
var one = new Twist2d(5, 1, 3);
var two = new Twist2d(5, 1, 3);
assertEquals(one, two);
}
@Test
void testInequality() {
var one = new Twist2d(5, 1, 3);
var two = new Twist2d(5, 1.2, 3);
assertNotEquals(one, two);
}
void testPose2dLog() {
final var start = new Pose2d();
final var end = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
final var twist = start.log(end);
assertAll(
() -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon),
() -> assertEquals(twist.dy, 0.0, kEpsilon),
() -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon)
);
}
}

32
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java Normal file
View File

@@ -0,0 +1,32 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
class ChassisSpeedsTest {
private static final double kEpsilon = 1E-9;
@Test
void testFieldRelativeConstruction() {
final var chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0)
);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
}
}

88
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java Normal file
View File

@@ -0,0 +1,88 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
class DifferentialDriveKinematicsTest {
private static final double kEpsilon = 1E-9;
private final DifferentialDriveKinematics m_kinematics
= new DifferentialDriveKinematics(0.381 * 2);
@Test
void testInverseKinematicsForZeros() {
var chassisSpeeds = new ChassisSpeeds();
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
assertAll(
() -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
);
}
@Test
void testForwardKinematicsForZeros() {
var wheelSpeeds = new DifferentialDriveWheelSpeeds();
var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
}
@Test
void testInverseKinematicsForStraightLine() {
var chassisSpeeds = new ChassisSpeeds(3, 0, 0);
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
assertAll(
() -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
);
}
@Test
void testForwardKinematicsForStraightLine() {
var wheelSpeeds = new DifferentialDriveWheelSpeeds(3, 3);
var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
}
@Test
void testInverseKinematicsForRotateInPlace() {
var chassisSpeeds = new ChassisSpeeds(0, 0, Math.PI);
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
assertAll(
() -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon)
);
}
@Test
void testForwardKinematicsForRotateInPlace() {
var wheelSpeeds = new DifferentialDriveWheelSpeeds(+0.381 * Math.PI, -0.381 * Math.PI);
var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
}
}

34
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java Normal file
View File

@@ -0,0 +1,34 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
class DifferentialDriveOdometryTest {
private static final double kEpsilon = 1E-9;
private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(
new Rotation2d());
@Test
void testOdometryWithEncoderDistances() {
m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
assertAll(
() -> assertEquals(pose.getX(), 5.0, kEpsilon),
() -> assertEquals(pose.getY(), 5.0, kEpsilon),
() -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)
);
}
}

263
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java Normal file
View File

@@ -0,0 +1,263 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
@SuppressWarnings("PMD.TooManyMethods")
class MecanumDriveKinematicsTest {
private static final double kEpsilon = 1E-9;
private final Translation2d m_fl = new Translation2d(12, 12);
private final Translation2d m_fr = new Translation2d(12, -12);
private final Translation2d m_bl = new Translation2d(-12, 12);
private final Translation2d m_br = new Translation2d(-12, -12);
private final MecanumDriveKinematics m_kinematics =
new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
@Test
void testStraightLineInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
*/
assertAll(
() -> assertEquals(3.536, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(3.536, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(3.536, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(3.536, moduleStates.rearRightMetersPerSecond, 0.1)
);
}
@Test
void testStraightLineForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]]
*/
assertAll(
() -> assertEquals(5, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
);
}
@Test
void testStrafeInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
*/
assertAll(
() -> assertEquals(-2.828427, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(2.828427, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(2.828427, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(-2.828427, moduleStates.rearRightMetersPerSecond, 0.1)
);
}
@Test
void testStrafeForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-2.828427, 2.828427, 2.828427, -2.828427);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be [[5][0][0]]
*/
assertAll(
() -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(4, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
);
}
@Test
void testRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
*/
assertAll(
() -> assertEquals(-106.629191, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(106.629191, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-106.629191, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(106.629191, moduleStates.rearRightMetersPerSecond, 0.1)
);
}
@Test
void testRotationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-106.629191, 106.629191, -106.629191, 106.629191);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should be [[0][0][2pi]]
*/
assertAll(
() -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1)
);
}
@Test
void testMixedTranslationRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
*/
assertAll(
() -> assertEquals(-17.677670, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(20.506097, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-13.435, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(16.26, moduleStates.rearRightMetersPerSecond, 0.1)
);
}
@Test
void testMixedTranslationRotationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.677670, 20.51, -13.44, 16.26);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be [[2][3][1]]
*/
assertAll(
() -> assertEquals(2, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(3, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
);
}
@Test
void testOffCenterRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
*/
assertAll(
() -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(16.971, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-16.971, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(33.941, moduleStates.rearRightMetersPerSecond, 0.1)
);
}
@Test
void testOffCenterRotationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from the wheel
velocities should be [[12][-12][1]]
*/
assertAll(
() -> assertEquals(12, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(-12, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
);
}
@Test
void testOffCenterTranslationRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
*/
assertAll(
() -> assertEquals(2.12, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(21.92, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-12.02, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(36.06, moduleStates.rearRightMetersPerSecond, 0.1)
);
}
@Test
void testOffCenterRotationTranslationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(2.12, 21.92, -12.02, 36.06);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from the wheel
velocities should be [[17][-10][1]]
*/
assertAll(
() -> assertEquals(17, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(-10, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
);
}
@Test
void testNormalize() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
wheelSpeeds.normalize(5.5);
double factor = 5.5 / 7.0;
assertAll(
() -> assertEquals(5.0 * factor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
() -> assertEquals(6.0 * factor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
() -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
() -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon)
);
}
}

94
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java Normal file
View File

@@ -0,0 +1,94 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
class MecanumDriveOdometryTest {
private final Translation2d m_fl = new Translation2d(12, 12);
private final Translation2d m_fr = new Translation2d(12, -12);
private final Translation2d m_bl = new Translation2d(-12, 12);
private final Translation2d m_br = new Translation2d(-12, -12);
private final MecanumDriveKinematics m_kinematics =
new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
new Rotation2d());
@Test
void testMultipleConsecutiveUpdates() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
assertAll(
() -> assertEquals(secondPose.getX(), 0.0, 0.01),
() -> assertEquals(secondPose.getY(), 0.0, 0.01),
() -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01)
);
}
@Test
void testTwoIterations() {
// 5 units/sec in the x axis (forward)
final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
assertAll(
() -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
() -> assertEquals(0, pose.getY(), 0.01),
() -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
);
}
@Test
void test90degreeTurn() {
// This is a 90 degree turn about the point between front left and rear left wheels
// fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
assertAll(
() -> assertEquals(12.0, pose.getX(), 0.01),
() -> assertEquals(12.0, pose.getY(), 0.01),
() -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
);
}
@Test
void testGyroAngleReset() {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536,
3.536, 3.536);
m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
assertAll(
() -> assertEquals(5.0, pose.getX(), 0.1),
() -> assertEquals(0.00, pose.getY(), 0.1),
() -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
);
}
}

263
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java Normal file
View File

@@ -0,0 +1,263 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
@SuppressWarnings("PMD.TooManyMethods")
class SwerveDriveKinematicsTest {
private static final double kEpsilon = 1E-9;
private final Translation2d m_fl = new Translation2d(12, 12);
private final Translation2d m_fr = new Translation2d(12, -12);
private final Translation2d m_bl = new Translation2d(-12, 12);
private final Translation2d m_br = new Translation2d(-12, -12);
private final SwerveDriveKinematics m_kinematics =
new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
@Test
void testStraightLineInverseKinematics() { // test inverse kinematics going in a straight line
ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
assertAll(
() -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, 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)
);
}
@Test
void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
}
@Test
void testStraightStrafeInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
assertAll(
() -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, 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)
);
}
@Test
void testStraightStrafeForwardKinematics() {
SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
}
@Test
void testTurnInPlaceInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
/*
The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius
the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels
trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second,
our wheels must trace out 1 rotation (or 106.63 inches) per second.
*/
assertAll(
() -> assertEquals(106.63, moduleStates[0].speedMetersPerSecond, 0.1),
() -> assertEquals(106.63, moduleStates[1].speedMetersPerSecond, 0.1),
() -> assertEquals(106.63, moduleStates[2].speedMetersPerSecond, 0.1),
() -> assertEquals(106.63, moduleStates[3].speedMetersPerSecond, 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)
);
}
@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));
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
);
}
@Test
void testOffCenterCORRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds, 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 with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
radius sqrt(24^2 + 24^2) 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].speedMetersPerSecond, 0.1),
() -> assertEquals(150.796, moduleStates[1].speedMetersPerSecond, 0.1),
() -> assertEquals(150.796, moduleStates[2].speedMetersPerSecond, 0.1),
() -> assertEquals(213.258, moduleStates[3].speedMetersPerSecond, 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)
);
}
@Test
void testOffCenterCORRotationForwardKinematics() {
SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0));
SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(0.0));
SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(-90));
SwerveModuleState brState = new SwerveModuleState(213.258, Rotation2d.fromDegrees(-45));
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
/*
We already know that our omega should be 2pi 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 the center,
we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
a full revolution about the center of revolution once every second. Therefore, the center of
mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
*/
assertAll(
() -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1),
() -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1),
() -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
);
}
private void assertModuleState(SwerveModuleState expected, SwerveModuleState actual,
SwerveModuleState tolerance) {
assertAll(
() -> assertEquals(expected.speedMetersPerSecond, actual.speedMetersPerSecond,
tolerance.speedMetersPerSecond),
() -> assertEquals(expected.angle.getDegrees(), actual.angle.getDegrees(),
tolerance.angle.getDegrees())
);
}
/**
* Test the rotation of the robot about a non-central point with
* both linear and angular velocities.
*/
@Test
void testOffCenterCORRotationAndTranslationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0.0, 3.0, 1.5);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds, 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))
};
var stateTolerance = new SwerveModuleState(0.1, Rotation2d.fromDegrees(0.1));
for (int i = 0; i < expectedStates.length; i++) {
assertModuleState(expectedStates[i], moduleStates[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));
var chassisSpeeds = m_kinematics.toChassisSpeeds(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
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.vxMetersPerSecond, 0.1),
() -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1),
() -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1)
);
}
@Test
void testNormalize() {
SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
SwerveModuleState[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.normalizeWheelSpeeds(arr, 5.5);
double factor = 5.5 / 7.0;
assertAll(
() -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon)
);
}
}

96
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java Normal file
View File

@@ -0,0 +1,96 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
class SwerveDriveOdometryTest {
private final Translation2d m_fl = new Translation2d(12, 12);
private final Translation2d m_fr = new Translation2d(12, -12);
private final Translation2d m_bl = new Translation2d(-12, 12);
private final Translation2d m_br = new Translation2d(-12, -12);
private final SwerveDriveKinematics m_kinematics =
new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
new Rotation2d());
@Test
void testTwoIterations() {
// 5 units/sec in the x axis (forward)
final SwerveModuleState[] wheelSpeeds = {
new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
new SwerveModuleState(5, Rotation2d.fromDegrees(0))
};
m_odometry.updateWithTime(0.0, new Rotation2d(),
new SwerveModuleState(), new SwerveModuleState(),
new SwerveModuleState(), new SwerveModuleState());
var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
assertAll(
() -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
() -> assertEquals(0, pose.getY(), 0.01),
() -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
);
}
@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
final SwerveModuleState[] wheelSpeeds = {
new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
};
final var zero = new SwerveModuleState();
m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero);
final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
assertAll(
() -> assertEquals(12.0, pose.getX(), 0.01),
() -> assertEquals(12.0, pose.getY(), 0.01),
() -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
);
}
@Test
void testGyroAngleReset() {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
var state = new SwerveModuleState();
m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
assertAll(
() -> assertEquals(1.0, pose.getX(), 0.1),
() -> assertEquals(0.00, pose.getY(), 0.1),
() -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
);
}
}

162
wpimath/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java Normal file
View File

@@ -0,0 +1,162 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.spline;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
class CubicHermiteSplineTest {
private static final double kMaxDx = 0.127;
private static final double kMaxDy = 0.00127;
private static final double kMaxDtheta = 0.0872;
@SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
// Start the timer.
//var start = System.nanoTime();
// Generate and parameterize the spline.
var controlVectors =
SplineHelper.getCubicControlVectorsFromWaypoints(a,
waypoints.toArray(new Translation2d[0]), b);
var splines
= SplineHelper.getCubicSplinesFromControlVectors(
controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]);
var poses = new ArrayList<PoseWithCurvature>();
poses.add(splines[0].getPoint(0.0));
for (var spline : splines) {
poses.addAll(SplineParameterizer.parameterize(spline));
}
// End the timer.
//var end = System.nanoTime();
// Calculate the duration (used when benchmarking)
//var durationMicroseconds = (end - start) / 1000.0;
for (int i = 0; i < poses.size() - 1; i++) {
var p0 = poses.get(i);
var p1 = poses.get(i + 1);
// Make sure the twist is under the tolerance defined by the Spline class.
var twist = p0.poseMeters.log(p1.poseMeters);
assertAll(
() -> assertTrue(Math.abs(twist.dx) < kMaxDx),
() -> assertTrue(Math.abs(twist.dy) < kMaxDy),
() -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta)
);
}
// Check first point
assertAll(
() -> assertEquals(a.getX(),
poses.get(0).poseMeters.getX(), 1E-9),
() -> assertEquals(a.getY(),
poses.get(0).poseMeters.getY(), 1E-9),
() -> assertEquals(a.getRotation().getRadians(),
poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)
);
// Check interior waypoints
boolean interiorsGood = true;
for (var waypoint : waypoints) {
boolean found = false;
for (var state : poses) {
if (waypoint.getDistance(state.poseMeters.getTranslation()) == 0) {
found = true;
}
}
interiorsGood &= found;
}
assertTrue(interiorsGood);
// Check last point
assertAll(
() -> assertEquals(b.getX(),
poses.get(poses.size() - 1).poseMeters.getX(), 1E-9),
() -> assertEquals(b.getY(),
poses.get(poses.size() - 1).poseMeters.getY(), 1E-9),
() -> assertEquals(b.getRotation().getRadians(),
poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)
);
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testStraightLine() {
run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d()));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testSCurve() {
var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0));
ArrayList<Translation2d> waypoints = new ArrayList<>();
waypoints.add(new Translation2d(1, 1));
waypoints.add(new Translation2d(2, -1));
var end = new Pose2d(3, 0, Rotation2d.fromDegrees(90.0));
run(start, waypoints, end);
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testOneInterior() {
var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
ArrayList<Translation2d> waypoints = new ArrayList<>();
waypoints.add(new Translation2d(2.0, 0.0));
var end = new Pose2d(4, 0, Rotation2d.fromDegrees(0.0));
run(start, waypoints, end);
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testWindyPath() {
final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
final ArrayList<Translation2d> waypoints = new ArrayList<>();
waypoints.add(new Translation2d(0.5, 0.5));
waypoints.add(new Translation2d(0.5, 0.5));
waypoints.add(new Translation2d(1.0, 0.0));
waypoints.add(new Translation2d(1.5, 0.5));
waypoints.add(new Translation2d(2.0, 0.0));
waypoints.add(new Translation2d(2.5, 0.5));
final var end = new Pose2d(3.0, 0.0, Rotation2d.fromDegrees(0.0));
run(start, waypoints, end);
}
@Test
void testMalformed() {
assertThrows(MalformedSplineException.class, () -> run(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
assertThrows(MalformedSplineException.class, () -> run(
new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
Arrays.asList(new Translation2d(10, 10.5)),
new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
}
}

108
wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java Normal file
View File

@@ -0,0 +1,108 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.spline;
import java.util.List;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
class QuinticHermiteSplineTest {
private static final double kMaxDx = 0.127;
private static final double kMaxDy = 0.00127;
private static final double kMaxDtheta = 0.0872;
@SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" })
private void run(Pose2d a, Pose2d b) {
// Start the timer.
//var start = System.nanoTime();
// Generate and parameterize the spline.
var controlVectors = SplineHelper.getQuinticControlVectorsFromWaypoints(List.of(a, b));
var spline = SplineHelper.getQuinticSplinesFromControlVectors(
controlVectors.toArray(new Spline.ControlVector[0]))[0];
var poses = SplineParameterizer.parameterize(spline);
// End the timer.
//var end = System.nanoTime();
// Calculate the duration (used when benchmarking)
//var durationMicroseconds = (end - start) / 1000.0;
for (int i = 0; i < poses.size() - 1; i++) {
var p0 = poses.get(i);
var p1 = poses.get(i + 1);
// Make sure the twist is under the tolerance defined by the Spline class.
var twist = p0.poseMeters.log(p1.poseMeters);
assertAll(
() -> assertTrue(Math.abs(twist.dx) < kMaxDx),
() -> assertTrue(Math.abs(twist.dy) < kMaxDy),
() -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
}
// Check first point
assertAll(
() -> assertEquals(
a.getX(), poses.get(0).poseMeters.getX(), 1E-9),
() -> assertEquals(
a.getY(), poses.get(0).poseMeters.getY(), 1E-9),
() -> assertEquals(
a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(),
1E-9));
// Check last point
assertAll(
() -> assertEquals(b.getX(), poses.get(poses.size() - 1)
.poseMeters.getX(), 1E-9),
() -> assertEquals(b.getY(), poses.get(poses.size() - 1)
.poseMeters.getY(), 1E-9),
() -> assertEquals(b.getRotation().getRadians(),
poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testStraightLine() {
run(new Pose2d(), new Pose2d(3, 0, new Rotation2d()));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testSimpleSCurve() {
run(new Pose2d(), new Pose2d(1, 1, new Rotation2d()));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testSquiggly() {
run(
new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
}
@Test
void testMalformed() {
assertThrows(MalformedSplineException.class,
() -> run(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
assertThrows(MalformedSplineException.class,
() -> run(
new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
}
}

43
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java Normal file
View File

@@ -0,0 +1,43 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
import java.util.Collections;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.trajectory.constraint.CentripetalAccelerationConstraint;
import edu.wpi.first.wpilibj.util.Units;
import static org.junit.jupiter.api.Assertions.assertTrue;
class CentripetalAccelerationConstraintTest {
@SuppressWarnings("LocalVariableName")
@Test
void testCentripetalAccelerationConstraint() {
double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared
var constraint = new CentripetalAccelerationConstraint(maxCentripetalAcceleration);
Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var t = 0.0;
var dt = 0.02;
while (t < duration) {
var point = trajectory.sample(t);
var centripetalAcceleration
= Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter;
t += dt;
assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05);
}
}
}

53
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java Normal file
View File

@@ -0,0 +1,53 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
import java.util.Collections;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
import edu.wpi.first.wpilibj.util.Units;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertTrue;
class DifferentialDriveKinematicsConstraintTest {
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
@Test
void testDifferentialDriveKinematicsConstraint() {
double maxVelocity = Units.feetToMeters(12.0); // 12 feet per second
var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(27));
var constraint = new DifferentialDriveKinematicsConstraint(kinematics, maxVelocity);
Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var t = 0.0;
var dt = 0.02;
while (t < duration) {
var point = trajectory.sample(t);
var chassisSpeeds = new ChassisSpeeds(
point.velocityMetersPerSecond, 0,
point.velocityMetersPerSecond * point.curvatureRadPerMeter
);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
t += dt;
assertAll(
() -> assertTrue(wheelSpeeds.leftMetersPerSecond <= maxVelocity + 0.05),
() -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05)
);
}
}
}

103
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java Normal file
View File

@@ -0,0 +1,103 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
import java.util.Collections;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertTrue;
class DifferentialDriveVoltageConstraintTest {
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
@Test
void testDifferentialDriveVoltageConstraint() {
// Pick an unreasonably large kA to ensure the constraint has to do some work
var feedforward = new SimpleMotorFeedforward(1, 1, 3);
var kinematics = new DifferentialDriveKinematics(0.5);
double maxVoltage = 10;
var constraint = new DifferentialDriveVoltageConstraint(feedforward,
kinematics,
maxVoltage);
Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var t = 0.0;
var dt = 0.02;
while (t < duration) {
var point = trajectory.sample(t);
var chassisSpeeds = new ChassisSpeeds(
point.velocityMetersPerSecond, 0,
point.velocityMetersPerSecond * point.curvatureRadPerMeter
);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
t += dt;
// 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 a reasonable check anyway
assertAll(
() -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
point.accelerationMetersPerSecondSq)
<= maxVoltage + 0.05),
() -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
point.accelerationMetersPerSecondSq)
>= -maxVoltage - 0.05),
() -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
point.accelerationMetersPerSecondSq)
<= maxVoltage + 0.05),
() -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
point.accelerationMetersPerSecondSq)
>= -maxVoltage - 0.05)
);
}
}
@Test
void testEndpointHighCurvature() {
var feedforward = new SimpleMotorFeedforward(1, 1, 3);
// Large trackwidth - need to test with radius of curvature less than half of trackwidth
var kinematics = new DifferentialDriveKinematics(3);
double maxVoltage = 10;
var constraint = new DifferentialDriveVoltageConstraint(feedforward,
kinematics,
maxVoltage);
var config = new TrajectoryConfig(12, 12).addConstraint(constraint);
// Radius of curvature should be ~1 meter.
assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
new ArrayList<Translation2d>(),
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
config));
assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
new ArrayList<Translation2d>(),
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
config.setReversed(true)));
}
}

77
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java Normal file
View File

@@ -0,0 +1,77 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
import java.util.List;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.constraint.EllipticalRegionConstraint;
import edu.wpi.first.wpilibj.trajectory.constraint.MaxVelocityConstraint;
import edu.wpi.first.wpilibj.util.Units;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
public class EllipticalRegionConstraintTest {
@Test
void testConstraint() {
// Create constraints
double maxVelocity = Units.feetToMeters(3.0);
var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
var regionConstraint = new EllipticalRegionConstraint(
new Translation2d(Units.feetToMeters(5.0), Units.feetToMeters(5.0)),
Units.feetToMeters(10.0), Units.feetToMeters(5.0), Rotation2d.fromDegrees(180.0),
maxVelocityConstraint
);
// Get trajectory
var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
// Iterate through trajectory and check constraints
boolean exceededConstraintOutsideRegion = false;
for (var point : trajectory.getStates()) {
var translation = point.poseMeters.getTranslation();
if (translation.getX() < Units.feetToMeters(10)
&& translation.getY() < Units.feetToMeters(5)) {
assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
} else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
exceededConstraintOutsideRegion = true;
}
}
assertTrue(exceededConstraintOutsideRegion);
}
@Test
void testIsPoseWithinRegion() {
double maxVelocity = Units.feetToMeters(3.0);
var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
var regionConstraintNoRotation = new EllipticalRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
Units.feetToMeters(2.0), Units.feetToMeters(4.0), new Rotation2d(),
maxVelocityConstraint);
assertFalse(regionConstraintNoRotation.isPoseInRegion(new Pose2d(
Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d()
)));
var regionConstraintWithRotation = new EllipticalRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
Units.feetToMeters(2.0), Units.feetToMeters(4.0), Rotation2d.fromDegrees(90.0),
maxVelocityConstraint);
assertTrue(regionConstraintWithRotation.isPoseInRegion(new Pose2d(
Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d()
)));
}
}

65
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java Normal file
View File

@@ -0,0 +1,65 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
import java.util.List;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.constraint.MaxVelocityConstraint;
import edu.wpi.first.wpilibj.trajectory.constraint.RectangularRegionConstraint;
import edu.wpi.first.wpilibj.util.Units;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
public class RectangularRegionConstraintTest {
@Test
void testConstraint() {
// Create constraints
double maxVelocity = Units.feetToMeters(3.0);
var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
var regionConstraint = new RectangularRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
maxVelocityConstraint
);
// Get trajectory
var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
// Iterate through trajectory and check constraints
boolean exceededConstraintOutsideRegion = false;
for (var point : trajectory.getStates()) {
if (regionConstraint.isPoseInRegion(point.poseMeters)) {
assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
} else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
exceededConstraintOutsideRegion = true;
}
}
assertTrue(exceededConstraintOutsideRegion);
}
@Test
void testIsPoseWithinRegion() {
double maxVelocity = Units.feetToMeters(3.0);
var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
var regionConstraint = new RectangularRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
maxVelocityConstraint
);
assertFalse(regionConstraint.isPoseInRegion(new Pose2d()));
assertTrue(regionConstraint.isPoseInRegion(new Pose2d(Units.feetToMeters(3.0),
Units.feetToMeters(14.5), new Rotation2d())));
}
}

89
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java Normal file
View File

@@ -0,0 +1,89 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
import static edu.wpi.first.wpilibj.util.Units.feetToMeters;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
class TrajectoryGeneratorTest {
static Trajectory getTrajectory(List<? extends TrajectoryConstraint> constraints) {
final double maxVelocity = feetToMeters(12.0);
final double maxAccel = feetToMeters(12);
// 2018 cross scale auto waypoints.
var sideStart = new Pose2d(feetToMeters(1.54), feetToMeters(23.23),
Rotation2d.fromDegrees(-180));
var crossScale = new Pose2d(feetToMeters(23.7), feetToMeters(6.8),
Rotation2d.fromDegrees(-160));
var waypoints = new ArrayList<Pose2d>();
waypoints.add(sideStart);
waypoints.add(sideStart.plus(
new Transform2d(new Translation2d(feetToMeters(-13), feetToMeters(0)),
new Rotation2d())));
waypoints.add(sideStart.plus(
new Transform2d(new Translation2d(feetToMeters(-19.5), feetToMeters(5)),
Rotation2d.fromDegrees(-90))));
waypoints.add(crossScale);
TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAccel)
.setReversed(true)
.addConstraints(constraints);
return TrajectoryGenerator.generateTrajectory(waypoints, config);
}
@Test
@SuppressWarnings("LocalVariableName")
void testGenerationAndConstraints() {
Trajectory trajectory = getTrajectory(new ArrayList<>());
double duration = trajectory.getTotalTimeSeconds();
double t = 0.0;
double dt = 0.02;
while (t < duration) {
var point = trajectory.sample(t);
t += dt;
assertAll(
() -> assertTrue(Math.abs(point.velocityMetersPerSecond) < feetToMeters(12.0) + 0.05),
() -> assertTrue(Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0)
+ 0.05)
);
}
}
@Test
void testMalformedTrajectory() {
var traj =
TrajectoryGenerator.generateTrajectory(
Arrays.asList(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(1, 0, Rotation2d.fromDegrees(180))
),
new TrajectoryConfig(feetToMeters(12), feetToMeters(12))
);
assertEquals(traj.getStates().size(), 1);
assertEquals(traj.getTotalTimeSeconds(), 0);
}
}

33
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java Normal file
View File

@@ -0,0 +1,33 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
import java.util.List;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
public class TrajectoryJsonTest {
@Test
void deserializeMatches() {
var config = List.of(new DifferentialDriveKinematicsConstraint(
new DifferentialDriveKinematics(20), 3));
var trajectory = TrajectoryGeneratorTest.getTrajectory(config);
var deserialized =
assertDoesNotThrow(() ->
TrajectoryUtil.deserializeTrajectory(TrajectoryUtil.serializeTrajectory(trajectory)));
assertEquals(trajectory.getStates(), deserialized.getStates());
}
}

68
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java Normal file
View File

@@ -0,0 +1,68 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
import java.util.List;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import static org.junit.jupiter.api.Assertions.assertEquals;
class TrajectoryTransformTest {
@Test
void testTransformBy() {
var config = new TrajectoryConfig(3, 3);
var trajectory = TrajectoryGenerator.generateTrajectory(
new Pose2d(), List.of(), new Pose2d(1, 1, Rotation2d.fromDegrees(90)),
config
);
var transformedTrajectory = trajectory.transformBy(
new Transform2d(new Translation2d(1, 2), Rotation2d.fromDegrees(30)));
// Test initial pose.
assertEquals(new Pose2d(1, 2, Rotation2d.fromDegrees(30)),
transformedTrajectory.sample(0).poseMeters);
testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
}
@Test
void testRelativeTo() {
var config = new TrajectoryConfig(3, 3);
var trajectory = TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 2, Rotation2d.fromDegrees(30.0)),
List.of(), new Pose2d(5, 7, Rotation2d.fromDegrees(90)),
config
);
var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));
// Test initial pose.
assertEquals(new Pose2d(), transformedTrajectory.sample(0).poseMeters);
testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
}
void testSameShapedTrajectory(List<Trajectory.State> statesA, List<Trajectory.State> statesB) {
for (int i = 0; i < statesA.size() - 1; i++) {
var a1 = statesA.get(i).poseMeters;
var a2 = statesA.get(i + 1).poseMeters;
var b1 = statesB.get(i).poseMeters;
var b2 = statesB.get(i + 1).poseMeters;
assertEquals(a2.relativeTo(a1), b2.relativeTo(b1));
}
}
}

257
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java Normal file
View File

@@ -0,0 +1,257 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.trajectory;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
@SuppressWarnings({"PMD.TooManyMethods", "PMD.AvoidInstantiatingObjectsInLoops"})
class TrapezoidProfileTest {
private static final double kDt = 0.01;
/**
* Asserts "val1" is less than or equal to "val2".
*
* @param val1 First operand in comparison.
* @param val2 Second operand in comparison.
*/
private static void assertLessThanOrEquals(double val1, double val2) {
assertTrue(val1 <= val2, val1 + " is greater than " + val2);
}
/**
* Asserts "val1" is within "eps" of "val2".
*
* @param val1 First operand in comparison.
* @param val2 Second operand in comparison.
* @param eps Tolerance for whether values are near to each other.
*/
private static void assertNear(double val1, double val2, double eps) {
assertTrue(Math.abs(val1 - val2) <= eps, "Difference between " + val1 + " and " + val2
+ " is greater than " + eps);
}
/**
* Asserts "val1" is less than or within "eps" of "val2".
*
* @param val1 First operand in comparison.
* @param val2 Second operand in comparison.
* @param eps Tolerance for whether values are near to each other.
*/
private static void assertLessThanOrNear(double val1, double val2, double eps) {
if (val1 <= val2) {
assertLessThanOrEquals(val1, val2);
} else {
assertNear(val1, val2, eps);
}
}
@Test
void reachesGoal() {
TrapezoidProfile.Constraints constraints =
new TrapezoidProfile.Constraints(1.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
for (int i = 0; i < 450; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertEquals(state, goal);
}
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
@Test
void posContinousUnderVelChange() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
double lastPos = state.position;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
constraints.maxVelocity = 0.75;
}
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
double estimatedVel = (state.position - lastPos) / kDt;
if (i >= 400) {
// Since estimatedVel can have floating point rounding errors, we check
// whether value is less than or within an error delta of the new
// constraint.
assertLessThanOrNear(estimatedVel, constraints.maxVelocity, 1e-4);
assertLessThanOrEquals(state.velocity, constraints.maxVelocity);
}
lastPos = state.position;
}
assertEquals(state, goal);
}
// There is some somewhat tricky code for dealing with going backwards
@Test
void backwards() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
for (int i = 0; i < 400; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertEquals(state, goal);
}
@Test
void switchGoalInMiddle() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
for (int i = 0; i < 200; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertNotEquals(state, goal);
goal = new TrapezoidProfile.State(0.0, 0.0);
for (int i = 0; i < 550; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertEquals(state, goal);
}
// Checks to make sure that it hits top speed
@Test
void topSpeed() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
for (int i = 0; i < 200; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertNear(constraints.maxVelocity, state.velocity, 10e-5);
for (int i = 0; i < 2000; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertEquals(state, goal);
}
@Test
void timingToCurrent() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
for (int i = 0; i < 400; i++) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
}
}
@Test
void timingToGoal() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
if (!reachedGoal && state.equals(goal)) {
// Expected value using for loop index is just an approximation since
// the time left in the profile doesn't increase linearly at the
// endpoints
assertNear(predictedTimeLeft, i / 100.0, 0.25);
reachedGoal = true;
}
}
}
@Test
void timingBeforeGoal() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
double predictedTimeLeft = profile.timeLeftUntil(1);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
if (!reachedGoal && (Math.abs(state.velocity - 1) < 10e-5)) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;
}
}
}
@Test
void timingToNegativeGoal() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
if (!reachedGoal && state.equals(goal)) {
// Expected value using for loop index is just an approximation since
// the time left in the profile doesn't increase linearly at the
// endpoints
assertNear(predictedTimeLeft, i / 100.0, 0.25);
reachedGoal = true;
}
}
}
@Test
void timingBeforeNegativeGoal() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
double predictedTimeLeft = profile.timeLeftUntil(-1);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
if (!reachedGoal && (Math.abs(state.velocity + 1) < 10e-5)) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;
}
}
}
}

60
wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java Normal file
View File

@@ -0,0 +1,60 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.util;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.UtilityClassTest;
import static org.junit.jupiter.api.Assertions.assertEquals;
class UnitsTest extends UtilityClassTest<Units> {
UnitsTest() {
super(Units.class);
}
@Test
void metersToFeetTest() {
assertEquals(3.28, Units.metersToFeet(1), 1e-2);
}
@Test
void feetToMetersTest() {
assertEquals(0.30, Units.feetToMeters(1), 1e-2);
}
@Test
void metersToInchesTest() {
assertEquals(39.37, Units.metersToInches(1), 1e-2);
}
@Test
void inchesToMetersTest() {
assertEquals(0.0254, Units.inchesToMeters(1), 1e-3);
}
@Test
void degreesToRadiansTest() {
assertEquals(0.017, Units.degreesToRadians(1), 1e-3);
}
@Test
void radiansToDegreesTest() {
assertEquals(114.59, Units.radiansToDegrees(2), 1e-2);
}
@Test
void rotationsPerMinuteToRadiansPerSecondTest() {
assertEquals(6.28, Units.rotationsPerMinuteToRadiansPerSecond(60), 1e-2);
}
@Test
void radiansPerSecondToRotationsPerMinute() {
assertEquals(76.39, Units.radiansPerSecondToRotationsPerMinute(8), 1e-2);
}
}

19
wpimath/src/test/java/edu/wpi/first/wpiutil/math/DrakeJNITest.java Normal file
View File

@@ -0,0 +1,19 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpiutil.math;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
public class DrakeJNITest {
@Test
public void testLink() {
assertDoesNotThrow(DrakeJNI::forceLoad);
}
}

69
wpimath/src/test/java/edu/wpi/first/wpiutil/math/DrakeTest.java Normal file
View File

@@ -0,0 +1,69 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpiutil.math;
import org.ejml.simple.SimpleMatrix;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public class DrakeTest {
public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
for (int i = 0; i < A.numRows(); i++) {
for (int j = 0; j < A.numCols(); j++) {
assertEquals(A.get(i, j), B.get(i, j), 1e-4);
}
}
}
private boolean solveDAREandVerify(SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q,
SimpleMatrix R) {
var X = Drake.discreteAlgebraicRiccatiEquation(A, B, Q, R);
// expect that x is the same as it's transpose
assertEquals(X.numRows(), X.numCols());
assertMatrixEqual(X, X.transpose());
// Verify that this is a solution to the DARE.
SimpleMatrix Y = A.transpose().mult(X).mult(A)
.minus(X)
.minus(A.transpose().mult(X).mult(B)
.mult(((B.transpose().mult(X).mult(B)).plus(R))
.invert()).mult(B.transpose()).mult(X).mult(A))
.plus(Q);
assertMatrixEqual(Y, new SimpleMatrix(Y.numRows(), Y.numCols()));
return true;
}
@Test
public void testDiscreteAlgebraicRicattiEquation() {
int n1 = 4;
int m1 = 1;
// we know from Scipy that this should be [[0.05048525 0.10097051 0.20194102 0.40388203]]
SimpleMatrix A1 = new SimpleMatrix(n1, n1, true, new double[]{0.5, 1, 0, 0, 0, 0, 1,
0, 0, 0, 0, 1, 0, 0, 0, 0}).transpose();
SimpleMatrix B1 = new SimpleMatrix(n1, m1, true, new double[]{0, 0, 0, 1});
SimpleMatrix Q1 = new SimpleMatrix(n1, n1, true, new double[]{1, 0,
0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0});
SimpleMatrix R1 = new SimpleMatrix(m1, m1, true, new double[]{0.25});
assertTrue(solveDAREandVerify(A1, B1, Q1, R1));
SimpleMatrix A2 = new SimpleMatrix(2, 2, true, new double[]{1, 1, 0, 1});
SimpleMatrix B2 = new SimpleMatrix(2, 1, true, new double[]{0, 1});
SimpleMatrix Q2 = new SimpleMatrix(2, 2, true, new double[]{1, 0, 0, 0});
SimpleMatrix R2 = new SimpleMatrix(1, 1, true, new double[]{0.3});
assertTrue(solveDAREandVerify(A2, B2, Q2, R2));
}
}

22
wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java Normal file
View File

@@ -0,0 +1,22 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpiutil.math;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
class MathUtilTest {
@Test
void testAngleNormalize() {
assertEquals(MathUtil.normalizeAngle(5 * Math.PI), Math.PI);
assertEquals(MathUtil.normalizeAngle(-5 * Math.PI), Math.PI);
assertEquals(MathUtil.normalizeAngle(Math.PI / 2), Math.PI / 2);
assertEquals(MathUtil.normalizeAngle(-Math.PI / 2), -Math.PI / 2);
}
}

210
wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java Normal file
View File

@@ -0,0 +1,210 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpiutil.math;
import org.ejml.data.SingularMatrixException;
import org.ejml.dense.row.MatrixFeatures_DDRM;
import org.ejml.simple.SimpleMatrix;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import edu.wpi.first.wpiutil.math.numbers.N3;
import edu.wpi.first.wpiutil.math.numbers.N4;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
public class MatrixTest {
@Test
void testMatrixMultiplication() {
var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(2.0, 1.0,
0.0, 1.0);
var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(3.0, 0.0,
0.0, 2.5);
Matrix<N2, N2> result = mat1.times(mat2);
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(6.0, 2.5,
0.0, 2.5).getStorage().getDDRM(),
result.getStorage().getDDRM()
));
var mat3 = MatrixUtils.mat(Nat.N2(), Nat.N3())
.fill(1.0, 3.0, 0.5,
2.0, 4.3, 1.2);
var mat4 = MatrixUtils.mat(Nat.N3(), Nat.N4())
.fill(3.0, 1.5, 2.0, 4.5,
2.3, 1.0, 1.6, 3.1,
5.2, 2.1, 2.0, 1.0);
Matrix<N2, N4> result2 = mat3.times(mat4);
assertTrue(MatrixFeatures_DDRM.isIdentical(
MatrixUtils.mat(Nat.N2(), Nat.N4())
.fill(12.5, 5.55, 7.8, 14.3,
22.13, 9.82, 13.28, 23.53).getStorage().getDDRM(),
result2.getStorage().getDDRM(),
1E-9
));
}
@Test
void testMatrixVectorMultiplication() {
var mat = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(1.0, 1.0,
0.0, 1.0);
var vec = MatrixUtils.vec(Nat.N2())
.fill(3.0,
2.0);
Matrix<N2, N1> result = mat.times(vec);
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.vec(Nat.N2())
.fill(5.0,
2.0).getStorage().getDDRM(),
result.getStorage().getDDRM()
));
}
@Test
void testTranspose() {
Matrix<N3, N1> vec = MatrixUtils.vec(Nat.N3())
.fill(1.0,
2.0,
3.0);
Matrix<N1, N3> transpose = vec.transpose();
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0).getStorage()
.getDDRM(),
transpose.getStorage().getDDRM()
));
}
@Test
void testInverse() {
var mat = MatrixUtils.mat(Nat.N3(), Nat.N3())
.fill(1.0, 3.0, 2.0,
5.0, 2.0, 1.5,
0.0, 1.3, 2.5);
var inv = mat.inv();
assertTrue(MatrixFeatures_DDRM.isIdentical(
MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(),
mat.times(inv).getStorage().getDDRM(),
1E-9
));
assertTrue(MatrixFeatures_DDRM.isIdentical(
MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(),
inv.times(mat).getStorage().getDDRM(),
1E-9
));
}
@Test
void testUninvertableMatrix() {
var singularMatrix = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(2.0, 1.0,
2.0, 1.0);
assertThrows(SingularMatrixException.class, singularMatrix::inv);
}
@Test
void testMatrixScalarArithmetic() {
var mat = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(1.0, 2.0,
3.0, 4.0);
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(3.0, 4.0,
5.0, 6.0).getStorage().getDDRM(),
mat.plus(2.0).getStorage().getDDRM()
));
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(0.0, 1.0,
2.0, 3.0).getStorage().getDDRM(),
mat.minus(1.0).getStorage().getDDRM()
));
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(2.0, 4.0,
6.0, 8.0).getStorage().getDDRM(),
mat.times(2.0).getStorage().getDDRM()
));
assertTrue(MatrixFeatures_DDRM.isIdentical(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(0.5, 1.0,
1.5, 2.0).getStorage().getDDRM(),
mat.div(2.0).getStorage().getDDRM(),
1E-3
));
}
@Test
void testMatrixMatrixArithmetic() {
var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(1.0, 2.0,
3.0, 4.0);
var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(5.0, 6.0,
7.0, 8.0);
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(-4.0, -4.0,
-4.0, -4.0).getStorage().getDDRM(),
mat1.minus(mat2).getStorage().getDDRM()
));
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(6.0, 8.0,
10.0, 12.0).getStorage().getDDRM(),
mat1.plus(mat2).getStorage().getDDRM()
));
}
@Test
void testMatrixExponential() {
SimpleMatrix matrix = MatrixUtils.eye(Nat.N2()).getStorage();
var result = SimpleMatrixUtils.expm(matrix);
assertTrue(MatrixFeatures_DDRM.isIdentical(
result.getDDRM(),
new SimpleMatrix(2, 2, true, new double[]{Math.E, 0, 0, Math.E}).getDDRM(),
1E-9
));
matrix = new SimpleMatrix(2, 2, true, new double[]{1, 2, 3, 4});
result = SimpleMatrixUtils.expm(matrix.scale(0.01));
assertTrue(MatrixFeatures_DDRM.isIdentical(
result.getDDRM(),
new SimpleMatrix(2, 2, true, new double[]{1.01035625, 0.02050912,
0.03076368, 1.04111993}).getDDRM(),
1E-8
));
}
}