Merge "Ignore camera-to-robot pitch/roll in y2020 localizer"
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) {