mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
[wpimath] Add ChassisAccelerations and drivetrain accelerations classes and add forward and inverse kinematics for accelerations to the interface (#8185)
ChassisAccelerations and the drivetrain acceleration types are added in both Java and C++. `ChassisAccelerations` is basically just `ChassisSpeeds` but for accelerations! `DifferentialDriveWheelAccelerations`, `MecanumDriveWheelAccelerations`, and `SwerveModuleAccelerations` are the acceleration equivalent of the drivetrain speeds types. In Java, the `Kinematics` interface now has an additional generic parameter `A` which represents the accelerations, and `toChassisAccelerations` and `toWheelAccelerations` methods, which are implemented the same way as `toChassisSpeeds` and `toWheelSpeeds`. Protobuf and struct classes were also added for all four classes in Java and C++. --------- Signed-off-by: Zach Harel <zach@zharel.me> Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Zach Harel
@@ -0,0 +1,160 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.math.kinematics;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.wpilib.units.Units.MetersPerSecondPerSecond;
|
||||
import static org.wpilib.units.Units.RadiansPerSecondPerSecond;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.wpilib.math.geometry.Rotation2d;
|
||||
|
||||
class ChassisAccelerationsTest {
|
||||
private static final double kEpsilon = 1E-9;
|
||||
|
||||
@Test
|
||||
void testDefaultConstructor() {
|
||||
var accelerations = new ChassisAccelerations();
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, accelerations.ax, kEpsilon),
|
||||
() -> assertEquals(0.0, accelerations.ay, kEpsilon),
|
||||
() -> assertEquals(0.0, accelerations.alpha, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testParameterizedConstructor() {
|
||||
var accelerations = new ChassisAccelerations(1.0, 2.0, 3.0);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, accelerations.ax, kEpsilon),
|
||||
() -> assertEquals(2.0, accelerations.ay, kEpsilon),
|
||||
() -> assertEquals(3.0, accelerations.alpha, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMeasureConstructor() {
|
||||
var ax = MetersPerSecondPerSecond.of(2.5);
|
||||
var ay = MetersPerSecondPerSecond.of(1.5);
|
||||
var alpha = RadiansPerSecondPerSecond.of(0.75);
|
||||
var accelerations = new ChassisAccelerations(ax, ay, alpha);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(2.5, accelerations.ax, kEpsilon),
|
||||
() -> assertEquals(1.5, accelerations.ay, kEpsilon),
|
||||
() -> assertEquals(0.75, accelerations.alpha, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testToRobotRelative() {
|
||||
final var chassisAccelerations =
|
||||
new ChassisAccelerations(1.0, 0.0, 0.5).toRobotRelative(Rotation2d.kCW_Pi_2);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, chassisAccelerations.ax, kEpsilon),
|
||||
() -> assertEquals(1.0, chassisAccelerations.ay, kEpsilon),
|
||||
() -> assertEquals(0.5, chassisAccelerations.alpha, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testToFieldRelative() {
|
||||
final var chassisAccelerations =
|
||||
new ChassisAccelerations(1.0, 0.0, 0.5).toFieldRelative(Rotation2d.fromDegrees(45.0));
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisAccelerations.ax, kEpsilon),
|
||||
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisAccelerations.ay, kEpsilon),
|
||||
() -> assertEquals(0.5, chassisAccelerations.alpha, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testPlus() {
|
||||
final var left = new ChassisAccelerations(1.0, 0.5, 0.75);
|
||||
final var right = new ChassisAccelerations(2.0, 1.5, 0.25);
|
||||
|
||||
final var chassisAccelerations = left.plus(right);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, chassisAccelerations.ax),
|
||||
() -> assertEquals(2.0, chassisAccelerations.ay),
|
||||
() -> assertEquals(1.0, chassisAccelerations.alpha));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMinus() {
|
||||
final var left = new ChassisAccelerations(1.0, 0.5, 0.75);
|
||||
final var right = new ChassisAccelerations(2.0, 0.5, 0.25);
|
||||
|
||||
final var chassisAccelerations = left.minus(right);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(-1.0, chassisAccelerations.ax),
|
||||
() -> assertEquals(0.0, chassisAccelerations.ay),
|
||||
() -> assertEquals(0.5, chassisAccelerations.alpha));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testUnaryMinus() {
|
||||
final var chassisAccelerations = new ChassisAccelerations(1.0, 0.5, 0.75).unaryMinus();
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(-1.0, chassisAccelerations.ax),
|
||||
() -> assertEquals(-0.5, chassisAccelerations.ay),
|
||||
() -> assertEquals(-0.75, chassisAccelerations.alpha));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMultiplication() {
|
||||
final var chassisAccelerations = new ChassisAccelerations(1.0, 0.5, 0.75).times(2.0);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(2.0, chassisAccelerations.ax),
|
||||
() -> assertEquals(1.0, chassisAccelerations.ay),
|
||||
() -> assertEquals(1.5, chassisAccelerations.alpha));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDivision() {
|
||||
final var chassisAccelerations = new ChassisAccelerations(2.0, 1.0, 1.5).div(2.0);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, chassisAccelerations.ax),
|
||||
() -> assertEquals(0.5, chassisAccelerations.ay),
|
||||
() -> assertEquals(0.75, chassisAccelerations.alpha));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInterpolation() {
|
||||
var start = new ChassisAccelerations(0.0, 0.0, 0.0);
|
||||
var end = new ChassisAccelerations(10.0, 20.0, 30.0);
|
||||
var result = start.interpolate(end, 0.5);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, result.ax, kEpsilon),
|
||||
() -> assertEquals(10.0, result.ay, kEpsilon),
|
||||
() -> assertEquals(15.0, result.alpha, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInterpolationAtBounds() {
|
||||
var start = new ChassisAccelerations(1.0, 2.0, 3.0);
|
||||
var end = new ChassisAccelerations(4.0, 5.0, 6.0);
|
||||
|
||||
// Test t = 0 (should return start)
|
||||
var resultStart = start.interpolate(end, 0.0);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, resultStart.ax, kEpsilon),
|
||||
() -> assertEquals(2.0, resultStart.ay, kEpsilon),
|
||||
() -> assertEquals(3.0, resultStart.alpha, kEpsilon));
|
||||
|
||||
// Test t = 1 (should return end)
|
||||
var resultEnd = start.interpolate(end, 1.0);
|
||||
assertAll(
|
||||
() -> assertEquals(4.0, resultEnd.ax, kEpsilon),
|
||||
() -> assertEquals(5.0, resultEnd.ay, kEpsilon),
|
||||
() -> assertEquals(6.0, resultEnd.alpha, kEpsilon));
|
||||
}
|
||||
}
|
||||
@@ -127,4 +127,36 @@ class ChassisSpeedsTest {
|
||||
() -> assertEquals(0.25, chassisSpeeds.vy),
|
||||
() -> assertEquals(0.375, chassisSpeeds.omega));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInterpolation() {
|
||||
var start = new ChassisSpeeds(0.0, 0.0, 0.0);
|
||||
var end = new ChassisSpeeds(10.0, 20.0, 30.0);
|
||||
var result = start.interpolate(end, 0.5);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, result.vx, kEpsilon),
|
||||
() -> assertEquals(10.0, result.vy, kEpsilon),
|
||||
() -> assertEquals(15.0, result.omega, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInterpolationAtBounds() {
|
||||
var start = new ChassisSpeeds(1.0, 2.0, 3.0);
|
||||
var end = new ChassisSpeeds(4.0, 5.0, 6.0);
|
||||
|
||||
// Test t = 0 (should return start)
|
||||
var resultStart = start.interpolate(end, 0.0);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, resultStart.vx, kEpsilon),
|
||||
() -> assertEquals(2.0, resultStart.vy, kEpsilon),
|
||||
() -> assertEquals(3.0, resultStart.omega, kEpsilon));
|
||||
|
||||
// Test t = 1 (should return end)
|
||||
var resultEnd = start.interpolate(end, 1.0);
|
||||
assertAll(
|
||||
() -> assertEquals(4.0, resultEnd.vx, kEpsilon),
|
||||
() -> assertEquals(5.0, resultEnd.vy, kEpsilon),
|
||||
() -> assertEquals(6.0, resultEnd.omega, kEpsilon));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -76,4 +76,68 @@ class DifferentialDriveKinematicsTest {
|
||||
() -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
|
||||
() -> assertEquals(-Math.PI, chassisSpeeds.omega, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInverseAccelerationsForZeros() {
|
||||
var chassisAccelerations = new ChassisAccelerations();
|
||||
var wheelAccelerations = m_kinematics.toWheelAccelerations(chassisAccelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, wheelAccelerations.left, kEpsilon),
|
||||
() -> assertEquals(0.0, wheelAccelerations.right, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testForwardAccelerationsForZeros() {
|
||||
var wheelAccelerations = new DifferentialDriveWheelAccelerations();
|
||||
var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, chassisAccelerations.ax, kEpsilon),
|
||||
() -> assertEquals(0.0, chassisAccelerations.ay, kEpsilon),
|
||||
() -> assertEquals(0.0, chassisAccelerations.alpha, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInverseAccelerationsForStraightLine() {
|
||||
var chassisAccelerations = new ChassisAccelerations(3, 0, 0);
|
||||
var wheelAccelerations = m_kinematics.toWheelAccelerations(chassisAccelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, wheelAccelerations.left, kEpsilon),
|
||||
() -> assertEquals(3.0, wheelAccelerations.right, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testForwardAccelerationsForStraightLine() {
|
||||
var wheelAccelerations = new DifferentialDriveWheelAccelerations(3, 3);
|
||||
var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, chassisAccelerations.ax, kEpsilon),
|
||||
() -> assertEquals(0.0, chassisAccelerations.ay, kEpsilon),
|
||||
() -> assertEquals(0.0, chassisAccelerations.alpha, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInverseAccelerationsForRotateInPlace() {
|
||||
var chassisAccelerations = new ChassisAccelerations(0, 0, Math.PI);
|
||||
var wheelAccelerations = m_kinematics.toWheelAccelerations(chassisAccelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(-0.381 * Math.PI, wheelAccelerations.left, kEpsilon),
|
||||
() -> assertEquals(+0.381 * Math.PI, wheelAccelerations.right, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testForwardAccelerationsForRotateInPlace() {
|
||||
var wheelAccelerations =
|
||||
new DifferentialDriveWheelAccelerations(+0.381 * Math.PI, -0.381 * Math.PI);
|
||||
var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, chassisAccelerations.ax, kEpsilon),
|
||||
() -> assertEquals(0.0, chassisAccelerations.ay, kEpsilon),
|
||||
() -> assertEquals(-Math.PI, chassisAccelerations.alpha, kEpsilon));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,137 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.math.kinematics;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.wpilib.units.Units.MetersPerSecondPerSecond;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class DifferentialDriveWheelAccelerationsTest {
|
||||
private static final double kEpsilon = 1E-9;
|
||||
|
||||
@Test
|
||||
void testDefaultConstructor() {
|
||||
var wheelAccelerations = new DifferentialDriveWheelAccelerations();
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, wheelAccelerations.left, kEpsilon),
|
||||
() -> assertEquals(0.0, wheelAccelerations.right, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testParameterizedConstructor() {
|
||||
var wheelAccelerations = new DifferentialDriveWheelAccelerations(1.5, 2.5);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(1.5, wheelAccelerations.left, kEpsilon),
|
||||
() -> assertEquals(2.5, wheelAccelerations.right, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMeasureConstructor() {
|
||||
var left = MetersPerSecondPerSecond.of(3.0);
|
||||
var right = MetersPerSecondPerSecond.of(4.5);
|
||||
var wheelAccelerations = new DifferentialDriveWheelAccelerations(left, right);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, wheelAccelerations.left, kEpsilon),
|
||||
() -> assertEquals(4.5, wheelAccelerations.right, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testPlus() {
|
||||
final var left = new DifferentialDriveWheelAccelerations(1.0, 0.5);
|
||||
final var right = new DifferentialDriveWheelAccelerations(2.0, 1.5);
|
||||
|
||||
final var wheelAccelerations = left.plus(right);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, wheelAccelerations.left),
|
||||
() -> assertEquals(2.0, wheelAccelerations.right));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMinus() {
|
||||
final var left = new DifferentialDriveWheelAccelerations(1.0, 0.5);
|
||||
final var right = new DifferentialDriveWheelAccelerations(2.0, 0.5);
|
||||
|
||||
final var wheelAccelerations = left.minus(right);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(-1.0, wheelAccelerations.left),
|
||||
() -> assertEquals(0.0, wheelAccelerations.right));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testUnaryMinus() {
|
||||
final var wheelAccelerations = new DifferentialDriveWheelAccelerations(1.0, 0.5).unaryMinus();
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(-1.0, wheelAccelerations.left),
|
||||
() -> assertEquals(-0.5, wheelAccelerations.right));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMultiplication() {
|
||||
final var wheelAccelerations = new DifferentialDriveWheelAccelerations(1.0, 0.5).times(2.0);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(2.0, wheelAccelerations.left),
|
||||
() -> assertEquals(1.0, wheelAccelerations.right));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDivision() {
|
||||
final var wheelAccelerations = new DifferentialDriveWheelAccelerations(1.0, 0.5).div(2.0);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.5, wheelAccelerations.left),
|
||||
() -> assertEquals(0.25, wheelAccelerations.right));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInterpolate() {
|
||||
final var start = new DifferentialDriveWheelAccelerations(1.0, 2.0);
|
||||
final var end = new DifferentialDriveWheelAccelerations(5.0, 6.0);
|
||||
|
||||
// Test interpolation at t=0 (should return start)
|
||||
final var atStart = start.interpolate(end, 0.0);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, atStart.left, kEpsilon),
|
||||
() -> assertEquals(2.0, atStart.right, kEpsilon));
|
||||
|
||||
// Test interpolation at t=1 (should return end)
|
||||
final var atEnd = start.interpolate(end, 1.0);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, atEnd.left, kEpsilon),
|
||||
() -> assertEquals(6.0, atEnd.right, kEpsilon));
|
||||
|
||||
// Test interpolation at t=0.5 (should return midpoint)
|
||||
final var atMidpoint = start.interpolate(end, 0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, atMidpoint.left, kEpsilon),
|
||||
() -> assertEquals(4.0, atMidpoint.right, kEpsilon));
|
||||
|
||||
// Test interpolation at t=0.25
|
||||
final var atQuarter = start.interpolate(end, 0.25);
|
||||
assertAll(
|
||||
() -> assertEquals(2.0, atQuarter.left, kEpsilon),
|
||||
() -> assertEquals(3.0, atQuarter.right, kEpsilon));
|
||||
|
||||
// Test clamping: t < 0 should clamp to 0
|
||||
final var belowRange = start.interpolate(end, -0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, belowRange.left, kEpsilon),
|
||||
() -> assertEquals(2.0, belowRange.right, kEpsilon));
|
||||
|
||||
// Test clamping: t > 1 should clamp to 1
|
||||
final var aboveRange = start.interpolate(end, 1.5);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, aboveRange.left, kEpsilon),
|
||||
() -> assertEquals(6.0, aboveRange.right, kEpsilon));
|
||||
}
|
||||
}
|
||||
@@ -55,4 +55,44 @@ class DifferentialDriveWheelSpeedsTest {
|
||||
assertAll(
|
||||
() -> assertEquals(0.5, wheelSpeeds.left), () -> assertEquals(0.25, wheelSpeeds.right));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInterpolate() {
|
||||
final var start = new DifferentialDriveWheelSpeeds(1.0, 2.0);
|
||||
final var end = new DifferentialDriveWheelSpeeds(5.0, 6.0);
|
||||
|
||||
// Test interpolation at t=0 (should return start)
|
||||
final var atStart = start.interpolate(end, 0.0);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, atStart.left, 1e-9), () -> assertEquals(2.0, atStart.right, 1e-9));
|
||||
|
||||
// Test interpolation at t=1 (should return end)
|
||||
final var atEnd = start.interpolate(end, 1.0);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, atEnd.left, 1e-9), () -> assertEquals(6.0, atEnd.right, 1e-9));
|
||||
|
||||
// Test interpolation at t=0.5 (should return midpoint)
|
||||
final var atMidpoint = start.interpolate(end, 0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, atMidpoint.left, 1e-9),
|
||||
() -> assertEquals(4.0, atMidpoint.right, 1e-9));
|
||||
|
||||
// Test interpolation at t=0.25
|
||||
final var atQuarter = start.interpolate(end, 0.25);
|
||||
assertAll(
|
||||
() -> assertEquals(2.0, atQuarter.left, 1e-9),
|
||||
() -> assertEquals(3.0, atQuarter.right, 1e-9));
|
||||
|
||||
// Test clamping: t < 0 should clamp to 0
|
||||
final var belowRange = start.interpolate(end, -0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, belowRange.left, 1e-9),
|
||||
() -> assertEquals(2.0, belowRange.right, 1e-9));
|
||||
|
||||
// Test clamping: t > 1 should clamp to 1
|
||||
final var aboveRange = start.interpolate(end, 1.5);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, aboveRange.left, 1e-9),
|
||||
() -> assertEquals(6.0, aboveRange.right, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -169,6 +169,112 @@ class MecanumDriveKinematicsTest {
|
||||
() -> assertEquals(48.0, moduleStates.rearRight, 0.1));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testStraightLineInverseAccelerations() {
|
||||
ChassisAccelerations accelerations = new ChassisAccelerations(5, 0, 0);
|
||||
var wheelAccelerations = m_kinematics.toWheelAccelerations(accelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, wheelAccelerations.frontLeft, 0.1),
|
||||
() -> assertEquals(5.0, wheelAccelerations.frontRight, 0.1),
|
||||
() -> assertEquals(5.0, wheelAccelerations.rearLeft, 0.1),
|
||||
() -> assertEquals(5.0, wheelAccelerations.rearRight, 0.1));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testStraightLineForwardAccelerations() {
|
||||
var wheelAccelerations = new MecanumDriveWheelAccelerations(3.536, 3.536, 3.536, 3.536);
|
||||
var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(3.536, chassisAccelerations.ax, 0.1),
|
||||
() -> assertEquals(0, chassisAccelerations.ay, 0.1),
|
||||
() -> assertEquals(0, chassisAccelerations.alpha, 0.1));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testStrafeInverseAccelerations() {
|
||||
ChassisAccelerations accelerations = new ChassisAccelerations(0, 4, 0);
|
||||
var wheelAccelerations = m_kinematics.toWheelAccelerations(accelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(-4.0, wheelAccelerations.frontLeft, 0.1),
|
||||
() -> assertEquals(4.0, wheelAccelerations.frontRight, 0.1),
|
||||
() -> assertEquals(4.0, wheelAccelerations.rearLeft, 0.1),
|
||||
() -> assertEquals(-4.0, wheelAccelerations.rearRight, 0.1));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testStrafeForwardAccelerations() {
|
||||
var wheelAccelerations =
|
||||
new MecanumDriveWheelAccelerations(-2.828427, 2.828427, 2.828427, -2.828427);
|
||||
var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0, chassisAccelerations.ax, 0.1),
|
||||
() -> assertEquals(2.8284, chassisAccelerations.ay, 0.1),
|
||||
() -> assertEquals(0, chassisAccelerations.alpha, 0.1));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testRotationInverseAccelerations() {
|
||||
ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 2 * Math.PI);
|
||||
var wheelAccelerations = m_kinematics.toWheelAccelerations(accelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(-150.79645, wheelAccelerations.frontLeft, 0.1),
|
||||
() -> assertEquals(150.79645, wheelAccelerations.frontRight, 0.1),
|
||||
() -> assertEquals(-150.79645, wheelAccelerations.rearLeft, 0.1),
|
||||
() -> assertEquals(150.79645, wheelAccelerations.rearRight, 0.1));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testRotationForwardAccelerations() {
|
||||
var wheelAccelerations =
|
||||
new MecanumDriveWheelAccelerations(-150.79645, 150.79645, -150.79645, 150.79645);
|
||||
var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0, chassisAccelerations.ax, 0.1),
|
||||
() -> assertEquals(0, chassisAccelerations.ay, 0.1),
|
||||
() -> assertEquals(2 * Math.PI, chassisAccelerations.alpha, 0.1));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMixedTranslationRotationInverseAccelerations() {
|
||||
ChassisAccelerations accelerations = new ChassisAccelerations(2, 3, 1);
|
||||
var wheelAccelerations = m_kinematics.toWheelAccelerations(accelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(-25.0, wheelAccelerations.frontLeft, 0.1),
|
||||
() -> assertEquals(29.0, wheelAccelerations.frontRight, 0.1),
|
||||
() -> assertEquals(-19.0, wheelAccelerations.rearLeft, 0.1),
|
||||
() -> assertEquals(23.0, wheelAccelerations.rearRight, 0.1));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMixedTranslationRotationForwardAccelerations() {
|
||||
var wheelAccelerations = new MecanumDriveWheelAccelerations(-17.677670, 20.51, -13.44, 16.26);
|
||||
var chassisAccelerations = m_kinematics.toChassisAccelerations(wheelAccelerations);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(1.413, chassisAccelerations.ax, 0.1),
|
||||
() -> assertEquals(2.122, chassisAccelerations.ay, 0.1),
|
||||
() -> assertEquals(0.707, chassisAccelerations.alpha, 0.1));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testOffCenterRotationInverseAccelerations() {
|
||||
ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 1);
|
||||
var wheelAccelerations = m_kinematics.toWheelAccelerations(accelerations, m_fl);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0, wheelAccelerations.frontLeft, 0.1),
|
||||
() -> assertEquals(24.0, wheelAccelerations.frontRight, 0.1),
|
||||
() -> assertEquals(-24.0, wheelAccelerations.rearLeft, 0.1),
|
||||
() -> assertEquals(48.0, wheelAccelerations.rearRight, 0.1));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testOffCenterRotationForwardKinematicsKinematics() {
|
||||
var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941);
|
||||
|
||||
@@ -0,0 +1,170 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.math.kinematics;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.wpilib.units.Units.MetersPerSecondPerSecond;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
class MecanumDriveWheelAccelerationsTest {
|
||||
private static final double kEpsilon = 1E-9;
|
||||
|
||||
@Test
|
||||
void testDefaultConstructor() {
|
||||
var wheelAccelerations = new MecanumDriveWheelAccelerations();
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, wheelAccelerations.frontLeft, kEpsilon),
|
||||
() -> assertEquals(0.0, wheelAccelerations.frontRight, kEpsilon),
|
||||
() -> assertEquals(0.0, wheelAccelerations.rearLeft, kEpsilon),
|
||||
() -> assertEquals(0.0, wheelAccelerations.rearRight, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testParameterizedConstructor() {
|
||||
var wheelAccelerations = new MecanumDriveWheelAccelerations(1.0, 2.0, 3.0, 4.0);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, wheelAccelerations.frontLeft, kEpsilon),
|
||||
() -> assertEquals(2.0, wheelAccelerations.frontRight, kEpsilon),
|
||||
() -> assertEquals(3.0, wheelAccelerations.rearLeft, kEpsilon),
|
||||
() -> assertEquals(4.0, wheelAccelerations.rearRight, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMeasureConstructor() {
|
||||
var frontLeft = MetersPerSecondPerSecond.of(1.5);
|
||||
var frontRight = MetersPerSecondPerSecond.of(2.5);
|
||||
var rearLeft = MetersPerSecondPerSecond.of(3.5);
|
||||
var rearRight = MetersPerSecondPerSecond.of(4.5);
|
||||
var wheelAccelerations =
|
||||
new MecanumDriveWheelAccelerations(frontLeft, frontRight, rearLeft, rearRight);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(1.5, wheelAccelerations.frontLeft, kEpsilon),
|
||||
() -> assertEquals(2.5, wheelAccelerations.frontRight, kEpsilon),
|
||||
() -> assertEquals(3.5, wheelAccelerations.rearLeft, kEpsilon),
|
||||
() -> assertEquals(4.5, wheelAccelerations.rearRight, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testPlus() {
|
||||
final var left = new MecanumDriveWheelAccelerations(1.0, 0.5, 2.0, 1.5);
|
||||
final var right = new MecanumDriveWheelAccelerations(2.0, 1.5, 0.5, 1.0);
|
||||
|
||||
final var wheelAccelerations = left.plus(right);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, wheelAccelerations.frontLeft),
|
||||
() -> assertEquals(2.0, wheelAccelerations.frontRight),
|
||||
() -> assertEquals(2.5, wheelAccelerations.rearLeft),
|
||||
() -> assertEquals(2.5, wheelAccelerations.rearRight));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMinus() {
|
||||
final var left = new MecanumDriveWheelAccelerations(5.0, 4.0, 6.0, 2.5);
|
||||
final var right = new MecanumDriveWheelAccelerations(1.0, 2.0, 3.0, 0.5);
|
||||
|
||||
final var wheelAccelerations = left.minus(right);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(4.0, wheelAccelerations.frontLeft),
|
||||
() -> assertEquals(2.0, wheelAccelerations.frontRight),
|
||||
() -> assertEquals(3.0, wheelAccelerations.rearLeft),
|
||||
() -> assertEquals(2.0, wheelAccelerations.rearRight));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testUnaryMinus() {
|
||||
final var wheelAccelerations =
|
||||
new MecanumDriveWheelAccelerations(1.0, -2.0, 3.0, -4.0).unaryMinus();
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(-1.0, wheelAccelerations.frontLeft),
|
||||
() -> assertEquals(2.0, wheelAccelerations.frontRight),
|
||||
() -> assertEquals(-3.0, wheelAccelerations.rearLeft),
|
||||
() -> assertEquals(4.0, wheelAccelerations.rearRight));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMultiplication() {
|
||||
final var wheelAccelerations =
|
||||
new MecanumDriveWheelAccelerations(2.0, 2.5, 3.0, 3.5).times(2.0);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(4.0, wheelAccelerations.frontLeft),
|
||||
() -> assertEquals(5.0, wheelAccelerations.frontRight),
|
||||
() -> assertEquals(6.0, wheelAccelerations.rearLeft),
|
||||
() -> assertEquals(7.0, wheelAccelerations.rearRight));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDivision() {
|
||||
final var wheelAccelerations = new MecanumDriveWheelAccelerations(2.0, 2.5, 1.5, 1.0).div(2.0);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, wheelAccelerations.frontLeft),
|
||||
() -> assertEquals(1.25, wheelAccelerations.frontRight),
|
||||
() -> assertEquals(0.75, wheelAccelerations.rearLeft),
|
||||
() -> assertEquals(0.5, wheelAccelerations.rearRight));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInterpolate() {
|
||||
final var start = new MecanumDriveWheelAccelerations(1.0, 2.0, 3.0, 4.0);
|
||||
final var end = new MecanumDriveWheelAccelerations(5.0, 6.0, 7.0, 8.0);
|
||||
|
||||
// Test interpolation at t=0 (should return start)
|
||||
final var atStart = start.interpolate(end, 0.0);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, atStart.frontLeft, kEpsilon),
|
||||
() -> assertEquals(2.0, atStart.frontRight, kEpsilon),
|
||||
() -> assertEquals(3.0, atStart.rearLeft, kEpsilon),
|
||||
() -> assertEquals(4.0, atStart.rearRight, kEpsilon));
|
||||
|
||||
// Test interpolation at t=1 (should return end)
|
||||
final var atEnd = start.interpolate(end, 1.0);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, atEnd.frontLeft, kEpsilon),
|
||||
() -> assertEquals(6.0, atEnd.frontRight, kEpsilon),
|
||||
() -> assertEquals(7.0, atEnd.rearLeft, kEpsilon),
|
||||
() -> assertEquals(8.0, atEnd.rearRight, kEpsilon));
|
||||
|
||||
// Test interpolation at t=0.5 (should return midpoint)
|
||||
final var atMidpoint = start.interpolate(end, 0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, atMidpoint.frontLeft, kEpsilon),
|
||||
() -> assertEquals(4.0, atMidpoint.frontRight, kEpsilon),
|
||||
() -> assertEquals(5.0, atMidpoint.rearLeft, kEpsilon),
|
||||
() -> assertEquals(6.0, atMidpoint.rearRight, kEpsilon));
|
||||
|
||||
// Test interpolation at t=0.25
|
||||
final var atQuarter = start.interpolate(end, 0.25);
|
||||
assertAll(
|
||||
() -> assertEquals(2.0, atQuarter.frontLeft, kEpsilon),
|
||||
() -> assertEquals(3.0, atQuarter.frontRight, kEpsilon),
|
||||
() -> assertEquals(4.0, atQuarter.rearLeft, kEpsilon),
|
||||
() -> assertEquals(5.0, atQuarter.rearRight, kEpsilon));
|
||||
|
||||
// Test clamping: t < 0 should clamp to 0
|
||||
final var belowRange = start.interpolate(end, -0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, belowRange.frontLeft, kEpsilon),
|
||||
() -> assertEquals(2.0, belowRange.frontRight, kEpsilon),
|
||||
() -> assertEquals(3.0, belowRange.rearLeft, kEpsilon),
|
||||
() -> assertEquals(4.0, belowRange.rearRight, kEpsilon));
|
||||
|
||||
// Test clamping: t > 1 should clamp to 1
|
||||
final var aboveRange = start.interpolate(end, 1.5);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, aboveRange.frontLeft, kEpsilon),
|
||||
() -> assertEquals(6.0, aboveRange.frontRight, kEpsilon),
|
||||
() -> assertEquals(7.0, aboveRange.rearLeft, kEpsilon),
|
||||
() -> assertEquals(8.0, aboveRange.rearRight, kEpsilon));
|
||||
}
|
||||
}
|
||||
@@ -70,4 +70,58 @@ class MecanumDriveWheelSpeedsTest {
|
||||
() -> assertEquals(1.0, wheelSpeeds.rearLeft),
|
||||
() -> assertEquals(0.75, wheelSpeeds.rearRight));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInterpolate() {
|
||||
final var start = new MecanumDriveWheelSpeeds(1.0, 2.0, 3.0, 4.0);
|
||||
final var end = new MecanumDriveWheelSpeeds(5.0, 6.0, 7.0, 8.0);
|
||||
|
||||
// Test interpolation at t=0 (should return start)
|
||||
final var atStart = start.interpolate(end, 0.0);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, atStart.frontLeft, 1e-9),
|
||||
() -> assertEquals(2.0, atStart.frontRight, 1e-9),
|
||||
() -> assertEquals(3.0, atStart.rearLeft, 1e-9),
|
||||
() -> assertEquals(4.0, atStart.rearRight, 1e-9));
|
||||
|
||||
// Test interpolation at t=1 (should return end)
|
||||
final var atEnd = start.interpolate(end, 1.0);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, atEnd.frontLeft, 1e-9),
|
||||
() -> assertEquals(6.0, atEnd.frontRight, 1e-9),
|
||||
() -> assertEquals(7.0, atEnd.rearLeft, 1e-9),
|
||||
() -> assertEquals(8.0, atEnd.rearRight, 1e-9));
|
||||
|
||||
// Test interpolation at t=0.5 (should return midpoint)
|
||||
final var atMidpoint = start.interpolate(end, 0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, atMidpoint.frontLeft, 1e-9),
|
||||
() -> assertEquals(4.0, atMidpoint.frontRight, 1e-9),
|
||||
() -> assertEquals(5.0, atMidpoint.rearLeft, 1e-9),
|
||||
() -> assertEquals(6.0, atMidpoint.rearRight, 1e-9));
|
||||
|
||||
// Test interpolation at t=0.25
|
||||
final var atQuarter = start.interpolate(end, 0.25);
|
||||
assertAll(
|
||||
() -> assertEquals(2.0, atQuarter.frontLeft, 1e-9),
|
||||
() -> assertEquals(3.0, atQuarter.frontRight, 1e-9),
|
||||
() -> assertEquals(4.0, atQuarter.rearLeft, 1e-9),
|
||||
() -> assertEquals(5.0, atQuarter.rearRight, 1e-9));
|
||||
|
||||
// Test clamping: t < 0 should clamp to 0
|
||||
final var belowRange = start.interpolate(end, -0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, belowRange.frontLeft, 1e-9),
|
||||
() -> assertEquals(2.0, belowRange.frontRight, 1e-9),
|
||||
() -> assertEquals(3.0, belowRange.rearLeft, 1e-9),
|
||||
() -> assertEquals(4.0, belowRange.rearRight, 1e-9));
|
||||
|
||||
// Test clamping: t > 1 should clamp to 1
|
||||
final var aboveRange = start.interpolate(end, 1.5);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, aboveRange.frontLeft, 1e-9),
|
||||
() -> assertEquals(6.0, aboveRange.frontRight, 1e-9),
|
||||
() -> assertEquals(7.0, aboveRange.rearLeft, 1e-9),
|
||||
() -> assertEquals(8.0, aboveRange.rearRight, 1e-9));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -7,6 +7,7 @@ package org.wpilib.math.kinematics;
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import java.util.Arrays;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.wpilib.math.geometry.Rotation2d;
|
||||
import org.wpilib.math.geometry.Translation2d;
|
||||
@@ -146,12 +147,10 @@ class SwerveDriveKinematicsTest {
|
||||
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
|
||||
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
|
||||
|
||||
/*
|
||||
The circumference of the wheels about the COR is π * diameter, or 2π * radius
|
||||
the radius is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels
|
||||
trace out is 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.
|
||||
*/
|
||||
// The circumference of the wheels about the COR is π * diameter, or 2π * radius. The radius
|
||||
// is the √(12²in + 12²in), or 16.9706in, so the circumference the wheels trace out is
|
||||
// 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].speed, 0.1),
|
||||
@@ -199,14 +198,12 @@ class SwerveDriveKinematicsTest {
|
||||
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 √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel
|
||||
should be pointing straight forward, the back-left wheel should be pointing straight right,
|
||||
and the back-right wheel should be at a -45 degree angle
|
||||
*/
|
||||
// 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 √(24² + 24²) and circumference 213.2584. As for angles, the front-right wheel should
|
||||
// be pointing straight forward, the back-left wheel should be pointing straight right, and the
|
||||
// back-right wheel should be at a -45 degree angle.
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, moduleStates[0].speed, 0.1),
|
||||
@@ -228,14 +225,12 @@ class SwerveDriveKinematicsTest {
|
||||
|
||||
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
|
||||
|
||||
/*
|
||||
We already know that our omega should be 2π from the previous test. Next, we need to determine
|
||||
the vx and vy of our chassis center. Because our COR is at a 45 degree angle from 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 triangle are
|
||||
1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
|
||||
*/
|
||||
// We already know that our omega should be 2π from the previous test. Next, we need to
|
||||
// determine the vx and vy of our chassis center. Because our COR is at a 45 degree angle from
|
||||
// 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
|
||||
// triangle are 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(75.398, chassisSpeeds.vx, 0.1),
|
||||
@@ -252,14 +247,12 @@ class SwerveDriveKinematicsTest {
|
||||
|
||||
var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta);
|
||||
|
||||
/*
|
||||
We already know that our omega should be 2π from the previous test. Next, we need to determine
|
||||
the vx and vy of our chassis center. Because our COR is at a 45 degree angle from 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 triangle are
|
||||
1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
|
||||
*/
|
||||
// We already know that our omega should be 2π from the previous test. Next, we need to
|
||||
// determine the vx and vy of our chassis center. Because our COR is at a 45 degree angle from
|
||||
// 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
|
||||
// triangle are 1:√(2)/2:√(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(75.398, twist.dx, 0.1),
|
||||
@@ -312,14 +305,12 @@ class SwerveDriveKinematicsTest {
|
||||
|
||||
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.
|
||||
*/
|
||||
// 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.vx, 0.1),
|
||||
@@ -336,14 +327,12 @@ class SwerveDriveKinematicsTest {
|
||||
|
||||
var twist = m_kinematics.toTwist2d(flDelta, frDelta, blDelta, brDelta);
|
||||
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
// 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, twist.dx, 0.1),
|
||||
@@ -406,4 +395,218 @@ class SwerveDriveKinematicsTest {
|
||||
() -> assertEquals(-1.0, arr[2].speed, kEpsilon),
|
||||
() -> assertEquals(-1.0, arr[3].speed, kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testTurnInPlaceInverseAccelerations() {
|
||||
ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 2 * Math.PI);
|
||||
double angularVelocity = 2 * Math.PI;
|
||||
var moduleAccelerations =
|
||||
m_kinematics.toSwerveModuleAccelerations(accelerations, angularVelocity);
|
||||
|
||||
// For turn-in-place with angular acceleration of 2π rad/s² and angular velocity of 2π rad/s,
|
||||
// each module has both tangential acceleration (from angular acceleration) and centripetal
|
||||
// acceleration (from angular velocity). The total acceleration magnitude is approximately
|
||||
// 678.4.
|
||||
//
|
||||
// For each swerve module at position (x, y) relative to the robot center:
|
||||
// - Distance from center: r = √(x² + y²) = √(12² + 12²) = 16.97 m
|
||||
// - Current tangential velocity: v_t = ω × r = 2π × 16.97 = 106.63 m/s
|
||||
//
|
||||
// Two acceleration components:
|
||||
// 1) Tangential acceleration (from angular acceleration α = 2π rad/s²):
|
||||
// a_tangential = α × r = 2π × 16.97 = 106.63 m/s²
|
||||
// Direction: perpendicular to radius vector
|
||||
//
|
||||
// 2) Centripetal acceleration (from angular velocity ω = 2π rad/s):
|
||||
// a_centripetal = ω² × r = (2π)² × 16.97 = 668.7 m/s²
|
||||
// Direction: toward center of rotation
|
||||
//
|
||||
// Total acceleration magnitude: |a_total| = √(a_tangential² + a_centripetal²)
|
||||
// = √(106.63² + 668.7²) = 678.4 m/s²
|
||||
//
|
||||
// For module positions:
|
||||
// FL (12, 12): radius angle = 135°, tangential = 45°, centripetal = -135° → total angle = -144°
|
||||
// FR (12, -12): radius angle = 45°, tangential = -45°, centripetal = -135° → total angle = 126°
|
||||
// BL (-12, 12): radius angle = 135°, tangential = 45°, centripetal = 45° → total angle = -54°
|
||||
// BR (-12, -12): radius angle = -45°, tangential = 45°, centripetal = 135° → total angle = 36°
|
||||
//
|
||||
// The acceleration angles are not simply tangential because the centripetal component from the
|
||||
// existing angular velocity dominates and affects the direction.
|
||||
|
||||
double centerRadius = m_kinematics.getModules()[0].getNorm();
|
||||
double tangentialAccel = centerRadius * accelerations.alpha; // α * r
|
||||
double centripetalAccel = centerRadius * angularVelocity * angularVelocity; // ω² * r
|
||||
double totalAccel = Math.hypot(tangentialAccel, centripetalAccel);
|
||||
|
||||
Rotation2d[] expectedAngles =
|
||||
Arrays.stream(m_kinematics.getModules())
|
||||
.map(
|
||||
m -> {
|
||||
Rotation2d radiusAngle = m.getAngle();
|
||||
|
||||
// Tangential acceleration: perpendicular to radius (90° CCW from radius)
|
||||
Rotation2d tangentialDirection = radiusAngle.rotateBy(Rotation2d.kCCW_Pi_2);
|
||||
double tangentialX = tangentialAccel * tangentialDirection.getCos();
|
||||
double tangentialY = tangentialAccel * tangentialDirection.getSin();
|
||||
|
||||
// Centripetal acceleration: toward center (opposite of radius)
|
||||
Rotation2d centripetalDirection = radiusAngle.rotateBy(Rotation2d.kPi);
|
||||
double centripetalX = centripetalAccel * centripetalDirection.getCos();
|
||||
double centripetalY = centripetalAccel * centripetalDirection.getSin();
|
||||
|
||||
// Vector sum of tangential and centripetal accelerations
|
||||
double totalX = tangentialX + centripetalX;
|
||||
double totalY = tangentialY + centripetalY;
|
||||
|
||||
return new Rotation2d(totalX, totalY);
|
||||
})
|
||||
.toArray(Rotation2d[]::new);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(totalAccel, moduleAccelerations[0].acceleration, kEpsilon),
|
||||
() -> assertEquals(totalAccel, moduleAccelerations[1].acceleration, kEpsilon),
|
||||
() -> assertEquals(totalAccel, moduleAccelerations[2].acceleration, kEpsilon),
|
||||
() -> assertEquals(totalAccel, moduleAccelerations[3].acceleration, kEpsilon),
|
||||
() ->
|
||||
assertEquals(
|
||||
expectedAngles[0].getDegrees(),
|
||||
moduleAccelerations[0].angle.getDegrees(),
|
||||
kEpsilon),
|
||||
() ->
|
||||
assertEquals(
|
||||
expectedAngles[1].getDegrees(),
|
||||
moduleAccelerations[1].angle.getDegrees(),
|
||||
kEpsilon),
|
||||
() ->
|
||||
assertEquals(
|
||||
expectedAngles[2].getDegrees(),
|
||||
moduleAccelerations[2].angle.getDegrees(),
|
||||
kEpsilon),
|
||||
() ->
|
||||
assertEquals(
|
||||
expectedAngles[3].getDegrees(),
|
||||
moduleAccelerations[3].angle.getDegrees(),
|
||||
kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testTurnInPlaceForwardAccelerations() {
|
||||
SwerveModuleAcceleration flAccel =
|
||||
new SwerveModuleAcceleration(106.629, Rotation2d.fromDegrees(135));
|
||||
SwerveModuleAcceleration frAccel =
|
||||
new SwerveModuleAcceleration(106.629, Rotation2d.fromDegrees(45));
|
||||
SwerveModuleAcceleration blAccel =
|
||||
new SwerveModuleAcceleration(106.629, Rotation2d.fromDegrees(-135));
|
||||
SwerveModuleAcceleration brAccel =
|
||||
new SwerveModuleAcceleration(106.629, Rotation2d.fromDegrees(-45));
|
||||
|
||||
var chassisAccelerations =
|
||||
m_kinematics.toChassisAccelerations(flAccel, frAccel, blAccel, brAccel);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, chassisAccelerations.ax, kEpsilon),
|
||||
() -> assertEquals(0.0, chassisAccelerations.ay, kEpsilon),
|
||||
() -> assertEquals(2 * Math.PI, chassisAccelerations.alpha, 0.1));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testOffCenterRotationInverseAccelerations() {
|
||||
ChassisAccelerations accelerations = new ChassisAccelerations(0, 0, 1);
|
||||
// For this test, assume an angular velocity of 1 rad/s
|
||||
double angularVelocity = 1.0;
|
||||
var moduleAccelerations =
|
||||
m_kinematics.toSwerveModuleAccelerations(accelerations, angularVelocity, m_fl);
|
||||
|
||||
// When rotating about the front-left module position with both angular acceleration (1 rad/s²)
|
||||
// and angular velocity (1 rad/s), each module experiences both tangential and centripetal
|
||||
// accelerations that combine vectorially.
|
||||
//
|
||||
// Center of rotation: FL module at (12, 12) inches
|
||||
// Module positions relative to center of rotation:
|
||||
// - FL: (0, 0) - at center of rotation
|
||||
// - FR: (0, -24) - 24 m south of center
|
||||
// - BL: (-24, 0) - 24 m west of center
|
||||
// - BR: (-24, -24) - distance = √(24² + 24²) = 33.94 m southwest
|
||||
//
|
||||
// For each module at distance r from center of rotation:
|
||||
// 1) Tangential acceleration: a_t = α × r = 1 × r
|
||||
// Direction: perpendicular to radius vector (90° CCW from radius)
|
||||
//
|
||||
// 2) Centripetal acceleration: a_c = ω² × r = 1² × r = r
|
||||
// Direction: toward center of rotation
|
||||
|
||||
double[] expectedAccelerations =
|
||||
Arrays.stream(m_kinematics.getModules())
|
||||
.mapToDouble(
|
||||
m -> {
|
||||
Translation2d relativePos = m.minus(m_fl);
|
||||
double r = relativePos.getNorm();
|
||||
|
||||
if (r < 1e-9) {
|
||||
return 0.0; // No acceleration at center of rotation
|
||||
}
|
||||
|
||||
double tangentialAccel = r * accelerations.alpha; // α * r = 1 * r
|
||||
double centripetalAccel = r * angularVelocity * angularVelocity; // ω² * r = 1 * r
|
||||
return Math.hypot(tangentialAccel, centripetalAccel);
|
||||
})
|
||||
.toArray();
|
||||
|
||||
Rotation2d[] expectedAngles =
|
||||
Arrays.stream(m_kinematics.getModules())
|
||||
.map(
|
||||
m -> {
|
||||
Translation2d relativePos = m.minus(m_fl);
|
||||
double r = relativePos.getNorm();
|
||||
|
||||
if (r < 1e-9) {
|
||||
return Rotation2d.kZero; // Angle is undefined at center of rotation
|
||||
}
|
||||
|
||||
Rotation2d radiusAngle = new Rotation2d(relativePos.getX(), relativePos.getY());
|
||||
|
||||
// Tangential acceleration: perpendicular to radius (90° CCW from radius)
|
||||
Rotation2d tangentialDirection = radiusAngle.rotateBy(Rotation2d.kCCW_Pi_2);
|
||||
double tangentialX = tangentialDirection.getCos() * r; // α * r = 1 * r
|
||||
double tangentialY = tangentialDirection.getSin() * r;
|
||||
|
||||
// Centripetal acceleration: toward center (opposite of radius)
|
||||
Rotation2d centripetalDirection = radiusAngle.rotateBy(Rotation2d.kPi);
|
||||
double centripetalX = centripetalDirection.getCos() * r; // ω² * r = 1 * r
|
||||
double centripetalY = centripetalDirection.getSin() * r;
|
||||
|
||||
// Vector sum of tangential and centripetal accelerations
|
||||
double totalX = tangentialX + centripetalX;
|
||||
double totalY = tangentialY + centripetalY;
|
||||
|
||||
return new Rotation2d(totalX, totalY);
|
||||
})
|
||||
.toArray(Rotation2d[]::new);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(expectedAccelerations[0], moduleAccelerations[0].acceleration, kEpsilon),
|
||||
() -> assertEquals(expectedAccelerations[1], moduleAccelerations[1].acceleration, kEpsilon),
|
||||
() -> assertEquals(expectedAccelerations[2], moduleAccelerations[2].acceleration, kEpsilon),
|
||||
() -> assertEquals(expectedAccelerations[3], moduleAccelerations[3].acceleration, kEpsilon),
|
||||
() ->
|
||||
assertEquals(
|
||||
expectedAngles[0].getDegrees(),
|
||||
moduleAccelerations[0].angle.getDegrees(),
|
||||
kEpsilon),
|
||||
() ->
|
||||
assertEquals(
|
||||
expectedAngles[1].getDegrees(),
|
||||
moduleAccelerations[1].angle.getDegrees(),
|
||||
kEpsilon),
|
||||
() ->
|
||||
assertEquals(
|
||||
expectedAngles[2].getDegrees(),
|
||||
moduleAccelerations[2].angle.getDegrees(),
|
||||
kEpsilon),
|
||||
() ->
|
||||
assertEquals(
|
||||
expectedAngles[3].getDegrees(),
|
||||
moduleAccelerations[3].angle.getDegrees(),
|
||||
kEpsilon));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,109 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.math.kinematics;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertNotEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
import static org.wpilib.units.Units.MetersPerSecondPerSecond;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.wpilib.math.geometry.Rotation2d;
|
||||
|
||||
class SwerveModuleAccelerationTest {
|
||||
private static final double kEpsilon = 1E-9;
|
||||
|
||||
@Test
|
||||
void testDefaultConstructor() {
|
||||
var moduleAccelerations = new SwerveModuleAcceleration();
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.0, moduleAccelerations.acceleration, kEpsilon),
|
||||
() -> assertEquals(0.0, moduleAccelerations.angle.getRadians(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testParameterizedConstructor() {
|
||||
var moduleAccelerations = new SwerveModuleAcceleration(2.5, new Rotation2d(1.5));
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(2.5, moduleAccelerations.acceleration, kEpsilon),
|
||||
() -> assertEquals(1.5, moduleAccelerations.angle.getRadians(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMeasureConstructor() {
|
||||
var acceleration = MetersPerSecondPerSecond.of(3.0);
|
||||
var angle = new Rotation2d(2.0);
|
||||
var moduleAccelerations = new SwerveModuleAcceleration(acceleration, angle);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, moduleAccelerations.acceleration, kEpsilon),
|
||||
() -> assertEquals(2.0, moduleAccelerations.angle.getRadians(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testEquals() {
|
||||
var moduleAccelerations1 = new SwerveModuleAcceleration(2.0, new Rotation2d(1.5));
|
||||
var moduleAccelerations2 = new SwerveModuleAcceleration(2.0, new Rotation2d(1.5));
|
||||
var moduleAccelerations3 = new SwerveModuleAcceleration(2.1, new Rotation2d(1.5));
|
||||
|
||||
assertEquals(moduleAccelerations1, moduleAccelerations2);
|
||||
assertNotEquals(moduleAccelerations1, moduleAccelerations3);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testCompareTo() {
|
||||
var slower = new SwerveModuleAcceleration(1.0, new Rotation2d(2.0));
|
||||
var faster = new SwerveModuleAcceleration(2.0, new Rotation2d(1.0));
|
||||
|
||||
assertTrue(slower.compareTo(faster) < 0);
|
||||
assertTrue(faster.compareTo(slower) > 0);
|
||||
assertEquals(0, slower.compareTo(new SwerveModuleAcceleration(1.0, new Rotation2d(2.0))));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInterpolate() {
|
||||
final var start = new SwerveModuleAcceleration(1.0, new Rotation2d(0.0));
|
||||
final var end = new SwerveModuleAcceleration(5.0, new Rotation2d(Math.PI / 2));
|
||||
|
||||
// Test interpolation at t=0 (should return start)
|
||||
final var atStart = start.interpolate(end, 0.0);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, atStart.acceleration, kEpsilon),
|
||||
() -> assertEquals(0.0, atStart.angle.getRadians(), kEpsilon));
|
||||
|
||||
// Test interpolation at t=1 (should return end)
|
||||
final var atEnd = start.interpolate(end, 1.0);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, atEnd.acceleration, kEpsilon),
|
||||
() -> assertEquals(Math.PI / 2, atEnd.angle.getRadians(), kEpsilon));
|
||||
|
||||
// Test interpolation at t=0.5 (should return midpoint)
|
||||
final var atMidpoint = start.interpolate(end, 0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, atMidpoint.acceleration, kEpsilon),
|
||||
() -> assertEquals(Math.PI / 4, atMidpoint.angle.getRadians(), kEpsilon));
|
||||
|
||||
// Test interpolation at t=0.25
|
||||
final var atQuarter = start.interpolate(end, 0.25);
|
||||
assertAll(
|
||||
() -> assertEquals(2.0, atQuarter.acceleration, kEpsilon),
|
||||
() -> assertEquals(Math.PI / 8, atQuarter.angle.getRadians(), kEpsilon));
|
||||
|
||||
// Test clamping: t < 0 should clamp to 0
|
||||
final var belowRange = start.interpolate(end, -0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, belowRange.acceleration, kEpsilon),
|
||||
() -> assertEquals(0.0, belowRange.angle.getRadians(), kEpsilon));
|
||||
|
||||
// Test clamping: t > 1 should clamp to 1
|
||||
final var aboveRange = start.interpolate(end, 1.5);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, aboveRange.acceleration, kEpsilon),
|
||||
() -> assertEquals(Math.PI / 2, aboveRange.angle.getRadians(), kEpsilon));
|
||||
}
|
||||
}
|
||||
@@ -149,4 +149,55 @@ class SwerveModuleStateTest {
|
||||
() -> assertEquals(Math.sqrt(2.0), refL.speed, kEpsilon),
|
||||
() -> assertEquals(45.0, refL.angle.getDegrees(), kEpsilon));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testInterpolate() {
|
||||
// Test basic interpolation with simple angles
|
||||
final var start = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0.0));
|
||||
final var end = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
|
||||
|
||||
// Test interpolation at t=0 (should return start)
|
||||
final var atStart = start.interpolate(end, 0.0);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, atStart.speed, kEpsilon),
|
||||
() -> assertEquals(0.0, atStart.angle.getDegrees(), kEpsilon));
|
||||
|
||||
// Test interpolation at t=1 (should return end)
|
||||
final var atEnd = start.interpolate(end, 1.0);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, atEnd.speed, kEpsilon),
|
||||
() -> assertEquals(90.0, atEnd.angle.getDegrees(), kEpsilon));
|
||||
|
||||
// Test interpolation at t=0.5 (should return midpoint)
|
||||
final var atMidpoint = start.interpolate(end, 0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, atMidpoint.speed, kEpsilon),
|
||||
() -> assertEquals(45.0, atMidpoint.angle.getDegrees(), kEpsilon));
|
||||
|
||||
// Test interpolation at t=0.25
|
||||
final var atQuarter = start.interpolate(end, 0.25);
|
||||
assertAll(
|
||||
() -> assertEquals(2.0, atQuarter.speed, kEpsilon),
|
||||
() -> assertEquals(22.5, atQuarter.angle.getDegrees(), kEpsilon));
|
||||
|
||||
// Test clamping: t < 0 should clamp to 0
|
||||
final var belowRange = start.interpolate(end, -0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(1.0, belowRange.speed, kEpsilon),
|
||||
() -> assertEquals(0.0, belowRange.angle.getDegrees(), kEpsilon));
|
||||
|
||||
// Test clamping: t > 1 should clamp to 1
|
||||
final var aboveRange = start.interpolate(end, 1.5);
|
||||
assertAll(
|
||||
() -> assertEquals(5.0, aboveRange.speed, kEpsilon),
|
||||
() -> assertEquals(90.0, aboveRange.angle.getDegrees(), kEpsilon));
|
||||
|
||||
// Test angle wrapping with crossing 180/-180 boundary
|
||||
final var startWrap = new SwerveModuleState(2.0, Rotation2d.fromDegrees(170.0));
|
||||
final var endWrap = new SwerveModuleState(4.0, Rotation2d.fromDegrees(-170.0));
|
||||
final var midpointWrap = startWrap.interpolate(endWrap, 0.5);
|
||||
assertAll(
|
||||
() -> assertEquals(3.0, midpointWrap.speed, kEpsilon),
|
||||
() -> assertEquals(180.0, Math.abs(midpointWrap.angle.getDegrees()), kEpsilon));
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user