mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
Add kinematics suite (#1787)
Co-authored-by: Tyler Veness <calcmogul@gmail.com> Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Prateek Machiraju
committed by
Peter Johnson
Peter Johnson
parent
561cbbd144
commit
f405582f86
@@ -0,0 +1,32 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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 DifferentialDriveKinematics m_kinematics
|
||||
= new DifferentialDriveKinematics(0.381 * 2);
|
||||
private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(m_kinematics);
|
||||
|
||||
@Test
|
||||
void testOneIteration() {
|
||||
m_odometry.resetPosition(new Pose2d());
|
||||
var speeds = new DifferentialDriveWheelSpeeds(0.02, 0.02);
|
||||
m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds());
|
||||
var pose = m_odometry.updateWithTime(1.0, new Rotation2d(), speeds);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(0.02, pose.getTranslation().getX(), kEpsilon),
|
||||
() -> assertEquals(0.00, pose.getTranslation().getY(), kEpsilon),
|
||||
() -> assertEquals(0.00, pose.getRotation().getRadians(), kEpsilon)
|
||||
);
|
||||
}
|
||||
|
||||
@Test
|
||||
void testQuarterCircle() {
|
||||
m_odometry.resetPosition(new Pose2d());
|
||||
var speeds = new DifferentialDriveWheelSpeeds(0.0, 5 * Math.PI);
|
||||
m_odometry.updateWithTime(0.0, new Rotation2d(), new DifferentialDriveWheelSpeeds());
|
||||
var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), speeds);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(pose.getTranslation().getX(), 5.0, kEpsilon),
|
||||
() -> assertEquals(pose.getTranslation().getY(), 5.0, kEpsilon),
|
||||
() -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,263 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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)
|
||||
);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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;
|
||||
|
||||
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);
|
||||
|
||||
@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.getTranslation().getX(), 0.0, 0.01),
|
||||
() -> assertEquals(secondPose.getTranslation().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.getTranslation().getX(), 0.01),
|
||||
() -> assertEquals(0, pose.getTranslation().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.getTranslation().getX(), 0.01),
|
||||
() -> assertEquals(12.0, pose.getTranslation().getY(), 0.01),
|
||||
() -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
|
||||
);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,263 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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)
|
||||
);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 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;
|
||||
|
||||
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);
|
||||
|
||||
@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.getTranslation().getX(), 0.01),
|
||||
() -> assertEquals(0, pose.getTranslation().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.getTranslation().getX(), 0.01),
|
||||
() -> assertEquals(12.0, pose.getTranslation().getY(), 0.01),
|
||||
() -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
|
||||
);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user