Now execute the trajectory for the arm.
We can now do a runtime forwards pass to bound accelerations and execute
a sample path. Also add a TrajectoryFollower object to hold that logic.
This also includes a LQR controller (pending Brian's library addition.)
Change-Id: I3e7ebf7186f07a742a866ff3144f98d0ee929725
diff --git a/y2018/control_loops/python/arm_trajectory.py b/y2018/control_loops/python/arm_trajectory.py
index 06664cb..464d16e 100755
--- a/y2018/control_loops/python/arm_trajectory.py
+++ b/y2018/control_loops/python/arm_trajectory.py
@@ -1009,7 +1009,6 @@
pylab.legend()
pylab.figure()
- pylab.title("Path Velocity Plan")
pylab.plot(t_array, alpha0_goal_t_array, label="alpha0_t_goal")
pylab.plot(t_array, alpha0_t_array, label="alpha0_t")
pylab.plot(t_array, alpha1_goal_t_array, label="alpha1_t_goal")
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index dfdaef4..ae2ed51 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -4,10 +4,12 @@
"trajectory.cc",
],
hdrs = [
+ "dlqr.h",
"trajectory.h",
],
deps = [
":dynamics",
+ "//frc971/control_loops:jacobian",
"//third_party/eigen",
],
)
@@ -17,7 +19,11 @@
srcs = [
"trajectory_test.cc",
],
+ linkopts = ["-lslicot"],
+ restricted_to = ["//tools:k8"],
deps = [
+ ":demo_path",
+ ":dynamics",
":trajectory",
"//aos/testing:googletest",
"//third_party/eigen",
@@ -38,6 +44,15 @@
],
)
+cc_library(
+ name = "demo_path",
+ srcs = [
+ "demo_path.cc",
+ ],
+ hdrs = ["demo_path.h"],
+ deps = [":trajectory"],
+)
+
cc_test(
name = "dynamics_test",
srcs = [
@@ -48,3 +63,19 @@
"//aos/testing:googletest",
],
)
+
+cc_binary(
+ name = "trajectory_plot",
+ srcs = [
+ "trajectory_plot.cc",
+ ],
+ linkopts = ["-lslicot"],
+ restricted_to = ["//tools:k8"],
+ deps = [
+ ":demo_path",
+ ":trajectory",
+ "//third_party/eigen",
+ "//third_party/matplotlib-cpp",
+ ],
+)
+
diff --git a/y2018/control_loops/superstructure/arm/demo_path.cc b/y2018/control_loops/superstructure/arm/demo_path.cc
new file mode 100644
index 0000000..6be0a06
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/demo_path.cc
@@ -0,0 +1,210 @@
+#include "y2018/control_loops/superstructure/arm/demo_path.h"
+
+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;
+}
+
+
+Path MakeDemoPath() {
+ return 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}}}));
+}
+
+} // 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
new file mode 100644
index 0000000..ccf4276
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/demo_path.h
@@ -0,0 +1,18 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_DEMO_PATH_H_
+
+#include "y2018/control_loops/superstructure/arm/trajectory.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+Path MakeDemoPath();
+
+} // 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/dlqr.h b/y2018/control_loops/superstructure/arm/dlqr.h
new file mode 100644
index 0000000..d33ee2b
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/dlqr.h
@@ -0,0 +1,98 @@
+#ifndef FRC971_CONTROL_LOOPS_DLQR_H_
+#define FRC971_CONTROL_LOOPS_DLQR_H_
+
+#include <Eigen/Dense>
+
+namespace frc971 {
+namespace controls {
+
+extern "C" {
+// Forward declaration for slicot fortran library.
+void sb02od_(char *DICO, char *JOBB, char *FACT, char *UPLO, char *JOBL,
+ char *SORT, int *N, int *M, int *P, double *A, int *LDA, double *B,
+ int *LDB, double *Q, int *LDQ, double *R, int *LDR, double *L,
+ int *LDL, double *RCOND, double *X, int *LDX, double *ALFAR,
+ double *ALFAI, double *BETA, double *S, int *LDS, double *T,
+ int *LDT, double *U, int *LDU, double *TOL, int *IWORK,
+ double *DWORK, int *LDWORK, int *BWORK, int *INFO);
+}
+
+template <int kN, int kM>
+void dlqr(::Eigen::Matrix<double, kN, kN> A, ::Eigen::Matrix<double, kN, kM> B,
+ ::Eigen::Matrix<double, kN, kN> Q, ::Eigen::Matrix<double, kM, kM> R,
+ ::Eigen::Matrix<double, kM, kN> *K,
+ ::Eigen::Matrix<double, kN, kN> *S) {
+ // Discrete (not continuous)
+ char DICO = 'D';
+ // B and R are provided instead of G.
+ char JOBB = 'B';
+ // Not factored, Q and R are provide.
+ char FACT = 'N';
+ // Store the upper triangle of Q and R.
+ char UPLO = 'U';
+ // L is zero.
+ char JOBL = 'Z';
+ // Stable eigenvalues first in the sort order
+ char SORT = 'S';
+
+ int N = 4;
+ int M = 2;
+ // Not needed since FACT = N
+ int P = 0;
+
+ int LDL = 1;
+
+ double RCOND = 0.0;
+ ::Eigen::Matrix<double, kN, kN> X = ::Eigen::Matrix<double, kN, kN>::Zero();
+
+ double ALFAR[kN * 2];
+ double ALFAI[kN * 2];
+ double BETA[kN * 2];
+ memset(ALFAR, 0, kN * 2);
+ memset(ALFAI, 0, kN * 2);
+ memset(BETA, 0, kN * 2);
+
+ int LDS = 2 * kN + kM;
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 *kN + kM> S_schur =
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 * kN + kM>::Zero();
+
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 *kN> T =
+ ::Eigen::Matrix<double, 2 * kN + kM, 2 * kN>::Zero();
+ int LDT = 2 * kN + kM;
+
+ ::Eigen::Matrix<double, 2 * kN, 2 *kN> U =
+ ::Eigen::Matrix<double, 2 * kN, 2 * kN>::Zero();
+ int LDU = 2 * kN;
+
+ double TOL = 0.0;
+
+ int IWORK[2 * kN > kM ? 2 * kN : kM];
+ memset(IWORK, 0, 2 * kN > kM ? 2 * kN : kM);
+
+ int LDWORK = 16 * kN + 3 * kM + 16;
+
+ double DWORK[LDWORK];
+ memset(DWORK, 0, LDWORK);
+
+ int INFO = 0;
+
+ int BWORK[2 * kN];
+ memset(BWORK, 0, 2 * kN);
+
+ // TODO(austin): I can't tell if anything here is transposed...
+ sb02od_(&DICO, &JOBB, &FACT, &UPLO, &JOBL, &SORT, &N, &M, &P, A.data(), &N,
+ B.data(), &N, Q.data(), &N, R.data(), &M, nullptr, &LDL, &RCOND,
+ X.data(), &N, ALFAR, ALFAI, BETA, S_schur.data(), &LDS, T.data(),
+ &LDT, U.data(), &LDU, &TOL, IWORK, DWORK, &LDWORK, BWORK, &INFO);
+ //::std::cout << "slycot result: " << INFO << ::std::endl;
+
+ *K = (R + B.transpose() * X * B).inverse() * B.transpose() * X * A;
+ if (S != nullptr) {
+ *S = X;
+ }
+}
+
+} // namespace controls
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_DLQR_H_
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/y2018/control_loops/superstructure/arm/trajectory.cc
index fb1e0a5..fbe38eb 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory.cc
@@ -1,5 +1,10 @@
#include "y2018/control_loops/superstructure/arm/trajectory.h"
+#include "Eigen/Dense"
+#include "frc971/control_loops/jacobian.h"
+#include "y2018/control_loops/superstructure/arm/dlqr.h"
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
+
namespace y2018 {
namespace control_loops {
namespace superstructure {
@@ -245,7 +250,7 @@
// Subdivide the plan step size into kNumSteps steps.
double integrated_distance = 0.0;
double integrated_velocity = previous_velocity;
- constexpr int kNumSteps = 10;
+ constexpr int kNumSteps = 50;
for (int j = 0; j < kNumSteps; ++j) {
// Compute the acceleration with respect to time.
const double acceleration = FeasableBackwardsAcceleration(
@@ -303,6 +308,191 @@
return max_dvelocity_forward_pass;
}
+void TrajectoryFollower::Reset() {
+ next_goal_ = last_goal_ = goal_ = ::Eigen::Matrix<double, 2, 1>::Zero();
+ U_ = ::Eigen::Matrix<double, 2, 1>::Zero();
+ goal_acceleration_ = 0.0;
+}
+
+::Eigen::Matrix<double, 2, 4> TrajectoryFollower::K_at_state(
+ const ::Eigen::Matrix<double, 4, 1> &X,
+ const ::Eigen::Matrix<double, 2, 1> &U) {
+ constexpr double q_pos = 0.2;
+ constexpr double q_vel = 4.0;
+ const ::Eigen::DiagonalMatrix<double, 4> Q =
+ (::Eigen::DiagonalMatrix<double, 4>().diagonal()
+ << 1.0 / ::std::pow(q_pos, 2),
+ 1.0 / ::std::pow(q_vel, 2), 1.0 / ::std::pow(q_pos, 2),
+ 1.0 / ::std::pow(q_vel, 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(
+ Dynamics::UnboundedDiscreteDynamics, X, U, 0.00505);
+ const ::Eigen::Matrix<double, 4, 2> final_B =
+ ::frc971::control_loops::NumericalJacobianU(
+ Dynamics::UnboundedDiscreteDynamics, X, U, 0.00505);
+
+ ::Eigen::Matrix<double, 2, 4> K;
+ ::Eigen::Matrix<double, 4, 4> S;
+ ::frc971::controls::dlqr<4, 2>(final_A, final_B, Q, R, &K, &S);
+ return K;
+}
+
+void TrajectoryFollower::USaturationSearch(
+ double goal_distance, double last_goal_distance, double goal_velocity,
+ double last_goal_velocity, double fraction_along_path,
+ const ::Eigen::Matrix<double, 2, 4> &K,
+ const ::Eigen::Matrix<double, 4, 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) * 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, 4, 1> R = trajectory.R(theta_t, omega_t);
+ const ::Eigen::Matrix<double, 2, 1> U_ff =
+ Dynamics::FF_U(R, 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 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, &vmax](const ::Eigen::Matrix<double, 2, 1> &X) {
+ return (::Eigen::Matrix<double, 2, 1>() << X(1),
+ trajectory_->FeasableForwardsAcceleration(X(0), X(1), vmax,
+ 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, 4, 1> &X,
+ double dt, double vmax) {
+ // To avoid exposing the new goals before the outer code has a chance to
+ // querry the internal state, move to the new goals here.
+ last_goal_ = goal_;
+ 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_, 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, 4, 1> R = trajectory_->R(theta_t, omega_t);
+
+ U_ff_ = Dynamics::FF_U(R, omega_t, alpha_t);
+
+ const ::Eigen::Matrix<double, 2, 4> 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.
+ fraction_along_path_ = 1.0;
+ if ((U_.array().abs() > 12.0).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.
+ fraction_along_path_ = 0.0;
+ step_size = 0.0;
+ } else {
+ 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),
+ fraction_along_path_, K, X, *trajectory_, &U_,
+ &saturation_goal_velocity,
+ &saturation_goal_acceleration);
+ step_size = step_size * 0.5;
+ if ((U_.array().abs() > 12.0).any()) {
+ fraction_along_path_ -= step_size;
+ } else {
+ fraction_along_path_ += step_size;
+ }
+ }
+
+ goal_(0) =
+ ((goal_(0) - last_goal_(0)) * fraction_along_path_ + last_goal_(0));
+ goal_(1) = saturation_goal_velocity;
+
+ next_goal_ = PlanNextGoal(goal_, vmax, dt);
+
+ goal_acceleration_ = trajectory_->InterpolateAcceleration(
+ goal_(0), next_goal_(0), goal_(1), next_goal_(1));
+ }
+}
+
} // namespace arm
} // namespace superstructure
} // namespace control_loops
diff --git a/y2018/control_loops/superstructure/arm/trajectory.h b/y2018/control_loops/superstructure/arm/trajectory.h
index 2c9d68c..5ecc783 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.h
+++ b/y2018/control_loops/superstructure/arm/trajectory.h
@@ -3,8 +3,6 @@
#include "Eigen/Dense"
-#include "y2018/control_loops/superstructure/arm/dynamics.h"
-
namespace y2018 {
namespace control_loops {
namespace superstructure {
@@ -168,6 +166,87 @@
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.
+ if (v0 < 0 || v1 < 0) {
+ abort();
+ }
+ 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 d) {
+ ::std::pair<size_t, size_t> indices = IndicesForDistance(d);
+ const double v0 = max_dvelocity_[indices.first];
+ const double v1 = max_dvelocity_[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 {
+ ::std::pair<size_t, size_t> indices = IndicesForDistance(distance);
+ const double v0 = max_dvelocity_[indices.first];
+ const double v1 = max_dvelocity_[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, 4, 1> R(
+ ::Eigen::Matrix<double, 2, 1> theta_t,
+ ::Eigen::Matrix<double, 2, 1> omega_t) {
+ return (::Eigen::Matrix<double, 4, 1>() << theta_t(0, 0), omega_t(0, 0),
+ theta_t(1, 0), omega_t(1, 0))
+ .finished();
+ }
+
private:
friend class testing::TrajectoryTest_IndicesForDistanceTest_Test;
@@ -202,6 +281,82 @@
::std::vector<double> max_dvelocity_forward_pass_;
};
+// This class tracks the current goal along trajectories and paths.
+class TrajectoryFollower {
+ public:
+ TrajectoryFollower(Path *const path, Trajectory *const trajectory,
+ const ::Eigen::Matrix<double, 2, 2> alpha_unitizer)
+ : path_(path), trajectory_(trajectory), alpha_unitizer_(alpha_unitizer) {
+ Reset();
+ }
+
+ // 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.
+ // TODO(austin): Allow switching the path.
+ void Reset();
+
+ // Returns the controller gain at the provided state.
+ ::Eigen::Matrix<double, 2, 4> K_at_state(
+ const ::Eigen::Matrix<double, 4, 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 fraction_along_path,
+ const ::Eigen::Matrix<double, 2, 4> &K,
+ const ::Eigen::Matrix<double, 4, 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 vmax and timestep. This ignores the
+ // backwards pass.
+ ::Eigen::Matrix<double, 2, 1> PlanNextGoal(
+ const ::Eigen::Matrix<double, 2, 1> &goal, double vmax, double dt);
+
+ // Plans the next cycle and updates the internal state for consumption.
+ void Update(const ::Eigen::Matrix<double, 4, 1> &X, double dt, double vmax);
+
+ // 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_; }
+ 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 fraction_along_path() const { return fraction_along_path_; }
+
+ private:
+ // The path to follow.
+ Path *const path_ = nullptr;
+ // The trajectory plan.
+ Trajectory *const trajectory_ = nullptr;
+ const ::Eigen::Matrix<double, 2, 2> alpha_unitizer_;
+
+ // 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 fraction_along_path_ = 1.0;
+};
+
} // namespace arm
} // namespace superstructure
} // namespace control_loops
diff --git a/y2018/control_loops/superstructure/arm/trajectory_plot.cc b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
new file mode 100644
index 0000000..0b808f8
--- /dev/null
+++ b/y2018/control_loops/superstructure/arm/trajectory_plot.cc
@@ -0,0 +1,232 @@
+#include "y2018/control_loops/superstructure/arm/trajectory.h"
+
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#include "y2018/control_loops/superstructure/arm/demo_path.h"
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+void Main() {
+ Path p = MakeDemoPath();
+ Trajectory trajectory(&p, 0.001);
+
+ constexpr double kAlpha0Max = 40.0;
+ constexpr double kAlpha1Max = 60.0;
+ constexpr double vmax = 11.95;
+ constexpr double sim_dt = 0.00505;
+
+ 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);
+
+ ::std::vector<double> distance_array = trajectory.DistanceArray();
+
+ ::std::vector<double> theta0_array;
+ ::std::vector<double> theta1_array;
+ ::std::vector<double> omega0_array;
+ ::std::vector<double> omega1_array;
+ ::std::vector<double> alpha0_array;
+ ::std::vector<double> alpha1_array;
+
+ for (const double d : distance_array) {
+ const ::Eigen::Matrix<double, 2, 1> theta = p.Theta(d);
+ const ::Eigen::Matrix<double, 2, 1> omega = p.Omega(d);
+ const ::Eigen::Matrix<double, 2, 1> alpha = p.Alpha(d);
+ theta0_array.push_back(theta(0, 0));
+ theta1_array.push_back(theta(1, 0));
+ omega0_array.push_back(omega(0, 0));
+ omega1_array.push_back(omega(1, 0));
+ alpha0_array.push_back(alpha(0, 0));
+ alpha1_array.push_back(alpha(1, 0));
+ }
+
+ // Next step: see what U is as a function of distance.
+ ::std::vector<double> Uff0_distance_array;
+ ::std::vector<double> Uff1_distance_array;
+
+ for (const double distance : distance_array) {
+ const double goal_velocity = trajectory.GetDVelocity(distance);
+ const double goal_acceleration = trajectory.GetDAcceleration(distance);
+ const ::Eigen::Matrix<double, 2, 1> theta_t = trajectory.ThetaT(distance);
+ const ::Eigen::Matrix<double, 2, 1> omega_t =
+ trajectory.OmegaT(distance, goal_velocity);
+ const ::Eigen::Matrix<double, 2, 1> alpha_t =
+ trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+ const ::Eigen::Matrix<double, 4, 1> R = trajectory.R(theta_t, omega_t);
+ const ::Eigen::Matrix<double, 2, 1> U = Dynamics::FF_U(R, omega_t, alpha_t);
+
+ Uff0_distance_array.push_back(U(0));
+ Uff1_distance_array.push_back(U(1));
+ }
+
+ 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;
+ }
+
+ TrajectoryFollower follower(&p, &trajectory, alpha_unitizer);
+
+ ::std::vector<double> t_array;
+ ::std::vector<double> theta0_goal_t_array;
+ ::std::vector<double> theta1_goal_t_array;
+ ::std::vector<double> omega0_goal_t_array;
+ ::std::vector<double> omega1_goal_t_array;
+ ::std::vector<double> alpha0_goal_t_array;
+ ::std::vector<double> alpha1_goal_t_array;
+ ::std::vector<double> theta0_t_array;
+ ::std::vector<double> omega0_t_array;
+ ::std::vector<double> theta1_t_array;
+ ::std::vector<double> omega1_t_array;
+ ::std::vector<double> distance_t_array;
+ ::std::vector<double> velocity_t_array;
+ ::std::vector<double> acceleration_t_array;
+ ::std::vector<double> u0_unsaturated_array;
+ ::std::vector<double> u1_unsaturated_array;
+ ::std::vector<double> alpha0_t_array;
+ ::std::vector<double> alpha1_t_array;
+ ::std::vector<double> uff0_array;
+ ::std::vector<double> uff1_array;
+ ::std::vector<double> u0_array;
+ ::std::vector<double> u1_array;
+
+ while (t < 1.0) {
+ t_array.push_back(t);
+ follower.Update(X, sim_dt, vmax);
+
+ const ::Eigen::Matrix<double, 2, 1> theta_t =
+ trajectory.ThetaT(follower.goal()(0));
+ const ::Eigen::Matrix<double, 2, 1> omega_t =
+ trajectory.OmegaT(follower.goal()(0), follower.goal()(1));
+ const ::Eigen::Matrix<double, 2, 1> alpha_t = trajectory.AlphaT(
+ follower.goal()(0), follower.goal()(1), follower.goal_acceleration());
+
+ theta0_goal_t_array.push_back(theta_t(0));
+ theta1_goal_t_array.push_back(theta_t(1));
+ omega0_goal_t_array.push_back(omega_t(0));
+ omega1_goal_t_array.push_back(omega_t(1));
+ alpha0_goal_t_array.push_back(alpha_t(0));
+ alpha1_goal_t_array.push_back(alpha_t(1));
+ theta0_t_array.push_back(X(0));
+ omega0_t_array.push_back(X(1));
+ theta1_t_array.push_back(X(2));
+ omega1_t_array.push_back(X(3));
+
+ distance_t_array.push_back(follower.goal()(0));
+ velocity_t_array.push_back(follower.goal()(1));
+ acceleration_t_array.push_back(follower.goal_acceleration());
+
+ u0_unsaturated_array.push_back(follower.U_unsaturated()(0));
+ u1_unsaturated_array.push_back(follower.U_unsaturated()(1));
+
+ const ::Eigen::Matrix<double, 4, 1> xdot =
+ Dynamics::Acceleration(X, follower.U());
+
+ X = Dynamics::UnboundedDiscreteDynamics(X, follower.U(), sim_dt);
+
+ alpha0_t_array.push_back(xdot(1));
+ alpha1_t_array.push_back(xdot(3));
+
+ uff0_array.push_back(follower.U_ff()(0));
+ uff1_array.push_back(follower.U_ff()(1));
+ u0_array.push_back(follower.U()(0));
+ u1_array.push_back(follower.U()(1));
+
+ t += sim_dt;
+ }
+
+ matplotlibcpp::figure();
+ matplotlibcpp::title("Trajectory");
+ matplotlibcpp::plot(theta0_array, theta1_array, {{"label", "desired path"}});
+ matplotlibcpp::plot(theta0_t_array, theta1_t_array,
+ {{"label", "actual path"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(distance_array, theta0_array, {{"label", "theta0"}});
+ matplotlibcpp::plot(distance_array, theta1_array, {{"label", "theta1"}});
+ matplotlibcpp::plot(distance_array, omega0_array, {{"label", "omega0"}});
+ matplotlibcpp::plot(distance_array, omega1_array, {{"label", "omega1"}});
+ matplotlibcpp::plot(distance_array, alpha0_array, {{"label", "alpha0"}});
+ matplotlibcpp::plot(distance_array, alpha1_array, {{"label", "alpha1"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(distance_array, trajectory.max_dvelocity_unfiltered(),
+ {{"label", "pass0"}});
+ matplotlibcpp::plot(distance_array, trajectory.max_dvelocity(),
+ {{"label", "passb"}});
+ matplotlibcpp::plot(distance_array, trajectory.max_dvelocity_forward_pass(),
+ {{"label", "passf"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(t_array, alpha0_goal_t_array,
+ {{"label", "alpha0_t_goal"}});
+ matplotlibcpp::plot(t_array, alpha0_t_array, {{"label", "alpha0_t"}});
+ matplotlibcpp::plot(t_array, alpha1_goal_t_array,
+ {{"label", "alpha1_t_goal"}});
+ matplotlibcpp::plot(t_array, alpha1_t_array, {{"label", "alpha1_t"}});
+ matplotlibcpp::plot(t_array, distance_t_array, {{"label", "distance_t"}});
+ matplotlibcpp::plot(t_array, velocity_t_array, {{"label", "velocity_t"}});
+ matplotlibcpp::plot(t_array, acceleration_t_array,
+ {{"label", "acceleration_t"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::title("Angular Velocities");
+ matplotlibcpp::plot(t_array, omega0_goal_t_array,
+ {{"label", "omega0_t_goal"}});
+ matplotlibcpp::plot(t_array, omega0_t_array, {{"label", "omega0_t"}});
+ matplotlibcpp::plot(t_array, omega1_goal_t_array,
+ {{"label", "omega1_t_goal"}});
+ matplotlibcpp::plot(t_array, omega1_t_array, {{"label", "omega1_t"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::title("Voltages");
+ matplotlibcpp::plot(t_array, u0_unsaturated_array, {{"label", "u0_full"}});
+ matplotlibcpp::plot(t_array, u0_array, {{"label", "u0"}});
+ matplotlibcpp::plot(t_array, uff0_array, {{"label", "uff0"}});
+ matplotlibcpp::plot(t_array, u1_unsaturated_array, {{"label", "u1_full"}});
+ matplotlibcpp::plot(t_array, u1_array, {{"label", "u1"}});
+ matplotlibcpp::plot(t_array, uff1_array, {{"label", "uff1"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::title("Angles");
+ matplotlibcpp::plot(t_array, theta0_goal_t_array,
+ {{"label", "theta0_t_goal"}});
+ matplotlibcpp::plot(t_array, theta0_t_array, {{"label", "theta0_t"}});
+ matplotlibcpp::plot(t_array, theta1_goal_t_array,
+ {{"label", "theta1_t_goal"}});
+ matplotlibcpp::plot(t_array, theta1_t_array, {{"label", "theta1_t"}});
+ matplotlibcpp::legend();
+
+/*
+ matplotlibcpp::figure();
+ matplotlibcpp::title("ff for distance");
+ matplotlibcpp::plot(distance_array, Uff0_distance_array, {{"label", "ff0"}});
+ matplotlibcpp::plot(distance_array, Uff1_distance_array, {{"label", "ff1"}});
+ matplotlibcpp::legend();
+*/
+
+ matplotlibcpp::show();
+}
+
+} // namespace arm
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+int main(int /*argc*/, const char * /*argv*/ []) {
+ ::y2018::control_loops::superstructure::arm::Main();
+ return 0;
+}
diff --git a/y2018/control_loops/superstructure/arm/trajectory_test.cc b/y2018/control_loops/superstructure/arm/trajectory_test.cc
index a4e7822..3c5d6a1 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_test.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_test.cc
@@ -1,6 +1,8 @@
#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"
namespace y2018 {
namespace control_loops {
@@ -113,6 +115,69 @@
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 follow a path. Look at :trajectory_plot if you want to see
+// the path.
+TEST(TrajectoryTest, RunTrajectory) {
+ Path path = MakeDemoPath();
+ Trajectory trajectory(&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;
+ }
+
+ TrajectoryFollower follower(&path, &trajectory, alpha_unitizer);
+ constexpr double sim_dt = 0.00505;
+ while (t < 1.0) {
+ follower.Update(X, sim_dt, vmax);
+ X = Dynamics::UnboundedDiscreteDynamics(X, follower.U(), sim_dt);
+ t += sim_dt;
+ }
+
+ ::Eigen::Matrix<double, 4, 1> final_X;
+ ::Eigen::Matrix<double, 2, 1> final_theta_t =
+ trajectory.ThetaT(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(path.Theta(follower.goal(0))));
+}
+
} // namespace testing
} // namespace arm
} // namespace superstructure