Add more functionality to year-agnostic localization utils
* Allow retrieving encoder values from the roborio.
* Move a utility for calculating some H matrices from y2019
into the year-agnostic code.
Change-Id: I2ef0e391fc615ccceeb2b0fc035233bb995d6e1e
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index cfd443a..e2985af 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -5,6 +5,9 @@
#include "aos/events/event_loop.h"
#include "aos/network/message_bridge_server_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+#include "frc971/control_loops/pose.h"
#include "frc971/input/joystick_state_generated.h"
#include "frc971/vision/calibration_generated.h"
@@ -16,6 +19,7 @@
// has timed out.
// * Offsets between monotonic clocks on different devices.
// * Whether we are in autonomous mode.
+// * Drivetrain encoder voltages, as reported by the roborio.
class LocalizationUtils {
public:
LocalizationUtils(aos::EventLoop *event_loop);
@@ -26,6 +30,11 @@
// [left_voltage, right_voltage]
Eigen::Vector2d VoltageOrZero(aos::monotonic_clock::time_point now);
+ // Returns the latest drivetrain encoder values (in meters), or nullopt if no
+ // position message is available (or if the message is stale).
+ // Returns encoders as [left_position, right_position]
+ std::optional<Eigen::Vector2d> Encoders(aos::monotonic_clock::time_point now);
+
// Returns true if either there is no JoystickState message available or if
// we are currently in autonomous mode.
bool MaybeInAutonomous();
@@ -39,6 +48,7 @@
private:
aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Position> position_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
};
@@ -47,6 +57,63 @@
Eigen::Matrix<double, 4, 4> FlatbufferToTransformationMatrix(
const frc971::vision::calibration::TransformationMatrix &flatbuffer);
+// This approximates the Jacobian of a vector of [heading, distance, skew]
+// of a target with respect to the full state of a drivetrain EKF.
+// Note that the only nonzero values in the returned matrix will be in the
+// columns corresponding to the X, Y, and Theta components of the state.
+// This is suitable for use as the H matrix in the kalman updates of the EKF,
+// although due to the approximation it should not be used to actually
+// calculate the expected measurement.
+// target_pose is the global pose of the target that we have identified.
+// camera_pose is the current estimate of the global pose of
+// the camera that can see the target.
+template <typename Scalar>
+Eigen::Matrix<double, 3, HybridEkf<Scalar>::kNStates>
+HMatrixForCameraHeadingDistanceSkew(const TypedPose<Scalar> &target_pose,
+ const TypedPose<Scalar> &camera_pose) {
+ // For all of the below calculations, we will assume to a first
+ // approximation that:
+ //
+ // dcamera_theta / dtheta ~= 1
+ // dcamera_x / dx ~= 1
+ // dcamera_y / dy ~= 1
+ //
+ // For cameras sufficiently far from the robot's origin, or if the robot were
+ // spinning extremely rapidly, this would not hold.
+
+ // To calculate dheading/d{x,y,theta}:
+ // heading = arctan2(target_pos - camera_pos) - camera_theta
+ Eigen::Matrix<Scalar, 3, 1> target_pos = target_pose.abs_pos();
+ Eigen::Matrix<Scalar, 3, 1> camera_pos = camera_pose.abs_pos();
+ Scalar diffx = target_pos.x() - camera_pos.x();
+ Scalar diffy = target_pos.y() - camera_pos.y();
+ Scalar norm2 = diffx * diffx + diffy * diffy;
+ Scalar dheadingdx = diffy / norm2;
+ Scalar dheadingdy = -diffx / norm2;
+ Scalar dheadingdtheta = -1.0;
+
+ // To calculate ddistance/d{x,y}:
+ // distance = sqrt(diffx^2 + diffy^2)
+ Scalar distance = ::std::sqrt(norm2);
+ Scalar ddistdx = -diffx / distance;
+ Scalar ddistdy = -diffy / distance;
+
+ // Skew = target.theta - camera.theta - heading
+ // = target.theta - arctan2(target_pos - camera_pos)
+ Scalar dskewdx = -dheadingdx;
+ Scalar dskewdy = -dheadingdy;
+ Eigen::Matrix<Scalar, 3, HybridEkf<Scalar>::kNStates> H;
+ H.setZero();
+ H(0, HybridEkf<Scalar>::kX) = dheadingdx;
+ H(0, HybridEkf<Scalar>::kY) = dheadingdy;
+ H(0, HybridEkf<Scalar>::kTheta) = dheadingdtheta;
+ H(1, HybridEkf<Scalar>::kX) = ddistdx;
+ H(1, HybridEkf<Scalar>::kY) = ddistdy;
+ H(2, HybridEkf<Scalar>::kX) = dskewdx;
+ H(2, HybridEkf<Scalar>::kY) = dskewdy;
+ return H;
+}
+
} // namespace frc971::control_loops::drivetrain
#endif // FRC971_CONTROL_LOOPS_DRIVETRAIN_LOCALIZATION_UTILS_H_