Add Pose object for handling transformations.
Change-Id: Ie09a21b80f1d4da74294b50dc4f8ce1691605737
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_