Add Pose object for handling transformations.
Change-Id: Ie09a21b80f1d4da74294b50dc4f8ce1691605737
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index d46bca6..d12d3da 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -30,6 +30,24 @@
)
cc_library(
+ name = "pose",
+ hdrs = ["pose.h"],
+ deps = [
+ "//aos/util:math",
+ "//third_party/eigen",
+ ],
+)
+
+cc_test(
+ name = "pose_test",
+ srcs = ["pose_test.cc"],
+ deps = [
+ ":pose",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
name = "hall_effect_tracker",
hdrs = [
"hall_effect_tracker.h",
diff --git a/frc971/control_loops/pose.h b/frc971/control_loops/pose.h
new file mode 100644
index 0000000..508a6a6
--- /dev/null
+++ b/frc971/control_loops/pose.h
@@ -0,0 +1,175 @@
+#ifndef FRC971_CONTROL_LOOPS_POSE_H_
+#define FRC971_CONTROL_LOOPS_POSE_H_
+
+#include "Eigen/Dense"
+#include "aos/util/math.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// Provides a representation of a transformation on the field.
+// Currently, this is heavily geared towards things that occur in a 2-D plane.
+// The Z-axis is rarely used (but still relevant; e.g., in 2019 some of the
+// targets are at a different height).
+// For rotations, we currently just represent the yaw axis (the rotation about
+// the Z-axis).
+// As a convention, we use right-handed coordinate systems; the positive Z
+// axis will go up on the field, the positive X axis shall be "forwards" for
+// some relevant meaning of forwards, and the origin shall be chosen as
+// appropriate.
+// For 2019, at least, the global origin will be on the ground at the center
+// of the driver's station wall of your current alliance and the positive X-axis
+// will point straight into the field from the driver's station.
+// In future years this may need to change if the field's symmetry changes and
+// we can't interchangeably handle which side of the field we are on.
+// This means that if we had a Pose for the center of mass of the robot with a
+// position of (10, -5, 0) and a yaw of pi / 2, that suggests the robot is
+// facing straight to the left from the driver's perspective and is placed 10m
+// from the driver's station wall and 5m to the right of the center of the wall.
+//
+// Furthermore, Poses can be chained such that a Pose can be placed relative to
+// another Pose; the other Pose can dynamically update, thus allowing us to,
+// e.g., provide a Pose for a camera that is relative to the Pose of the robot.
+// Poses can also be in the global frame with no parent Pose.
+template <typename Scalar = double>
+class TypedPose {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ // The type that contains the translational (x, y, z) component of the Pose.
+ typedef Eigen::Matrix<Scalar, 3, 1> Pos;
+
+ // Construct a Pose in the absolute frame with a particular position and yaw.
+ TypedPose(const Pos &abs_pos, Scalar theta) : pos_(abs_pos), theta_(theta) {}
+ // Construct a Pose relative to another Pose (base).
+ // If you provide a base of nullptr, then this will
+ // construct a Pose in the global frame.
+ // Note that the lifetime of base should be greater than the lifetime of
+ // the object being constructed.
+ TypedPose(const TypedPose<Scalar> *base, const Pos &rel_pos, Scalar rel_theta)
+ : base_(base), pos_(rel_pos), theta_(rel_theta) {}
+
+ // Calculate the current global position of this Pose.
+ Pos abs_pos() const {
+ if (base_ == nullptr) {
+ return pos_;
+ }
+ Pos base_pos = base_->abs_pos();
+ Scalar base_theta = base_->abs_theta();
+ return base_pos + YawRotation(base_theta) * pos_;
+ }
+ // Calculate the absolute yaw of this Pose. Since we only have a single
+ // rotational axis, we can just sum the angle with that of the base Pose.
+ Scalar abs_theta() const {
+ if (base_ == nullptr) {
+ return theta_;
+ }
+ return aos::math::NormalizeAngle(theta_ + base_->abs_theta());
+ }
+ // Provide access to the position and yaw relative to the base Pose.
+ Pos rel_pos() const { return pos_; }
+ Scalar rel_theta() const { return theta_; }
+
+ Pos *mutable_pos() { return &pos_; }
+ void set_theta(Scalar theta) { theta_ = theta; }
+
+ // For 2-D calculation, provide the heading, which is distinct from the
+ // yaw/theta value. heading is the heading relative to the base Pose if you
+ // were to draw a line from the base to this Pose. i.e., if heading() is zero
+ // then you are directly in front of the base Pose.
+ Scalar heading() const { return ::std::atan2(pos_.y(), pos_.x()); }
+ // The 2-D distance from the base Pose to this Pose.
+ Scalar xy_norm() const { return pos_.template topRows<2>().norm(); }
+ // Return the absolute xy position.
+ Eigen::Matrix<Scalar, 2, 1> abs_xy() const {
+ return abs_pos().template topRows<2>();
+ }
+
+ // Provide a copy of this that is set to have the same
+ // current absolute Pose as this, but have a different base.
+ // This can be used, e.g., to compute a Pose for a vision target that is
+ // relative to the camera instead of relative to the field. You can then
+ // access the rel_* variables to get what the position of the target is
+ // relative to the robot/camera.
+ // If new_base == nullptr, provides a Pose referenced to the global frame.
+ // Note that the lifetime of new_base should be greater than the lifetime of
+ // the returned object (unless new_base == nullptr).
+ TypedPose Rebase(const TypedPose<Scalar> *new_base) const;
+
+ private:
+ // A rotation-matrix like representation of the rotation for a given angle.
+ inline static Eigen::AngleAxis<Scalar> YawRotation(double theta) {
+ return Eigen::AngleAxis<Scalar>(theta, Pos::UnitZ());
+ }
+
+ // A pointer to the base Pose. If uninitialized, then this Pose is in the
+ // global frame.
+ const TypedPose<Scalar> *base_ = nullptr;
+ // Position and yaw relative to base_.
+ Pos pos_;
+ Scalar theta_;
+}; // class TypedPose
+
+typedef TypedPose<double> Pose;
+
+template <typename Scalar>
+TypedPose<Scalar> TypedPose<Scalar>::Rebase(
+ const TypedPose<Scalar> *new_base) const {
+ if (new_base == nullptr) {
+ return TypedPose<Scalar>(nullptr, abs_pos(), abs_theta());
+ }
+ // Calculate the absolute position/yaws of this and of the new_base, and then
+ // calculate where we are relative to new_base, essentially reversing the
+ // calculation in abs_*.
+ Pos base_pos = new_base->abs_pos();
+ Scalar base_theta = new_base->abs_theta();
+ Pos self_pos = abs_pos();
+ Scalar self_theta = abs_theta();
+ Scalar diff_theta = ::aos::math::DiffAngle(self_theta, base_theta);
+ Pos diff_pos = YawRotation(-base_theta) * (self_pos - base_pos);
+ return TypedPose<Scalar>(new_base, diff_pos, diff_theta);
+}
+
+// Represents a 2D line segment constructed from a pair of Poses.
+// The line segment goes between the two Poses, but for calculating
+// intersections we use the 2D projection of the Poses onto the global X-Y
+// plane.
+template <typename Scalar = double>
+class TypedLineSegment {
+ public:
+ TypedLineSegment(const TypedPose<Scalar> &pose1,
+ const TypedPose<Scalar> &pose2)
+ : pose1_(pose1), pose2_(pose2) {}
+ // Detects if two line segments intersect.
+ // When at least one end of one line segment is collinear with the other,
+ // the line segments are treated as not intersecting.
+ bool Intersects(const TypedLineSegment<Scalar> &other) const {
+ // Source for algorithm:
+ // https://bryceboe.com/2006/10/23/line-segment-intersection-algorithm/
+ // Method:
+ // We will consider the four triangles that can be made out of any 3 points
+ // from the pair of line segments.
+ // Basically, if you consider one line segment the base of the triangle,
+ // then the two points of the other line segment should be on opposite
+ // sides of the first line segment (we use the PointsAreCCW function for
+ // this). This must hold when splitting off of both line segments.
+ Eigen::Matrix<Scalar, 2, 1> p1 = pose1_.abs_xy();
+ Eigen::Matrix<Scalar, 2, 1> p2 = pose2_.abs_xy();
+ Eigen::Matrix<Scalar, 2, 1> q1 = other.pose1_.abs_xy();
+ Eigen::Matrix<Scalar, 2, 1> q2 = other.pose2_.abs_xy();
+ return (::aos::math::PointsAreCCW<Scalar>(p1, q1, q2) !=
+ ::aos::math::PointsAreCCW<Scalar>(p2, q1, q2)) &&
+ (::aos::math::PointsAreCCW<Scalar>(p1, p2, q1) !=
+ ::aos::math::PointsAreCCW<Scalar>(p1, p2, q2));
+ }
+ private:
+ const TypedPose<Scalar> pose1_;
+ const TypedPose<Scalar> pose2_;
+}; // class TypedLineSegment
+
+typedef TypedLineSegment<double> LineSegment;
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_POSE_H_
diff --git a/frc971/control_loops/pose_test.cc b/frc971/control_loops/pose_test.cc
new file mode 100644
index 0000000..cb994a2
--- /dev/null
+++ b/frc971/control_loops/pose_test.cc
@@ -0,0 +1,131 @@
+#include "frc971/control_loops/pose.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+
+// Test that basic accessors on an individual Pose object work as expected.
+TEST(PoseTest, BasicPoseTest) {
+ // Provide a basic Pose with non-zero components for everything.
+ Pose pose({1, 1, 0.5}, 0.5);
+ // The xy_norm should just be based on the x/y positions, not the Z; hence
+ // sqrt(2) rather than sqrt(1^2 + 1^2 + 0.5^2).
+ EXPECT_DOUBLE_EQ(::std::sqrt(2.0), pose.xy_norm());
+ // Similarly, heading should just be atan2(y, x).
+ EXPECT_DOUBLE_EQ(M_PI / 4.0, pose.heading());
+ // Global and relative poses should be the same since we did not construct
+ // this off of a separate Pose.
+ EXPECT_EQ(1.0, pose.rel_pos().x());
+ EXPECT_EQ(1.0, pose.rel_pos().y());
+ EXPECT_EQ(0.5, pose.rel_pos().z());
+
+ EXPECT_EQ(1.0, pose.abs_pos().x());
+ EXPECT_EQ(1.0, pose.abs_pos().y());
+ EXPECT_EQ(0.5, pose.abs_pos().z());
+
+ EXPECT_EQ(0.5, pose.rel_theta());
+ EXPECT_EQ(0.5, pose.abs_theta());
+
+ pose.set_theta(3.14);
+ EXPECT_EQ(3.14, pose.rel_theta());
+ pose.mutable_pos()->x() = 9.71;
+ EXPECT_EQ(9.71, pose.rel_pos().x());
+}
+
+// Check that Poses behave as expected when constructed relative to another
+// POse.
+TEST(PoseTest, BaseTest) {
+ // Tolerance for the EXPECT_NEARs. Because we are doing enough trig operations
+ // under the hood we actually start to lose some precision.
+ constexpr double kEps = 1e-15;
+ // The points we will construct have absolute positions at:
+ // base1: (1, 1)
+ // base2: (-1, 1)
+ // rel1: (0, 2)
+ // Where rel1 is expressed as compared to base1, noting that because base1
+ // has a yaw of M_PI, the position of rel1 compared to base1 is (1, -1)
+ // rather than (-1, 1).
+ Pose base1({1, 1, 0}, M_PI);
+ Pose base2({-1, 1, 0}, -M_PI / 2.0);
+ Pose rel1(&base1, {1, -1, 0}, 0.0);
+ EXPECT_NEAR(0.0, rel1.abs_pos().x(), kEps);
+ EXPECT_NEAR(2.0, rel1.abs_pos().y(), kEps);
+ EXPECT_NEAR(M_PI, rel1.abs_theta(), kEps);
+ // Check that, when rebasing to base2, the absolute position does not change
+ // and the relative POse changes to be relative to base2.
+ Pose rel2 = rel1.Rebase(&base2);
+ EXPECT_NEAR(rel1.abs_pos().x(), rel2.abs_pos().x(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().y(), rel2.abs_pos().y(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().z(), rel2.abs_pos().z(), kEps);
+ EXPECT_NEAR(rel1.abs_theta(), rel2.abs_theta(), kEps);
+ EXPECT_NEAR(-1.0, rel2.rel_pos().x(), kEps);
+ EXPECT_NEAR(1.0, rel2.rel_pos().y(), kEps);
+ EXPECT_NEAR(-M_PI / 2.0, rel2.rel_theta(), kEps);
+ // Check that rebasing onto nullptr results in a Pose based in the global
+ // frame.
+ Pose abs = rel1.Rebase(nullptr);
+ EXPECT_NEAR(rel1.abs_pos().x(), abs.abs_pos().x(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().y(), abs.abs_pos().y(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().z(), abs.abs_pos().z(), kEps);
+ EXPECT_NEAR(rel1.abs_theta(), abs.abs_theta(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().x(), abs.rel_pos().x(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().y(), abs.rel_pos().y(), kEps);
+ EXPECT_NEAR(rel1.abs_pos().z(), abs.rel_pos().z(), kEps);
+ EXPECT_NEAR(rel1.abs_theta(), abs.rel_theta(), kEps);
+}
+
+// Tests that basic checks for intersection function as expected.
+TEST(LineSegmentTest, TrivialIntersectTest) {
+ Pose p1({0, 0, 0}, 0.0), p2({2, 0, 0}, 0.0);
+ // A line segment from (0, 0) to (0, 2).
+ LineSegment l1(p1, p2);
+ Pose q1({1, -1, 0}, 0.0), q2({1, 1, 0}, 0.0);
+ // A line segment from (1, -1) to (1, 1).
+ LineSegment l2(q1, q2);
+ // The two line segments should intersect.
+ EXPECT_TRUE(l1.Intersects(l2));
+ EXPECT_TRUE(l2.Intersects(l1));
+
+ // If we switch around the orderings such that the line segments are
+ // (0, 0) -> (1, -1) and (2, 0)->(1, 1) then the line segments do not
+ // intersect.
+ LineSegment l3(p1, q1);
+ LineSegment l4(p2, q2);
+ EXPECT_FALSE(l3.Intersects(l4));
+ EXPECT_FALSE(l4.Intersects(l3));
+}
+
+// Check that when we construct line segments that are collinear, both with
+// overlapping bits and without overlapping bits, they register as not
+// intersecting.
+// We may want this behavior to change in the future, but for now check for
+// consistency.
+TEST(LineSegmentTest, CollinearIntersectTest) {
+ Pose p1({0, 0, 0}, 0.0), p2({1, 0, 0}, 0.0), p3({2, 0, 0}, 0.0),
+ p4({3, 0, 0}, 0.0);
+ // These two line segments overlap and are collinear, one going from 0 to 2
+ // and the other from 1 to 3 on the X-axis.
+ LineSegment l1(p1, p3);
+ LineSegment l2(p2, p4);
+ EXPECT_FALSE(l1.Intersects(l2));
+ EXPECT_FALSE(l2.Intersects(l1));
+
+ // These two line segments do not overlap and are collinear, one going from 0
+ // to 1 and the other from 2 to 3 on the X-axis.
+ LineSegment l3(p1, p2);
+ LineSegment l4(p3, p4);
+ EXPECT_FALSE(l3.Intersects(l4));
+ EXPECT_FALSE(l4.Intersects(l3));
+
+ // Test when one line segment is completely contained within the other.
+ LineSegment l5(p1, p4);
+ LineSegment l6(p3, p2);
+ EXPECT_FALSE(l5.Intersects(l6));
+ EXPECT_FALSE(l6.Intersects(l5));
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace frc971