mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
[wpimath] Move math functionality into new wpimath library (#2629)
The wpimath library is a new library designed to separate the reusable math functionality from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j). Package names / include file names were NOT changed to minimize breakage. In a future year it would be good to revamp these for a more uniform user experience and to reduce the risk of accidental naming conflicts. While theoretically all of this functionality could be placed into wpiutil, several pieces of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this expense for users who only want cscore or ntcore. It also allows for easy future separation of build tasks vs number of workers on memory-constrained machines. This moves the following functionality from wpiutil into wpimath: - Eigen - ejml - Drake - DARE - wpiutil.math package (Matrix etc) - units And the following functionality from wpilibc/j into wpimath: - Geometry - Kinematics - Spline - Trajectory - LinearFilter - MedianFilter - Feed-forward controllers
This commit is contained in:
Peter Johnson
20
wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
Normal file
20
wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
Normal file
@@ -0,0 +1,20 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "frc/kinematics/ChassisSpeeds.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
TEST(ChassisSpeeds, FieldRelativeConstruction) {
|
||||
const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
|
||||
1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
|
||||
|
||||
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), kEpsilon);
|
||||
EXPECT_NEAR(1.0, chassisSpeeds.vy.to<double>(), kEpsilon);
|
||||
EXPECT_NEAR(0.5, chassisSpeeds.omega.to<double>(), kEpsilon);
|
||||
}
|
||||
@@ -0,0 +1,79 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <wpi/math>
|
||||
|
||||
#include "frc/kinematics/ChassisSpeeds.h"
|
||||
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
||||
#include "gtest/gtest.h"
|
||||
#include "units/angular_velocity.h"
|
||||
#include "units/length.h"
|
||||
#include "units/velocity.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
TEST(DifferentialDriveKinematics, InverseKinematicsFromZero) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const ChassisSpeeds chassisSpeeds;
|
||||
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
|
||||
|
||||
EXPECT_NEAR(wheelSpeeds.left.to<double>(), 0, kEpsilon);
|
||||
EXPECT_NEAR(wheelSpeeds.right.to<double>(), 0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveKinematics, ForwardKinematicsFromZero) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const DifferentialDriveWheelSpeeds wheelSpeeds;
|
||||
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
|
||||
|
||||
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveKinematics, InverseKinematicsForStraightLine) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
|
||||
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
|
||||
|
||||
EXPECT_NEAR(wheelSpeeds.left.to<double>(), 3, kEpsilon);
|
||||
EXPECT_NEAR(wheelSpeeds.right.to<double>(), 3, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveKinematics, ForwardKinematicsForStraightLine) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
|
||||
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
|
||||
|
||||
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 3, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveKinematics, InverseKinematicsForRotateInPlace) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const ChassisSpeeds chassisSpeeds{0.0_mps, 0.0_mps,
|
||||
units::radians_per_second_t{wpi::math::pi}};
|
||||
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
|
||||
|
||||
EXPECT_NEAR(wheelSpeeds.left.to<double>(), -0.381 * wpi::math::pi, kEpsilon);
|
||||
EXPECT_NEAR(wheelSpeeds.right.to<double>(), +0.381 * wpi::math::pi, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveKinematics, ForwardKinematicsForRotateInPlace) {
|
||||
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
const DifferentialDriveWheelSpeeds wheelSpeeds{
|
||||
units::meters_per_second_t(+0.381 * wpi::math::pi),
|
||||
units::meters_per_second_t(-0.381 * wpi::math::pi)};
|
||||
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
|
||||
|
||||
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), -wpi::math::pi, kEpsilon);
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <wpi/math>
|
||||
|
||||
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
||||
#include "frc/kinematics/DifferentialDriveOdometry.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
static constexpr double kEpsilon = 1E-9;
|
||||
|
||||
using namespace frc;
|
||||
|
||||
TEST(DifferentialDriveOdometry, EncoderDistances) {
|
||||
DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
|
||||
|
||||
const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
|
||||
units::meter_t(5 * wpi::math::pi));
|
||||
|
||||
EXPECT_NEAR(pose.X().to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(pose.Y().to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
|
||||
}
|
||||
@@ -0,0 +1,230 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <wpi/math>
|
||||
|
||||
#include "frc/geometry/Translation2d.h"
|
||||
#include "frc/kinematics/MecanumDriveKinematics.h"
|
||||
#include "gtest/gtest.h"
|
||||
#include "units/angular_velocity.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
class MecanumDriveKinematicsTest : public ::testing::Test {
|
||||
protected:
|
||||
Translation2d m_fl{12_m, 12_m};
|
||||
Translation2d m_fr{12_m, -12_m};
|
||||
Translation2d m_bl{-12_m, 12_m};
|
||||
Translation2d m_br{-12_m, -12_m};
|
||||
|
||||
MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
|
||||
};
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
|
||||
ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
|
||||
auto moduleStates = 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
|
||||
*/
|
||||
|
||||
EXPECT_NEAR(3.536, moduleStates.frontLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(3.536, moduleStates.frontRight.to<double>(), 0.1);
|
||||
EXPECT_NEAR(3.536, moduleStates.rearLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(3.536, moduleStates.rearRight.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
|
||||
MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
|
||||
3.536_mps};
|
||||
auto chassisSpeeds = 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]]
|
||||
*/
|
||||
|
||||
EXPECT_NEAR(5.0, chassisSpeeds.vx.to<double>(), 0.1);
|
||||
EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
|
||||
EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
|
||||
ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
|
||||
auto moduleStates = 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
|
||||
*/
|
||||
|
||||
EXPECT_NEAR(-2.828427, moduleStates.frontLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(2.828427, moduleStates.frontRight.to<double>(), 0.1);
|
||||
EXPECT_NEAR(2.828427, moduleStates.rearLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(-2.828427, moduleStates.rearRight.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
|
||||
MecanumDriveWheelSpeeds wheelSpeeds{-2.828427_mps, 2.828427_mps, 2.828427_mps,
|
||||
-2.828427_mps};
|
||||
auto chassisSpeeds = 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]]
|
||||
*/
|
||||
|
||||
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
|
||||
EXPECT_NEAR(4.0, chassisSpeeds.vy.to<double>(), 0.1);
|
||||
EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
|
||||
ChassisSpeeds speeds{0_mps, 0_mps,
|
||||
units::radians_per_second_t(2 * wpi::math::pi)};
|
||||
auto moduleStates = 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
|
||||
*/
|
||||
|
||||
EXPECT_NEAR(-106.62919, moduleStates.frontLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(106.62919, moduleStates.frontRight.to<double>(), 0.1);
|
||||
EXPECT_NEAR(-106.62919, moduleStates.rearLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(106.62919, moduleStates.rearRight.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
|
||||
MecanumDriveWheelSpeeds wheelSpeeds{-106.62919_mps, 106.62919_mps,
|
||||
-106.62919_mps, 106.62919_mps};
|
||||
auto chassisSpeeds = 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]]
|
||||
*/
|
||||
|
||||
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
|
||||
EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
|
||||
EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
|
||||
ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
|
||||
auto moduleStates = 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
|
||||
*/
|
||||
|
||||
EXPECT_NEAR(-17.677670, moduleStates.frontLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(20.506097, moduleStates.frontRight.to<double>(), 0.1);
|
||||
EXPECT_NEAR(-13.435, moduleStates.rearLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(16.26, moduleStates.rearRight.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
|
||||
MecanumDriveWheelSpeeds wheelSpeeds{-17.677670_mps, 20.506097_mps,
|
||||
-13.435_mps, 16.26_mps};
|
||||
|
||||
auto chassisSpeeds = 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]]
|
||||
*/
|
||||
|
||||
EXPECT_NEAR(2.0, chassisSpeeds.vx.to<double>(), 0.1);
|
||||
EXPECT_NEAR(3.0, chassisSpeeds.vy.to<double>(), 0.1);
|
||||
EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
|
||||
ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
|
||||
auto moduleStates = 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
|
||||
*/
|
||||
|
||||
EXPECT_NEAR(0, moduleStates.frontLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(16.971, moduleStates.frontRight.to<double>(), 0.1);
|
||||
EXPECT_NEAR(-16.971, moduleStates.rearLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(33.941, moduleStates.rearRight.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
|
||||
MecanumDriveWheelSpeeds wheelSpeeds{0_mps, 16.971_mps, -16.971_mps,
|
||||
33.941_mps};
|
||||
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
|
||||
|
||||
/*
|
||||
By equation (13.13) of the state-space-guide, the chassis motion from the
|
||||
wheel velocities should be [[12][-12][1]]
|
||||
*/
|
||||
|
||||
EXPECT_NEAR(12.0, chassisSpeeds.vx.to<double>(), 0.1);
|
||||
EXPECT_NEAR(-12, chassisSpeeds.vy.to<double>(), 0.1);
|
||||
EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest,
|
||||
OffCenterTranslationRotationInverseKinematics) {
|
||||
ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
|
||||
auto moduleStates = 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
|
||||
*/
|
||||
EXPECT_NEAR(2.12, moduleStates.frontLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(21.92, moduleStates.frontRight.to<double>(), 0.1);
|
||||
EXPECT_NEAR(-12.02, moduleStates.rearLeft.to<double>(), 0.1);
|
||||
EXPECT_NEAR(36.06, moduleStates.rearRight.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest,
|
||||
OffCenterTranslationRotationForwardKinematics) {
|
||||
MecanumDriveWheelSpeeds wheelSpeeds{2.12_mps, 21.92_mps, -12.02_mps,
|
||||
36.06_mps};
|
||||
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
|
||||
|
||||
/*
|
||||
By equation (13.13) of the state-space-guide, the chassis motion from the
|
||||
wheel velocities should be [[17][-10][1]]
|
||||
*/
|
||||
|
||||
EXPECT_NEAR(17.0, chassisSpeeds.vx.to<double>(), 0.1);
|
||||
EXPECT_NEAR(-10, chassisSpeeds.vy.to<double>(), 0.1);
|
||||
EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveKinematicsTest, NormalizeTest) {
|
||||
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
|
||||
wheelSpeeds.Normalize(5.5_mps);
|
||||
|
||||
double kFactor = 5.5 / 7.0;
|
||||
|
||||
EXPECT_NEAR(wheelSpeeds.frontLeft.to<double>(), 5.0 * kFactor, 1E-9);
|
||||
EXPECT_NEAR(wheelSpeeds.frontRight.to<double>(), 6.0 * kFactor, 1E-9);
|
||||
EXPECT_NEAR(wheelSpeeds.rearLeft.to<double>(), 4.0 * kFactor, 1E-9);
|
||||
EXPECT_NEAR(wheelSpeeds.rearRight.to<double>(), 7.0 * kFactor, 1E-9);
|
||||
}
|
||||
@@ -0,0 +1,72 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "frc/kinematics/MecanumDriveOdometry.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
class MecanumDriveOdometryTest : public ::testing::Test {
|
||||
protected:
|
||||
Translation2d m_fl{12_m, 12_m};
|
||||
Translation2d m_fr{12_m, -12_m};
|
||||
Translation2d m_bl{-12_m, 12_m};
|
||||
Translation2d m_br{-12_m, -12_m};
|
||||
|
||||
MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
|
||||
MecanumDriveOdometry odometry{kinematics, 0_rad};
|
||||
};
|
||||
|
||||
TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
|
||||
odometry.ResetPosition(Pose2d(), 0_rad);
|
||||
MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
|
||||
3.536_mps};
|
||||
|
||||
odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
|
||||
auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
|
||||
|
||||
EXPECT_NEAR(secondPose.X().to<double>(), 0.0, 0.01);
|
||||
EXPECT_NEAR(secondPose.Y().to<double>(), 0.0, 0.01);
|
||||
EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
|
||||
odometry.ResetPosition(Pose2d(), 0_rad);
|
||||
MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
|
||||
|
||||
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
|
||||
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
|
||||
|
||||
EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
|
||||
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
|
||||
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
|
||||
odometry.ResetPosition(Pose2d(), 0_rad);
|
||||
MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
|
||||
39.986_mps};
|
||||
odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
|
||||
auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
|
||||
|
||||
EXPECT_NEAR(pose.X().to<double>(), 12, 0.01);
|
||||
EXPECT_NEAR(pose.Y().to<double>(), 12, 0.01);
|
||||
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
|
||||
}
|
||||
|
||||
TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
|
||||
odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
|
||||
|
||||
MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
|
||||
|
||||
odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
|
||||
auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
|
||||
|
||||
EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
|
||||
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
|
||||
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
|
||||
}
|
||||
@@ -0,0 +1,183 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <wpi/math>
|
||||
|
||||
#include "frc/geometry/Translation2d.h"
|
||||
#include "frc/kinematics/SwerveDriveKinematics.h"
|
||||
#include "gtest/gtest.h"
|
||||
#include "units/angular_velocity.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr double kEpsilon = 0.1;
|
||||
|
||||
class SwerveDriveKinematicsTest : public ::testing::Test {
|
||||
protected:
|
||||
Translation2d m_fl{12_m, 12_m};
|
||||
Translation2d m_fr{12_m, -12_m};
|
||||
Translation2d m_bl{-12_m, 12_m};
|
||||
Translation2d m_br{-12_m, -12_m};
|
||||
|
||||
SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
|
||||
};
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
|
||||
ChassisSpeeds speeds{5.0_mps, 0.0_mps, 0.0_rad_per_s};
|
||||
|
||||
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
|
||||
|
||||
EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
|
||||
|
||||
EXPECT_NEAR(fl.angle.Radians().to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(fr.angle.Radians().to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(bl.angle.Radians().to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(br.angle.Radians().to<double>(), 0.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
|
||||
SwerveModuleState state{5.0_mps, Rotation2d()};
|
||||
|
||||
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
|
||||
|
||||
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
|
||||
ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
|
||||
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
|
||||
|
||||
EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
|
||||
|
||||
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 90.0, kEpsilon);
|
||||
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 90.0, kEpsilon);
|
||||
EXPECT_NEAR(bl.angle.Degrees().to<double>(), 90.0, kEpsilon);
|
||||
EXPECT_NEAR(br.angle.Degrees().to<double>(), 90.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
|
||||
SwerveModuleState state{5_mps, Rotation2d(90_deg)};
|
||||
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
|
||||
|
||||
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
|
||||
ChassisSpeeds speeds{0_mps, 0_mps,
|
||||
units::radians_per_second_t(2 * wpi::math::pi)};
|
||||
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
|
||||
|
||||
EXPECT_NEAR(fl.speed.to<double>(), 106.63, kEpsilon);
|
||||
EXPECT_NEAR(fr.speed.to<double>(), 106.63, kEpsilon);
|
||||
EXPECT_NEAR(bl.speed.to<double>(), 106.63, kEpsilon);
|
||||
EXPECT_NEAR(br.speed.to<double>(), 106.63, kEpsilon);
|
||||
|
||||
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 135.0, kEpsilon);
|
||||
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 45.0, kEpsilon);
|
||||
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -135.0, kEpsilon);
|
||||
EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
|
||||
SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
|
||||
SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};
|
||||
SwerveModuleState bl{106.629_mps, Rotation2d(-135_deg)};
|
||||
SwerveModuleState br{106.629_mps, Rotation2d(-45_deg)};
|
||||
|
||||
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
|
||||
|
||||
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
|
||||
ChassisSpeeds speeds{0_mps, 0_mps,
|
||||
units::radians_per_second_t(2 * wpi::math::pi)};
|
||||
auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
|
||||
|
||||
EXPECT_NEAR(fl.speed.to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(fr.speed.to<double>(), 150.796, kEpsilon);
|
||||
EXPECT_NEAR(bl.speed.to<double>(), 150.796, kEpsilon);
|
||||
EXPECT_NEAR(br.speed.to<double>(), 213.258, kEpsilon);
|
||||
|
||||
EXPECT_NEAR(fl.angle.Degrees().to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(fr.angle.Degrees().to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -90.0, kEpsilon);
|
||||
EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
|
||||
SwerveModuleState fl{0.0_mps, Rotation2d(0_deg)};
|
||||
SwerveModuleState fr{150.796_mps, Rotation2d(0_deg)};
|
||||
SwerveModuleState bl{150.796_mps, Rotation2d(-90_deg)};
|
||||
SwerveModuleState br{213.258_mps, Rotation2d(-45_deg)};
|
||||
|
||||
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
|
||||
|
||||
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 75.398, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -75.398, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest,
|
||||
OffCenterCORRotationAndTranslationInverseKinematics) {
|
||||
ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
|
||||
auto [fl, fr, bl, br] =
|
||||
m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
|
||||
|
||||
EXPECT_NEAR(fl.speed.to<double>(), 23.43, kEpsilon);
|
||||
EXPECT_NEAR(fr.speed.to<double>(), 23.43, kEpsilon);
|
||||
EXPECT_NEAR(bl.speed.to<double>(), 54.08, kEpsilon);
|
||||
EXPECT_NEAR(br.speed.to<double>(), 54.08, kEpsilon);
|
||||
|
||||
EXPECT_NEAR(fl.angle.Degrees().to<double>(), -140.19, kEpsilon);
|
||||
EXPECT_NEAR(fr.angle.Degrees().to<double>(), -39.81, kEpsilon);
|
||||
EXPECT_NEAR(bl.angle.Degrees().to<double>(), -109.44, kEpsilon);
|
||||
EXPECT_NEAR(br.angle.Degrees().to<double>(), -70.56, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest,
|
||||
OffCenterCORRotationAndTranslationForwardKinematics) {
|
||||
SwerveModuleState fl{23.43_mps, Rotation2d(-140.19_deg)};
|
||||
SwerveModuleState fr{23.43_mps, Rotation2d(-39.81_deg)};
|
||||
SwerveModuleState bl{54.08_mps, Rotation2d(-109.44_deg)};
|
||||
SwerveModuleState br{54.08_mps, Rotation2d(-70.56_deg)};
|
||||
|
||||
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
|
||||
|
||||
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -33.0, kEpsilon);
|
||||
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 1.5, kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveKinematicsTest, NormalizeTest) {
|
||||
SwerveModuleState state1{5.0_mps, Rotation2d()};
|
||||
SwerveModuleState state2{6.0_mps, Rotation2d()};
|
||||
SwerveModuleState state3{4.0_mps, Rotation2d()};
|
||||
SwerveModuleState state4{7.0_mps, Rotation2d()};
|
||||
|
||||
std::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
|
||||
SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps);
|
||||
|
||||
double kFactor = 5.5 / 7.0;
|
||||
|
||||
EXPECT_NEAR(arr[0].speed.to<double>(), 5.0 * kFactor, kEpsilon);
|
||||
EXPECT_NEAR(arr[1].speed.to<double>(), 6.0 * kFactor, kEpsilon);
|
||||
EXPECT_NEAR(arr[2].speed.to<double>(), 4.0 * kFactor, kEpsilon);
|
||||
EXPECT_NEAR(arr[3].speed.to<double>(), 7.0 * kFactor, kEpsilon);
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "frc/kinematics/SwerveDriveKinematics.h"
|
||||
#include "frc/kinematics/SwerveDriveOdometry.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static constexpr double kEpsilon = 0.01;
|
||||
|
||||
class SwerveDriveOdometryTest : public ::testing::Test {
|
||||
protected:
|
||||
Translation2d m_fl{12_m, 12_m};
|
||||
Translation2d m_fr{12_m, -12_m};
|
||||
Translation2d m_bl{-12_m, 12_m};
|
||||
Translation2d m_br{-12_m, -12_m};
|
||||
|
||||
SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
|
||||
SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad};
|
||||
};
|
||||
|
||||
TEST_F(SwerveDriveOdometryTest, TwoIterations) {
|
||||
SwerveModuleState state{5_mps, Rotation2d()};
|
||||
|
||||
m_odometry.ResetPosition(Pose2d(), 0_rad);
|
||||
m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
|
||||
SwerveModuleState(), SwerveModuleState(),
|
||||
SwerveModuleState());
|
||||
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
|
||||
state, state);
|
||||
|
||||
EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
|
||||
SwerveModuleState fl{18.85_mps, Rotation2d(90_deg)};
|
||||
SwerveModuleState fr{42.15_mps, Rotation2d(26.565_deg)};
|
||||
SwerveModuleState bl{18.85_mps, Rotation2d(-90_deg)};
|
||||
SwerveModuleState br{42.15_mps, Rotation2d(-26.565_deg)};
|
||||
|
||||
SwerveModuleState zero{0_mps, Rotation2d()};
|
||||
|
||||
m_odometry.ResetPosition(Pose2d(), 0_rad);
|
||||
m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
|
||||
auto pose =
|
||||
m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
|
||||
|
||||
EXPECT_NEAR(12.0, pose.X().to<double>(), kEpsilon);
|
||||
EXPECT_NEAR(12.0, pose.Y().to<double>(), kEpsilon);
|
||||
EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
|
||||
}
|
||||
|
||||
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
|
||||
m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
|
||||
|
||||
SwerveModuleState state{5_mps, Rotation2d()};
|
||||
|
||||
m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(),
|
||||
SwerveModuleState(), SwerveModuleState(),
|
||||
SwerveModuleState());
|
||||
auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
|
||||
state, state);
|
||||
|
||||
EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
|
||||
}
|
||||
Reference in New Issue
Block a user