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

Add trajectory generation using hermite splines (#1843)

Browse Source
This commit is contained in:
Prateek Machiraju
2019-09-28 18:40:56 -04:00
committed by Peter Johnson
parent fd612052f3
commit 457f94ba26
56 changed files with 4185 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -52,3 +52,15 @@ TEST(Pose2dTest, Inequality) {
const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
EXPECT_TRUE(a != b);
}
TEST(Pose2dTest, Minus) {
const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
const auto transform = final - initial;
EXPECT_NEAR(transform.Translation().X().to<double>(), 5.0 * std::sqrt(2.0),
kEpsilon);
EXPECT_NEAR(transform.Translation().Y().to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
}

11
wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp
View File

@@ -56,3 +56,14 @@ TEST(Twist2dTest, Inequality) {
const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
EXPECT_TRUE(one != two);
}
TEST(Twist2dTest, Pose2dLog) {
const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
const Pose2d start{};
const auto twist = start.Log(end);
EXPECT_NEAR(twist.dx.to<double>(), 5 / 2.0 * wpi::math::pi, kEpsilon);
EXPECT_NEAR(twist.dy.to<double>(), 0.0, kEpsilon);
EXPECT_NEAR(twist.dtheta.to<double>(), wpi::math::pi / 2.0, kEpsilon);
}