Ignore camera-to-robot pitch/roll in y2020 localizer
Make it so that if camera pitch is significantly off, the lever arm from
the camera to the robot origin does not cause an offset in our position
estimates.
Also, add the superstructure to the drivetrain replay.
Change-Id: I7701e7f6a7cfc93045f76033effcd916e0c263df
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 01dd1a3..38e00da 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -190,6 +190,7 @@
"//aos/events/logging:log_writer",
"//frc971/control_loops/drivetrain:drivetrain_lib",
"//frc971/control_loops/drivetrain:trajectory_generator",
+ "//y2020/control_loops/superstructure:superstructure_lib",
"@com_github_gflags_gflags//:gflags",
"@com_github_google_glog//:glog",
],
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index fc50556..8f6ff63 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -5,8 +5,8 @@
// in some other way. The original drivetrain status data will be on the
// /original/drivetrain channel.
#include "aos/configuration.h"
-#include "aos/events/logging/log_writer.h"
#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/init.h"
#include "aos/json_to_flatbuffer.h"
@@ -16,6 +16,7 @@
#include "gflags/gflags.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
#include "y2020/control_loops/drivetrain/localizer.h"
+#include "y2020/control_loops/superstructure/superstructure.h"
DEFINE_string(config, "y2020/config.json",
"Name of the config file to replay using.");
@@ -74,6 +75,10 @@
"frc971.control_loops.drivetrain.Status");
reader.RemapLoggedChannel("/drivetrain",
"frc971.control_loops.drivetrain.Output");
+ reader.RemapLoggedChannel("/superstructure",
+ "y2020.control_loops.superstructure.Status");
+ reader.RemapLoggedChannel("/superstructure",
+ "y2020.control_loops.superstructure.Output");
reader.Register();
std::vector<std::unique_ptr<LoggerState>> loggers;
@@ -117,6 +122,13 @@
y2020::control_loops::drivetrain::GetDrivetrainConfig(),
drivetrain_event_loop.get(), &localizer);
+ std::unique_ptr<aos::EventLoop> superstructure_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("superstructure", node);
+ superstructure_event_loop->SkipTimingReport();
+
+ y2020::control_loops::superstructure::Superstructure superstructure(
+ superstructure_event_loop.get());
+
reader.event_loop_factory()->Run();
return 0;
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 1cab511..5169a64 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -227,8 +227,15 @@
// reading. Note that the Pose object ignores any roll/pitch components, so
// if the camera's extrinsics for pitch/roll are off, this should just
// ignore it.
- const Pose measured_pose(H_field_target *
- (H_robot_camera * H_camera_target).inverse());
+ const Pose measured_camera_pose(H_field_target * H_camera_target.inverse());
+ // Calculate the camera-to-robot transformation matrix ignoring the
+ // pitch/roll of the camera.
+ // TODO(james): This could probably be made a bit more efficient, but I
+ // don't think this is anywhere near our bottleneck currently.
+ const Eigen::Matrix<float, 4, 4> H_camera_robot_stripped =
+ Pose(H_robot_camera).AsTransformationMatrix().inverse();
+ const Pose measured_pose(measured_camera_pose.AsTransformationMatrix() *
+ H_camera_robot_stripped);
// This "Z" is the robot pose directly implied by the camera results.
// Currently, we do not actually use this result directly. However, it is
// kept around in case we want to quickly re-enable it.
diff --git a/y2020/control_loops/drivetrain/localizer_test.cc b/y2020/control_loops/drivetrain/localizer_test.cc
index a3e26b5..ab3b075 100644
--- a/y2020/control_loops/drivetrain/localizer_test.cc
+++ b/y2020/control_loops/drivetrain/localizer_test.cc
@@ -286,11 +286,11 @@
// TODO(james): Use non-zero turret angles.
camera_target->camera_to_target.reset(new TransformationMatrixT());
- camera_target->camera_to_target->data =
- MatrixToVector((robot_pose.AsTransformationMatrix() *
- TurretRobotTransformation() * H_turret_camera)
- .inverse() *
- H_field_target);
+ camera_target->camera_to_target->data = MatrixToVector(
+ (robot_pose.AsTransformationMatrix() * TurretRobotTransformation() *
+ H_turret_camera * camera_calibration_offset_)
+ .inverse() *
+ H_field_target);
frame->camera_poses.emplace_back(std::move(camera_target));
}
@@ -366,6 +366,11 @@
std::unique_ptr<ImageMatchResultT>>>
camera_delay_queue_;
+ // Offset to add to the camera for actually taking the images, to simulate an
+ // inaccurate calibration.
+ Eigen::Matrix<double, 4, 4> camera_calibration_offset_ =
+ Eigen::Matrix<double, 4, 4>::Identity();
+
void set_enable_cameras(bool enable) { enable_cameras_ = enable; }
void set_camera_is_turreted(bool turreted) { is_turreted_ = turreted; }
@@ -453,6 +458,24 @@
EXPECT_TRUE(VerifyEstimatorAccurate(2e-3));
}
+// Tests that camera updates with a perfect model but incorrect camera pitch
+// results in no errors.
+TEST_F(LocalizedDrivetrainTest, PerfectCameraUpdateWithBadPitch) {
+ // Introduce some camera pitch.
+ camera_calibration_offset_.template block<3, 3>(0, 0) =
+ Eigen::AngleAxis<double>(0.1, Eigen::Vector3d::UnitY())
+ .toRotationMatrix();
+ SetEnabled(true);
+ set_enable_cameras(true);
+ set_camera_is_turreted(true);
+
+ SendGoal(-1.0, 1.0);
+
+ RunFor(chrono::seconds(3));
+ VerifyNearGoal();
+ EXPECT_TRUE(VerifyEstimatorAccurate(2e-3));
+}
+
// Tests that camera updates with a constant initial error in the position
// results in convergence.
TEST_F(LocalizedDrivetrainTest, InitialPositionError) {