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

39
wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp Normal file
View File

@@ -0,0 +1,39 @@
/*----------------------------------------------------------------------------*/
/* 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(Pose2dTest, TransformBy) {
const Pose2d initial{1.0, 2.0, Rotation2d::FromDegrees(45.0)};
const Transform2d transform{Translation2d{5.0, 0.0},
Rotation2d::FromDegrees(5.0)};
const auto transformed = initial + transform;
EXPECT_NEAR(transformed.Translation().X(), 1 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Translation().Y(), 2 + 5 / std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(transformed.Rotation().Degrees(), 50.0, kEpsilon);
}
TEST(Pose2dTest, RelativeTo) {
const Pose2d initial{0.0, 0.0, Rotation2d::FromDegrees(45.0)};
const Pose2d final{5.0, 5.0, Rotation2d::FromDegrees(45.0)};
const auto finalRelativeToInitial = final.RelativeTo(initial);
EXPECT_NEAR(finalRelativeToInitial.Translation().X(), 5.0 * std::sqrt(2.0),
kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Translation().Y(), 0.0, kEpsilon);
EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees(), 0.0, kEpsilon);
}