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

Add geometry classes (#1766)

Browse Source 
These classes introduce ways to represent poses and provide easy ways to transform, rotate, and translate poses across 2d space. This classes will be especially useful for a planned odometry and kinematics suite.

Furthermore, these classes can also be used to simply represent waypoints on a field, do superstructure motion planning, etc.
This commit is contained in:
Prateek Machiraju
2019-07-24 02:57:39 -04:00
committed by Peter Johnson
parent 48fe54271a
commit ee24101696
22 changed files with 1898 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp Normal file
View File

@@ -0,0 +1,43 @@
/*----------------------------------------------------------------------------*/
/* 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 <cmath>
#include "frc/geometry/Pose2d.h"
#include "gtest/gtest.h"
using namespace frc;
static constexpr double kEpsilon = 1E-9;
TEST(Twist2dTest, Straight) {
const Twist2d straight{5.0, 0.0, 0.0};
const auto straightPose = Pose2d().Exp(straight);
EXPECT_NEAR(straightPose.Translation().X(), 5.0, kEpsilon);
EXPECT_NEAR(straightPose.Translation().Y(), 0.0, kEpsilon);
EXPECT_NEAR(straightPose.Rotation().Radians(), 0.0, kEpsilon);
}
TEST(Twist2dTest, QuarterCircle) {
const Twist2d quarterCircle{5.0 / 2.0 * 3.14159265358979323846, 0,
3.14159265358979323846 / 2.0};
const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
EXPECT_NEAR(quarterCirclePose.Translation().X(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Translation().Y(), 5.0, kEpsilon);
EXPECT_NEAR(quarterCirclePose.Rotation().Degrees(), 90.0, kEpsilon);
}
TEST(Twist2dTest, DiagonalNoDtheta) {
const Twist2d diagonal{2.0, 2.0, 0.0};
const auto diagonalPose = Pose2d().Exp(diagonal);
EXPECT_NEAR(diagonalPose.Translation().X(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Translation().Y(), 2.0, kEpsilon);
EXPECT_NEAR(diagonalPose.Rotation().Degrees(), 0.0, kEpsilon);
}