Removed Common
Change-Id: I01ea8f07220375c2ad9bc0092281d4f27c642303
diff --git a/aos/controls/BUILD b/aos/controls/BUILD
new file mode 100644
index 0000000..d529f04
--- /dev/null
+++ b/aos/controls/BUILD
@@ -0,0 +1,104 @@
+package(default_visibility = ["//visibility:public"])
+
+load("//aos/build:queues.bzl", "queue_library")
+load("//tools:environments.bzl", "mcu_cpus")
+
+cc_library(
+ name = "replay_control_loop",
+ hdrs = [
+ "replay_control_loop.h",
+ ],
+ deps = [
+ ":control_loop",
+ "//aos:queues",
+ "//aos/time:time",
+ "//aos/logging:queue_logging",
+ "//aos/logging:replay",
+ ],
+)
+
+cc_library(
+ name = "control_loop_test",
+ testonly = True,
+ srcs = [
+ "control_loop_test.cc",
+ ],
+ hdrs = [
+ "control_loop_test.h",
+ ],
+ deps = [
+ "//aos/time:time",
+ "//aos/logging:queue_logging",
+ "//aos/robot_state:robot_state",
+ "//aos/testing:googletest",
+ "//aos/testing:test_shm",
+ ],
+)
+
+cc_library(
+ name = "polytope_uc",
+ hdrs = [
+ "polytope.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ "//third_party/eigen",
+ ],
+)
+
+cc_library(
+ name = "polytope",
+ hdrs = [
+ "polytope.h",
+ ],
+ deps = [
+ "//aos/logging",
+ "//aos/logging:matrix_logging",
+ "//third_party/cddlib",
+ "//third_party/eigen",
+ ],
+)
+
+cc_test(
+ name = "polytope_test",
+ srcs = [
+ "polytope_test.cc",
+ ],
+ deps = [
+ ":polytope",
+ "//aos/testing:googletest",
+ "//aos/testing:test_logging",
+ "//third_party/eigen",
+ "//third_party/googletest:googlemock",
+ ],
+)
+
+queue_library(
+ name = "control_loop_queues",
+ srcs = [
+ "control_loops.q",
+ ],
+ compatible_with = [
+ "//tools:armhf-debian",
+ ],
+)
+
+cc_library(
+ name = "control_loop",
+ srcs = [
+ "control_loop.cc",
+ "control_loop-tmpl.h",
+ ],
+ hdrs = [
+ "control_loop.h",
+ ],
+ deps = [
+ ":control_loop_queues",
+ "//aos:queues",
+ "//aos/time:time",
+ "//aos/logging",
+ "//aos/logging:queue_logging",
+ "//aos/robot_state:robot_state",
+ "//aos/util:log_interval",
+ ],
+)
diff --git a/aos/controls/control_loop-tmpl.h b/aos/controls/control_loop-tmpl.h
new file mode 100644
index 0000000..1834728
--- /dev/null
+++ b/aos/controls/control_loop-tmpl.h
@@ -0,0 +1,126 @@
+#include <stddef.h>
+#include <inttypes.h>
+
+#include "aos/logging/logging.h"
+#include "aos/logging/queue_logging.h"
+#include "aos/robot_state/robot_state.q.h"
+
+namespace aos {
+namespace controls {
+
+// TODO(aschuh): Tests.
+
+template <class T>
+constexpr ::std::chrono::milliseconds ControlLoop<T>::kStaleLogInterval;
+template <class T>
+constexpr ::std::chrono::milliseconds ControlLoop<T>::kPwmDisableTime;
+
+template <class T>
+void ControlLoop<T>::ZeroOutputs() {
+ aos::ScopedMessagePtr<OutputType> output =
+ control_loop_->output.MakeMessage();
+ Zero(output.get());
+ output.Send();
+}
+
+template <class T>
+void ControlLoop<T>::Iterate() {
+ no_goal_.Print();
+ no_sensor_state_.Print();
+ motors_off_log_.Print();
+
+ control_loop_->position.FetchAnother();
+ const PositionType *const position = control_loop_->position.get();
+ LOG_STRUCT(DEBUG, "position", *position);
+
+ // Fetch the latest control loop goal. If there is no new
+ // goal, we will just reuse the old one.
+ control_loop_->goal.FetchLatest();
+ const GoalType *goal = control_loop_->goal.get();
+ if (goal) {
+ LOG_STRUCT(DEBUG, "goal", *goal);
+ } else {
+ LOG_INTERVAL(no_goal_);
+ }
+
+ const bool new_robot_state = ::aos::robot_state.FetchLatest();
+ if (!::aos::robot_state.get()) {
+ LOG_INTERVAL(no_sensor_state_);
+ return;
+ }
+ if (sensor_reader_pid_ != ::aos::robot_state->reader_pid) {
+ LOG(INFO, "new sensor reader PID %" PRId32 ", old was %" PRId32 "\n",
+ ::aos::robot_state->reader_pid, sensor_reader_pid_);
+ reset_ = true;
+ sensor_reader_pid_ = ::aos::robot_state->reader_pid;
+ }
+
+ bool outputs_enabled = ::aos::robot_state->outputs_enabled;
+
+ // Check to see if we got a driver station packet recently.
+ if (new_robot_state) {
+ if (::aos::robot_state->outputs_enabled) {
+ // If the driver's station reports being disabled, we're probably not
+ // actually going to send motor values regardless of what the FPGA
+ // reports.
+ last_pwm_sent_ = ::aos::robot_state->sent_time;
+ }
+ }
+
+ const ::aos::monotonic_clock::time_point now = ::aos::monotonic_clock::now();
+ const bool motors_off = now >= kPwmDisableTime + last_pwm_sent_;
+ ::aos::joystick_state.FetchLatest();
+ if (motors_off) {
+ if (::aos::joystick_state.get() && ::aos::joystick_state->enabled) {
+ LOG_INTERVAL(motors_off_log_);
+ }
+ outputs_enabled = false;
+ }
+
+ aos::ScopedMessagePtr<StatusType> status =
+ control_loop_->status.MakeMessage();
+ if (status.get() == nullptr) {
+ return;
+ }
+
+ if (outputs_enabled) {
+ aos::ScopedMessagePtr<OutputType> output =
+ control_loop_->output.MakeMessage();
+ RunIteration(goal, position, output.get(), status.get());
+
+ output->SetTimeToNow();
+ LOG_STRUCT(DEBUG, "output", *output);
+ output.Send();
+ } else {
+ // The outputs are disabled, so pass nullptr in for the output.
+ RunIteration(goal, position, nullptr, status.get());
+ ZeroOutputs();
+ }
+
+ status->SetTimeToNow();
+ LOG_STRUCT(DEBUG, "status", *status);
+ status.Send();
+}
+
+template <class T>
+void ControlLoop<T>::Run() {
+ struct sigaction action;
+ action.sa_handler = &ControlLoop<T>::Quit;
+ sigemptyset(&action.sa_mask);
+ action.sa_flags = SA_RESETHAND;
+
+ PCHECK(sigaction(SIGTERM, &action, nullptr));
+ PCHECK(sigaction(SIGQUIT, &action, nullptr));
+ PCHECK(sigaction(SIGINT, &action, nullptr));
+
+ while (run_) {
+ Iterate();
+ }
+ LOG(INFO, "Shutting down\n");
+}
+
+template <class T>
+::std::atomic<bool> ControlLoop<T>::run_{true};
+
+} // namespace controls
+} // namespace aos
diff --git a/aos/controls/control_loop.cc b/aos/controls/control_loop.cc
new file mode 100644
index 0000000..17e5d7e
--- /dev/null
+++ b/aos/controls/control_loop.cc
@@ -0,0 +1,7 @@
+#include "aos/controls/control_loop.h"
+
+namespace aos {
+namespace controls {
+
+} // namespace controls
+} // namespace aos
diff --git a/aos/controls/control_loop.h b/aos/controls/control_loop.h
new file mode 100644
index 0000000..fd63e3f
--- /dev/null
+++ b/aos/controls/control_loop.h
@@ -0,0 +1,177 @@
+#ifndef AOS_CONTROL_LOOP_CONTROL_LOOP_H_
+#define AOS_CONTROL_LOOP_CONTROL_LOOP_H_
+
+#include <string.h>
+#include <atomic>
+
+#include "aos/queue.h"
+#include "aos/time/time.h"
+#include "aos/type_traits/type_traits.h"
+#include "aos/util/log_interval.h"
+
+namespace aos {
+namespace controls {
+
+// Interface to describe runnable jobs.
+class Runnable {
+ public:
+ virtual ~Runnable() {}
+ // Runs forever.
+ virtual void Run() = 0;
+ // Does one quick piece of work and return. Does _not_ block.
+ virtual void Iterate() = 0;
+};
+
+class SerializableControlLoop : public Runnable {
+ public:
+ // Returns the size of all the data to be sent when serialized.
+ virtual size_t SeralizedSize() = 0;
+ // Serialize the current data.
+ virtual void Serialize(char *buffer) const = 0;
+ // Serialize zeroed data in case the data is out of date.
+ virtual void SerializeZeroMessage(char *buffer) const = 0;
+ // Deserialize data into the control loop.
+ virtual void Deserialize(const char *buffer) = 0;
+ // Unique identifier for the control loop.
+ // Most likely the hash of the queue group.
+ virtual uint32_t UniqueID() = 0;
+};
+
+// Control loops run this often, "starting" at time 0.
+constexpr ::std::chrono::nanoseconds kLoopFrequency =
+ ::std::chrono::milliseconds(5);
+
+// Provides helper methods to assist in writing control loops.
+// This template expects to be constructed with a queue group as an argument
+// that has a goal, position, status, and output queue.
+// It will then call the RunIteration method every cycle that it has enough
+// valid data for the control loop to run.
+template <class T>
+class ControlLoop : public SerializableControlLoop {
+ public:
+ // Create some convenient typedefs to reference the Goal, Position, Status,
+ // and Output structures.
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->goal.MakeMessage().get()))>::type
+ GoalType;
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->position.MakeMessage().get()))>::type
+ PositionType;
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->status.MakeMessage().get()))>::type
+ StatusType;
+ typedef typename std::remove_reference<
+ decltype(*(static_cast<T *>(NULL)->output.MakeMessage().get()))>::type
+ OutputType;
+
+ ControlLoop(T *control_loop) : control_loop_(control_loop) {}
+
+ // Returns true if all the counters etc in the sensor data have been reset.
+ // This will return true only a single time per reset.
+ bool WasReset() {
+ if (reset_) {
+ reset_ = false;
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ // Constructs and sends a message on the output queue which sets everything to
+ // a safe state (generally motors off). For some subclasses, this will be a
+ // bit different (ie pistons).
+ // The implementation here creates a new Output message, calls Zero() on it,
+ // and then sends it.
+ virtual void ZeroOutputs();
+
+ // Sets the output to zero.
+ // Over-ride if a value of zero is not "off" for this subsystem.
+ virtual void Zero(OutputType *output) { output->Zero(); }
+
+ // Runs the loop forever.
+ void Run() override;
+
+ // Runs one cycle of the loop.
+ void Iterate() override;
+
+ // Returns the name of the queue group.
+ const char *name() { return control_loop_->name(); }
+
+ // Methods to serialize all the data that should be sent over the network.
+ size_t SeralizedSize() override { return control_loop_->goal->Size(); }
+ void Serialize(char *buffer) const override {
+ control_loop_->goal->Serialize(buffer);
+ }
+ void SerializeZeroMessage(char *buffer) const override {
+ GoalType zero_goal;
+ zero_goal.Zero();
+ zero_goal.Serialize(buffer);
+ }
+
+ void Deserialize(const char *buffer) override {
+ ScopedMessagePtr<GoalType> new_msg = control_loop_->goal.MakeMessage();
+ new_msg->Deserialize(buffer);
+ new_msg.Send();
+ }
+
+ uint32_t UniqueID() override { return control_loop_->hash(); }
+
+
+ protected:
+ static void Quit(int /*signum*/) {
+ run_ = false;
+ }
+
+ // Runs an iteration of the control loop.
+ // goal is the last goal that was sent. It might be any number of cycles old
+ // or nullptr if we haven't ever received a goal.
+ // position is the current position, or nullptr if we didn't get a position
+ // this cycle.
+ // output is the values to be sent to the motors. This is nullptr if the
+ // output is going to be ignored and set to 0.
+ // status is the status of the control loop.
+ // Both output and status should be filled in by the implementation.
+ virtual void RunIteration(const GoalType *goal,
+ const PositionType *position,
+ OutputType *output,
+ StatusType *status) = 0;
+
+ T *queue_group() { return control_loop_; }
+ const T *queue_group() const { return control_loop_; }
+
+ private:
+ static constexpr ::std::chrono::milliseconds kStaleLogInterval =
+ ::std::chrono::milliseconds(100);
+ // The amount of time after the last PWM pulse we consider motors enabled for.
+ // 100ms is the result of using an oscilliscope to look at the input and
+ // output of a Talon. The Info Sheet also lists 100ms for Talon SR, Talon SRX,
+ // and Victor SP.
+ static constexpr ::std::chrono::milliseconds kPwmDisableTime =
+ ::std::chrono::milliseconds(100);
+
+ // Pointer to the queue group
+ T *control_loop_;
+
+ bool reset_ = false;
+ int32_t sensor_reader_pid_ = 0;
+
+ ::aos::monotonic_clock::time_point last_pwm_sent_ =
+ ::aos::monotonic_clock::min_time;
+
+ typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
+ SimpleLogInterval no_sensor_state_ =
+ SimpleLogInterval(kStaleLogInterval, ERROR, "no sensor state");
+ SimpleLogInterval motors_off_log_ =
+ SimpleLogInterval(kStaleLogInterval, WARNING, "motors disabled");
+ SimpleLogInterval no_goal_ =
+ SimpleLogInterval(kStaleLogInterval, ERROR, "no goal");
+
+ static ::std::atomic<bool> run_;
+};
+
+} // namespace controls
+} // namespace aos
+
+#include "aos/controls/control_loop-tmpl.h" // IWYU pragma: export
+
+#endif
diff --git a/aos/controls/control_loop_test.cc b/aos/controls/control_loop_test.cc
new file mode 100644
index 0000000..61c2fe0
--- /dev/null
+++ b/aos/controls/control_loop_test.cc
@@ -0,0 +1,64 @@
+#include "aos/controls/control_loop_test.h"
+
+#include "aos/robot_state/robot_state.q.h"
+#include "aos/logging/queue_logging.h"
+
+namespace aos {
+namespace testing {
+
+constexpr ::std::chrono::milliseconds ControlLoopTest::kTimeTick;
+constexpr ::std::chrono::milliseconds ControlLoopTest::kDSPacketTime;
+
+ControlLoopTest::ControlLoopTest() {
+ ::aos::joystick_state.Clear();
+ ::aos::robot_state.Clear();
+
+ ::aos::time::EnableMockTime(current_time_);
+
+ SendMessages(false);
+}
+
+ControlLoopTest::~ControlLoopTest() {
+ ::aos::joystick_state.Clear();
+ ::aos::robot_state.Clear();
+
+ ::aos::time::DisableMockTime();
+}
+
+void ControlLoopTest::SendMessages(bool enabled) {
+ if (current_time_ >= kDSPacketTime + last_ds_time_ ||
+ last_enabled_ != enabled) {
+ last_ds_time_ = current_time_;
+ auto new_state = ::aos::joystick_state.MakeMessage();
+ new_state->fake = true;
+
+ new_state->enabled = enabled;
+ new_state->autonomous = false;
+ new_state->team_id = team_id_;
+
+ new_state.Send();
+ last_enabled_ = enabled;
+ }
+
+ {
+ auto new_state = ::aos::robot_state.MakeMessage();
+
+ new_state->reader_pid = reader_pid_;
+ new_state->outputs_enabled = enabled;
+ new_state->browned_out = false;
+
+ new_state->is_3v3_active = true;
+ new_state->is_5v_active = true;
+ new_state->voltage_3v3 = 3.3;
+ new_state->voltage_5v = 5.0;
+
+ new_state->voltage_roborio_in = battery_voltage_;
+ new_state->voltage_battery = battery_voltage_;
+
+ LOG_STRUCT(INFO, "robot_state", *new_state);
+ new_state.Send();
+ }
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/controls/control_loop_test.h b/aos/controls/control_loop_test.h
new file mode 100644
index 0000000..1633daf
--- /dev/null
+++ b/aos/controls/control_loop_test.h
@@ -0,0 +1,70 @@
+#ifndef AOS_CONTROLS_CONTROL_LOOP_TEST_H_
+#define AOS_CONTROLS_CONTROL_LOOP_TEST_H_
+
+#include "gtest/gtest.h"
+
+#include "aos/testing/test_shm.h"
+#include "aos/time/time.h"
+
+namespace aos {
+namespace testing {
+
+// Handles setting up the environment that all control loops need to actually
+// run.
+// This includes sending the queue messages and Clear()ing the queues when
+// appropriate.
+// It also includes dealing with ::aos::time.
+class ControlLoopTest : public ::testing::Test {
+ public:
+ ControlLoopTest();
+ virtual ~ControlLoopTest();
+
+ void set_team_id(uint16_t team_id) { team_id_ = team_id; }
+ uint16_t team_id() const { return team_id_; }
+
+ // Sends out all of the required queue messages.
+ void SendMessages(bool enabled);
+ // Ticks time for a single control loop cycle.
+ void TickTime(::std::chrono::nanoseconds dt = kTimeTick) {
+ ::aos::time::SetMockTime(current_time_ += dt);
+ }
+
+ // Simulates everything that happens during 1 loop time step.
+ void SimulateTimestep(bool enabled) {
+ SendMessages(enabled);
+ TickTime();
+ }
+
+ // Simulate a reset of the process reading sensors, which tells loops that all
+ // index counts etc will be reset.
+ void SimulateSensorReset() {
+ ++reader_pid_;
+ }
+
+ // Sets the battery voltage in robot_state.
+ void set_battery_voltage(double battery_voltage) {
+ battery_voltage_ = battery_voltage;
+ }
+
+ private:
+ static constexpr ::std::chrono::milliseconds kTimeTick{5};
+ static constexpr ::std::chrono::milliseconds kDSPacketTime{20};
+
+ uint16_t team_id_ = 971;
+ int32_t reader_pid_ = 1;
+ double battery_voltage_ = 12.4;
+
+ ::aos::monotonic_clock::time_point last_ds_time_ =
+ ::aos::monotonic_clock::epoch();
+ ::aos::monotonic_clock::time_point current_time_ =
+ ::aos::monotonic_clock::epoch();
+
+ ::aos::testing::TestSharedMemory my_shm_;
+
+ bool last_enabled_ = false;
+};
+
+} // namespace testing
+} // namespace aos
+
+#endif // AOS_CONTROLS_CONTROL_LOOP_TEST_H_
diff --git a/aos/controls/control_loops.q b/aos/controls/control_loops.q
new file mode 100644
index 0000000..2b0bfb1
--- /dev/null
+++ b/aos/controls/control_loops.q
@@ -0,0 +1,34 @@
+package aos.control_loops;
+
+interface ControlLoop {
+ queue goal;
+ queue position;
+ queue output;
+ queue status;
+};
+
+message Goal {
+ double goal;
+};
+
+message Position {
+ double position;
+};
+
+message Output {
+ double voltage;
+};
+
+message Status {
+ bool done;
+};
+
+// Single Input Single Output control loop.
+queue_group SISO {
+ implements ControlLoop;
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
diff --git a/aos/controls/polytope.h b/aos/controls/polytope.h
new file mode 100644
index 0000000..145b904
--- /dev/null
+++ b/aos/controls/polytope.h
@@ -0,0 +1,237 @@
+#ifndef AOS_CONTROLS_POLYTOPE_H_
+#define AOS_CONTROLS_POLYTOPE_H_
+
+#include "Eigen/Dense"
+
+#ifdef __linux__
+#include "third_party/cddlib/lib-src/setoper.h"
+#include "third_party/cddlib/lib-src/cdd.h"
+
+#include "aos/logging/logging.h"
+#include "aos/logging/matrix_logging.h"
+#endif // __linux__
+
+namespace aos {
+namespace controls {
+
+// A number_of_dimensions dimensional polytope.
+// This represents the region consisting of all points X such that H * X <= k.
+// The vertices are calculated at construction time because we always use those
+// and libcdd is annoying about calculating vertices. In particular, for some
+// random-seeming polytopes it refuses to calculate the vertices completely. To
+// avoid issues with that, using the "shifting" constructor is recommended
+// whenever possible.
+template <int number_of_dimensions, typename Scalar = double>
+class Polytope {
+ public:
+ virtual ~Polytope() {}
+
+ // Returns a reference to H.
+ virtual Eigen::Ref<
+ const Eigen::Matrix<Scalar, Eigen::Dynamic, number_of_dimensions>>
+ H() const = 0;
+
+ // Returns a reference to k.
+ virtual Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> k()
+ const = 0;
+
+ // Returns the number of dimensions in the polytope.
+ constexpr int ndim() const { return number_of_dimensions; }
+
+ // Returns the number of constraints currently in the polytope.
+ int num_constraints() const { return k().rows(); }
+
+ // Returns true if the point is inside the polytope.
+ bool IsInside(Eigen::Matrix<Scalar, number_of_dimensions, 1> point) const;
+
+ // Returns the list of vertices inside the polytope.
+ virtual Eigen::Matrix<Scalar, number_of_dimensions, Eigen::Dynamic> Vertices()
+ const = 0;
+};
+
+template <int number_of_dimensions, int num_vertices, typename Scalar = double>
+Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> ShiftPoints(
+ const Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> &vertices,
+ const Eigen::Matrix<Scalar, number_of_dimensions, 1> &offset) {
+ Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> ans = vertices;
+ for (int i = 0; i < num_vertices; ++i) {
+ ans.col(i) += offset;
+ }
+ return ans;
+}
+
+template <int number_of_dimensions, int num_constraints, int num_vertices,
+ typename Scalar = double>
+class HVPolytope : public Polytope<number_of_dimensions, Scalar> {
+ public:
+ // Constructs a polytope given the H and k matrices.
+ HVPolytope(Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints,
+ number_of_dimensions>> H,
+ Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, 1>> k,
+ Eigen::Ref<const Eigen::Matrix<Scalar, number_of_dimensions,
+ num_vertices>> vertices)
+ : H_(H), k_(k), vertices_(vertices) {}
+
+ Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, number_of_dimensions>>
+ static_H() const {
+ return H_;
+ }
+
+ Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, number_of_dimensions>>
+ H() const override {
+ return H_;
+ }
+
+ Eigen::Ref<const Eigen::Matrix<Scalar, num_constraints, 1>> static_k()
+ const {
+ return k_;
+ }
+ Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> k()
+ const override {
+ return k_;
+ }
+
+ Eigen::Matrix<Scalar, number_of_dimensions, Eigen::Dynamic> Vertices()
+ const override {
+ return vertices_;
+ }
+
+ Eigen::Matrix<Scalar, number_of_dimensions, num_vertices>
+ StaticVertices() const {
+ return vertices_;
+ }
+
+ private:
+ const Eigen::Matrix<Scalar, num_constraints, number_of_dimensions> H_;
+ const Eigen::Matrix<Scalar, num_constraints, 1> k_;
+ const Eigen::Matrix<Scalar, number_of_dimensions, num_vertices> vertices_;
+};
+
+
+
+#ifdef __linux__
+template <int number_of_dimensions>
+class HPolytope : public Polytope<number_of_dimensions> {
+ public:
+ // Constructs a polytope given the H and k matrices.
+ HPolytope(Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic,
+ number_of_dimensions>> H,
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k)
+ : H_(H), k_(k), vertices_(CalculateVertices(H, k)) {}
+
+ // This is an initialization function shared across all instantiations of this
+ // template.
+ // This must be called at least once before creating any instances. It is
+ // not thread-safe.
+ static void Init() { dd_set_global_constants(); }
+
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>>
+ H() const override {
+ return H_;
+ }
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> k()
+ const override {
+ return k_;
+ }
+
+ Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> Vertices()
+ const override {
+ return vertices_;
+ }
+
+ private:
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions> H_;
+ const Eigen::Matrix<double, Eigen::Dynamic, 1> k_;
+ const Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> vertices_;
+
+ static Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
+ CalculateVertices(
+ Eigen::Ref<
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> &H,
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> &k);
+};
+
+#endif // __linux__
+
+template <int number_of_dimensions, typename Scalar>
+bool Polytope<number_of_dimensions, Scalar>::IsInside(
+ Eigen::Matrix<Scalar, number_of_dimensions, 1> point) const {
+ Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> k_ptr = k();
+ for (int i = 0; i < k_ptr.rows(); ++i) {
+ Scalar ev = (H().row(i) * point)(0, 0);
+ if (ev > k_ptr(i, 0)) {
+ return false;
+ }
+ }
+ return true;
+}
+
+#ifdef __linux__
+template <int number_of_dimensions>
+Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic>
+HPolytope<number_of_dimensions>::CalculateVertices(
+ Eigen::Ref<
+ const Eigen::Matrix<double, Eigen::Dynamic, number_of_dimensions>> &H,
+ Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>> &k) {
+ dd_MatrixPtr matrix = dd_CreateMatrix(k.rows(), number_of_dimensions + 1);
+
+ // Copy the data over. TODO(aschuh): Is there a better way? I hate copying...
+ for (int i = 0; i < k.rows(); ++i) {
+ dd_set_d(matrix->matrix[i][0], k(i, 0));
+ for (int j = 0; j < number_of_dimensions; ++j) {
+ dd_set_d(matrix->matrix[i][j + 1], -H(i, j));
+ }
+ }
+
+ matrix->representation = dd_Inequality;
+ matrix->numbtype = dd_Real;
+
+ dd_ErrorType error;
+ dd_PolyhedraPtr polyhedra = dd_DDMatrix2Poly(matrix, &error);
+ if (error != dd_NoError || polyhedra == NULL) {
+ dd_WriteErrorMessages(stderr, error);
+ dd_FreeMatrix(matrix);
+ LOG_MATRIX(ERROR, "bad H", H);
+ LOG_MATRIX(ERROR, "bad k_", k);
+ LOG(FATAL, "dd_DDMatrix2Poly failed\n");
+ }
+
+ dd_MatrixPtr vertex_matrix = dd_CopyGenerators(polyhedra);
+
+ int num_vertices = 0;
+ int num_rays = 0;
+ for (int i = 0; i < vertex_matrix->rowsize; ++i) {
+ if (dd_get_d(vertex_matrix->matrix[i][0]) == 0) {
+ num_rays += 1;
+ } else {
+ num_vertices += 1;
+ }
+ }
+
+ // Rays are unsupported right now. This may change in the future.
+ CHECK_EQ(0, num_rays);
+
+ Eigen::Matrix<double, number_of_dimensions, Eigen::Dynamic> vertices(
+ number_of_dimensions, num_vertices);
+
+ int vertex_index = 0;
+ for (int i = 0; i < vertex_matrix->rowsize; ++i) {
+ if (dd_get_d(vertex_matrix->matrix[i][0]) != 0) {
+ for (int j = 0; j < number_of_dimensions; ++j) {
+ vertices(j, vertex_index) = dd_get_d(vertex_matrix->matrix[i][j + 1]);
+ }
+ ++vertex_index;
+ }
+ }
+ dd_FreeMatrix(vertex_matrix);
+ dd_FreePolyhedra(polyhedra);
+ dd_FreeMatrix(matrix);
+
+ return vertices;
+}
+#endif // __linux__
+
+} // namespace controls
+} // namespace aos
+
+#endif // AOS_CONTROLS_POLYTOPE_H_
diff --git a/aos/controls/polytope_test.cc b/aos/controls/polytope_test.cc
new file mode 100644
index 0000000..d7ec5e5
--- /dev/null
+++ b/aos/controls/polytope_test.cc
@@ -0,0 +1,102 @@
+#include "aos/controls/polytope.h"
+
+#include <vector>
+
+#include "Eigen/Dense"
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+
+#include "aos/testing/test_logging.h"
+
+namespace aos {
+namespace controls {
+
+class HPolytopeTest : public ::testing::Test {
+ protected:
+ HPolytope<2> Polytope1() {
+ return HPolytope<2>{
+ (Eigen::Matrix<double, 4, 2>() << 1, 0, -1, 0, 0, 1, 0, -1).finished(),
+ (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0).finished()};
+ }
+ HPolytope<2> Polytope2() {
+ return HPolytope<2>{
+ (Eigen::Matrix<double, 4, 2>() << 1, 1, -1, -1, 0, 1, 0, -1).finished(),
+ (Eigen::Matrix<double, 4, 1>() << 12.0, 12.0, 12.0, 12.0).finished()};
+ }
+ HPolytope<1> Polytope3() {
+ return HPolytope<1>{(Eigen::Matrix<double, 2, 1>() << 1, -0.5).finished(),
+ (Eigen::Matrix<double, 2, 1>() << 5.0, 2.0).finished()};
+ }
+ HPolytope<2> Polytope4() {
+ return HPolytope<2>{
+ (Eigen::Matrix<double, 4, 2>() << 1, 1, -1, -1, 1, -1, -1, 1)
+ .finished(),
+ (Eigen::Matrix<double, 4, 1>() << 2, -1, 2, -1).finished()};
+ }
+ HPolytope<2> Polytope5() {
+ return HPolytope<2>{
+ (Eigen::Matrix<double, 4, 2>() << 1, 1, -1, -1, 1, -1, -1, 1)
+ .finished(),
+ (Eigen::Matrix<double, 4, 1>() << 1.5, -0.5, 1.5, -0.5).finished()};
+ }
+
+ void SetUp() override {
+ ::aos::testing::EnableTestLogging();
+ ::aos::testing::ForcePrintLogsDuringTests();
+ HPolytope<0>::Init();
+ }
+
+ template <typename T>
+ ::std::vector<::std::vector<double>> MatrixToVectors(const T &matrix) {
+ ::std::vector<::std::vector<double>> r;
+ for (int i = 0; i < matrix.cols(); ++i) {
+ ::std::vector<double> col;
+ for (int j = 0; j < matrix.rows(); ++j) {
+ col.emplace_back(matrix(j, i));
+ }
+ r.emplace_back(col);
+ }
+ return r;
+ }
+
+ template <typename T>
+ ::std::vector<::testing::Matcher<::std::vector<double>>> MatrixToMatchers(
+ const T &matrix) {
+ ::std::vector<::testing::Matcher<::std::vector<double>>> r;
+ for (int i = 0; i < matrix.cols(); ++i) {
+ ::std::vector<::testing::Matcher<double>> col;
+ for (int j = 0; j < matrix.rows(); ++j) {
+ col.emplace_back(::testing::DoubleNear(matrix(j, i), 0.000001));
+ }
+ r.emplace_back(::testing::ElementsAreArray(col));
+ }
+ return r;
+ }
+};
+
+// Tests that the vertices for various polytopes calculated from H and k are
+// correct.
+TEST_F(HPolytopeTest, CalculatedVertices) {
+ EXPECT_THAT(MatrixToVectors(Polytope1().Vertices()),
+ ::testing::UnorderedElementsAreArray(
+ MatrixToVectors((Eigen::Matrix<double, 2, 4>() << -12, -12,
+ 12, 12, -12, 12, 12, -12).finished())));
+ EXPECT_THAT(MatrixToVectors(Polytope2().Vertices()),
+ ::testing::UnorderedElementsAreArray(
+ MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 24, 0, -24,
+ 0, -12, 12, 12, -12).finished())));
+ EXPECT_THAT(MatrixToVectors(Polytope3().Vertices()),
+ ::testing::UnorderedElementsAreArray(MatrixToVectors(
+ (Eigen::Matrix<double, 1, 2>() << 5, -4).finished())));
+ EXPECT_THAT(MatrixToVectors(Polytope4().Vertices()),
+ ::testing::UnorderedElementsAreArray(
+ MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 1, 1.5, 1.5,
+ 2, 0, -0.5, 0.5, 0).finished())));
+ EXPECT_THAT(MatrixToVectors(Polytope5().Vertices()),
+ ::testing::UnorderedElementsAreArray(
+ MatrixToVectors((Eigen::Matrix<double, 2, 4>() << 0.5, 1, 1.5,
+ 1, 0, 0.5, 0, -0.5).finished())));
+}
+
+} // namespace controls
+} // namespace aos
diff --git a/aos/controls/replay_control_loop.h b/aos/controls/replay_control_loop.h
new file mode 100644
index 0000000..d1436dd
--- /dev/null
+++ b/aos/controls/replay_control_loop.h
@@ -0,0 +1,192 @@
+#ifndef AOS_CONTROLS_REPLAY_CONTROL_LOOP_H_
+#define AOS_CONTROLS_REPLAY_CONTROL_LOOP_H_
+
+#include <fcntl.h>
+
+#include "aos/queue.h"
+#include "aos/controls/control_loop.h"
+#include "aos/logging/replay.h"
+#include "aos/logging/queue_logging.h"
+#include "aos/time/time.h"
+#include "aos/macros.h"
+
+namespace aos {
+namespace controls {
+
+// Handles reading logged messages and running them through a control loop
+// again.
+// T should be a queue group suitable for use with ControlLoop.
+template <class T>
+class ControlLoopReplayer {
+ public:
+ typedef typename ControlLoop<T>::GoalType GoalType;
+ typedef typename ControlLoop<T>::PositionType PositionType;
+ typedef typename ControlLoop<T>::StatusType StatusType;
+ typedef typename ControlLoop<T>::OutputType OutputType;
+
+ // loop_group is where to send the messages out.
+ // process_name is the name of the process which wrote the log messages in the
+ // file(s).
+ ControlLoopReplayer(T *loop_group, const ::std::string &process_name)
+ : loop_group_(loop_group) {
+ // Clear out any old messages which will confuse the code.
+ loop_group_->status.FetchLatest();
+ loop_group_->output.FetchLatest();
+
+ replayer_.AddDirectQueueSender("wpilib_interface.DSReader",
+ "joystick_state", ::aos::joystick_state);
+ replayer_.AddDirectQueueSender("wpilib_interface.SensorReader",
+ "robot_state", ::aos::robot_state);
+
+ replayer_.AddHandler(
+ process_name, "position",
+ ::std::function<void(const PositionType &)>(::std::ref(position_)));
+ replayer_.AddHandler(
+ process_name, "output",
+ ::std::function<void(const OutputType &)>(::std::ref(output_)));
+ replayer_.AddHandler(
+ process_name, "status",
+ ::std::function<void(const StatusType &)>(::std::ref(status_)));
+ // The timing of goal messages doesn't matter, and we don't need to look
+ // back at them after running the loop.
+ replayer_.AddDirectQueueSender(process_name, "goal", loop_group_->goal);
+ }
+
+ template <class QT>
+ void AddDirectQueueSender(const ::std::string &process_name,
+ const ::std::string &log_message,
+ const ::aos::Queue<QT> &queue) {
+ replayer_.AddDirectQueueSender<QT>(process_name, log_message, queue);
+ }
+
+ // Replays messages from a file.
+ // filename can be straight from the command line; all sanity checking etc is
+ // handled by this function.
+ void ProcessFile(const char *filename);
+
+ private:
+ // A message handler which saves off messages and records whether it currently
+ // has a new one or not.
+ template <class S>
+ class CaptureMessage {
+ public:
+ CaptureMessage() {}
+
+ void operator()(const S &message) {
+ CHECK(!have_new_message_);
+ saved_message_ = message;
+ have_new_message_ = true;
+ }
+
+ const S &saved_message() const { return saved_message_; }
+ bool have_new_message() const { return have_new_message_; }
+ void clear_new_message() { have_new_message_ = false; }
+
+ private:
+ S saved_message_;
+ bool have_new_message_ = false;
+
+ DISALLOW_COPY_AND_ASSIGN(CaptureMessage);
+ };
+
+ // Runs through the file currently loaded in replayer_.
+ // Returns after going through the entire file.
+ void DoProcessFile();
+
+ T *const loop_group_;
+
+ CaptureMessage<PositionType> position_;
+ CaptureMessage<OutputType> output_;
+ CaptureMessage<StatusType> status_;
+
+ // The output that the loop sends for ZeroOutputs(). It might not actually be
+ // all fields zeroed, so we pick the first one and remember it to compare.
+ CaptureMessage<OutputType> zero_output_;
+
+ ::aos::logging::linux_code::LogReplayer replayer_;
+};
+
+template <class T>
+void ControlLoopReplayer<T>::ProcessFile(const char *filename) {
+ int fd;
+ if (strcmp(filename, "-") == 0) {
+ fd = STDIN_FILENO;
+ } else {
+ fd = open(filename, O_RDONLY);
+ }
+ if (fd == -1) {
+ PLOG(FATAL, "couldn't open file '%s' for reading", filename);
+ }
+
+ replayer_.OpenFile(fd);
+ DoProcessFile();
+ replayer_.CloseCurrentFile();
+
+ PCHECK(close(fd));
+}
+
+template <class T>
+void ControlLoopReplayer<T>::DoProcessFile() {
+ while (true) {
+ // Dig through messages until we get a status, which indicates the end of
+ // the control loop cycle.
+ while (!status_.have_new_message()) {
+ if (replayer_.ProcessMessage()) return;
+ }
+
+ // Send out the position message (after adjusting the time offset) so the
+ // loop will run right now.
+ if (!position_.have_new_message()) {
+ LOG(WARNING, "don't have a new position this cycle -> skipping\n");
+ status_.clear_new_message();
+ position_.clear_new_message();
+ output_.clear_new_message();
+ continue;
+ }
+ ::aos::time::OffsetToNow(position_.saved_message().sent_time);
+ {
+ auto position_message = loop_group_->position.MakeMessage();
+ *position_message = position_.saved_message();
+ CHECK(position_message.Send());
+ }
+ position_.clear_new_message();
+
+ // Wait for the loop to finish running.
+ loop_group_->status.FetchNextBlocking();
+
+ // Point out if the status is different.
+ if (!loop_group_->status->EqualsNoTime(status_.saved_message())) {
+ LOG_STRUCT(WARNING, "expected status", status_.saved_message());
+ LOG_STRUCT(WARNING, "got status", *loop_group_->status);
+ }
+ status_.clear_new_message();
+
+ // Point out if the output is different. This is a lot more complicated than
+ // for the status because there isn't always an output logged.
+ bool loop_new_output = loop_group_->output.FetchLatest();
+ if (output_.have_new_message()) {
+ if (!loop_new_output) {
+ LOG_STRUCT(WARNING, "no output, expected", output_.saved_message());
+ } else if (!loop_group_->output->EqualsNoTime(output_.saved_message())) {
+ LOG_STRUCT(WARNING, "expected output", output_.saved_message());
+ LOG_STRUCT(WARNING, "got output", *loop_group_->output);
+ }
+ } else if (loop_new_output) {
+ if (zero_output_.have_new_message()) {
+ if (!loop_group_->output->EqualsNoTime(zero_output_.saved_message())) {
+ LOG_STRUCT(WARNING, "expected null output",
+ zero_output_.saved_message());
+ LOG_STRUCT(WARNING, "got output", *loop_group_->output);
+ }
+ } else {
+ zero_output_(*loop_group_->output);
+ }
+ }
+ output_.clear_new_message();
+ }
+}
+
+} // namespace controls
+} // namespace aos
+
+#endif // AOS_CONTROLS_REPLAY_CONTROL_LOOP_H_