Move 2018 arm code to frc971 and make dynamics adjustable
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I16e7bbb0d293aca461abc931f560fb9066b19aaf
diff --git a/y2018/control_loops/python/graph_codegen.py b/y2018/control_loops/python/graph_codegen.py
index e7f8ce1..be267de 100644
--- a/y2018/control_loops/python/graph_codegen.py
+++ b/y2018/control_loops/python/graph_codegen.py
@@ -27,12 +27,12 @@
cc_file.append(" %s," % (alpha_unitizer))
if reverse:
cc_file.append(
- " Trajectory(Path::Reversed(%s()), 0.005));"
+ " Trajectory(dynamics, Path::Reversed(%s()), 0.005));"
% (path_function_name(str(name))))
else:
cc_file.append(
- " Trajectory(%s(), 0.005));" %
- (path_function_name(str(name))))
+ " Trajectory(dynamics, %s(), 0.005));"
+ % (path_function_name(str(name))))
start_index = None
end_index = None
@@ -68,9 +68,14 @@
cc_file.append("#include <memory>")
cc_file.append("")
cc_file.append(
- "#include \"y2018/control_loops/superstructure/arm/trajectory.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/trajectory.h\"")
cc_file.append(
- "#include \"y2018/control_loops/superstructure/arm/graph.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/graph.h\"")
+
+ cc_file.append("using frc971::control_loops::arm::Trajectory;")
+ cc_file.append("using frc971::control_loops::arm::Path;")
+ cc_file.append("using frc971::control_loops::arm::SearchGraph;")
+
cc_file.append("")
cc_file.append("namespace y2018 {")
cc_file.append("namespace control_loops {")
@@ -86,15 +91,21 @@
h_file.append("#include <memory>")
h_file.append("")
h_file.append(
- "#include \"y2018/control_loops/superstructure/arm/trajectory.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/trajectory.h\"")
h_file.append(
- "#include \"y2018/control_loops/superstructure/arm/graph.h\"")
+ "#include \"frc971/control_loops/double_jointed_arm/graph.h\"")
+ h_file.append(
+ "#include \"frc971/control_loops/double_jointed_arm/dynamics.h\"")
h_file.append("")
h_file.append("namespace y2018 {")
h_file.append("namespace control_loops {")
h_file.append("namespace superstructure {")
h_file.append("namespace arm {")
+ h_file.append("using frc971::control_loops::arm::Trajectory;")
+ h_file.append("using frc971::control_loops::arm::Path;")
+ h_file.append("using frc971::control_loops::arm::SearchGraph;")
+
h_file.append("")
h_file.append("struct TrajectoryAndParams {")
h_file.append(" TrajectoryAndParams(double new_vmax,")
@@ -182,11 +193,13 @@
h_file.append("")
h_file.append("// Builds a search graph.")
h_file.append("SearchGraph MakeSearchGraph("
+ "const frc971::control_loops::arm::Dynamics *dynamics, "
"::std::vector<TrajectoryAndParams> *trajectories,")
h_file.append(" "
"const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
h_file.append(" double vmax);")
cc_file.append("SearchGraph MakeSearchGraph("
+ "const frc971::control_loops::arm::Dynamics *dynamics, "
"::std::vector<TrajectoryAndParams> *trajectories,")
cc_file.append(" "
"const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,")
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 567810d..a3cb808 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -94,6 +94,7 @@
"//frc971/control_loops:control_loop_test",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
+ "//y2018/control_loops/superstructure/arm:arm_constants",
"//y2018/control_loops/superstructure/intake:intake_plants",
],
)
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index f90cc92..9f969ff 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -1,128 +1,4 @@
cc_library(
- name = "trajectory",
- srcs = [
- "trajectory.cc",
- ],
- hdrs = [
- "trajectory.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- ":dynamics",
- "//aos/logging",
- "//frc971/control_loops:dlqr",
- "//frc971/control_loops:jacobian",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_test(
- name = "trajectory_test",
- srcs = [
- "trajectory_test.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":demo_path",
- ":dynamics",
- ":ekf",
- ":trajectory",
- "//aos/testing:googletest",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "dynamics",
- srcs = [
- "dynamics.cc",
- ],
- hdrs = [
- "dynamics.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//frc971/control_loops:runge_kutta",
- "@com_github_gflags_gflags//:gflags",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "demo_path",
- srcs = [
- "demo_path.cc",
- ],
- hdrs = ["demo_path.h"],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [":trajectory"],
-)
-
-cc_test(
- name = "dynamics_test",
- srcs = [
- "dynamics_test.cc",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":dynamics",
- "//aos/testing:googletest",
- ],
-)
-
-cc_binary(
- name = "trajectory_plot",
- srcs = [
- "trajectory_plot.cc",
- ],
- target_compatible_with = ["@platforms//cpu:x86_64"],
- deps = [
- ":ekf",
- ":generated_graph",
- ":trajectory",
- "//frc971/analysis:in_process_plotter",
- "@com_github_gflags_gflags//:gflags",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "ekf",
- srcs = [
- "ekf.cc",
- ],
- hdrs = [
- "ekf.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- ":dynamics",
- "//frc971/control_loops:jacobian",
- "@org_tuxfamily_eigen//:eigen",
- ],
-)
-
-cc_library(
- name = "graph",
- srcs = ["graph.cc"],
- hdrs = ["graph.h"],
- target_compatible_with = ["@platforms//os:linux"],
-)
-
-cc_test(
- name = "graph_test",
- srcs = ["graph_test.cc"],
- target_compatible_with = ["@platforms//os:linux"],
- deps = [
- ":graph",
- "//aos/testing:googletest",
- ],
-)
-
-cc_library(
name = "arm",
srcs = [
"arm.cc",
@@ -133,15 +9,16 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- ":demo_path",
- ":ekf",
":generated_graph",
- ":graph",
- ":trajectory",
+ "//frc971/control_loops/double_jointed_arm:demo_path",
+ "//frc971/control_loops/double_jointed_arm:ekf",
+ "//frc971/control_loops/double_jointed_arm:graph",
+ "//frc971/control_loops/double_jointed_arm:trajectory",
"//frc971/zeroing",
"//y2018:constants",
"//y2018/control_loops/superstructure:superstructure_position_fbs",
"//y2018/control_loops/superstructure:superstructure_status_fbs",
+ "//y2018/control_loops/superstructure/arm:arm_constants",
],
)
@@ -170,7 +47,35 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- ":graph",
- ":trajectory",
+ ":arm_constants",
+ "//frc971/control_loops/double_jointed_arm:graph",
+ "//frc971/control_loops/double_jointed_arm:trajectory",
+ ],
+)
+
+cc_library(
+ name = "arm_constants",
+ hdrs = ["arm_constants.h"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/double_jointed_arm:dynamics",
+ ],
+)
+
+cc_binary(
+ name = "trajectory_plot",
+ srcs = [
+ "trajectory_plot.cc",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":arm_constants",
+ ":generated_graph",
+ "//frc971/analysis:in_process_plotter",
+ "//frc971/control_loops/double_jointed_arm:ekf",
+ "//frc971/control_loops/double_jointed_arm:trajectory",
+ "@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 1c29157..ab5deea 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -5,8 +5,9 @@
#include "aos/logging/logging.h"
#include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/demo_path.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
namespace y2018 {
@@ -29,9 +30,12 @@
alpha_unitizer_((::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max(),
0.0, 0.0, 1.0 / kAlpha1Max())
.finished()),
- search_graph_(MakeSearchGraph(&trajectories_, alpha_unitizer_, kVMax())),
+ dynamics_(kArmConstants),
+ search_graph_(MakeSearchGraph(&dynamics_, &trajectories_, alpha_unitizer_,
+ kVMax())),
+ arm_ekf_(&dynamics_),
// Go to the start of the first trajectory.
- follower_(ReadyAboveBoxPoint()),
+ follower_(&dynamics_, ReadyAboveBoxPoint()),
points_(PointList()) {
int i = 0;
for (const auto &trajectory : trajectories_) {
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 7ceb001..51d6c4d 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -4,14 +4,17 @@
#include "aos/time/time.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/arm/graph.h"
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
+#include "frc971/control_loops/double_jointed_arm/graph.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
+using frc971::control_loops::arm::TrajectoryFollower;
+using frc971::control_loops::arm::EKF;
+
namespace y2018 {
namespace control_loops {
namespace superstructure {
@@ -114,6 +117,8 @@
double vmax_ = kVMax();
+ frc971::control_loops::arm::Dynamics dynamics_;
+
::std::vector<TrajectoryAndParams> trajectories_;
SearchGraph search_graph_;
diff --git a/y2018/control_loops/superstructure/arm/arm_constants.h b/y2018/control_loops/superstructure/arm/arm_constants.h
new file mode 100644
index 0000000..9ac7020
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/arm_constants.h
@@ -0,0 +1,53 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
+
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+constexpr double kEfficiencyTweak = 0.95;
+constexpr double kStallTorque = 1.41 * kEfficiencyTweak;
+constexpr double kFreeSpeed = (5840.0 / 60.0) * 2.0 * M_PI;
+constexpr double kStallCurrent = 89.0;
+
+constexpr ::frc971::control_loops::arm::ArmConstants kArmConstants = {
+ .l1 = 46.25 * 0.0254,
+ .l2 = 41.80 * 0.0254,
+ .m1 = 9.34 / 2.2,
+ .m2 = 9.77 / 2.2,
+
+ // Moment of inertia of the joints in kg m^2
+ .j1 = 2957.05 * 0.0002932545454545454,
+ .j2 = 2824.70 * 0.0002932545454545454,
+
+ // Radius of the center of mass of the joints in meters.
+ .r1 = 21.64 * 0.0254,
+ .r2 = 26.70 * 0.0254,
+
+ // Gear ratios for the two joints.
+ .g1 = 140.0,
+ .g2 = 90.0,
+
+ // MiniCIM motor constants.
+ .efficiency_tweak = kEfficiencyTweak,
+ .stall_torque = kStallTorque,
+ .free_speed = kFreeSpeed,
+ .stall_current = kStallCurrent,
+ .resistance = 12.0 / kStallCurrent,
+ .Kv = kFreeSpeed / 12.0,
+ .Kt = kStallTorque / kStallCurrent,
+
+ // Number of motors on the distal joint.
+ .num_distal_motors = 2.0,
+};
+
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_CONSTANTS_H_
diff --git a/y2018/control_loops/superstructure/arm/demo_path.cc b/y2018/control_loops/superstructure/arm/demo_path.cc
deleted file mode 100644
index 03856c9..0000000
--- a/y2018/control_loops/superstructure/arm/demo_path.cc
+++ /dev/null
@@ -1,218 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
-
-#include <array>
-#include <initializer_list>
-#include <memory>
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-::std::vector<::std::array<double, 6>> FlipPath(
- ::std::initializer_list<::std::array<double, 6>> list) {
- ::std::vector<::std::array<double, 6>> result;
- result.reserve(list.size());
- for (::std::array<double, 6> point : list) {
- point[0] = M_PI_2 - point[0];
- point[1] = M_PI_2 - point[1];
- point[2] = -point[2];
- point[3] = -point[3];
- point[4] = -point[4];
- point[5] = -point[5];
- result.push_back(point);
- }
- return result;
-}
-
-
-::std::unique_ptr<Path> MakeDemoPath() {
- return ::std::unique_ptr<Path>(new Path(FlipPath(
- {{{1.3583511559969876, 0.99753029519739866, 0.63708920330895369,
- -0.77079007974101643, -0.21483375398380378, -0.17756921128311187}},
- {{1.4037744780290744, 0.94141413786797179, 0.62102298265172207,
- -0.78379235452915641, -0.23084099683729808, -0.18290283743090688}},
- {{1.4401396868545593, 0.89467165253531666, 0.606891738771278,
- -0.79478451004733031, -0.24685749210031543, -0.18849894149927623}},
- {{1.4710837705397868, 0.85345617877170399, 0.59376006111610313,
- -0.80464215016577489, -0.26311393698286972, -0.19415687958799988}},
- {{1.4982967960378542, 0.81597755723156562, 0.58119389263733834,
- -0.81376511301545618, -0.27984413029527577, -0.19986598102433009}},
- {{1.5227269571172215, 0.78122893136122973, 0.56894801707769227,
- -0.82237348805962973, -0.29704891442706877, -0.20550961547441812}},
- {{1.5449685847030388, 0.74857685672553398, 0.55686625320351735,
- -0.83060217676278458, -0.31482379304256025, -0.21106978462024978}},
- {{1.5654226738527495, 0.71759174238125301, 0.54484048864732038,
- -0.83853970802255351, -0.33321287621660045, -0.21650513557965106}},
- {{1.5843743655705165, 0.68796601612512265, 0.53279106733541426,
- -0.84624681894089859, -0.35226023809265267, -0.22178093106252236}},
- {{1.6020345343012865, 0.65947020123078393, 0.52065635488289785,
- -0.85376633812774205, -0.37188463022164508, -0.22678852570730737}},
- {{1.6185639244576269, 0.63192742228662813, 0.50838674358661662,
- -0.86112886314731996, -0.39227952043050979, -0.23159139751917215}},
- {{1.6340880277208036, 0.60519768777296334, 0.49594100637973321,
- -0.868356216187261, -0.41330592927121557, -0.23605025316897368}},
- {{1.6487067026409843, 0.57916772325998889, 0.48328397846038601,
- -0.87546364639743945, -0.43479093110952094, -0.24001893904573279}},
- {{1.6625006419641934, 0.55374413183503834, 0.47038503527185527,
- -0.88246128447218319, -0.45715591769925784, -0.2436817242766752}},
- {{1.6755358637715485, 0.52884863988931285, 0.45721707379330323,
- -0.88935513009814537, -0.48005035022840359, -0.24679412703629519}},
- {{1.6878669166029825, 0.50441469954774187, 0.44375581873319114,
- -0.89614774079971626, -0.50380737957408728, -0.24947659419788909}},
- {{1.6995392207123023, 0.48038500205371104, 0.42997935653089725,
- -0.90283871923908732, -0.52783372117172589, -0.25138272577381626}},
- {{1.7105908129416818, 0.45670961972392227, 0.41586782149756141,
- -0.90942506840468851, -0.55267961927927023, -0.25273346631273114}},
- {{1.7210536699519405, 0.43334459201815007, 0.40140320353386438,
- -0.91590145113584731, -0.57770009966870806, -0.25318354738409404}},
- {{1.7309547270317198, 0.41025083198931545, 0.38656923494570777,
- -0.92226038979969771, -0.60317645612010684, -0.25282447346857195}},
- {{1.7403166729890138, 0.38739326814551689, 0.37135135004406289,
- -0.92849242044318914, -0.62907036438628838, -0.25159784841893079}},
- {{1.7491585775728877, 0.36474016215239319, 0.35573669259875446,
- -0.93458622156486959, -0.6551663269414254, -0.24938020059543903}},
- {{1.7574963917487576, 0.34226255982896742, 0.33971416817223166,
- -0.94052872574050006, -0.68164592193873064, -0.24620769226541636}},
- {{1.7653433501187576, 0.31993384454025714, 0.32327453032164366,
- -0.94630522456833177, -0.70757451300020135, -0.24172061458903144}},
- {{1.7727102970907476, 0.29772937021195395, 0.30641049133414705,
- -0.95189947515500117, -0.73378453575677771, -0.23620138680600353}},
- {{1.7796059529520747, 0.27562615695138953, 0.28911685939500692,
- -0.95729381154041093, -0.75990897245769828, -0.22950448838605128}},
- {{1.7860371320859008, 0.25360263640618674, 0.27139068895919466,
- -0.9624692690918778, -0.78523056852177775, -0.22141489322699751}},
- {{1.7920089227108962, 0.23163843702152076, 0.25323144050682866,
- -0.96740572540110403, -0.81025448009679157, -0.21209581939563796}},
- {{1.7975248354162883, 0.20971420159976159, 0.2346411508301583,
- -0.97208205946674009, -0.83408589705170777, -0.20133249883457041}},
- {{1.8025869261905516, 0.187811431247703, 0.21562459548465329,
- -0.97647633551565383, -0.85752536967991499, -0.18935884543741083}},
- {{1.8071958984562269, 0.16591235107247332, 0.19618945367704119,
- -0.98056600913243175, -0.8793458743889202, -0.17593847769244828}},
- {{1.8113511877225346, 0.14399979396679621, 0.17634645105214913,
- -0.98432816133711831, -0.89983233183814704, -0.16120962647016274}},
- {{1.8150510317784807, 0.12205709958524935, 0.15610948821796866,
- -0.98773975706575867, -0.91852354093170219, -0.14517104001739675}},
- {{1.8182925288197413, 0.10006802621144681, 0.13549574094137548,
- -0.99077792879471627, -0.93602333307674279, -0.12800867856143744}},
- {{1.821071685494891, 0.078016673692446373, 0.11452572935697461,
- -0.99342028231522084, -0.95112385045254688, -0.10965064890920556}},
- {{1.8233834565427998, 0.055887416001137141, 0.093223341590826445,
- -0.99564522224667962, -0.96368362602021329, -0.090231797498176183}},
- {{1.8252217774528923, 0.03366484230234558, 0.071615815013953282,
- -0.99743229095507402, -0.97395276760288163, -0.069931055757802438}},
- {{1.8265795913984428, 0.011333705660752967, 0.049733665943173709,
- -0.99876251555204698, -0.98177893706111263, -0.048889055531294821}},
- {{1.8274488715589303, -0.011121121248675203, 0.02761056315188902,
- -0.99961875572762016, -0.98689335559342106, -0.027260179511112551}},
- {{1.8278206398523622, -0.033714683874300592, 0.0052831412013899706,
- -0.99998604411213976, -0.98885489268650051, -0.0052254487261787384}},
- {{1.8276849830361703, -0.056461977241467148, -0.017209244112191453,
- -0.99985190999321838, -0.98812791978941406, 0.017006330622021722}},
- {{1.8270310671011307, -0.079377971247817633, -0.039824819694542858,
- -0.99920667718760625, -0.98450390283226485, 0.039237693528591036}},
- {{1.8258471508733223, -0.10247762977900209, -0.062519753866286318,
- -0.99804372668560926, -0.97719363903338485, 0.061212532653782022}},
- {{1.8241205997516765, -0.12577592348843702, -0.085248569812080663,
- -0.99635971483445407, -0.9676884196650537, 0.08279433361404552}},
- {{1.8218379005412455, -0.14928783595476772, -0.10796459505798382,
- -0.99415473957224865, -0.95496997071328249, 0.103708042997466}},
- {{1.8189846783932371, -0.17302836280361969, -0.13062045461685196,
- -0.99143244693508337, -0.93935720014890423, 0.12375848445745616}},
- {{1.8155457169306066, -0.19701250325490266, -0.15316858191907523,
- -0.98820007362522466, -0.92129127811992539, 0.14279680293047756}},
- {{1.8115049827211411, -0.22125524343262917, -0.17556174489393769,
- -0.98446842190585071, -0.90059657633599033, 0.16060369204333666}},
- {{1.8068456553568204, -0.24577153065112339, -0.19775357131740673,
- -0.98025176614541798, -0.87754964758424281, 0.17703366424976713}},
- {{1.8015501645066898, -0.27057623777063011, -0.21969906367786421,
- -0.97556769186923664, -0.85256620853260556, 0.19199796793136439}},
- {{1.795600235427889, -0.29568411659857541, -0.24135508622397667,
- -0.97043687190554384, -0.82563764988942245, 0.20534143268497296}},
- {{1.7889769445422168, -0.32110973920321939, -0.26268082100774776,
- -0.96488278369690872, -0.79733324597202848, 0.2170659935022537}},
- {{1.7816607868090406, -0.34686742590848585, -0.2836381738799823,
- -0.95893137727265387, -0.76782873106500538, 0.22711183686998365}},
- {{1.7736317567432636, -0.37297115865829411, -0.30419213280652752,
- -0.95261070030659223, -0.73738784843863214, 0.23546528518662449}},
- {{1.7648694450316516, -0.39943447838330554, -0.32431106313855179,
- -0.94595049253433039, -0.70610753433104578, 0.2420821140887684}},
- {{1.7553531527820141, -0.42627036498226889, -0.34396694344403017,
- -0.93898175797923322, -0.67467492193224388, 0.24714550708779132}},
- {{1.7450620254853675, -0.45349109855564618, -0.36313553561943973,
- -0.93173632684916963, -0.64280482438985487, 0.25052642309609846}},
- {{1.7339752087663065, -0.48110810061485765, -0.38179649614837224,
- -0.92424641493966653, -0.61125522193929116, 0.2525024014306243}},
- {{1.7220720279237476, -0.50913175415228962, -0.39993342129901005,
- -0.91654419343972093, -0.58016017720123902, 0.25315181203742848}},
- {{1.7093321931028456, -0.53757120171300954, -0.41753384214477446,
- -0.90866137293483673, -0.54964898595174738, 0.25256545404213349}},
- {{1.6957360316664474, -0.56643412097840984, -0.43458916639025197,
- -0.9006288116955985, -0.51972307699036913, 0.25078642241782151}},
- {{1.6812647489267576, -0.59572647787451594, -0.45109457862962754,
- -0.89247615157546845, -0.49088885529812037, 0.24811508685641395}},
- {{1.6659007178300929, -0.62545225787260295, -0.46704890024189227,
- -0.88423148823305242, -0.46296888429931843, 0.24453849915066983}},
- {{1.6496277974361373, -0.65561317697342414, -0.48245442463761296,
- -0.87592107415428122, -0.43630053035835431, 0.24031239163872659}},
- {{1.6324316790779632, -0.68620837486997288, -0.49731672574028513,
- -0.86756906024763358, -0.41095471910587472, 0.23557116495526012}},
- {{1.6143002579172752, -0.7172340939700278, -0.51164445121259294,
- -0.85919727393850864, -0.38675403181117768, 0.23030820687574571}},
- {{1.5952240262184549, -0.748683349319481, -0.52544910775441078,
- -0.85082503204836046, -0.36408233991880601, 0.22484810685703166}},
- {{1.5751964830679697, -0.780545595975073, -0.53874483689504815,
- -0.84246899095392713, -0.34276938388236106, 0.21919491474821423}},
- {{1.5542145534957212, -0.81280640198469889, -0.55154819264623711,
- -0.83414302801658013, -0.32291038254364612, 0.21351295685970414}},
- {{1.5322790080695103, -0.84544713677503402, -0.56387791937564813,
- -0.82585815491559456, -0.30444498192901781, 0.20786805829508584}},
- {{1.509394872119298, -0.87844468632349793, -0.57575473429345714,
- -0.81762245929198296, -0.28737482236853346, 0.20236376870812761}},
- {{1.4855718119204786, -0.91177120788178356, -0.58720111439137757,
- -0.8094410733694728, -0.27173204177162957, 0.1971250636942595}},
- {{1.4608244835702884, -0.94539393807497152, -0.59824108887263983,
- -0.80131616705547515, -0.25741396251510884, 0.19217806498346204}},
- {{1.4351728290976418, -0.97927506876108006, -0.60890003734400222,
- -0.79324696313473042, -0.24447483076597848, 0.18765975242966434}},
- {{1.4086423037364302, -1.0133717049339268, -0.61920449072176709,
- -0.78522977444184905, -0.2328103488745005, 0.18358578028961331}},
- {{1.3812640184459468, -1.047635918033063, -0.62918193604047223,
- -0.77725805969469564, -0.22240924347971955, 0.18003762666126619}},
- {{1.3530747828369902, -1.0820149061676485, -0.63886062143319422,
- -0.76932249829443622, -0.21321450222713362, 0.17705731568712871}},
- {{1.324117035769417, -1.1164512699023377, -0.64826936036845306,
- -0.76141108240389876, -0.20523063810290823, 0.17473421668463424}},
- {{1.294438654066159, -1.1508834084076087, -0.6574373330314689,
- -0.75350922564788103, -0.19832793065660315, 0.17304104866571926}},
- {{1.2640926339871807, -1.1852460360561055, -0.66639388399919663,
- -0.74559988691553936, -0.1925723524639939, 0.17211494490833781}},
- {{1.2331366451651338, -1.2194708141674491, -0.67516831336894412,
- -0.73766370970960415, -0.18786875722059337, 0.17195217715824379}},
- {{1.2016324623545263, -1.2534870868845998, -0.68378966242702388,
- -0.72967917440333774, -0.18419866624510314, 0.17261421082264014}},
- {{1.1696452862242701, -1.2872227045046678, -0.6922864921531301,
- -0.72162276348679166, -0.18150423850780073, 0.17412530594411615}},
- {{1.1372429700984137, -1.3206049124326309, -0.70068665547523046,
- -0.71346913797229905, -0.17977253485932801, 0.17655152952198189}},
- {{1.104495174566031, -1.3535612797241663, -0.70901706362016093,
- -0.70519132403585671, -0.17898758721428376, 0.17995840096834861}},
- {{1.0714724758096574, -1.3860206383273435, -0.71730344800943324,
- -0.69676090839955884, -0.17907637813730631, 0.18435585053172451}},
- {{1.0382454559924987, -1.4179140029119093, -0.72557011960647111,
- -0.68814824095848082, -0.18003314612431659, 0.18982321746977993}},
- {{1.0048838048727911, -1.4491754417344611, -0.73383972725116353,
- -0.67932264404179699, -0.18184420373071258, 0.19643734513631667}},
- {{0.97145546090878643, -1.4797428713062153, -0.74213301743428883,
- -0.67025262732336799, -0.18446052990619061, 0.20424250843873848}}})));
-}
-
-::std::unique_ptr<Path> MakeReversedDemoPath() {
- return ::std::unique_ptr<Path>(new Path(Path::Reversed(*MakeDemoPath())));
-}
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/demo_path.h b/y2018/control_loops/superstructure/arm/demo_path.h
deleted file mode 100644
index 0c18103..0000000
--- a/y2018/control_loops/superstructure/arm/demo_path.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
-
-#include <memory>
-
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-::std::unique_ptr<Path> MakeDemoPath();
-::std::unique_ptr<Path> MakeReversedDemoPath();
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
-
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
diff --git a/y2018/control_loops/superstructure/arm/dynamics.cc b/y2018/control_loops/superstructure/arm/dynamics.cc
deleted file mode 100644
index 02a2861..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics.cc
+++ /dev/null
@@ -1,35 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-DEFINE_bool(gravity, true, "If true, enable gravity.");
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K3 =
- (::Eigen::Matrix<double, 2, 2>()
- << Dynamics::kG1 * Dynamics::Kt / Dynamics::kResistance,
- 0.0, 0.0, Dynamics::kG2 *Dynamics::kNumDistalMotors *Dynamics::Kt /
- Dynamics::kResistance)
- .finished();
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K3_inverse = K3.inverse();
-
-const ::Eigen::Matrix<double, 2, 2> Dynamics::K4 =
- (::Eigen::Matrix<double, 2, 2>()
- << Dynamics::kG1 * Dynamics::kG1 * Dynamics::Kt /
- (Dynamics::Kv * Dynamics::kResistance),
- 0.0, 0.0,
- Dynamics::kG2 *Dynamics::kG2 *Dynamics::Kt *Dynamics::kNumDistalMotors /
- (Dynamics::Kv * Dynamics::kResistance))
- .finished();
-
-constexpr double Dynamics::kAlpha;
-constexpr double Dynamics::kBeta;
-constexpr double Dynamics::kGamma;
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/dynamics.h b/y2018/control_loops/superstructure/arm/dynamics.h
deleted file mode 100644
index b1ebc97..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics.h
+++ /dev/null
@@ -1,189 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
-
-#include "Eigen/Dense"
-
-#include "frc971/control_loops/runge_kutta.h"
-#include "gflags/gflags.h"
-
-DECLARE_bool(gravity);
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-// This class captures the dynamics of our system. It doesn't actually need to
-// store state yet, so everything can be constexpr and/or static.
-class Dynamics {
- public:
- // Below, 1 refers to the proximal joint, and 2 refers to the distal joint.
- // Length of the joints in meters.
- static constexpr double kL1 = 46.25 * 0.0254;
- static constexpr double kL2 = 41.80 * 0.0254;
-
- // Mass of the joints in kilograms.
- static constexpr double kM1 = 9.34 / 2.2;
- static constexpr double kM2 = 9.77 / 2.2;
-
- // Moment of inertia of the joints in kg m^2
- static constexpr double kJ1 = 2957.05 * 0.0002932545454545454;
- static constexpr double kJ2 = 2824.70 * 0.0002932545454545454;
-
- // Radius of the center of mass of the joints in meters.
- static constexpr double r1 = 21.64 * 0.0254;
- static constexpr double r2 = 26.70 * 0.0254;
-
- // Gear ratios for the two joints.
- static constexpr double kG1 = 140.0;
- static constexpr double kG2 = 90.0;
-
- // MiniCIM motor constants.
- static constexpr double kEfficiencyTweak = 0.95;
- static constexpr double kStallTorque = 1.41 * kEfficiencyTweak;
- static constexpr double kFreeSpeed = (5840.0 / 60.0) * 2.0 * M_PI;
- static constexpr double kStallCurrent = 89.0;
- static constexpr double kResistance = 12.0 / kStallCurrent;
- static constexpr double Kv = kFreeSpeed / 12.0;
- static constexpr double Kt = kStallTorque / kStallCurrent;
-
- // Number of motors on the distal joint.
- static constexpr double kNumDistalMotors = 2.0;
-
- static constexpr double kAlpha = kJ1 + r1 * r1 * kM1 + kL1 * kL1 * kM2;
- static constexpr double kBeta = kL1 * r2 * kM2;
- static constexpr double kGamma = kJ2 + r2 * r2 * kM2;
-
- // K3, K4 matricies described below.
- static const ::Eigen::Matrix<double, 2, 2> K3;
- static const ::Eigen::Matrix<double, 2, 2> K3_inverse;
- static const ::Eigen::Matrix<double, 2, 2> K4;
-
- // Generates K1-2 for the arm ODE.
- // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
- // These matricies are missing the velocity factor for K2[1, 0], and K2[0, 1].
- // You probbaly want MatriciesForState.
- static void NormilizedMatriciesForState(
- const ::Eigen::Matrix<double, 4, 1> &X,
- ::Eigen::Matrix<double, 2, 2> *K1_result,
- ::Eigen::Matrix<double, 2, 2> *K2_result) {
- const double angle = X(0, 0) - X(2, 0);
- const double s = ::std::sin(angle);
- const double c = ::std::cos(angle);
- *K1_result << kAlpha, c * kBeta, c * kBeta, kGamma;
- *K2_result << 0.0, s * kBeta, -s * kBeta, 0.0;
- }
-
- // Generates K1-2 for the arm ODE.
- // K1 * d^2 theta / dt^2 + K2 * d theta / dt = K3 * V - K4 * d theta/dt
- static void MatriciesForState(const ::Eigen::Matrix<double, 4, 1> &X,
- ::Eigen::Matrix<double, 2, 2> *K1_result,
- ::Eigen::Matrix<double, 2, 2> *K2_result) {
- NormilizedMatriciesForState(X, K1_result, K2_result);
- (*K2_result)(1, 0) *= X(1, 0);
- (*K2_result)(0, 1) *= X(3, 0);
- }
-
- // TODO(austin): We may want a way to provide K1 and K2 to save CPU cycles.
-
- // Calculates the acceleration given the current state and control input.
- static const ::Eigen::Matrix<double, 4, 1> Acceleration(
- const ::Eigen::Matrix<double, 4, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- MatriciesForState(X, &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 1> velocity =
- (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
-
- const ::Eigen::Matrix<double, 2, 1> torque = K3 * U - K4 * velocity;
- const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
-
- const ::Eigen::Matrix<double, 2, 1> accel =
- K1.inverse() * (torque + gravity_torque - K2 * velocity);
-
- return (::Eigen::Matrix<double, 4, 1>() << X(1, 0), accel(0, 0), X(3, 0),
- accel(1, 0))
- .finished();
- }
-
- // Calculates the acceleration given the current augmented kalman filter state
- // and control input.
- static const ::Eigen::Matrix<double, 6, 1> EKFAcceleration(
- const ::Eigen::Matrix<double, 6, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- MatriciesForState(X.block<4, 1>(0, 0), &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 1> velocity =
- (::Eigen::Matrix<double, 2, 1>() << X(1, 0), X(3, 0)).finished();
-
- const ::Eigen::Matrix<double, 2, 1> torque =
- K3 *
- (U +
- (::Eigen::Matrix<double, 2, 1>() << X(4, 0), X(5, 0)).finished()) -
- K4 * velocity;
- const ::Eigen::Matrix<double, 2, 1> gravity_torque =
- GravityTorque(X.block<4, 1>(0, 0));
-
- const ::Eigen::Matrix<double, 2, 1> accel =
- K1.inverse() * (torque + gravity_torque - K2 * velocity);
-
- return (::Eigen::Matrix<double, 6, 1>() << X(1, 0), accel(0, 0), X(3, 0),
- accel(1, 0), 0.0, 0.0)
- .finished();
- }
-
- // Calculates the voltage required to follow the trajectory. This requires
- // knowing the current state, desired angular velocity and acceleration.
- static const ::Eigen::Matrix<double, 2, 1> FF_U(
- const ::Eigen::Matrix<double, 4, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &omega_t,
- const ::Eigen::Matrix<double, 2, 1> &alpha_t) {
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- MatriciesForState(X, &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 1> gravity_torque = GravityTorque(X);
-
- return K3_inverse *
- (K1 * alpha_t + K2 * omega_t + K4 * omega_t - gravity_torque);
- }
-
- static ::Eigen::Matrix<double, 2, 1> GravityTorque(
- const ::Eigen::Matrix<double, 4, 1> &X) {
- constexpr double kAccelDueToGravity = 9.8 * kEfficiencyTweak;
- return (::Eigen::Matrix<double, 2, 1>() << (r1 * kM1 + kL1 * kM2) *
- ::std::sin(X(0)) *
- kAccelDueToGravity,
- r2 * kM2 * ::std::sin(X(2)) * kAccelDueToGravity)
- .finished() *
- (FLAGS_gravity ? 1.0 : 0.0);
- }
-
- static const ::Eigen::Matrix<double, 4, 1> UnboundedDiscreteDynamics(
- const ::Eigen::Matrix<double, 4, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
- return ::frc971::control_loops::RungeKuttaU(Dynamics::Acceleration, X, U,
- dt);
- }
-
- static const ::Eigen::Matrix<double, 6, 1> UnboundedEKFDiscreteDynamics(
- const ::Eigen::Matrix<double, 6, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
- return ::frc971::control_loops::RungeKuttaU(Dynamics::EKFAcceleration, X, U,
- dt);
- }
-};
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
-
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DYNAMICS_H_
diff --git a/y2018/control_loops/superstructure/arm/dynamics_test.cc b/y2018/control_loops/superstructure/arm/dynamics_test.cc
deleted file mode 100644
index 23e5adc..0000000
--- a/y2018/control_loops/superstructure/arm/dynamics_test.cc
+++ /dev/null
@@ -1,52 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-#include "gtest/gtest.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-namespace testing {
-
-// Tests that zero inputs result in no acceleration and no motion.
-// This isn't all that rigerous, but it's a good start.
-TEST(DynamicsTest, Acceleration) {
- Dynamics dynamics;
-
- EXPECT_TRUE(dynamics
- .Acceleration(::Eigen::Matrix<double, 4, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero())
- .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
-
- EXPECT_TRUE(dynamics
- .UnboundedDiscreteDynamics(
- ::Eigen::Matrix<double, 4, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero(), 0.1)
- .isApprox(::Eigen::Matrix<double, 4, 1>::Zero()));
-
- const ::Eigen::Matrix<double, 4, 1> X =
- (::Eigen::Matrix<double, 4, 1>() << M_PI / 2.0, 0.0, 0.0, 0.0).finished();
-
- ::std::cout << dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero())
- << ::std::endl;
-
- ::std::cout << dynamics.UnboundedDiscreteDynamics(
- X, dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero()),
- 0.01)
- << ::std::endl;
-
- EXPECT_TRUE(dynamics
- .UnboundedDiscreteDynamics(
- X, dynamics.FF_U(X, ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero()),
- 0.01)
- .isApprox(X));
-}
-
-} // namespace testing
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/ekf.cc b/y2018/control_loops/superstructure/arm/ekf.cc
deleted file mode 100644
index 48a71f4..0000000
--- a/y2018/control_loops/superstructure/arm/ekf.cc
+++ /dev/null
@@ -1,111 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/ekf.h"
-
-#include "Eigen/Dense"
-#include <iostream>
-
-#include "frc971/control_loops/jacobian.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-DEFINE_double(proximal_voltage_error_uncertainty, 8.0,
- "Proximal joint voltage error uncertainty.");
-DEFINE_double(distal_voltage_error_uncertainty, 2.0,
- "Distal joint voltage error uncertainty.");
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-namespace {
-// TODO(austin): When tuning this, make sure to verify that you are waiting
-// enough cycles to make sure it converges at startup. Otherwise you will have a
-// bad day.
-::Eigen::Matrix<double, 6, 6> Q_covariance(
- (::Eigen::DiagonalMatrix<double, 6>().diagonal() << ::std::pow(0.1, 2),
- ::std::pow(2.0, 2), ::std::pow(0.1, 2), ::std::pow(2.0, 2),
- ::std::pow(FLAGS_proximal_voltage_error_uncertainty, 2),
- ::std::pow(FLAGS_distal_voltage_error_uncertainty, 2))
- .finished()
- .asDiagonal());
-} // namespace
-
-EKF::EKF() {
- X_hat_.setZero();
- Q_covariance =
- ((::Eigen::DiagonalMatrix<double, 6>().diagonal() << ::std::pow(0.1, 2),
- ::std::pow(2.0, 2), ::std::pow(0.1, 2), ::std::pow(2.0, 2),
- ::std::pow(FLAGS_proximal_voltage_error_uncertainty, 2),
- ::std::pow(FLAGS_distal_voltage_error_uncertainty, 2))
- .finished()
- .asDiagonal());
- P_ = Q_covariance;
- P_reset_ = P_;
- //::std::cout << "Reset P: " << P_ << ::std::endl;
- // TODO(austin): Running the EKF 2000 cycles works, but isn't super clever.
- // We could just solve the DARE.
-
- for (int i = 0; i < 1000; ++i) {
- Predict(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
- Correct(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
- }
- P_half_converged_ = P_;
- //::std::cout << "Stabilized P: " << P_ << ::std::endl;
- for (int i = 0; i < 1000; ++i) {
- Predict(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
- Correct(::Eigen::Matrix<double, 2, 1>::Zero(), 0.00505);
- }
- //::std::cout << "Really stabilized P: " << P_ << ::std::endl;
- P_converged_ = P_;
-
- Reset(::Eigen::Matrix<double, 4, 1>::Zero());
-}
-
-void EKF::Reset(const ::Eigen::Matrix<double, 4, 1> &X) {
- X_hat_.setZero();
- P_ = P_converged_;
- X_hat_.block<4, 1>(0, 0) = X;
-}
-
-void EKF::Predict(const ::Eigen::Matrix<double, 2, 1> &U, double dt) {
- const ::Eigen::Matrix<double, 6, 6> A =
- ::frc971::control_loops::NumericalJacobianX<6, 2>(
- Dynamics::UnboundedEKFDiscreteDynamics, X_hat_, U, dt);
-
- X_hat_ = Dynamics::UnboundedEKFDiscreteDynamics(X_hat_, U, dt);
- P_ = A * P_ * A.transpose() + Q_covariance;
-}
-
-void EKF::Correct(const ::Eigen::Matrix<double, 2, 1> &Y, double /*dt*/) {
- const ::Eigen::Matrix<double, 2, 2> R_covariance(
- (::Eigen::DiagonalMatrix<double, 2>().diagonal() << ::std::pow(0.01, 2),
- ::std::pow(0.01, 2))
- .finished()
- .asDiagonal());
- // H is the jacobian of the h(x) measurement prediction function
- const ::Eigen::Matrix<double, 2, 6> H_jacobian =
- (::Eigen::Matrix<double, 2, 6>() << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 1.0, 0.0, 0.0, 0.0)
- .finished();
-
- // Update step Measurement residual error of proximal and distal joint
- // angles.
- const ::Eigen::Matrix<double, 2, 1> Y_hat =
- Y - (::Eigen::Matrix<double, 2, 1>() << X_hat_(0), X_hat_(2)).finished();
- // Residual covariance
- const ::Eigen::Matrix<double, 2, 2> S =
- H_jacobian * P_ * H_jacobian.transpose() + R_covariance;
-
- // K is the Near-optimal Kalman gain
- const ::Eigen::Matrix<double, 6, 2> kalman_gain =
- P_ * H_jacobian.transpose() * S.inverse();
- // Updated state estimate
- X_hat_ = X_hat_ + kalman_gain * Y_hat;
- // Updated covariance estimate
- P_ = (::Eigen::Matrix<double, 6, 6>::Identity() - kalman_gain * H_jacobian) *
- P_;
-}
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/ekf.h b/y2018/control_loops/superstructure/arm/ekf.h
deleted file mode 100644
index 9ae8b47..0000000
--- a/y2018/control_loops/superstructure/arm/ekf.h
+++ /dev/null
@@ -1,53 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
-
-#include "Eigen/Dense"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-// An extended kalman filter for the Arm.
-// Our states are:
-// [theta0, omega0, theta1, omega1, voltage error0, voltage error1]
-class EKF {
- public:
- EKF();
-
- // Resets the internal state back to X. Resets the torque disturbance to 0.
- void Reset(const ::Eigen::Matrix<double, 4, 1> &X);
- // TODO(austin): Offset the internal state when we calibrate.
-
- // Corrects the state estimate with the provided sensor reading.
- void Correct(const ::Eigen::Matrix<double, 2, 1> &Y, double dt);
-
- // Predicts the next state and covariance given the control input.
- void Predict(const ::Eigen::Matrix<double, 2, 1> &U, double dt);
-
- // Returns the current state and covariance estimates.
- const ::Eigen::Matrix<double, 6, 1> &X_hat() const { return X_hat_; }
- double X_hat(int i) const { return X_hat_(i); }
- const ::Eigen::Matrix<double, 6, 6> &P() const { return P_; }
- const ::Eigen::Matrix<double, 6, 6> &P_reset() const { return P_reset_; }
- const ::Eigen::Matrix<double, 6, 6> &P_half_converged() const {
- return P_half_converged_;
- }
- const ::Eigen::Matrix<double, 6, 6> &P_converged() const {
- return P_converged_;
- }
-
- private:
- ::Eigen::Matrix<double, 6, 1> X_hat_;
- ::Eigen::Matrix<double, 6, 6> P_;
- ::Eigen::Matrix<double, 6, 6> P_half_converged_;
- ::Eigen::Matrix<double, 6, 6> P_converged_;
- ::Eigen::Matrix<double, 6, 6> P_reset_;
-};
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
-
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_EKF_H_
diff --git a/y2018/control_loops/superstructure/arm/graph.cc b/y2018/control_loops/superstructure/arm/graph.cc
deleted file mode 100644
index ad6f982..0000000
--- a/y2018/control_loops/superstructure/arm/graph.cc
+++ /dev/null
@@ -1,73 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/graph.h"
-
-#include <algorithm>
-#include <cassert>
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-SearchGraph::SearchGraph(size_t num_vertexes, std::initializer_list<Edge> edges)
- : SearchGraph(num_vertexes, ::std::vector<Edge>(edges)) {}
-
-SearchGraph::SearchGraph(size_t num_vertexes, std::vector<Edge> &&edges)
- : edges_(::std::move(edges)) {
- vertexes_.resize(num_vertexes);
- size_t i = 0;
- for (const auto &edge : edges_) {
- assert(edge.start < num_vertexes);
- assert(edge.end < num_vertexes);
- assert(edge.start != edge.end);
- assert(edge.cost > 0);
- vertexes_[edge.start].forward.emplace_back(HalfEdge{edge.end, i});
- vertexes_[edge.end].reverse.emplace_back(HalfEdge{edge.start, i});
- ++i;
- }
- for (auto &vertex : vertexes_) {
- ::std::sort(vertex.forward.begin(), vertex.forward.end());
- ::std::sort(vertex.reverse.begin(), vertex.reverse.end());
- }
- vertex_heap_.Reserve(num_vertexes);
- SetGoal(0);
-}
-
-SearchGraph::~SearchGraph() {}
-
-void SearchGraph::SetGoal(size_t vertex) {
- if (last_searched_vertex_ == vertex) {
- return;
- }
- assert(vertex < vertexes_.size());
- vertex_heap_.Clear();
-
- for (auto &vertex : vertexes_) {
- vertex.cached_distance = std::numeric_limits<double>::infinity();
- }
-
- vertexes_[vertex].cached_distance = 0.0;
- vertex_heap_.InsertOrDecrease(&vertexes_[vertex]);
-
- while (!vertex_heap_.Empty()) {
- Vertex *vertex = vertex_heap_.PopMin();
- for (const auto &half_edge : vertex->reverse) {
- Vertex *to_adjust = &vertexes_[half_edge.dest];
- double adjust_cost = vertex->cached_distance + GetCost(half_edge);
- if (adjust_cost < to_adjust->cached_distance) {
- to_adjust->cached_distance = adjust_cost;
- vertex_heap_.InsertOrDecrease(to_adjust);
- }
- }
- }
- last_searched_vertex_ = vertex;
-}
-
-double SearchGraph::GetCostToGoal(size_t vertex) {
- assert(vertex < vertexes_.size());
- return vertexes_[vertex].cached_distance;
-}
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/graph.h b/y2018/control_loops/superstructure/arm/graph.h
deleted file mode 100644
index 2300d6a..0000000
--- a/y2018/control_loops/superstructure/arm/graph.h
+++ /dev/null
@@ -1,190 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
-
-#include <algorithm>
-#include <cassert>
-#include <cstdlib>
-#include <limits>
-#include <memory>
-#include <vector>
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-// Grr... normal priority queues don't allow modifying the node cost.
-// This relies on pointer stability for the passed in T.
-template <typename T>
-class IntrusivePriorityQueue {
- public:
- // T is expected to be like so:
- //
- // struct T {
- // IntrusivePriorityQueue<T>::QueueEntry* entry_ = nullptr;
- // };
- //
- // This QueueEntry is like a unique_ptr that always maintains a link
- // back to the unique_ptr pointing to that entry. This maintains the
- // mapping between the elements and their indexes in the list.
- // This mapping is crucial for supporting O(log(n)) decrease-key
- // operations.
- class QueueEntry {
- public:
- QueueEntry(T *value) : value_(value) { value_->entry_ = this; }
-
- QueueEntry(QueueEntry &&o) : value_(::std::move(o.value_)) {
- if (value_) {
- value_->entry_ = this;
- }
- o.value_ = nullptr;
- }
-
- QueueEntry &operator=(QueueEntry &&o) {
- if (this == &o) return *this;
- std::swap(value_, o.value_);
- if (value_) value_->entry_ = this;
- if (o.value_) o.value_->entry_ = this;
- return *this;
- }
-
- ~QueueEntry() {
- if (value_) value_->entry_ = nullptr;
- }
-
- T *get() { return value_; }
-
- bool operator<(const QueueEntry &o) const { return *value_ < *o.value_; }
-
- private:
- friend class IntrusivePriorityQueue;
- T *value_ = nullptr;
- };
-
- // Conceptually, nodes start at inifinity and can only decrease in cost.
- // The infinity nodes are not in the queue.
- void InsertOrDecrease(T *t) {
- if (t->entry_ != nullptr) {
- std::push_heap(queue_.begin(), queue_.begin() + (GetIndex(t) + 1));
- } else {
- queue_.emplace_back(t);
- std::push_heap(queue_.begin(), queue_.end());
- }
- }
-
- // For testing.
- void VerifyCorrectness() {
- for (size_t i = 0; i < queue_.size(); ++i) {
- assert(queue_[i].value_->entry_ == &queue_[i]);
- }
-
- assert(std::is_heap(queue_.begin(), queue_.end()));
- }
-
- T *PopMin() {
- T *result = queue_.front().get();
- std::pop_heap(queue_.begin(), queue_.end());
- queue_.pop_back();
- return result;
- }
-
- bool Empty() const { return queue_.empty(); }
-
- void Clear() { queue_.clear(); }
-
- void Reserve(size_t n) { queue_.reserve(n); }
-
- private:
- size_t GetIndex(T *t) { return t->entry_ - &queue_[0]; }
-
- std::vector<QueueEntry> queue_;
-};
-
-// You pass in a graph (with num_vertexes) and a set of edges described by
-// edges.
-// Then, this will search that graph to find the optimal path from all points
-// to a goal point.
-//
-// A traversal algorithm would be as such:
-// // Initialize graph...
-// graph.SetGoal(my_goal);
-// size_t current_node = node;
-//
-// while (current_node != my_goal) {
-// edge = argmin(GetCostToGoal(edge.dest) for edge in
-// Neighbors(current_node));
-// Travel along edge # edge.edge_id.
-// current_node = edge.dest;
-// }
-class SearchGraph {
- public:
- struct Edge {
- size_t start;
- size_t end;
- double cost;
- };
-
- // The part of an edge stored at every vertex.
- struct HalfEdge {
- size_t dest;
- // This id matches the corresponding id in the edges array from the
- // constructor.
- size_t edge_id;
- bool operator<(const HalfEdge &o) const { return dest < o.dest; }
- };
-
- SearchGraph(size_t num_vertexes, std::vector<Edge> &&edges);
- SearchGraph(size_t num_vertexes, ::std::initializer_list<Edge> edges);
- SearchGraph(SearchGraph &&o)
- : vertexes_(::std::move(o.vertexes_)),
- edges_(::std::move(o.edges_)),
- vertex_heap_(::std::move(o.vertex_heap_)) {
- last_searched_vertex_ = std::numeric_limits<size_t>::max();
- }
-
- ~SearchGraph();
-
- // Set the goal vertex.
- void SetGoal(size_t vertex);
-
- // Compute the cost if you're at vertex.
- double GetCostToGoal(size_t vertex);
-
- // For walking the graph yourself.
- const std::vector<HalfEdge> &Neighbors(size_t vertex) {
- assert(vertex < vertexes_.size());
- return vertexes_[vertex].forward;
- }
-
- size_t num_vertexes() const { return vertexes_.size(); }
-
- const std::vector<Edge> &edges() const { return edges_; }
-
- private:
- struct Vertex {
- std::vector<HalfEdge> forward;
- std::vector<HalfEdge> reverse;
- double cached_distance = std::numeric_limits<double>::infinity();
-
- bool operator<(const Vertex &o) {
- return cached_distance > o.cached_distance;
- }
-
- IntrusivePriorityQueue<Vertex>::QueueEntry *entry_ = nullptr;
- };
-
- double GetCost(const HalfEdge &edge) { return edges_[edge.edge_id].cost; }
- std::vector<Vertex> vertexes_;
- std::vector<Edge> edges_;
-
- IntrusivePriorityQueue<Vertex> vertex_heap_;
-
- size_t last_searched_vertex_ = std::numeric_limits<size_t>::max();
-};
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
-
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_GRAPH_H_
diff --git a/y2018/control_loops/superstructure/arm/graph_test.cc b/y2018/control_loops/superstructure/arm/graph_test.cc
deleted file mode 100644
index 8f246a5..0000000
--- a/y2018/control_loops/superstructure/arm/graph_test.cc
+++ /dev/null
@@ -1,73 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/graph.h"
-
-#include "gtest/gtest.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-namespace testing {
-
-class HeapTest {
- public:
- explicit HeapTest(size_t id, double cost) : id_(id), cost_(cost) {}
-
- void SetCost(double cost) { cost_ = cost; }
-
- bool operator<(const HeapTest &o) { return cost_ > o.cost_; }
-
- private:
- size_t id_;
- double cost_;
-
- IntrusivePriorityQueue<HeapTest>::QueueEntry *entry_ = nullptr;
- friend class IntrusivePriorityQueue<HeapTest>;
-};
-
-TEST(IntrusivePriorityQueue, HeapTest) {
- std::vector<HeapTest> items;
- IntrusivePriorityQueue<HeapTest> queue;
- size_t i = 0;
- for (double value :
- {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0}) {
- items.push_back(HeapTest(i, value));
- ++i;
- }
- for (auto &item : items) {
- queue.InsertOrDecrease(&item);
- queue.VerifyCorrectness();
- }
-
- items[6].SetCost(-1.0);
- queue.InsertOrDecrease(&items[6]);
- queue.VerifyCorrectness();
-
- while (!queue.Empty()) {
- queue.PopMin();
- queue.VerifyCorrectness();
- }
-}
-
-TEST(GraphTest, Distances) {
- SearchGraph graph(6, {{0, 1, 4.0},
- {1, 5, 2.0},
- {2, 0, 7.0},
- {2, 3, 8.0},
- {3, 1, 2.0},
- {4, 0, 1.0},
- {5, 2, 9.0},
- {5, 4, 8.0}});
-
- graph.SetGoal(2);
- size_t i = 0;
- for (double expected_cost : {15, 11, 0, 13, 16, 9}) {
- EXPECT_EQ(graph.GetCostToGoal(i), expected_cost);
- ++i;
- }
-}
-
-} // namespace testing
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/y2018/control_loops/superstructure/arm/trajectory.cc
deleted file mode 100644
index c6e09dd..0000000
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ /dev/null
@@ -1,618 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
-
-#include "Eigen/Dense"
-#include "aos/logging/logging.h"
-#include "frc971/control_loops/dlqr.h"
-#include "frc971/control_loops/jacobian.h"
-#include "gflags/gflags.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
-DEFINE_double(lqr_proximal_pos, 0.15, "Position LQR gain");
-DEFINE_double(lqr_proximal_vel, 4.0, "Velocity LQR gain");
-DEFINE_double(lqr_distal_pos, 0.20, "Position LQR gain");
-DEFINE_double(lqr_distal_vel, 4.0, "Velocity LQR gain");
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-Path Path::Reversed(const Path &p) {
- ::std::vector<::std::array<double, 6>> list(p.distances().size());
- for (ssize_t i = list.size() - 1; i >= 0; --i) {
- list[i][0] = p.thetas_[list.size() - 1 - i][0];
- list[i][1] = p.thetas_[list.size() - 1 - i][1];
-
- list[i][2] = -p.omegas_[list.size() - 1 - i][0];
- list[i][3] = -p.omegas_[list.size() - 1 - i][1];
-
- list[i][4] = p.alphas_[list.size() - 1 - i][0];
- list[i][5] = p.alphas_[list.size() - 1 - i][1];
- }
- return Path(list);
-}
-
-::std::unique_ptr<Path> Path::Reversed(::std::unique_ptr<Path> p) {
- return ::std::unique_ptr<Path>(new Path(Path::Reversed(*p)));
-}
-
-Path::Path(::std::vector<::std::array<double, 6>> list) {
- distances_.reserve(list.size());
- ::Eigen::Matrix<double, 2, 1> last_theta;
- bool first = true;
-
- // Pull apart the initializer list into points, and compute the distance
- // lookup for efficient path lookups.
- for (const auto &e : list) {
- thetas_.push_back(
- (::Eigen::Matrix<double, 2, 1>() << e[0], e[1]).finished());
- omegas_.push_back(
- (::Eigen::Matrix<double, 2, 1>() << e[2], e[3]).finished());
- alphas_.push_back(
- (::Eigen::Matrix<double, 2, 1>() << e[4], e[5]).finished());
-
- if (first) {
- distances_.push_back(0.0);
- } else {
- distances_.push_back((thetas_.back() - last_theta).norm() +
- distances_.back());
- }
- first = false;
- last_theta = thetas_.back();
- }
-}
-
-Path::Path(::std::initializer_list<::std::array<double, 6>> list) {
- distances_.reserve(list.size());
- ::Eigen::Matrix<double, 2, 1> last_theta;
- bool first = true;
-
- // Pull apart the initializer list into points, and compute the distance
- // lookup for efficient path lookups.
- for (const auto &e : list) {
- thetas_.push_back(
- (::Eigen::Matrix<double, 2, 1>() << e[0], e[1]).finished());
- omegas_.push_back(
- (::Eigen::Matrix<double, 2, 1>() << e[2], e[3]).finished());
- alphas_.push_back(
- (::Eigen::Matrix<double, 2, 1>() << e[4], e[5]).finished());
-
- if (first) {
- distances_.push_back(0.0);
- } else {
- distances_.push_back((thetas_.back() - last_theta).norm() +
- distances_.back());
- }
- first = false;
- last_theta = thetas_.back();
- }
-}
-
-::std::vector<double> Trajectory::CurvatureOptimizationPass(
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax) {
- ::std::vector<double> max_dvelocity_unfiltered;
- max_dvelocity_unfiltered.reserve(num_plan_points_);
- for (size_t i = 0; i < num_plan_points_; ++i) {
- const double distance = DistanceForIndex(i);
-
- // TODO(austin): We are looking up the index in the path 3 times here.
- const ::Eigen::Matrix<double, 2, 1> theta = path().Theta(distance);
- const ::Eigen::Matrix<double, 2, 1> omega = path().Omega(distance);
- const ::Eigen::Matrix<double, 2, 1> alpha = path().Alpha(distance);
- const ::Eigen::Matrix<double, 4, 1> X =
- (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
- .finished();
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- const ::Eigen::Matrix<double, 2, 1> gravity_volts =
- Dynamics::K3_inverse * Dynamics::GravityTorque(X);
-
- Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 2> omega_square =
- (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
- .finished();
-
- // Here, we can say that
- // d^2/dt^2 theta = d^2/dd^2 theta(d) * (d d/dt)^2
- // Normalize so that the max accel is 1, and take magnitudes. This
- // gives us the max velocity we can be at each point due to
- // curvature.
- double min_good_ddot =
- ::std::sqrt(1.0 / ::std::max(0.001, (alpha_unitizer * alpha).norm()));
-
- const ::Eigen::Matrix<double, 2, 1> vk1 =
- Dynamics::K3_inverse * (K1 * alpha + K2 * omega_square * omega);
- const ::Eigen::Matrix<double, 2, 1> vk2 =
- Dynamics::K3_inverse * Dynamics::K4 * omega;
-
- // Loop through all the various vmin, plan_vmax combinations.
- for (const double c : {-plan_vmax, plan_vmax}) {
- // Also loop through saturating theta0 and theta1
- for (const ::std::tuple<double, double, double> &abgravity :
- {::std::tuple<double, double, double>{vk1(0), vk2(0),
- gravity_volts(0)},
- ::std::tuple<double, double, double>{vk1(1), vk2(1),
- gravity_volts(1)}}) {
- const double a = ::std::get<0>(abgravity);
- const double b = ::std::get<1>(abgravity);
- const double gravity = ::std::get<2>(abgravity);
- const double sqrt_number = b * b - 4.0 * a * (c - gravity);
- // Throw out imaginary solutions to the quadratic.
- if (sqrt_number > 0) {
- const double sqrt_result = ::std::sqrt(sqrt_number);
- const double ddot1 = (-b + sqrt_result) / (2.0 * a);
- const double ddot2 = (-b - sqrt_result) / (2.0 * a);
- // Loop through both solutions.
- for (const double ddot : {ddot1, ddot2}) {
- const ::Eigen::Matrix<double, 2, 1> U =
- vk1 * ddot * ddot + vk2 * ddot - gravity_volts;
-
- // Finally, make sure the velocity is positive and valid.
- if ((U.array().abs() <= plan_vmax + 1e-6).all() && ddot > 0.0) {
- min_good_ddot = ::std::min(min_good_ddot, ddot);
- }
- }
- }
- }
- }
-
- max_dvelocity_unfiltered.push_back(min_good_ddot);
- }
- return max_dvelocity_unfiltered;
-}
-
-double Trajectory::FeasableForwardsAcceleration(
- double goal_distance, double goal_velocity, double plan_vmax,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const {
- // TODO(austin): We are looking up the index in the path 3 times here.
- const ::Eigen::Matrix<double, 2, 1> theta = path().Theta(goal_distance);
- const ::Eigen::Matrix<double, 2, 1> omega = path().Omega(goal_distance);
- const ::Eigen::Matrix<double, 2, 1> alpha = path().Alpha(goal_distance);
-
- const ::Eigen::Matrix<double, 4, 1> X =
- (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
- .finished();
-
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 2> omega_square =
- (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
- .finished();
-
- const ::Eigen::Matrix<double, 2, 1> k_constant =
- Dynamics::K3_inverse *
- ((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
- goal_velocity +
- Dynamics::K4 * omega * goal_velocity - Dynamics::GravityTorque(X));
- const ::Eigen::Matrix<double, 2, 1> k_scalar =
- Dynamics::K3_inverse * K1 * omega;
-
- const double constraint_goal_acceleration =
- ::std::sqrt(
- ::std::max(0.0, 1.0 - ::std::pow((alpha_unitizer * alpha).norm() *
- goal_velocity * goal_velocity,
- 2))) /
- (alpha_unitizer * omega).norm();
-
- double goal_acceleration = -::std::numeric_limits<double>::infinity();
- for (double c : {-plan_vmax, plan_vmax}) {
- for (const ::std::pair<double, double> &ab :
- {::std::pair<double, double>{k_constant(0, 0), k_scalar(0, 0)},
- ::std::pair<double, double>{k_constant(1, 0), k_scalar(1, 0)}}) {
- const double a = ab.first;
- const double b = ab.second;
- const double voltage_accel = (c - a) / b;
- const ::Eigen::Matrix<double, 2, 1> U =
- k_constant + k_scalar * voltage_accel;
-
- if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
- goal_acceleration = ::std::max(voltage_accel, goal_acceleration);
- }
- }
- }
- if (goal_acceleration == -::std::numeric_limits<double>::infinity()) {
- // TODO(austin): The math above doesn't always give a valid solution.
- // Figure out why. It should be an affine transformation.
- return constraint_goal_acceleration;
- }
-
- return ::std::min(constraint_goal_acceleration, goal_acceleration);
-}
-
-double Trajectory::FeasableBackwardsAcceleration(
- double goal_distance, double goal_velocity, double plan_vmax,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const {
- const ::Eigen::Matrix<double, 2, 1> theta = path().Theta(goal_distance);
- const ::Eigen::Matrix<double, 2, 1> omega = path().Omega(goal_distance);
- const ::Eigen::Matrix<double, 2, 1> alpha = path().Alpha(goal_distance);
- const ::Eigen::Matrix<double, 4, 1> X =
- (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
- .finished();
- ::Eigen::Matrix<double, 2, 2> K1;
- ::Eigen::Matrix<double, 2, 2> K2;
-
- Dynamics::NormilizedMatriciesForState(X, &K1, &K2);
-
- const ::Eigen::Matrix<double, 2, 2> omega_square =
- (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
- .finished();
-
- const ::Eigen::Matrix<double, 2, 1> k_constant =
- Dynamics::K3_inverse *
- ((K1 * alpha + K2 * omega_square * omega) * goal_velocity *
- goal_velocity +
- Dynamics::K4 * omega * goal_velocity - Dynamics::GravityTorque(X));
- const ::Eigen::Matrix<double, 2, 1> k_scalar =
- Dynamics::K3_inverse * K1 * omega;
-
- const double constraint_goal_acceleration =
- ::std::sqrt(
- ::std::max(0.0, 1.0 - ::std::pow(((alpha_unitizer * alpha).norm() *
- goal_velocity * goal_velocity),
- 2))) /
- (alpha_unitizer * omega).norm();
-
- double goal_acceleration = -::std::numeric_limits<double>::infinity();
- for (double c : {-plan_vmax, plan_vmax}) {
- for (const ::std::pair<double, double> &ab :
- {::std::pair<double, double>{k_constant(0, 0), k_scalar(0, 0)},
- ::std::pair<double, double>{k_constant(1, 0), k_scalar(1, 0)}}) {
- const double a = ab.first;
- const double b = ab.second;
- // This time, we are doing the other pass. So, find all the
- // decelerations (and flip them) to find the prior velocity.
- const double voltage_accel = (c - a) / b;
-
- const ::Eigen::Matrix<double, 2, 1> U =
- k_constant + k_scalar * voltage_accel;
-
- // TODO(austin): This doesn't always give a valid solution. It really
- // should. Figure out why.
- if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
- goal_acceleration = ::std::max(-voltage_accel, goal_acceleration);
- }
- }
- }
- if (goal_acceleration == -::std::numeric_limits<double>::infinity()) {
- return constraint_goal_acceleration;
- }
-
- return ::std::min(goal_acceleration, constraint_goal_acceleration);
-}
-
-::std::vector<double> Trajectory::BackwardsOptimizationPass(
- const ::std::vector<double> &max_dvelocity,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax) {
- ::std::vector<double> max_dvelocity_back_pass = max_dvelocity;
-
- // Now, iterate over the list of velocities and constrain the acceleration.
- for (int i = num_plan_points_ - 2; i >= 0; --i) {
- const double previous_velocity = max_dvelocity_back_pass[i + 1];
- const double prev_distance = DistanceForIndex(i + 1);
-
- // Now, integrate with respect to distance (not time like normal).
- // Subdivide the plan step size into kNumSteps steps.
- double integrated_distance = 0.0;
- double integrated_velocity = previous_velocity;
- constexpr int kNumSteps = 50;
- for (int j = 0; j < kNumSteps; ++j) {
- // Compute the acceleration with respect to time.
- const double acceleration = FeasableBackwardsAcceleration(
- prev_distance - integrated_distance, integrated_velocity, plan_vmax,
- alpha_unitizer);
-
- // And then integrate it with respect to distance.
- const double integration_step_size_distance =
- step_size_ / static_cast<double>(kNumSteps);
- integrated_distance += integration_step_size_distance;
- integrated_velocity =
- ::std::sqrt(2.0 * acceleration * integration_step_size_distance +
- ::std::pow(integrated_velocity, 2));
- }
- max_dvelocity_back_pass[i] =
- ::std::min(integrated_velocity, max_dvelocity_back_pass[i]);
- }
-
- return max_dvelocity_back_pass;
-}
-
-::std::vector<double> Trajectory::ForwardsOptimizationPass(
- const ::std::vector<double> &max_dvelocity,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax) {
- ::std::vector<double> max_dvelocity_forward_pass = max_dvelocity;
- // Now, iterate over the list of velocities and constrain the acceleration.
- for (size_t i = 1; i < num_plan_points_; ++i) {
- const double previous_velocity = max_dvelocity_forward_pass[i - 1];
- const double previous_distance = DistanceForIndex(i - 1);
-
- // Now, integrate with respect to distance (not time like normal).
- // Subdivide the plan step size into kNumSteps steps.
- double integrated_distance = 0.0;
- double integrated_velocity = previous_velocity;
- constexpr int kNumSteps = 10;
- for (int j = 0; j < kNumSteps; ++j) {
- // Compute the acceleration with respect to time.
- const double acceleration = FeasableForwardsAcceleration(
- previous_distance + integrated_distance, integrated_velocity,
- plan_vmax, alpha_unitizer);
-
- // And then integrate it with respect to distance.
- const double integration_step_size_distance =
- step_size_ / static_cast<double>(kNumSteps);
- integrated_distance += integration_step_size_distance;
- integrated_velocity =
- ::std::sqrt(2.0 * acceleration * integration_step_size_distance +
- ::std::pow(integrated_velocity, 2));
- }
-
- max_dvelocity_forward_pass[i] =
- ::std::min(integrated_velocity, max_dvelocity_forward_pass[i]);
- }
-
- return max_dvelocity_forward_pass;
-}
-
-void TrajectoryFollower::Reset() {
- next_goal_ = last_goal_ = goal_ = ::Eigen::Matrix<double, 2, 1>::Zero();
- U_unsaturated_ = U_ff_ = U_ = ::Eigen::Matrix<double, 2, 1>::Zero();
- goal_acceleration_ = 0.0;
- saturation_fraction_along_path_ = 0.0;
- omega_.setZero();
- if (trajectory_ != nullptr) {
- set_theta(trajectory_->ThetaT(goal_(0)));
- }
-}
-
-::Eigen::Matrix<double, 2, 6> TrajectoryFollower::K_at_state(
- const ::Eigen::Matrix<double, 6, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U) {
- const double kProximalPos = FLAGS_lqr_proximal_pos;
- const double kProximalVel = FLAGS_lqr_proximal_vel;
- const double kDistalPos = FLAGS_lqr_distal_pos;
- const double kDistalVel = FLAGS_lqr_distal_vel;
- const ::Eigen::DiagonalMatrix<double, 4> Q =
- (::Eigen::DiagonalMatrix<double, 4>().diagonal()
- << 1.0 / ::std::pow(kProximalPos, 2),
- 1.0 / ::std::pow(kProximalVel, 2), 1.0 / ::std::pow(kDistalPos, 2),
- 1.0 / ::std::pow(kDistalVel, 2))
- .finished()
- .asDiagonal();
-
- const ::Eigen::DiagonalMatrix<double, 2> R =
- (::Eigen::DiagonalMatrix<double, 2>().diagonal()
- << 1.0 / ::std::pow(12.0, 2),
- 1.0 / ::std::pow(12.0, 2))
- .finished()
- .asDiagonal();
-
- const ::Eigen::Matrix<double, 4, 4> final_A =
- ::frc971::control_loops::NumericalJacobianX<4, 2>(
- Dynamics::UnboundedDiscreteDynamics, X.block<4, 1>(0, 0), U, 0.00505);
- const ::Eigen::Matrix<double, 4, 2> final_B =
- ::frc971::control_loops::NumericalJacobianU<4, 2>(
- Dynamics::UnboundedDiscreteDynamics, X.block<4, 1>(0, 0), U, 0.00505);
-
- ::Eigen::Matrix<double, 4, 4> S;
- ::Eigen::Matrix<double, 2, 4> sub_K;
- if (::frc971::controls::dlqr<4, 2>(final_A, final_B, Q, R, &sub_K, &S) == 0) {
- ::Eigen::EigenSolver<::Eigen::Matrix<double, 4, 4>> eigensolver(
- final_A - final_B * sub_K);
-
- ::Eigen::Matrix<double, 2, 6> K;
- K.setZero();
- K.block<2, 4>(0, 0) = sub_K;
- K(0, 4) = 1.0;
- K(1, 5) = 1.0;
- failed_solutions_ = 0;
- last_K_ = K;
- } else {
- ++failed_solutions_;
- }
- return last_K_;
-}
-
-void TrajectoryFollower::USaturationSearch(
- double goal_distance, double last_goal_distance, double goal_velocity,
- double last_goal_velocity, double saturation_fraction_along_path,
- const ::Eigen::Matrix<double, 2, 6> &K,
- const ::Eigen::Matrix<double, 6, 1> &X, const Trajectory &trajectory,
- ::Eigen::Matrix<double, 2, 1> *U, double *saturation_goal_velocity,
- double *saturation_goal_acceleration) {
- double saturation_goal_distance =
- ((goal_distance - last_goal_distance) * saturation_fraction_along_path +
- last_goal_distance);
-
- const ::Eigen::Matrix<double, 2, 1> theta_t =
- trajectory.ThetaT(saturation_goal_distance);
- *saturation_goal_velocity = trajectory.InterpolateVelocity(
- saturation_goal_distance, last_goal_distance, goal_distance,
- last_goal_velocity, goal_velocity);
- *saturation_goal_acceleration = trajectory.InterpolateAcceleration(
- last_goal_distance, goal_distance, last_goal_velocity, goal_velocity);
- const ::Eigen::Matrix<double, 2, 1> omega_t =
- trajectory.OmegaT(saturation_goal_distance, *saturation_goal_velocity);
- const ::Eigen::Matrix<double, 2, 1> alpha_t =
- trajectory.AlphaT(saturation_goal_distance, *saturation_goal_velocity,
- *saturation_goal_acceleration);
- const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
- const ::Eigen::Matrix<double, 2, 1> U_ff =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
-
- *U = U_ff + K * (R - X);
-}
-
-::Eigen::Matrix<double, 2, 1> TrajectoryFollower::PlanNextGoal(
- const ::Eigen::Matrix<double, 2, 1> &goal, double plan_vmax, double dt) {
- // Figure out where we would be if we were to accelerate as fast as
- // our constraints allow.
- ::Eigen::Matrix<double, 2, 1> next_goal = ::frc971::control_loops::RungeKutta(
- [this, &plan_vmax](const ::Eigen::Matrix<double, 2, 1> &X) {
- return (::Eigen::Matrix<double, 2, 1>() << X(1),
- trajectory_->FeasableForwardsAcceleration(
- X(0), X(1), plan_vmax, trajectory_->alpha_unitizer()))
- .finished();
- },
- goal, dt);
-
- // Min that with the backwards pass velocity in case the backwards pass
- // wants us to slow down.
- const double next_trajectory_velocity =
- trajectory_->GetDVelocity(next_goal(0));
- if (next_trajectory_velocity < next_goal(1)) {
- next_goal(1) = next_trajectory_velocity;
- // And then recompute how far to go with our new trajectory in mind.
- double goal_acceleration = trajectory_->InterpolateAcceleration(
- goal(0), next_goal(0), goal(1), next_goal(1));
- next_goal(0) = (goal(0) + goal(1) * dt + 0.5 * dt * dt * goal_acceleration);
- next_goal(1) = trajectory_->GetDVelocity(next_goal(0));
- }
- return next_goal;
-}
-
-void TrajectoryFollower::Update(const ::Eigen::Matrix<double, 6, 1> &X,
- bool disabled, double dt, double plan_vmax,
- double voltage_limit) {
- // TODO(austin): Separate voltage limit for shoulder for no path.
- last_goal_ = goal_;
-
- if (!has_path()) {
- // No path, so go to the last theta (no velocity).
- last_goal_.setZero();
- next_goal_.setZero();
- goal_.setZero();
- goal_acceleration_ = 0.0;
- saturation_fraction_along_path_ = 0.0;
-
- if (disabled) {
- U_ff_.setZero();
- U_.setZero();
- U_unsaturated_.setZero();
- } else {
- const ::Eigen::Matrix<double, 6, 1> R =
- trajectory_->R(theta_, ::Eigen::Matrix<double, 2, 1>::Zero());
-
- U_ff_ = Dynamics::FF_U(X.block<4, 1>(0, 0),
- ::Eigen::Matrix<double, 2, 1>::Zero(),
- ::Eigen::Matrix<double, 2, 1>::Zero());
- const ::Eigen::Matrix<double, 2, 6> K = K_at_state(X, U_ff_);
- U_ = U_unsaturated_ = U_ff_ + K * (R - X);
-
- U_ = U_.array().max(-voltage_limit).min(voltage_limit);
- }
- return;
- }
-
- if (disabled) {
- // If we are disabled, it's likely for a bit of time. So, lets freeze
- // ourselves on the path (accept the previous motion, but then zero out the
- // velocity). Set all outputs to 0 as well.
- next_goal_(1) = 0.0;
- goal_ = next_goal_;
- goal_acceleration_ = 0.0;
- U_unsaturated_.setZero();
- U_.setZero();
- U_ff_.setZero();
- saturation_fraction_along_path_ = 1.0;
- theta_ = trajectory_->ThetaT(goal_(0));
- omega_.setZero();
- return;
- }
-
- // To avoid exposing the new goals before the outer code has a chance to
- // querry the internal state, move to the new goals here.
- goal_ = next_goal_;
-
- if (::std::abs(goal_(0) - path()->length()) < 1e-2) {
- // If we go backwards along the path near the goal, snap us to the end
- // point or we'll never actually finish.
- if (goal_acceleration_ * dt + goal_(1) < 0.0 ||
- goal_(0) > path()->length()) {
- goal_(0) = path()->length();
- goal_(1) = 0.0;
- }
- }
-
- if (goal_(0) == path()->length()) {
- next_goal_(0) = goal_(0);
- next_goal_(1) = 0.0;
- goal_acceleration_ = 0.0;
- } else {
- // Figure out where we would be if we were to accelerate as fast as
- // our constraints allow.
- next_goal_ = PlanNextGoal(goal_, plan_vmax, dt);
-
- goal_acceleration_ = trajectory_->InterpolateAcceleration(
- goal_(0), next_goal_(0), goal_(1), next_goal_(1));
- }
-
- const ::Eigen::Matrix<double, 2, 1> theta_t = trajectory_->ThetaT(goal_(0));
- const ::Eigen::Matrix<double, 2, 1> omega_t =
- trajectory_->OmegaT(goal_(0), goal_(1));
- const ::Eigen::Matrix<double, 2, 1> alpha_t =
- trajectory_->AlphaT(goal_(0), goal_(1), goal_acceleration_);
-
- const ::Eigen::Matrix<double, 6, 1> R = trajectory_->R(theta_t, omega_t);
-
- U_ff_ = Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t);
-
- const ::Eigen::Matrix<double, 2, 6> K = K_at_state(X, U_ff_);
- U_ = U_unsaturated_ = U_ff_ + K * (R - X);
-
- // Ok, now we know if we are staturated or not. If we are, time to search
- // between here and our previous goal either until we find a state where we
- // aren't saturated, or we are really close to our starting point.
- saturation_fraction_along_path_ = 1.0;
- if ((U_.array().abs() > voltage_limit).any()) {
- // Saturated. Let's do a binary search.
- double step_size;
- if ((goal_(0) - last_goal_(0)) < 1e-8) {
- // print "Not bothering to move"
- // Avoid the divide by 0 when interpolating. Just don't move since we
- // are saturated.
- saturation_fraction_along_path_ = 0.0;
- step_size = 0.0;
- } else {
- saturation_fraction_along_path_ = 0.5;
- step_size = 0.5;
- }
-
- // Pull us back to the previous point until we aren't saturated anymore.
- double saturation_goal_velocity;
- double saturation_goal_acceleration;
- while (step_size > 0.01) {
- USaturationSearch(goal_(0), last_goal_(0), goal_(1), last_goal_(1),
- saturation_fraction_along_path_, K, X, *trajectory_,
- &U_, &saturation_goal_velocity,
- &saturation_goal_acceleration);
- step_size = step_size * 0.5;
- if ((U_.array().abs() > voltage_limit).any()) {
- saturation_fraction_along_path_ -= step_size;
- } else {
- saturation_fraction_along_path_ += step_size;
- }
- }
-
- goal_(0) = ((goal_(0) - last_goal_(0)) * saturation_fraction_along_path_ +
- last_goal_(0));
- goal_(1) = saturation_goal_velocity;
-
- next_goal_ = PlanNextGoal(goal_, plan_vmax, dt);
-
- goal_acceleration_ = trajectory_->InterpolateAcceleration(
- goal_(0), next_goal_(0), goal_(1), next_goal_(1));
-
- U_ = U_.array().max(-voltage_limit).min(voltage_limit);
- }
- theta_ = trajectory_->ThetaT(goal_(0));
- omega_ = trajectory_->OmegaT(goal_(0), goal_(1));
-}
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/arm/trajectory.h b/y2018/control_loops/superstructure/arm/trajectory.h
deleted file mode 100644
index a3a4246..0000000
--- a/y2018/control_loops/superstructure/arm/trajectory.h
+++ /dev/null
@@ -1,438 +0,0 @@
-#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
-#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
-
-#include <array>
-#include <initializer_list>
-#include <memory>
-#include <vector>
-
-#include "Eigen/Dense"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-
-// This class represents a path in theta0, theta1 space. It also returns the
-// position, velocity and acceleration of the path as a function of path
-// distance.
-class Path {
- public:
- // Constructs a path from an initializer list of (theta0, theta1, omega0,
- // omega1, alpha0, alpha1)
- Path(::std::initializer_list<::std::array<double, 6>> list);
- // TODO(austin): I'd like to not have 2 constructors, but it looks like C++
- // doesn't want that.
- Path(::std::vector<::std::array<double, 6>> list);
-
- static ::std::unique_ptr<Path> Reversed(::std::unique_ptr<Path> p);
- static Path Reversed(const Path &p);
-
- // Returns the length of the path in radians.
- double length() const { return distances_.back(); }
-
- // Returns theta, omega, and alpha as a function of path distance. Distances
- // before the beginning of the path or after the end of the path will get
- // truncated to the nearest end.
- ::Eigen::Matrix<double, 2, 1> Theta(double distance) const {
- return Interpolate(thetas_, distance);
- }
- ::Eigen::Matrix<double, 2, 1> Omega(double distance) const {
- return Interpolate(omegas_, distance);
- }
- ::Eigen::Matrix<double, 2, 1> Alpha(double distance) const {
- return Interpolate(alphas_, distance);
- }
-
- const ::std::vector<double> &distances() const { return distances_; }
-
- private:
- // Interpolates the function represented by the list of points at the provided
- // distance. Distances outside the range will get truncated. This is a
- // log(n) algorithm so we can run it live on the bot.
- ::Eigen::Matrix<double, 2, 1> Interpolate(
- const ::std::vector<::Eigen::Matrix<double, 2, 1>> &points,
- double distance) const {
- if (distance <= 0.0) {
- return points[0];
- }
- if (distance >= length()) {
- return points.back();
- }
- // Keep a max and min, and pull them in towards eachother until they bound
- // our point exactly.
- size_t after = ::std::distance(
- distances_.begin(),
- ::std::lower_bound(distances_.begin(), distances_.end(), distance));
- size_t before = after - 1;
- return (distance - distances_[before]) * (points[after] - points[before]) /
- (distances_[after] - distances_[before]) +
- points[before];
- }
-
- // The list of theta points in the path.
- ::std::vector<::Eigen::Matrix<double, 2, 1>> thetas_;
- // The list of omega points in the path.
- ::std::vector<::Eigen::Matrix<double, 2, 1>> omegas_;
- // The list of alpha points in the path.
- ::std::vector<::Eigen::Matrix<double, 2, 1>> alphas_;
-
- // The distance along the path of each point.
- ::std::vector<double> distances_;
-};
-
-namespace testing {
-class TrajectoryTest_IndicesForDistanceTest_Test;
-} // namespace testing
-
-// A trajectory is a path and a set of velocities as a function of distance.
-class Trajectory {
- public:
- // Constructs a trajectory (but doesn't calculate it) given a path and a step
- // size.
- Trajectory(::std::unique_ptr<const Path> path, double gridsize)
- : path_(::std::move(path)),
- num_plan_points_(
- static_cast<size_t>(::std::ceil(path_->length() / gridsize) + 1)),
- step_size_(path_->length() /
- static_cast<double>(num_plan_points_ - 1)) {
- alpha_unitizer_.setZero();
- }
-
- // Optimizes the trajectory. The path will adhere to the constraints that
- // || angular acceleration * alpha_unitizer || < 1, and the applied voltage <
- // plan_vmax.
- void OptimizeTrajectory(const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,
- double plan_vmax) {
- if (path_ == nullptr) {
- abort();
- }
- alpha_unitizer_ = alpha_unitizer;
-
- max_dvelocity_unfiltered_ =
- CurvatureOptimizationPass(alpha_unitizer, plan_vmax);
-
- // We need to start and end the trajectory with 0 velocity.
- max_dvelocity_unfiltered_[0] = 0.0;
- max_dvelocity_unfiltered_[max_dvelocity_unfiltered_.size() - 1] = 0.0;
-
- max_dvelocity_ = BackwardsOptimizationPass(max_dvelocity_unfiltered_,
- alpha_unitizer, plan_vmax);
- max_dvelocity_forward_pass_ =
- ForwardsOptimizationPass(max_dvelocity_, alpha_unitizer, plan_vmax);
- }
-
- // Returns an array of the distances used in the plan. The starting point
- // (0.0), and end point (path->length()) are included in the array.
- ::std::vector<double> DistanceArray() const {
- ::std::vector<double> result;
- result.reserve(num_plan_points_);
- for (size_t i = 0; i < num_plan_points_; ++i) {
- result.push_back(DistanceForIndex(i));
- }
- return result;
- }
-
- // Computes the maximum velocity that we can follow the path while adhering to
- // the constraints that || angular acceleration * alpha_unitizer || < 1, and
- // the applied voltage < plan_vmax. Returns the velocities.
- ::std::vector<double> CurvatureOptimizationPass(
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax);
-
- // Computes the maximum forwards feasable acceleration at a given position
- // while adhering to the plan_vmax and alpha_unitizer constraints.
- // This gives us the maximum path distance acceleration (d^2d/dt^2) for any
- // initial position and velocity for the forwards path.
- double FeasableForwardsAcceleration(
- double goal_distance, double goal_velocity, double plan_vmax,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const;
-
- // Computes the maximum backwards feasable acceleration at a given position
- // while adhering to the plan_vmax and alpha_unitizer constraints.
- // This gives us the maximum path distance acceleration (d^2d/dt^2) for any
- // initial position and velocity for the backwards path.
- // Note: positive acceleration means speed up while going in the negative
- // direction on the path.
- double FeasableBackwardsAcceleration(
- double goal_distance, double goal_velocity, double plan_vmax,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) const;
-
- // Executes the backwards path optimization pass.
- ::std::vector<double> BackwardsOptimizationPass(
- const ::std::vector<double> &max_dvelocity,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax);
-
- // Executes the forwards path optimization pass.
- ::std::vector<double> ForwardsOptimizationPass(
- const ::std::vector<double> &max_dvelocity,
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double plan_vmax);
-
- // Returns the number of points used for the plan.
- size_t num_plan_points() const { return num_plan_points_; }
-
- // Returns the curvature only velocity plan.
- const ::std::vector<double> &max_dvelocity_unfiltered() const {
- return max_dvelocity_unfiltered_;
- }
- // Returns the backwards pass + curvature plan.
- const ::std::vector<double> &max_dvelocity() const { return max_dvelocity_; }
- // Returns the full plan. This isn't useful at runtime since we don't want to
- // be tied to a specific acceleration plan when we hit saturation.
- const ::std::vector<double> &max_dvelocity_forward_pass() const {
- return max_dvelocity_forward_pass_;
- }
-
- // Returns the interpolated velocity at the distance d along the path. The
- // math assumes constant acceleration from point (d0, v0), to point (d1, v1),
- // and that we are at point d between the two.
- static double InterpolateVelocity(double d, double d0, double d1, double v0,
- double v1) {
- // We don't support negative velocities. Report 0 velocity in that case.
- // TODO(austin): Verify this doesn't show up in the real world.
- if (v0 < 0 || v1 < 0) {
- return 0.0;
- }
- if (d <= d0) {
- return v0;
- }
- if (d >= d1) {
- return v1;
- }
- return ::std::sqrt(v0 * v0 + (v1 * v1 - v0 * v0) * (d - d0) / (d1 - d0));
- }
-
- // Computes the path distance velocity of the plan as a function of the
- // distance.
- double GetDVelocity(double distance) const {
- return GetDVelocity(distance, max_dvelocity_);
- }
- double GetDVelocity(double d, const ::std::vector<double> &plan) const {
- ::std::pair<size_t, size_t> indices = IndicesForDistance(d);
- const double v0 = plan[indices.first];
- const double v1 = plan[indices.second];
- const double d0 = DistanceForIndex(indices.first);
- const double d1 = DistanceForIndex(indices.second);
- return InterpolateVelocity(d, d0, d1, v0, v1);
- }
-
- // Computes the path distance acceleration of the plan as a function of the
- // distance.
- double GetDAcceleration(double distance) const {
- return GetDAcceleration(distance, max_dvelocity_);
- }
- double GetDAcceleration(double distance, const ::std::vector<double> &plan) const {
- ::std::pair<size_t, size_t> indices = IndicesForDistance(distance);
- const double v0 = plan[indices.first];
- const double v1 = plan[indices.second];
- const double d0 = DistanceForIndex(indices.first);
- const double d1 = DistanceForIndex(indices.second);
- return InterpolateAcceleration(d0, d1, v0, v1);
- }
-
- // Returns the acceleration along the path segment assuming constant
- // acceleration.
- static double InterpolateAcceleration(double d0, double d1, double v0,
- double v1) {
- return 0.5 * (::std::pow(v1, 2) - ::std::pow(v0, 2)) / (d1 - d0);
- }
-
- ::Eigen::Matrix<double, 2, 1> ThetaT(double d) const {
- return path_->Theta(d);
- }
-
- // Returns d theta/dt at a specified path distance and velocity.
- ::Eigen::Matrix<double, 2, 1> OmegaT(double distance, double velocity) const {
- if (distance > path_->length() || distance < 0.0) {
- return ::Eigen::Matrix<double, 2, 1>::Zero();
- } else {
- return path_->Omega(distance) * velocity;
- }
- }
-
- // Returns d^2 theta/dt^2 at a specified path distance, velocity and
- // acceleration.
- ::Eigen::Matrix<double, 2, 1> AlphaT(double distance, double velocity,
- double acceleration) const {
- if (distance > path_->length() || distance < 0.0) {
- return ::Eigen::Matrix<double, 2, 1>::Zero();
- } else {
- return path_->Alpha(distance) * ::std::pow(velocity, 2) +
- path_->Omega(distance) * acceleration;
- }
- }
-
- // Converts a theta and omega vector to a full state vector.
- static ::Eigen::Matrix<double, 6, 1> R(
- ::Eigen::Matrix<double, 2, 1> theta_t,
- ::Eigen::Matrix<double, 2, 1> omega_t) {
- return (::Eigen::Matrix<double, 6, 1>() << theta_t(0, 0), omega_t(0, 0),
- theta_t(1, 0), omega_t(1, 0), 0.0, 0.0)
- .finished();
- }
-
- const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer() const {
- return alpha_unitizer_;
- }
-
- const Path &path() const { return *path_; }
-
- private:
- friend class testing::TrajectoryTest_IndicesForDistanceTest_Test;
-
- // Returns the distance along the path for a specific index in the plan.
- double DistanceForIndex(size_t index) const {
- return static_cast<double>(index) * step_size_;
- }
-
- // Returns the before and after indices for a specific distance in the plan.
- ::std::pair<size_t, size_t> IndicesForDistance(double distance) const {
- const double path_length = path_->length();
- if (distance <= 0.0) {
- return ::std::pair<size_t, size_t>(0, 1);
- }
- const size_t lower_index =
- ::std::min(static_cast<size_t>(num_plan_points_ - 2),
- static_cast<size_t>(::std::floor((num_plan_points_ - 1) *
- distance / path_length)));
-
- return ::std::pair<size_t, size_t>(lower_index, lower_index + 1);
- }
-
- // The path to follow.
- ::std::unique_ptr<const Path> path_;
- // The number of points in the plan.
- const size_t num_plan_points_;
- // A cached version of the step size since we need this a *lot*.
- const double step_size_;
-
- ::std::vector<double> max_dvelocity_unfiltered_;
- ::std::vector<double> max_dvelocity_;
- ::std::vector<double> max_dvelocity_forward_pass_;
-
- ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
-};
-
-// This class tracks the current goal along trajectories and paths.
-class TrajectoryFollower {
- public:
- TrajectoryFollower(const ::Eigen::Matrix<double, 2, 1> &theta)
- : trajectory_(nullptr), theta_(theta) {
- omega_.setZero();
- last_K_.setZero();
- Reset();
- }
-
- TrajectoryFollower(Trajectory *const trajectory) : trajectory_(trajectory) {
- last_K_.setZero();
- Reset();
- }
-
- bool has_path() const { return trajectory_ != nullptr; }
-
- void set_theta(const ::Eigen::Matrix<double, 2, 1> &theta) { theta_ = theta; }
-
- // Returns the goal distance along the path.
- const ::Eigen::Matrix<double, 2, 1> &goal() const { return goal_; }
- double goal(int i) const { return goal_(i); }
-
- // Starts over at the beginning of the path.
- void Reset();
-
- // Switches paths and starts at the beginning of the path.
- void SwitchTrajectory(const Trajectory *trajectory) {
- trajectory_ = trajectory;
- Reset();
- }
-
- // Returns the controller gain at the provided state.
- ::Eigen::Matrix<double, 2, 6> K_at_state(
- const ::Eigen::Matrix<double, 6, 1> &X,
- const ::Eigen::Matrix<double, 2, 1> &U);
-
- // Returns the voltage, velocity and acceleration if we were to be partially
- // along the path.
- void USaturationSearch(double goal_distance, double last_goal_distance,
- double goal_velocity, double last_goal_velocity,
- double saturation_fraction_along_path,
- const ::Eigen::Matrix<double, 2, 6> &K,
- const ::Eigen::Matrix<double, 6, 1> &X,
- const Trajectory &trajectory,
- ::Eigen::Matrix<double, 2, 1> *U,
- double *saturation_goal_velocity,
- double *saturation_goal_acceleration);
-
- // Returns the next goal given a planning plan_vmax and timestep. This
- // ignores the
- // backwards pass.
- ::Eigen::Matrix<double, 2, 1> PlanNextGoal(
- const ::Eigen::Matrix<double, 2, 1> &goal, double plan_vmax, double dt);
-
- // Plans the next cycle and updates the internal state for consumption.
- void Update(const ::Eigen::Matrix<double, 6, 1> &X, bool disabled, double dt,
- double plan_vmax, double voltage_limit);
-
- // Returns the goal acceleration for this cycle.
- double goal_acceleration() const { return goal_acceleration_; }
-
- // Returns U(s) for this cycle.
- const ::Eigen::Matrix<double, 2, 1> &U() const { return U_; }
- double U(int i) const { return U_(i); }
- const ::Eigen::Matrix<double, 2, 1> &U_unsaturated() const {
- return U_unsaturated_;
- }
- const ::Eigen::Matrix<double, 2, 1> &U_ff() const { return U_ff_; }
-
- double saturation_fraction_along_path() const {
- return saturation_fraction_along_path_;
- }
-
- const ::Eigen::Matrix<double, 2, 1> &theta() const { return theta_; }
- double theta(int i) const { return theta_(i); }
- const ::Eigen::Matrix<double, 2, 1> &omega() const { return omega_; }
- double omega(int i) const { return omega_(i); }
-
- // Distance left on the path before we get to the end of the path.
- double path_distance_to_go() const {
- if (has_path()) {
- return ::std::max(0.0, path()->length() - goal_(0));
- } else {
- return 0.0;
- }
- }
-
- const Path *path() const { return &trajectory_->path(); }
-
- int failed_solutions() const { return failed_solutions_; }
-
- private:
- // The trajectory plan.
- const Trajectory *trajectory_ = nullptr;
-
- // The current goal.
- ::Eigen::Matrix<double, 2, 1> goal_;
- // The previously executed goal.
- ::Eigen::Matrix<double, 2, 1> last_goal_;
- // The goal to use next cycle. We always plan 1 cycle ahead.
- ::Eigen::Matrix<double, 2, 1> next_goal_;
-
- ::Eigen::Matrix<double, 2, 1> U_;
- ::Eigen::Matrix<double, 2, 1> U_unsaturated_;
- ::Eigen::Matrix<double, 2, 1> U_ff_;
- double goal_acceleration_ = 0.0;
-
- double saturation_fraction_along_path_ = 1.0;
-
- // Holds the last valid goal position for when we loose our path.
- ::Eigen::Matrix<double, 2, 1> theta_;
- ::Eigen::Matrix<double, 2, 1> omega_;
-
- ::Eigen::Matrix<double, 2, 6> last_K_;
- int failed_solutions_ = 0;
-};
-
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
-
-#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
diff --git a/y2018/control_loops/superstructure/arm/trajectory_plot.cc b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
index 10b39bc..0cee664 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_plot.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
@@ -1,10 +1,11 @@
#include "aos/init.h"
#include "frc971/analysis/in_process_plotter.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
+#include "frc971/control_loops/double_jointed_arm/trajectory.h"
#include "gflags/gflags.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
DEFINE_bool(forwards, true, "If true, run the forwards simulation.");
DEFINE_bool(plot, true, "If true, plot");
@@ -16,10 +17,12 @@
namespace arm {
void Main() {
- Trajectory trajectory(FLAGS_forwards
- ? MakeNeutralToFrontHighPath()
- : Path::Reversed(MakeNeutralToFrontHighPath()),
- 0.001);
+ frc971::control_loops::arm::Dynamics dynamics(kArmConstants);
+ frc971::control_loops::arm::Trajectory trajectory(
+ &dynamics,
+ FLAGS_forwards ? MakeNeutralToFrontHighPath()
+ : Path::Reversed(MakeNeutralToFrontHighPath()),
+ 0.001);
constexpr double kAlpha0Max = 30.0;
constexpr double kAlpha1Max = 50.0;
@@ -96,7 +99,7 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
.array()
.max(-20)
.min(20);
@@ -118,7 +121,7 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
.array()
.max(-20)
.min(20);
@@ -140,7 +143,7 @@
const ::Eigen::Matrix<double, 6, 1> R = trajectory.R(theta_t, omega_t);
const ::Eigen::Matrix<double, 2, 1> U =
- Dynamics::FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
+ dynamics.FF_U(R.block<4, 1>(0, 0), omega_t, alpha_t)
.array()
.max(-20)
.min(20);
@@ -156,7 +159,8 @@
X << theta_t(0), 0.0, theta_t(1), 0.0;
}
- TrajectoryFollower follower(&trajectory);
+ frc971::control_loops::arm::TrajectoryFollower follower(&dynamics,
+ &trajectory);
::std::vector<double> t_array;
::std::vector<double> theta0_goal_t_array;
@@ -187,7 +191,7 @@
::std::vector<double> torque0_hat_t_array;
::std::vector<double> torque1_hat_t_array;
- EKF arm_ekf;
+ frc971::control_loops::arm::EKF arm_ekf(&dynamics);
arm_ekf.Reset(X);
::std::cout << "Reset P: " << arm_ekf.P_reset() << ::std::endl;
::std::cout << "Stabilized P: " << arm_ekf.P_half_converged() << ::std::endl;
@@ -236,9 +240,9 @@
actual_U(0) += 1.0;
const ::Eigen::Matrix<double, 4, 1> xdot =
- Dynamics::Acceleration(X, actual_U);
+ dynamics.Acceleration(X, actual_U);
- X = Dynamics::UnboundedDiscreteDynamics(X, actual_U, sim_dt);
+ X = dynamics.UnboundedDiscreteDynamics(X, actual_U, sim_dt);
arm_ekf.Predict(follower.U(), sim_dt);
alpha0_t_array.push_back(xdot(1));
@@ -270,10 +274,14 @@
plotter.AddLine(distance_array, alpha0_array, "alpha0");
plotter.AddLine(distance_array, alpha1_array, "alpha1");
- plotter.AddLine(integrated_distance, integrated_theta0_array, "integrated theta0");
- plotter.AddLine(integrated_distance, integrated_theta1_array, "integrated theta1");
- plotter.AddLine(integrated_distance, integrated_omega0_array, "integrated omega0");
- plotter.AddLine(integrated_distance, integrated_omega1_array, "integrated omega1");
+ plotter.AddLine(integrated_distance, integrated_theta0_array,
+ "integrated theta0");
+ plotter.AddLine(integrated_distance, integrated_theta1_array,
+ "integrated theta1");
+ plotter.AddLine(integrated_distance, integrated_omega0_array,
+ "integrated omega0");
+ plotter.AddLine(integrated_distance, integrated_omega1_array,
+ "integrated omega1");
plotter.Publish();
plotter.AddFigure();
diff --git a/y2018/control_loops/superstructure/arm/trajectory_test.cc b/y2018/control_loops/superstructure/arm/trajectory_test.cc
deleted file mode 100644
index b91c5a4..0000000
--- a/y2018/control_loops/superstructure/arm/trajectory_test.cc
+++ /dev/null
@@ -1,209 +0,0 @@
-#include "y2018/control_loops/superstructure/arm/trajectory.h"
-
-#include "gtest/gtest.h"
-#include "y2018/control_loops/superstructure/arm/demo_path.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-#include "y2018/control_loops/superstructure/arm/ekf.h"
-
-namespace y2018 {
-namespace control_loops {
-namespace superstructure {
-namespace arm {
-namespace testing {
-
-// Tests that we can pull out values along the path.
-TEST(TrajectoryTest, Theta) {
- Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{4.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
-
- EXPECT_TRUE(p.Theta(0.5).isApprox(
- (::Eigen::Matrix<double, 2, 1>() << 0.5, 0.0).finished()));
- EXPECT_TRUE(p.Omega(0.5).isApprox(
- (::Eigen::Matrix<double, 2, 1>() << 1.0, 0.0).finished()));
- EXPECT_TRUE(p.Alpha(0.5).isApprox(
- (::Eigen::Matrix<double, 2, 1>() << 0.0, 0.0).finished()));
-
- EXPECT_TRUE(p.Theta(1.5).isApprox(
- (::Eigen::Matrix<double, 2, 1>() << 1.5, 0.0).finished()));
-
- for (double d = 0.0; d <= 4.0; d += 0.01) {
- EXPECT_TRUE(p.Theta(d).isApprox(
- (::Eigen::Matrix<double, 2, 1>() << d, 0.0).finished()));
- }
-}
-
-// Tests a path where picking the right point to interpolate for matters.
-TEST(TrajectoryTest, HarderPath) {
- Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{1.0, 1.0, 0.0, 1.0, 0.0, 0.0}}});
-
- EXPECT_TRUE(p.Theta(0.5).isApprox(
- (::Eigen::Matrix<double, 2, 1>() << 0.5, 0.0).finished()));
-
- EXPECT_TRUE(p.Theta(1.5).isApprox(
- (::Eigen::Matrix<double, 2, 1>() << 1.0, 0.5).finished()));
-}
-
-// Tests that a path with an even number of points works as expected.
-TEST(TrajectoryTest, EvenPointNumbers) {
- Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
- EXPECT_TRUE(p.Theta(1.5).isApprox(
- (::Eigen::Matrix<double, 2, 1>() << 1.5, 0.0).finished()));
-
- for (double d = 0.0; d <= 3.0; d += 0.01) {
- EXPECT_TRUE(p.Theta(d).isApprox(
- (::Eigen::Matrix<double, 2, 1>() << d, 0.0).finished()));
- }
-}
-
-// Tests that out of range points work as expected.
-TEST(TrajectoryTest, OutOfBounds) {
- Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
- EXPECT_TRUE(p.Theta(-1.0).isApprox(
- (::Eigen::Matrix<double, 2, 1>() << 0.0, 0.0).finished()));
- EXPECT_TRUE(p.Theta(10.0).isApprox(
- (::Eigen::Matrix<double, 2, 1>() << 3.0, 0.0).finished()));
-}
-
-
-// Tests that we can compute the indices of the plan for a given distance correctly.
-TEST(TrajectoryTest, IndicesForDistanceTest) {
- // Start with a stupid simple plan.
- Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
- {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
- Trajectory t(::std::unique_ptr<Path>(new Path(p)), 0.1);
-
- // 0 - 3.0 every 0.1 should be 31 points.
- EXPECT_EQ(t.num_plan_points(), 31);
-
- // Verify that something centered in a grid cell returns the points on either side.
- EXPECT_EQ(::std::make_pair(static_cast<size_t>(0), static_cast<size_t>(1)),
- t.IndicesForDistance(0.05));
- EXPECT_EQ(::std::make_pair(static_cast<size_t>(1), static_cast<size_t>(2)),
- t.IndicesForDistance(0.15));
-
- // Verify that something on an edge returns the expected result.
- EXPECT_EQ(::std::make_pair(static_cast<size_t>(1), static_cast<size_t>(2)),
- t.IndicesForDistance(0.1));
-
- // Verify what small deviations result.
- EXPECT_EQ(::std::make_pair(static_cast<size_t>(1), static_cast<size_t>(2)),
- t.IndicesForDistance(0.1001));
- EXPECT_EQ(::std::make_pair(static_cast<size_t>(0), static_cast<size_t>(1)),
- t.IndicesForDistance(0.0999));
-
- // Verify that blowing past the ends works.
- EXPECT_EQ(::std::make_pair(static_cast<size_t>(29), static_cast<size_t>(30)),
- t.IndicesForDistance(4.0));
- EXPECT_EQ(::std::make_pair(static_cast<size_t>(0), static_cast<size_t>(1)),
- t.IndicesForDistance(-0.1));
-
- // Verify that the index to distance calculation also works.
- EXPECT_EQ(0.0, t.DistanceForIndex(0));
- EXPECT_EQ(0.1, t.DistanceForIndex(1));
- EXPECT_EQ(3.0, t.DistanceForIndex(30));
-}
-
-// Tests that we can correctly interpolate velocities between two points
-TEST(TrajectoryTest, InterpolateVelocity) {
- // x = 0.5 * a * t^2
- // v = a * t
- // a = 2.0
- EXPECT_EQ(0.0, Trajectory::InterpolateVelocity(0.0, 0.0, 1.0, 0.0, 2.0));
- EXPECT_EQ(2.0, Trajectory::InterpolateVelocity(1.0, 0.0, 1.0, 0.0, 2.0));
- EXPECT_EQ(0.0, Trajectory::InterpolateVelocity(-1.0, 0.0, 1.0, 0.0, 2.0));
- EXPECT_EQ(2.0, Trajectory::InterpolateVelocity(20.0, 0.0, 1.0, 0.0, 2.0));
-}
-
-// Tests that we can correctly interpolate velocities between two points
-TEST(TrajectoryTest, InterpolateAcceleration) {
- // x = 0.5 * a * t^2
- // v = a * t
- // a = 2.0
- EXPECT_EQ(2.0, Trajectory::InterpolateAcceleration(0.0, 1.0, 0.0, 2.0));
-}
-
-// Tests that we can correctly interpolate velocities between two points
-TEST(TrajectoryTest, ReversedPath) {
- // Tests that a reversed path is actually reversed.
- ::std::unique_ptr<Path> path = MakeDemoPath();
- ::std::unique_ptr<Path> reversed_path(
- new Path(Path::Reversed(*MakeDemoPath())));
-
- EXPECT_NEAR(path->length(), reversed_path->length(), 1e-6);
-
- for (double d = 0; d < path->length(); d += 0.01) {
- EXPECT_TRUE(path->Theta(d).isApprox(reversed_path->Theta(path->length() - d)));
- EXPECT_TRUE(path->Omega(d).isApprox(-reversed_path->Omega(path->length() - d)));
- EXPECT_TRUE(path->Alpha(d).isApprox(reversed_path->Alpha(path->length() - d)));
- }
-}
-
-// Tests that we can follow a path. Look at :trajectory_plot if you want to see
-// the path.
-TEST(TrajectoryTest, RunTrajectory) {
- ::std::unique_ptr<Path> path = MakeDemoPath();
- Trajectory trajectory(::std::move(path), 0.001);
-
- constexpr double kAlpha0Max = 40.0;
- constexpr double kAlpha1Max = 60.0;
- constexpr double vmax = 11.95;
-
- const ::Eigen::Matrix<double, 2, 2> alpha_unitizer =
- (::Eigen::Matrix<double, 2, 2>() << 1.0 / kAlpha0Max, 0.0, 0.0,
- 1.0 / kAlpha1Max)
- .finished();
- trajectory.OptimizeTrajectory(alpha_unitizer, vmax);
-
- double t = 0;
- ::Eigen::Matrix<double, 4, 1> X;
- {
- ::Eigen::Matrix<double, 2, 1> theta_t = trajectory.ThetaT(0.0);
- X << theta_t(0), 0.0, theta_t(1), 0.0;
- }
-
- EKF arm_ekf;
- arm_ekf.Reset(X);
-
- TrajectoryFollower follower(&trajectory);
- constexpr double sim_dt = 0.00505;
- while (t < 1.0) {
- arm_ekf.Correct((::Eigen::Matrix<double, 2, 1>() << X(0), X(2)).finished(),
- sim_dt);
- follower.Update(arm_ekf.X_hat(), false, sim_dt, vmax, 12.0);
- X = Dynamics::UnboundedDiscreteDynamics(X, follower.U(), sim_dt);
- arm_ekf.Predict(follower.U(), sim_dt);
- t += sim_dt;
- }
-
- ::Eigen::Matrix<double, 4, 1> final_X;
- ::Eigen::Matrix<double, 2, 1> final_theta_t =
- trajectory.ThetaT(trajectory.path().length());
- final_X << final_theta_t(0), 0.0, final_theta_t(1), 0.0;
-
- // Verify that we got to the end.
- EXPECT_TRUE(X.isApprox(final_X, 0.01))
- << ": X is " << X.transpose() << " final_X is " << final_X.transpose();
-
- // Verify that our goal is at the end.
- EXPECT_TRUE(
- final_theta_t.isApprox(trajectory.path().Theta(follower.goal(0))));
-}
-
-} // namespace testing
-} // namespace arm
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2018
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 285c8c3..54342c5 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -8,7 +8,8 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2018/constants.h"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "y2018/control_loops/superstructure/arm/arm_constants.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
#include "y2018/control_loops/superstructure/superstructure.h"
@@ -129,7 +130,8 @@
constants::Values::kProximalEncoderRatio()),
distal_zeroing_constants_(distal_zeroing_constants),
distal_pot_encoder_(M_PI * 2.0 *
- constants::Values::kDistalEncoderRatio()) {
+ constants::Values::kDistalEncoderRatio()),
+ dynamics_(arm::kArmConstants) {
X_.setZero();
}
@@ -174,7 +176,7 @@
AOS_CHECK_LE(::std::abs(U(1)), voltage_check);
if (release_arm_brake) {
- X_ = arm::Dynamics::UnboundedDiscreteDynamics(X_, U, 0.00505);
+ X_ = dynamics_.UnboundedDiscreteDynamics(X_, U, 0.00505);
} else {
// Well, the brake shouldn't stop both joints, but this will get the tests
// to pass.
@@ -197,6 +199,8 @@
const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
distal_zeroing_constants_;
PositionSensorSimulator distal_pot_encoder_;
+
+ ::frc971::control_loops::arm::Dynamics dynamics_;
};
class SuperstructureSimulation {