mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +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
20
wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
Normal file
20
wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
Normal file
@@ -0,0 +1,20 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#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,77 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <units/units.h>
|
||||
#include <wpi/math>
|
||||
|
||||
#include "frc/kinematics/ChassisSpeeds.h"
|
||||
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
||||
#include "gtest/gtest.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,46 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#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, OneIteration) {
|
||||
DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
DifferentialDriveOdometry odometry{kinematics};
|
||||
|
||||
odometry.ResetPosition(Pose2d());
|
||||
DifferentialDriveWheelSpeeds wheelSpeeds{0.02_mps, 0.02_mps};
|
||||
odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
|
||||
const auto& pose = odometry.UpdateWithTime(1_s, Rotation2d(), wheelSpeeds);
|
||||
|
||||
EXPECT_NEAR(pose.Translation().X().to<double>(), 0.02, kEpsilon);
|
||||
EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, kEpsilon);
|
||||
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
|
||||
}
|
||||
|
||||
TEST(DifferentialDriveOdometry, QuarterCircle) {
|
||||
DifferentialDriveKinematics kinematics{0.381_m * 2};
|
||||
DifferentialDriveOdometry odometry{kinematics};
|
||||
|
||||
odometry.ResetPosition(Pose2d());
|
||||
DifferentialDriveWheelSpeeds wheelSpeeds{
|
||||
0.0_mps, units::meters_per_second_t(5 * wpi::math::pi)};
|
||||
odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
|
||||
const auto& pose =
|
||||
odometry.UpdateWithTime(1_s, Rotation2d(90_deg), wheelSpeeds);
|
||||
|
||||
EXPECT_NEAR(pose.Translation().X().to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
|
||||
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
|
||||
}
|
||||
@@ -0,0 +1,230 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <units/units.h>
|
||||
#include <wpi/math>
|
||||
|
||||
#include "frc/geometry/Translation2d.h"
|
||||
#include "frc/kinematics/MecanumDriveKinematics.h"
|
||||
#include "gtest/gtest.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,59 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#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};
|
||||
};
|
||||
|
||||
TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
|
||||
odometry.ResetPosition(Pose2d());
|
||||
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.Translation().X().to<double>(), 0.0, 0.01);
|
||||
EXPECT_NEAR(secondPose.Translation().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());
|
||||
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.Translation().X().to<double>(), 0.5, 0.01);
|
||||
EXPECT_NEAR(pose.Translation().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());
|
||||
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.Translation().X().to<double>(), 12, 0.01);
|
||||
EXPECT_NEAR(pose.Translation().Y().to<double>(), 12, 0.01);
|
||||
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
|
||||
}
|
||||
@@ -0,0 +1,183 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <units/units.h>
|
||||
#include <wpi/math>
|
||||
|
||||
#include "frc/geometry/Translation2d.h"
|
||||
#include "frc/kinematics/SwerveDriveKinematics.h"
|
||||
#include "gtest/gtest.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,58 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#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};
|
||||
};
|
||||
|
||||
TEST_F(SwerveDriveOdometryTest, TwoIterations) {
|
||||
SwerveModuleState state{5_mps, Rotation2d()};
|
||||
|
||||
m_odometry.ResetPosition(Pose2d());
|
||||
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.Translation().X().to<double>(), kEpsilon);
|
||||
EXPECT_NEAR(0.0, pose.Translation().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());
|
||||
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.Translation().X().to<double>(), kEpsilon);
|
||||
EXPECT_NEAR(12.0, pose.Translation().Y().to<double>(), kEpsilon);
|
||||
EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
|
||||
}
|
||||
Reference in New Issue
Block a user