blob: 845dda78e0d81d136d26a735c2d59b0d280a2977 [file] [log] [blame]
James Kuszmaul9f9676d2019-01-25 21:27:58 -08001#include "frc971/control_loops/pose.h"
2
3#include "gtest/gtest.h"
4
5namespace frc971 {
6namespace control_loops {
7namespace testing {
8
9// Test that basic accessors on an individual Pose object work as expected.
10TEST(PoseTest, BasicPoseTest) {
11 // Provide a basic Pose with non-zero components for everything.
12 Pose pose({1, 1, 0.5}, 0.5);
13 // The xy_norm should just be based on the x/y positions, not the Z; hence
14 // sqrt(2) rather than sqrt(1^2 + 1^2 + 0.5^2).
15 EXPECT_DOUBLE_EQ(::std::sqrt(2.0), pose.xy_norm());
16 // Similarly, heading should just be atan2(y, x).
17 EXPECT_DOUBLE_EQ(M_PI / 4.0, pose.heading());
18 // Global and relative poses should be the same since we did not construct
19 // this off of a separate Pose.
20 EXPECT_EQ(1.0, pose.rel_pos().x());
21 EXPECT_EQ(1.0, pose.rel_pos().y());
22 EXPECT_EQ(0.5, pose.rel_pos().z());
23
24 EXPECT_EQ(1.0, pose.abs_pos().x());
25 EXPECT_EQ(1.0, pose.abs_pos().y());
James Kuszmaul090563a2019-02-09 14:43:20 -080026 EXPECT_EQ(1.0, pose.abs_xy().x());
27 EXPECT_EQ(1.0, pose.abs_xy().y());
James Kuszmaul9f9676d2019-01-25 21:27:58 -080028 EXPECT_EQ(0.5, pose.abs_pos().z());
29
30 EXPECT_EQ(0.5, pose.rel_theta());
31 EXPECT_EQ(0.5, pose.abs_theta());
32
33 pose.set_theta(3.14);
34 EXPECT_EQ(3.14, pose.rel_theta());
35 pose.mutable_pos()->x() = 9.71;
36 EXPECT_EQ(9.71, pose.rel_pos().x());
James Kuszmaul090563a2019-02-09 14:43:20 -080037
38 EXPECT_EQ(nullptr, pose.base());
39 Pose new_base;
40 pose.set_base(&new_base);
41 EXPECT_EQ(&new_base, pose.base());
James Kuszmaul9f9676d2019-01-25 21:27:58 -080042}
43
44// Check that Poses behave as expected when constructed relative to another
45// POse.
46TEST(PoseTest, BaseTest) {
47 // Tolerance for the EXPECT_NEARs. Because we are doing enough trig operations
48 // under the hood we actually start to lose some precision.
49 constexpr double kEps = 1e-15;
50 // The points we will construct have absolute positions at:
51 // base1: (1, 1)
52 // base2: (-1, 1)
53 // rel1: (0, 2)
54 // Where rel1 is expressed as compared to base1, noting that because base1
55 // has a yaw of M_PI, the position of rel1 compared to base1 is (1, -1)
56 // rather than (-1, 1).
57 Pose base1({1, 1, 0}, M_PI);
58 Pose base2({-1, 1, 0}, -M_PI / 2.0);
59 Pose rel1(&base1, {1, -1, 0}, 0.0);
60 EXPECT_NEAR(0.0, rel1.abs_pos().x(), kEps);
61 EXPECT_NEAR(2.0, rel1.abs_pos().y(), kEps);
62 EXPECT_NEAR(M_PI, rel1.abs_theta(), kEps);
63 // Check that, when rebasing to base2, the absolute position does not change
64 // and the relative POse changes to be relative to base2.
65 Pose rel2 = rel1.Rebase(&base2);
66 EXPECT_NEAR(rel1.abs_pos().x(), rel2.abs_pos().x(), kEps);
67 EXPECT_NEAR(rel1.abs_pos().y(), rel2.abs_pos().y(), kEps);
68 EXPECT_NEAR(rel1.abs_pos().z(), rel2.abs_pos().z(), kEps);
69 EXPECT_NEAR(rel1.abs_theta(), rel2.abs_theta(), kEps);
70 EXPECT_NEAR(-1.0, rel2.rel_pos().x(), kEps);
71 EXPECT_NEAR(1.0, rel2.rel_pos().y(), kEps);
72 EXPECT_NEAR(-M_PI / 2.0, rel2.rel_theta(), kEps);
73 // Check that rebasing onto nullptr results in a Pose based in the global
74 // frame.
75 Pose abs = rel1.Rebase(nullptr);
76 EXPECT_NEAR(rel1.abs_pos().x(), abs.abs_pos().x(), kEps);
77 EXPECT_NEAR(rel1.abs_pos().y(), abs.abs_pos().y(), kEps);
78 EXPECT_NEAR(rel1.abs_pos().z(), abs.abs_pos().z(), kEps);
79 EXPECT_NEAR(rel1.abs_theta(), abs.abs_theta(), kEps);
80 EXPECT_NEAR(rel1.abs_pos().x(), abs.rel_pos().x(), kEps);
81 EXPECT_NEAR(rel1.abs_pos().y(), abs.rel_pos().y(), kEps);
82 EXPECT_NEAR(rel1.abs_pos().z(), abs.rel_pos().z(), kEps);
83 EXPECT_NEAR(rel1.abs_theta(), abs.rel_theta(), kEps);
84}
85
James Kuszmaul3ca28612020-02-15 17:52:27 -080086// Tests that we can go between transformation matrices and Pose objects.
87TEST(PoseTest, TransformationMatrixTest) {
88 // First, sanity check the basic case.
89 Pose pose({0, 0, 0}, 0);
90 typedef Eigen::Matrix<double, 4, 4> TransformationMatrix;
91 ASSERT_EQ(TransformationMatrix::Identity(), pose.AsTransformationMatrix());
92 Pose reproduced_pose(pose.AsTransformationMatrix());
93 ASSERT_EQ(reproduced_pose.rel_pos(), pose.rel_pos());
94 ASSERT_EQ(reproduced_pose.rel_theta(), pose.rel_theta());
95 // Check a basic case of rotation + translation.
96 *pose.mutable_pos() << 1, 2, 3;
97 pose.set_theta(M_PI_2);
98 TransformationMatrix expected;
99 expected << 0, -1, 0, 1, 1, 0, 0, 2, 0, 0, 1, 3, 0, 0, 0, 1;
Philipp Schrader790cb542023-07-05 21:06:52 -0700100 TransformationMatrix pose_transformation = pose.AsTransformationMatrix();
James Kuszmaul3ca28612020-02-15 17:52:27 -0800101 ASSERT_LT((expected - pose_transformation).norm(), 1e-15)
102 << "expected:\n"
103 << expected << "\nBut got:\n"
104 << pose_transformation;
105 ASSERT_EQ(Eigen::Vector4d(1, 2, 3, 1),
106 pose_transformation * Eigen::Vector4d(0, 0, 0, 1));
107 ASSERT_LT((Eigen::Vector4d(0, 3, 3, 1) -
108 pose_transformation * Eigen::Vector4d(1, 1, 0, 1))
109 .norm(),
110 1e-15)
111 << "got " << pose_transformation * Eigen::Vector4d(1, 1, 0, 1);
112
113 // Also, confirm that setting a new base does not affect the pose.
114 Pose faux_base({1, 1, 1}, 1);
115 pose.set_base(&faux_base);
116
117 ASSERT_EQ(pose_transformation, pose.AsTransformationMatrix());
118
119 reproduced_pose = Pose(pose_transformation);
120 ASSERT_EQ(reproduced_pose.rel_pos(), pose.rel_pos());
121 ASSERT_EQ(reproduced_pose.rel_theta(), pose.rel_theta());
122 // And check that if we introduce a pitch to the transformation matrix that it
123 // does not impact the resulting Pose (which only has a yaw component).
124 pose_transformation.block<3, 3>(0, 0) =
125 Eigen::AngleAxis<double>(0.5, Eigen::Vector3d::UnitX()) *
126 pose_transformation.block<3, 3>(0, 0);
127 reproduced_pose = Pose(pose_transformation);
128 ASSERT_EQ(reproduced_pose.rel_pos(), pose.rel_pos());
129 ASSERT_EQ(reproduced_pose.rel_theta(), pose.rel_theta());
130}
131
James Kuszmaul090563a2019-02-09 14:43:20 -0800132// Tests that basic accessors for LineSegment behave as expected.
133TEST(LineSegmentTest, BasicAccessorTest) {
134 LineSegment l;
135 EXPECT_EQ(0.0, l.pose1().rel_theta());
136 l.mutable_pose1()->set_theta(1.234);
137 EXPECT_EQ(1.234, l.pose1().rel_theta());
138 EXPECT_EQ(0.0, l.pose2().rel_theta());
139 l.mutable_pose2()->set_theta(5.678);
140 EXPECT_EQ(5.678, l.pose2().rel_theta());
141
142 const ::std::vector<Pose> plot_pts = l.PlotPoints();
143 ASSERT_EQ(2u, plot_pts.size());
144 EXPECT_EQ(l.pose1().rel_theta(), plot_pts[0].rel_theta());
145 EXPECT_EQ(l.pose2().rel_theta(), plot_pts[1].rel_theta());
146}
147
James Kuszmaul9f9676d2019-01-25 21:27:58 -0800148// Tests that basic checks for intersection function as expected.
149TEST(LineSegmentTest, TrivialIntersectTest) {
150 Pose p1({0, 0, 0}, 0.0), p2({2, 0, 0}, 0.0);
151 // A line segment from (0, 0) to (0, 2).
152 LineSegment l1(p1, p2);
153 Pose q1({1, -1, 0}, 0.0), q2({1, 1, 0}, 0.0);
154 // A line segment from (1, -1) to (1, 1).
155 LineSegment l2(q1, q2);
156 // The two line segments should intersect.
157 EXPECT_TRUE(l1.Intersects(l2));
158 EXPECT_TRUE(l2.Intersects(l1));
159
160 // If we switch around the orderings such that the line segments are
161 // (0, 0) -> (1, -1) and (2, 0)->(1, 1) then the line segments do not
162 // intersect.
163 LineSegment l3(p1, q1);
164 LineSegment l4(p2, q2);
165 EXPECT_FALSE(l3.Intersects(l4));
166 EXPECT_FALSE(l4.Intersects(l3));
167}
168
169// Check that when we construct line segments that are collinear, both with
170// overlapping bits and without overlapping bits, they register as not
171// intersecting.
172// We may want this behavior to change in the future, but for now check for
173// consistency.
174TEST(LineSegmentTest, CollinearIntersectTest) {
175 Pose p1({0, 0, 0}, 0.0), p2({1, 0, 0}, 0.0), p3({2, 0, 0}, 0.0),
176 p4({3, 0, 0}, 0.0);
177 // These two line segments overlap and are collinear, one going from 0 to 2
178 // and the other from 1 to 3 on the X-axis.
179 LineSegment l1(p1, p3);
180 LineSegment l2(p2, p4);
181 EXPECT_FALSE(l1.Intersects(l2));
182 EXPECT_FALSE(l2.Intersects(l1));
183
184 // These two line segments do not overlap and are collinear, one going from 0
185 // to 1 and the other from 2 to 3 on the X-axis.
186 LineSegment l3(p1, p2);
187 LineSegment l4(p3, p4);
188 EXPECT_FALSE(l3.Intersects(l4));
189 EXPECT_FALSE(l4.Intersects(l3));
190
191 // Test when one line segment is completely contained within the other.
192 LineSegment l5(p1, p4);
193 LineSegment l6(p3, p2);
194 EXPECT_FALSE(l5.Intersects(l6));
195 EXPECT_FALSE(l6.Intersects(l5));
196}
197
198} // namespace testing
199} // namespace control_loops
200} // namespace frc971