From d1a2a78e893386814f3bd95a3c56891fc040c6cc Mon Sep 17 00:00:00 2001 From: Joe Rivera Date: Tue, 27 Oct 2026 19:26:32 -0500 Subject: [PATCH] nav: DynBikePH.Golden, the pH form and its books for GRL-SNAM's twin A golden for dyn_bike's port-Hamiltonian form, as DynBike.Golden is for the plant: tests/data/dyn_bike_ph_golden.json, written with CVC_DYN_BIKE_PH_GOLDEN_WRITE=1 and otherwise checked to 1e-9 relative. On DynBike.Golden's Humvee (lags, low-speed blend), at its seven states (cruising, a hard corner, trail braking, past the tire's peak, backing up, creeping, pulling away from rest) and a stop at the speed clamp, each case records: - the parts at its state (dyn_ph_parts_at): u, sg, Fxf, Fxr, Fyf, Fyr, wf, wr, w0f, the secants Sf and Sr, Dx, cos delta; - the rates at its state (dyn_ph_rates_at): dissipated, supplied, floor, grade; - one dyn_plant_books step: the next state, the twist before the limiters (dyn_step_free), and the books (dH, the trapezoid's dissipated, supplied, floor and grade, limiter, residual); - the gradient of a fixed weighting of the books on the state and the commands, by forward-mode duals. The test also checks that dyn_plant_books steps exactly as dyn_plant. DynBike.Golden and dyn_bike_golden.json are unchanged. nav_dyn_bike_test: 22 tests pass (MSVC 14.44 Release). --- src/cvc/tests/data/dyn_bike_ph_golden.json | 80 +++++++++ src/cvc/tests/nav_dyn_bike_test.cpp | 195 +++++++++++++++++++++ 2 files changed, 275 insertions(+) create mode 100644 src/cvc/tests/data/dyn_bike_ph_golden.json diff --git a/src/cvc/tests/data/dyn_bike_ph_golden.json b/src/cvc/tests/data/dyn_bike_ph_golden.json new file mode 100644 index 000000000..fcd01fb80 --- /dev/null +++ b/src/cvc/tests/data/dyn_bike_ph_golden.json @@ -0,0 +1,80 @@ +{ + "format": "cvc-dyn-bike-ph-golden-1", + "order": ["x", "y", "psi", "vx", "vy", "r", "delta", "a"], + "params": {"m": 2676, "Iz": 6283.69384765625, "lf": 1.5369999408721924, "lr": 1.7630000114440918, "h": 0.74000000953674316, "g": 9.8066501617431641, "mu": 0.89999997615814209, "B_f": 10.708646774291992, "B_r": 10.708646774291992, "C": 1.2021652460098267, "front_drive": 0.5, "front_brake": 0.70588237047195435, "drag": 1.5006250143051147, "c_rr": 0.014999999664723873, "tau_steer": 0.05000000074505806, "steer_rate": 0.34999999403953552, "tau_drive": 0.10000000149011612, "v_blend": 1, "v_width": 0.30000001192092896, "eps": 0.5, "fz_min": 0.05000000074505806, "iters": 3}, + "g_books": [0.90000000000000002, -0.40000000000000002, 0.69999999999999996, 1.3, -0.59999999999999998, 0.80000000000000004, 1.1000000000000001], + "cases": [ + {"name": "cruising", "state": [0, 0, 0, 12, 0.10000000000000001, 0.050000000000000003, 0.02, 0.5], "delta_cmd": 0.050000000000000003, "a_cmd": 1, "grade": 0, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "parts": {"u": 12.010412149464313, "sg": 0.999133073092352, "Fxf": 668.99999999379133, "Fxr": 668.99999998711121, "Fyf": 833.2364968298391, "Fyr": -142.89201640763156, "wf": 0.17684999704360962, "wr": 0.01184999942779541, "w0f": 0.24003194964382116, "Sf": 13187.887720124836, "Sr": 12058.398591350429, "Dx": 50.797930946489572, "cd": 0.99980000666657776}, + "rates": {"dissipated": 7369.2303991363142, "supplied": 16056.760548503509, "floor": 4.6216263526531047e-05, "grade": -0}, + "next": [0.72170075978362558, 0.0014655898551522932, 0.0048612338851567365, 12.028322883477824, 0.10879309514548365, 0.08102056656356306, 0.035978411112368727, 0.72559417581953878], + "free": {"vx": 12.028322883477824, "vy": 0.10879309514548363, "r": 0.08102056656356306}, + "books": {"dH": 925.80375282801106, "dissipated": 480.2778572362302, "supplied": 1399.6434267494701, "floor": 5.6032430529408864e-05, "grade": -0, "limiter": 0, "residual": 6.438127282340588}, + "g_state": [0, 0, 0, 94.572062704034721, 266.26204849829253, 178.23833422073534, -2291.7756449200788, 1696.8050928496623], + "g_delta_cmd": -955.48663759140084, "g_a_cmd": 1394.9752983233941}, + {"name": "hard corner", "state": [1, -2, 0.29999999999999999, 15, -1.2, 0.59999999999999998, 0.25, 0], "delta_cmd": 0.29999999999999999, "a_cmd": 0, "grade": 0.02, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "parts": {"u": 15.008331019803634, "sg": 0.99944490697915433, "Fxf": 0, "Fxr": 0, "Fyf": 12572.367058289443, "Fyr": 10317.129343040195, "wf": -0.27780003547668453, "wr": -2.257800006866455, "w0f": 3.8300376059888634, "Sf": 3060.5803236673237, "Sr": 4569.54969955868, "Dx": 48.749905116075631, "cd": 0.96891242171064473}, + "rates": {"dissipated": 84304.682707770335, "supplied": 0, "floor": 1.1111296915389401, "grade": -7872.7787498474127}, + "next": [1.8867829073011033, -1.8363951317071947, 0.3362258629537182, 14.851775313286971, -1.2377173130869106, 0.60376439605715648, 0.26954533644854789, 0], + "free": {"vx": 14.851775313286971, "vy": -1.2377173130869106, "r": 0.60376439605715648}, + "books": {"dH": -5783.0839400431141, "dissipated": 5285.4247061505548, "supplied": 0, "floor": 0.084403202039299102, "grade": -470.0328341572777, "limiter": 0, "residual": -27.710802937321205}, + "g_state": [0, 0, 0, -463.19990518332662, 1821.5216108242723, -2334.1217865019594, -12976.335618546113, 2130.7992682734298], + "g_delta_cmd": -1337.7832678807169, "g_a_cmd": 1751.7700515237402}, + {"name": "trail braking", "state": [-3, 4, -1.1000000000000001, 14, 0.80000000000000004, 0.40000000000000002, 0.14999999999999999, -4], "delta_cmd": 0.14999999999999999, "a_cmd": -6, "grade": -0.029999999999999999, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "parts": {"u": 14.0089257261219, "sg": 0.99936285434754957, "Fxf": -7551.3663373512209, "Fxr": -3148.1333008982315, "Fyf": 7001.6152821298219, "Fyr": -717.59662961739537, "wf": 1.414799976348877, "wr": 0.094799995422363281, "w0f": 2.1158726232176082, "Sf": 9987.003933760383, "Sr": 7569.5850661203158, "Dx": 49.121296036861125, "cd": 0.98877107793604224}, + "rates": {"dissipated": 14549.607263992975, "supplied": -150202.43120684219, "floor": 0.14143812354461546, "grade": 11021.890249786376}, + "next": [-2.6188025127373895, 3.2714360882174414, -1.0723766257472249, 13.702551861063213, 0.58732269798076042, 0.4603895811700956, 0.14999999999999999, -4.9023767032781551], + "free": {"vx": 13.702551861063213, "vy": 0.58732269798076042, "r": 0.4603895811700956}, + "books": {"dH": -11256.750776780944, "dissipated": 886.6551612188855, "supplied": -10910.422291267738, "floor": 0.0085982852894107929, "grade": 654.28817020049235, "limiter": 0, "residual": -113.97009278010228}, + "g_state": [0, 0, 0, -1495.197249568253, 595.46430683739527, -493.32365527179718, -3135.8971941691698, 1906.0968547428911], + "g_delta_cmd": -7275.6476827216156, "g_a_cmd": 1567.0379820187352}, + {"name": "past the peak", "state": [2, 2, 2.5, 10, 2.5, 1.1000000000000001, 0.40000000000000002, 1], "delta_cmd": 0.45000000000000001, "a_cmd": 2, "grade": 0, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "parts": {"u": 10.012492197250394, "sg": 0.99875233887784454, "Fxf": 1337.9999962059987, "Fxr": 1337.9999945396203, "Fyf": 480.06651262852165, "Fyr": -6926.5853953834903, "wf": 4.1906999349594116, "wr": 0.56069998741149885, "w0f": 4.2273249542110252, "Sf": 13107.611202344198, "Sr": 12353.460943276346, "Dx": 54.339776317364368, "cd": 0.9210609940028851}, + "rates": {"dissipated": 9334.4499331538391, "supplied": 27887.325598366322, "floor": 0.27295228804807259, "grade": -0}, + "next": [1.4909879252982987, 2.3339382319443089, 2.5710190927434646, 10.145752585238649, 1.9848686819797339, 1.1836515721810814, 0.41954533644854791, 1.4511883516390776], + "free": {"vx": 10.145752585238649, "vy": 1.9848686819797099, "r": 1.183651572181071}, + "books": {"dH": 1437.7772360406525, "dissipated": 624.33651714749806, "supplied": 2434.9378987666028, "floor": 0.20744834669852991, "grade": -0, "limiter": 2.3283064365386963e-10, "residual": -373.03159392538373}, + "g_state": [0, 0, 0, -196.40742046722073, 258.4249427185905, 162.64432813704843, -6670.0189402365349, 1471.2756168733499], + "g_delta_cmd": -687.63940737960604, "g_a_cmd": 1209.5632852663059}, + {"name": "backing up", "state": [0.5, 0.5, 0.20000000000000001, -2, 0.050000000000000003, -0.10000000000000001, 0.20000000000000001, -0.5], "delta_cmd": 0.25, "a_cmd": -0.80000000000000004, "grade": 0, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 1}, + "parts": {"u": 2.0615528128088303, "sg": -0.97014250014533188, "Fxf": -944.4706115932695, "Fxr": -393.5293883083641, "Fyf": -11960.444792630304, "Fyr": -9240.0034610141138, "wf": -0.10369999408721924, "wr": 0.22630000114440918, "w0f": -0.40509636691446466, "Sf": 39683.439719049973, "Sr": 40830.77072155106, "Dx": 194.03654493800514, "cd": 0.98006657784124163}, + "rates": {"dissipated": 6407.7299751548862, "supplied": 2657.8049284691688, "floor": 3.7985452324499418, "grade": 0}, + "next": [0.38163390582407375, 0.4801677888170704, 0.19227964555052293, -1.9995770449242993, -0.17430505462442358, -0.12867257703400767, 0.2195453364485479, -0.63535650549172329], + "free": {"vx": -1.9995770449242993, "vy": -0.17244089459456319, "r": -0.12848239017639973}, + "books": {"dH": 55.642971390501771, "dissipated": 250.1420964398819, "supplied": 204.26131283110482, "floor": 0.22447606907508749, "grade": 0, "limiter": 1.0185296015806671, "residual": 100.2807493286231}, + "g_state": [0, 0, 0, -14.765472460519435, -755.8199706286432, -373.902656142647, -935.21641114208364, -285.87310632729543], + "g_delta_cmd": -96.415267256589999, "g_a_cmd": -235.02164359479943}, + {"name": "creeping", "state": [0, 0, 0, 0.59999999999999998, 0.02, 0.050000000000000003, 0.29999999999999999, 0.29999999999999999], "delta_cmd": 0.34999999999999998, "a_cmd": 0.5, "grade": 0, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "parts": {"u": 0.78102496759066542, "sg": 0.76822127959737585, "Fxf": 401.39999999994171, "Fxr": 401.39999999985969, "Fyf": 10648.87473784531, "Fyr": 8751.4437535048728, "wf": 0.096849997043609623, "wr": -0.068150000572204592, "w0f": 0.18325608116457207, "Sf": 123242.18654484602, "Sr": 128414.43404292801, "Dx": 505.17502810405216, "cd": 0.95533648912560598}, + "rates": {"dissipated": 1705.6793444363263, "supplied": 482.41176208108084, "floor": 24.510905403308719, "grade": -0}, + "next": [0.036071137283442252, -1.6430282470171074e-05, 0.0036051455458450143, 0.60118074078138972, 0.10349000305763768, 0.060085760440438306, 0.31954533644854788, 0.39023767032781553], + "free": {"vx": 0.60118074078138972, "vy": 0.097734281068398154, "r": 0.05968543769212728}, + "books": {"dH": 19.181072855414982, "dissipated": 62.662709369358424, "supplied": 38.087341472711124, "floor": 1.0740101336170835, "grade": -0, "limiter": 1.7003021666398581, "residual": 40.982128451805337}, + "g_state": [0, 0, 0, 73.806285726693346, -138.28621683960841, 132.72750314154956, 36.853793319321852, 86.942492019706492], + "g_delta_cmd": 3.7994075916207333, "g_a_cmd": 71.47705370124973}, + {"name": "pulling away", "state": [0, 0, 0, 0, 0, 0, 0, 0], "delta_cmd": 0.10000000000000001, "a_cmd": 2, "grade": 0, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "parts": {"u": 0.5, "sg": 0, "Fxf": 0, "Fxr": 0, "Fyf": 0, "Fyr": -0, "wf": 0, "wr": 0, "w0f": 0, "Sf": 324875.10216936003, "Sr": 283229.16028579313, "Dx": 788.028169894862, "cd": 1}, + "rates": {"dissipated": 0, "supplied": 0, "floor": 0, "grade": -0}, + "next": [0.0032481985805457771, 1.4550464025321322e-08, 2.063503148721843e-05, 0.054136644212622924, 0.00060545142240215992, 0.00034391719914078894, 0.020946006673345115, 0.90237670327815533], + "free": {"vx": 0.054136644212622924, "vy": 0.00060314686984607434, "r": 0.00034521109633305774}, + "books": {"dH": 3.9222407051603652, "dissipated": 0.068884470032634745, "supplied": 3.9222400422277417, "floor": 5.7135242476698058e-10, "grade": -0, "limiter": 9.2523945038891497e-07, "residual": 0.068884207154455535}, + "g_state": [0, 0, 0, 228.92649419811758, 2.465602765855218, 3.2564308892131937, -0.0010988264864153741, 7.6917420463825845], + "g_delta_cmd": -3.9576052137056676e-06, "g_a_cmd": 6.323525430819652}, + {"name": "stopping at the clamp", "state": [0, 0, 0, 0.29999999999999999, 0, 0.050000000000000003, 0.20000000000000001, -6], "delta_cmd": 0.20000000000000001, "a_cmd": -6, "grade": 0, + "limits": {"L": 3.2999999523162842, "vmax": 40, "hdt": 0.059999998658895493, "allow_reverse": 0}, + "parts": {"u": 0.58309518948452999, "sg": 0.51449575542752657, "Fxf": -11240.746548567124, "Fxr": -4711.3658366127747, "Fyf": -3860.3908643551163, "Fyr": 5792.8950502960515, "wf": 0.076849997043609619, "wr": -0.088150000572204595, "w0f": 0.060212665452055336, "Sf": 232031.85217001935, "Sr": 65716.335935256517, "Dx": 675.96020050715163, "cd": 0.98006657784124163}, + "rates": {"dissipated": 629.96568739769941, "supplied": -4890.0345190848939, "floor": -2.1894094567378808, "grade": -0}, + "next": [1.7498980678721112e-09, -8.6535871941968698e-05, 2.0221649454310458e-05, 0, -0.00084808508129440973, 0.00033702749843832593, 0.20000000000000001, -6], + "free": {"vx": -0.059860071445697161, "vy": -0.0053382628087870069, "r": 0.0021214161174794573}, + "books": {"dH": -128.27329808069157, "dissipated": 20.195298564663304, "supplied": -118.31738102777641, "floor": -0.052474467419384233, "grade": 0, "limiter": -4.845308680962499, "residual": 15.137164660130026}, + "g_state": [0, 0, 0, -1327.1231135289845, 41.284887027157239, -65.788762611175002, -12.594168378664648, 0.89760497109159187], + "g_delta_cmd": -29.219941313896982, "g_a_cmd": 0.73793788550114403} + ] +} diff --git a/src/cvc/tests/nav_dyn_bike_test.cpp b/src/cvc/tests/nav_dyn_bike_test.cpp index cf3354e2c..7e6535087 100644 --- a/src/cvc/tests/nav_dyn_bike_test.cpp +++ b/src/cvc/tests/nav_dyn_bike_test.cpp @@ -1005,6 +1005,201 @@ TEST(DynBikePH, TheSecantStaysPSDWhereTheTangentDoesNot) { } } +// The values GRL-SNAM's torch twin of the form and its books is held to, on DynBike.Golden's +// Humvee (lags and the low-speed blend). Per case: the parts (dyn_ph_parts_at) and the rates +// (dyn_ph_rates_at) at its state; one dyn_plant_books step from it, with the twist the step +// reached before its limiters (dyn_step_free) and its books; and the gradient of a fixed weighting +// of the books on the state and the commands, by forward-mode duals (the twin's is autograd). The +// cases are DynBike.Golden's, and a stop at the speed clamp. CVC_DYN_BIKE_PH_GOLDEN_WRITE=1 +// rewrites tests/data/dyn_bike_ph_golden.json; otherwise this build must reproduce it to 1e-9. +TEST(DynBikePH, Golden) { + namespace fs = std::filesystem; + const fs::path path = fs::path(__FILE__).parent_path() / "data" / "dyn_bike_ph_golden.json"; + dyn_bike_params p = humvee(); + p.tau_steer = 0.05f; + p.v_blend = 1.0f; + p.v_width = 0.3f; + struct gcase { + const char *name; + double s[8], dcmd, acmd, grade; + bool reverse; + }; + const gcase cases[] = { + {"cruising", {0.0, 0.0, 0.0, 12.0, 0.1, 0.05, 0.02, 0.5}, 0.05, 1.0, 0.0, false}, + {"hard corner", {1.0, -2.0, 0.3, 15.0, -1.2, 0.6, 0.25, 0.0}, 0.3, 0.0, 0.02, false}, + {"trail braking", {-3.0, 4.0, -1.1, 14.0, 0.8, 0.4, 0.15, -4.0}, 0.15, -6.0, -0.03, false}, + {"past the peak", {2.0, 2.0, 2.5, 10.0, 2.5, 1.1, 0.4, 1.0}, 0.45, 2.0, 0.0, false}, + {"backing up", {0.5, 0.5, 0.2, -2.0, 0.05, -0.1, 0.2, -0.5}, 0.25, -0.8, 0.0, true}, + {"creeping", {0.0, 0.0, 0.0, 0.6, 0.02, 0.05, 0.3, 0.3}, 0.35, 0.5, 0.0, false}, + {"pulling away", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 0.1, 2.0, 0.0, false}, + {"stopping at the clamp", {0.0, 0.0, 0.0, 0.3, 0.0, 0.05, 0.2, -6.0}, 0.2, -6.0, 0.0, false}, + }; + const char *const part_keys[] = {"u", "sg", "Fxf", "Fxr", "Fyf", "Fyr", "wf", + "wr", "w0f", "Sf", "Sr", "Dx", "cd"}; + const char *const rate_keys[] = {"dissipated", "supplied", "floor", "grade"}; + const char *const free_keys[] = {"vx", "vy", "r"}; + const char *const book_keys[] = {"dH", "dissipated", "supplied", "floor", + "grade", "limiter", "residual"}; + const double g_books[7] = {0.9, -0.4, 0.7, 1.3, -0.6, 0.8, 1.1}; + // Every value this build computes, in the file's order, and what it is: per case, parts[13], + // rates[4], next[8], free[3], books[7], g_state[8], g_delta_cmd, g_a_cmd. + std::vector now; + std::vector what; + std::ostringstream js; + const auto num = [](double x) { + char b[40]; + std::snprintf(b, sizeof b, "%.17g", x); + return std::string(b); + }; + const auto arr = [&](const double *v, int n) { + std::string out = "["; + for (int k = 0; k < n; ++k) + out += (k ? ", " : "") + num(v[k]); + return out + "]"; + }; + const auto obj = [&](const char *const *keys, const double *v, int n) { + std::string out = "{"; + for (int k = 0; k < n; ++k) + out += std::string(k ? ", " : "") + "\"" + keys[k] + "\": " + num(v[k]); + return out + "}"; + }; + const auto record = [&](const char *name, const char *group, const char *const *keys, + const double *v, int n) { + for (int k = 0; k < n; ++k) { + now.push_back(v[k]); + std::string label = std::string(name) + " " + group; + if (n > 1) + label += "." + (keys ? std::string(keys[k]) : std::to_string(k)); + what.push_back(label); + } + }; + js << "{\n \"format\": \"cvc-dyn-bike-ph-golden-1\",\n"; + js << " \"order\": [\"x\", \"y\", \"psi\", \"vx\", \"vy\", \"r\", \"delta\", \"a\"],\n"; + js << " \"params\": {"; + const std::pair params[] = {{"m", p.m}, + {"Iz", p.Iz}, + {"lf", p.lf}, + {"lr", p.lr}, + {"h", p.h}, + {"g", p.g}, + {"mu", p.mu}, + {"B_f", p.B_f}, + {"B_r", p.B_r}, + {"C", p.C}, + {"front_drive", p.front_drive}, + {"front_brake", p.front_brake}, + {"drag", p.drag}, + {"c_rr", p.c_rr}, + {"tau_steer", p.tau_steer}, + {"steer_rate", p.steer_rate}, + {"tau_drive", p.tau_drive}, + {"v_blend", p.v_blend}, + {"v_width", p.v_width}, + {"eps", p.eps}, + {"fz_min", p.fz_min}}; + bool first = true; + for (const auto &kv : params) { + js << (first ? "" : ", ") << "\"" << kv.first << "\": " << num(kv.second); + first = false; + } + js << ", \"iters\": " << p.iters << "},\n \"g_books\": " << arr(g_books, 7) + << ",\n \"cases\": [\n"; + const int ncase = static_cast(sizeof cases / sizeof cases[0]); + for (int c = 0; c < ncase; ++c) { + const gcase &gc = cases[c]; + const bike_veh lim = plant_limits(p, gc.reverse); + dyn_state s; + for (int k = 0; k < 8; ++k) + s[k] = gc.s[k]; + diff::dyn_ph_parts q; + diff::dyn_ph_parts_at(p, s, q); + const double parts[] = {q.u, q.sg, q.Fxf, q.Fxr, q.Fyf, q.Fyr, q.wf, + q.wr, q.w0f, q.Sf, q.Sr, q.Dx, q.cd}; + const diff::dyn_ph_rates rt = diff::dyn_ph_rates_at(p, s, gc.grade); + const double rates[] = {rt.dissipated, rt.supplied, rt.floor, rt.grade}; + dyn_state o, o_books; + diff::dyn_step_free seen; + diff::dyn_plant(p, lim, s, gc.dcmd, gc.acmd, gc.grade, o, &seen); + diff::dyn_ph_books b; + diff::dyn_plant_books(p, lim, s, gc.dcmd, gc.acmd, gc.grade, o_books, b); + for (int k = 0; k < 8; ++k) + EXPECT_EQ(o_books[k], o[k]) << gc.name << ": the books' step is not dyn_plant's"; + const double next[] = {o.x, o.y, o.psi, o.vx, o.vy, o.r, o.delta, o.a}; + const double twist[] = {seen.vx, seen.vy, seen.r}; + const double books[] = {b.dH, b.dissipated, b.supplied, b.floor, + b.grade, b.limiter, b.residual}; + // d(g_books . books) / d(state, delta_cmd, a_cmd), one dual pass per input. + typedef diff::dual D; + double grad[10]; + for (int j = 0; j < 10; ++j) { + dyn_state ds, od; + for (int k = 0; k < 8; ++k) + ds[k] = D(gc.s[k], k == j ? 1.0 : 0.0); + diff::dyn_ph_books bd; + diff::dyn_plant_books(p, lim, ds, D(gc.dcmd, j == 8 ? 1.0 : 0.0), + D(gc.acmd, j == 9 ? 1.0 : 0.0), D(gc.grade), od, bd); + const D bv[] = {bd.dH, bd.dissipated, bd.supplied, bd.floor, + bd.grade, bd.limiter, bd.residual}; + grad[j] = 0.0; + for (int k = 0; k < 7; ++k) + grad[j] += g_books[k] * bv[k].d; + } + record(gc.name, "parts", part_keys, parts, 13); + record(gc.name, "rates", rate_keys, rates, 4); + record(gc.name, "next", nullptr, next, 8); + record(gc.name, "free", free_keys, twist, 3); + record(gc.name, "books", book_keys, books, 7); + record(gc.name, "g_state", nullptr, grad, 8); + record(gc.name, "g_delta_cmd", nullptr, grad + 8, 1); + record(gc.name, "g_a_cmd", nullptr, grad + 9, 1); + js << " {\"name\": \"" << gc.name << "\", \"state\": " << arr(gc.s, 8) + << ", \"delta_cmd\": " << num(gc.dcmd) << ", \"a_cmd\": " << num(gc.acmd) + << ", \"grade\": " << num(gc.grade) << ",\n \"limits\": {\"L\": " << num(lim.L) + << ", \"vmax\": " << num(lim.vmax) << ", \"hdt\": " << num(lim.hdt) + << ", \"allow_reverse\": " << lim.allow_reverse << "}" + << ",\n \"parts\": " << obj(part_keys, parts, 13) + << ",\n \"rates\": " << obj(rate_keys, rates, 4) << ",\n \"next\": " << arr(next, 8) + << ",\n \"free\": " << obj(free_keys, twist, 3) + << ",\n \"books\": " << obj(book_keys, books, 7) << ",\n \"g_state\": " << arr(grad, 8) + << ",\n \"g_delta_cmd\": " << num(grad[8]) << ", \"g_a_cmd\": " << num(grad[9]) << "}" + << (c + 1 < ncase ? "," : "") << "\n"; + } + js << " ]\n}\n"; + const char *write = std::getenv("CVC_DYN_BIKE_PH_GOLDEN_WRITE"); + if (write && std::string(write) == "1") { + std::ofstream(path, std::ios::binary) << js.str(); + std::printf(" wrote %s\n", path.string().c_str()); + return; + } + boost::property_tree::ptree want; + ASSERT_NO_THROW(boost::property_tree::read_json(path.string(), want)) << path; + std::vector golden; + for (const auto &c : want.get_child("cases")) { + const boost::property_tree::ptree &gc = c.second; + const auto named = [&](const char *group, const char *const *keys, int n) { + for (int k = 0; k < n; ++k) + golden.push_back(gc.get(std::string(group) + "." + keys[k])); + }; + const auto listed = [&](const char *key) { + for (const auto &v : gc.get_child(key)) + golden.push_back(v.second.get_value()); + }; + named("parts", part_keys, 13); + named("rates", rate_keys, 4); + listed("next"); + named("free", free_keys, 3); + named("books", book_keys, 7); + listed("g_state"); + golden.push_back(gc.get("g_delta_cmd")); + golden.push_back(gc.get("g_a_cmd")); + } + ASSERT_EQ(golden.size(), now.size()); + for (std::size_t k = 0; k < now.size(); ++k) + EXPECT_LE(std::fabs(now[k] - golden[k]), + 1e-9 * std::max(1.0, std::max(std::fabs(now[k]), std::fabs(golden[k])))) + << what[k] << ": golden " << golden[k] << ", now " << now[k]; +} + // ── The external-force port: the deployed drive's ext_force in the differentiable rollout ── namespace {