Merge "Added support for matrix exponential to Eigen"
diff --git a/aos/common/util/BUILD b/aos/common/util/BUILD
index b62e93c..4e0455b 100644
--- a/aos/common/util/BUILD
+++ b/aos/common/util/BUILD
@@ -188,6 +188,24 @@
)
cc_library(
+ name = 'global_factory',
+ hdrs = [
+ 'global_factory.h'
+ ],
+)
+
+cc_test(
+ name = 'global_factory_test',
+ srcs = [
+ 'global_factory_test.cc'
+ ],
+ deps = [
+ '//aos/testing:googletest',
+ ':global_factory'
+ ],
+)
+
+cc_library(
name = 'linked_list',
hdrs = [
'linked_list.h',
diff --git a/aos/common/util/global_factory.h b/aos/common/util/global_factory.h
new file mode 100644
index 0000000..148fc1e
--- /dev/null
+++ b/aos/common/util/global_factory.h
@@ -0,0 +1,88 @@
+#ifndef AOS_COMMON_UTIL_GLOBAL_FACTORY_H_
+#define AOS_COMMON_UTIL_GLOBAL_FACTORY_H_
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <unordered_map>
+
+// // File Usage Description:
+// class ExampleBaseClass { virtual ~ExampleBaseClass(); }
+// class ExampleSubClass : public ExampleBaseClass {}
+//
+// // At namespace scope in header file:
+// SETUP_FACTORY(ExampleBaseClass);
+// // At namespace scope in cc file:
+// REGISTER_SUBCLASS("ExampleSubClass", ExampleBaseClass, ExampleSubClass);
+//
+// // When you want an object of type "ExampleSubClass".
+// std::unique_ptr<ExampleBaseClass> constructed_item =
+// ExampleBaseClassGlobalFactory::Get("ExampleSubClass")();
+
+// Helper macro to set up a Factory Family for a particular type.
+// Put this is the header file along-side the base class.
+#define SETUP_FACTORY(BaseClass, ...) \
+ using BaseClass##GlobalFactory = \
+ ::aos::GlobalFactory<BaseClass, ##__VA_ARGS__>
+
+// Helper macro to set up a Factory for a subtype. For BaseClass
+// This should happen in a .cc file not a header file to avoid multiple
+// linkage.
+#define REGISTER_SUBCLASS_BY_KEY(key, BaseClass, SubClass) \
+ BaseClass##GlobalFactory::SubClassRegisterer<SubClass> \
+ register_for_##SubClass(key)
+
+// Proxy to above but where SubClass name is the key.
+#define REGISTER_SUBCLASS(BaseClass, SubClass) \
+ REGISTER_SUBCLASS_BY_KEY(#SubClass, BaseClass, SubClass)
+
+namespace aos {
+
+// Maintains a static std::unordered_map<std::string, FactoryFunction> for
+// BaseClass. Best to use with macros above.
+template <typename BaseClass, typename... FactoryArgs>
+class GlobalFactory {
+ public:
+ using FactoryFunction =
+ std::function<std::unique_ptr<BaseClass>(FactoryArgs&&...)>;
+
+ // Gets the factory function by named. This will return a null factory
+ // std::function if the factory is not available, so one would be wise
+ // to check this function before use.
+ // It is an error to call this during static initialization.
+ static const FactoryFunction &Get(const std::string &name) {
+ const auto &map = *GetMap();
+ auto item = map.find(name);
+ if (item == map.end()) {
+ static FactoryFunction null_create_fn;
+ return null_create_fn;
+ }
+ return item->second;
+ }
+
+ // Installs a factory function for constructing SubClass
+ // using name "name". It is an error not call this at namespace scope
+ // through the REGISTER_SUBCLASS macro above.
+ template <typename SubClass>
+ class SubClassRegisterer {
+ public:
+ explicit SubClassRegisterer(const char *name) {
+ (*GetMap())[name] = [](FactoryArgs&&... args) {
+ return std::unique_ptr<BaseClass>(
+ new SubClass(std::forward<FactoryArgs>(args)...));
+ };
+ }
+ };
+
+ private:
+ // Actual map. (Protected by static from concurrent construction
+ // if there is nothing registered at static linkage time).
+ static std::unordered_map<std::string, FactoryFunction> *GetMap() {
+ static std::unordered_map<std::string, FactoryFunction> map;
+ return ↦
+ }
+};
+
+} // namespace aos
+
+#endif // AOS_COMMON_UTIL_GLOBAL_FACTORY_H_
diff --git a/aos/common/util/global_factory_test.cc b/aos/common/util/global_factory_test.cc
new file mode 100644
index 0000000..8da7dcb
--- /dev/null
+++ b/aos/common/util/global_factory_test.cc
@@ -0,0 +1,67 @@
+#include "aos/common/util/global_factory.h"
+#include "gtest/gtest.h"
+
+namespace aos {
+
+namespace test_a {
+class BaseType {
+ public:
+ virtual ~BaseType() {}
+
+ virtual std::pair<int, int> Get() = 0;
+};
+
+SETUP_FACTORY(BaseType, int, int);
+
+class BaseTypeNoArgs {
+ public:
+ virtual ~BaseTypeNoArgs() {}
+
+ virtual int Get() = 0;
+};
+
+SETUP_FACTORY(BaseTypeNoArgs);
+
+} // namespace test_a
+
+namespace test_b {
+
+class SubType : public test_a::BaseType {
+ public:
+ SubType(int t1, int t2) : value_(t1, t2) {}
+ std::pair<int, int> Get() override { return value_; }
+
+ private:
+ std::pair<int, int> value_;
+};
+
+REGISTER_SUBCLASS(test_a::BaseType, SubType);
+
+} // namespace test_b
+
+namespace {
+
+class SubType1 : public test_a::BaseTypeNoArgs {
+ public:
+ int Get() override { return 1; }
+};
+
+class SubType2 : public test_a::BaseTypeNoArgs {
+ public:
+ int Get() override { return 2; }
+};
+REGISTER_SUBCLASS(test_a::BaseTypeNoArgs, SubType1);
+REGISTER_SUBCLASS(test_a::BaseTypeNoArgs, SubType2);
+
+TEST(GlobalFactoryTest, CheckFactory) {
+ auto val = test_a::BaseTypeGlobalFactory::Get("SubType")(2, 7)->Get();
+ EXPECT_EQ(val.first, 2);
+ EXPECT_EQ(val.second, 7);
+}
+TEST(GlobalFactoryTest, CheckFactoryNoArgs) {
+ EXPECT_EQ(1, test_a::BaseTypeNoArgsGlobalFactory::Get("SubType1")()->Get());
+ EXPECT_EQ(2, test_a::BaseTypeNoArgsGlobalFactory::Get("SubType2")()->Get());
+}
+
+} // namespace
+} // namespace aos
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index c7cef55..d03c48c 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -417,10 +417,63 @@
bool enabled = true) {
const auto start_time = monotonic_clock::now();
while (monotonic_clock::now() < start_time + run_for) {
+ const auto loop_start_time = monotonic_clock::now();
+ double begin_intake_velocity = superstructure_plant_.intake_velocity();
+ double begin_turret_velocity =
+ superstructure_plant_.turret_angular_velocity();
+ double begin_hood_velocity =
+ superstructure_plant_.hood_angular_velocity();
+
RunIteration(enabled);
+
+ const double loop_time =
+ chrono::duration_cast<chrono::duration<double>>(
+ monotonic_clock::now() - loop_start_time).count();
+ const double intake_acceleration =
+ (superstructure_plant_.intake_velocity() - begin_intake_velocity) /
+ loop_time;
+ const double turret_acceleration =
+ (superstructure_plant_.turret_angular_velocity() -
+ begin_turret_velocity) /
+ loop_time;
+ const double hood_acceleration =
+ (superstructure_plant_.hood_angular_velocity() -
+ begin_hood_velocity) /
+ loop_time;
+ EXPECT_GE(peak_intake_acceleration_, intake_acceleration);
+ EXPECT_LE(-peak_intake_acceleration_, intake_acceleration);
+ EXPECT_GE(peak_turret_acceleration_, turret_acceleration);
+ EXPECT_LE(-peak_turret_acceleration_, turret_acceleration);
+ EXPECT_GE(peak_hood_acceleration_, hood_acceleration);
+ EXPECT_LE(-peak_hood_acceleration_, hood_acceleration);
+
+ EXPECT_GE(peak_intake_velocity_, superstructure_plant_.intake_velocity());
+ EXPECT_LE(-peak_intake_velocity_,
+ superstructure_plant_.intake_velocity());
+ EXPECT_GE(peak_turret_velocity_,
+ superstructure_plant_.turret_angular_velocity());
+ EXPECT_LE(-peak_turret_velocity_,
+ superstructure_plant_.turret_angular_velocity());
+ EXPECT_GE(peak_hood_velocity_,
+ superstructure_plant_.hood_angular_velocity());
+ EXPECT_LE(-peak_hood_velocity_,
+ superstructure_plant_.hood_angular_velocity());
}
}
+ void set_peak_intake_acceleration(double value) {
+ peak_intake_acceleration_ = value;
+ }
+ void set_peak_turret_acceleration(double value) {
+ peak_turret_acceleration_ = value;
+ }
+ void set_peak_hood_acceleration(double value) {
+ peak_hood_acceleration_ = value;
+ }
+ void set_peak_intake_velocity(double value) { peak_intake_velocity_ = value; }
+ void set_peak_turret_velocity(double value) { peak_turret_velocity_ = value; }
+ void set_peak_hood_velocity(double value) { peak_hood_velocity_ = value; }
+
// Create a new instance of the test queue so that it invalidates the queue
// that it points to. Otherwise, we will have a pointer to shared memory
// that is no longer valid.
@@ -429,6 +482,16 @@
// Create a control loop and simulation.
Superstructure superstructure_;
SuperstructureSimulation superstructure_plant_;
+
+ private:
+ // The acceleration limits to check for while moving.
+ double peak_intake_acceleration_ = 1e10;
+ double peak_turret_acceleration_ = 1e10;
+ double peak_hood_acceleration_ = 1e10;
+ // The velocity limits to check for while moving.
+ double peak_intake_velocity_ = 1e10;
+ double peak_turret_velocity_ = 1e10;
+ double peak_hood_velocity_ = 1e10;
};
// Tests that the superstructure does nothing when the goal is zero.
@@ -473,6 +536,56 @@
VerifyNearGoal();
}
+// Makes sure that the voltage on a motor is properly pulled back after
+// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
+TEST_F(SuperstructureTest, SaturationTest) {
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.distance = constants::Values::kIntakeRange.lower;
+ goal->intake.profile_params.max_velocity = 20.0;
+ goal->intake.profile_params.max_acceleration = 0.1;
+ goal->turret.angle = constants::Values::kTurretRange.lower;
+ goal->turret.profile_params.max_velocity = 20.0;
+ goal->turret.profile_params.max_acceleration = 1.0;
+ goal->hood.angle = constants::Values::kHoodRange.lower;
+ goal->hood.profile_params.max_velocity = 20.0;
+ goal->hood.profile_params.max_acceleration = 1.0;
+ ASSERT_TRUE(goal.Send());
+ }
+ set_peak_intake_velocity(23.0);
+ set_peak_turret_velocity(23.0);
+ set_peak_hood_velocity(23.0);
+ set_peak_intake_acceleration(0.2);
+ set_peak_turret_acceleration(1.1);
+ set_peak_hood_acceleration(1.1);
+
+ RunForTime(chrono::seconds(8));
+
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.distance = constants::Values::kIntakeRange.upper;
+ goal->intake.profile_params.max_velocity = 0.1;
+ goal->intake.profile_params.max_acceleration = 100;
+ goal->turret.angle = constants::Values::kTurretRange.upper;
+ goal->turret.profile_params.max_velocity = 1;
+ goal->turret.profile_params.max_acceleration = 100;
+ goal->hood.angle = constants::Values::kHoodRange.upper;
+ goal->hood.profile_params.max_velocity = 1;
+ goal->hood.profile_params.max_acceleration = 100;
+ ASSERT_TRUE(goal.Send());
+ }
+
+ set_peak_intake_velocity(0.2);
+ set_peak_turret_velocity(1.1);
+ set_peak_hood_velocity(1.1);
+ set_peak_intake_acceleration(103);
+ set_peak_turret_acceleration(103);
+ set_peak_hood_acceleration(103);
+ RunForTime(chrono::seconds(8));
+
+ VerifyNearGoal();
+}
+
// Tests that the hood, turret and intake loops doesn't try and go beyond the
// physical range of the mechanisms.
TEST_F(SuperstructureTest, RespectsRange) {