feat:finsh joint space planner

CMakeLists.txt

@@ -67,6 +67,7 @@ include_directories(
         ${PROJECT_SOURCE_DIR}/src/utils
         ${PROJECT_SOURCE_DIR}/third_party
         ${PROJECT_SOURCE_DIR}/src
+        ${PROJECT_SOURCE_DIR}/third_party/toppra/0.6.2/include
 
 )
 
@@ -75,7 +76,7 @@ include_directories(
 ############################################################
 
 add_subdirectory(src)
-add_subdirectory(example)
+#add_subdirectory(example)
 #add_subdirectory(test)
 
 ############################################################
@@ -100,4 +101,5 @@ target_link_libraries(cmvr_es PRIVATE
         cmvr_es::ctrl::controller
         cmvr_es::data_center
         cmvr_es::ik_solver
+        cmvr_es::planner
 )

data/planner/plot_traj.py

@@ -0,0 +1,63 @@
+import os
+import re
+import sys
+import pandas as pd
+import matplotlib.pyplot as plt
+
+def main(csv_path: str):
+    # 更健壮的读取
+    df = pd.read_csv(csv_path, sep=None, engine="python", comment="#")
+    if 't' not in df.columns:
+        raise RuntimeError("CSV missing 't' column")
+
+    # ——严格区分列名——
+    # 允许列名形如：q0, q1, ... | qd0, qd1, ... | qdd0, qdd1, ...
+    def grep(pattern):
+        r = re.compile(pattern)
+        return [c for c in df.columns if r.fullmatch(c)]
+
+    q_cols   = grep(r"q\d+")
+    qd_cols  = grep(r"qd\d+")
+    qdd_cols = grep(r"qdd\d+")
+
+    # 如果你的列名不是纯数字后缀（比如 q_joint1），把上面的正则改成更宽松的：
+    # q_cols   = [c for c in df.columns if c.startswith("q")   and not c.startswith(("qd","qdd"))]
+    # qd_cols  = [c for c in df.columns if c.startswith("qd")  and not c.startswith("qdd")]
+    # qdd_cols = [c for c in df.columns if c.startswith("qdd")]
+
+    t = df['t'].values
+
+    # 位置
+    plt.figure()
+    for c in q_cols:
+        plt.plot(t, df[c].values, label=c)
+    plt.xlabel('time [s]')
+    plt.ylabel('position [rad]')
+    plt.title('Joint Positions')
+    plt.legend()
+    plt.grid(True)
+
+    # 速度（只画 qd*）
+    plt.figure()
+    for c in qd_cols:
+        plt.plot(t, df[c].values, label=c)
+    plt.xlabel('time [s]')
+    plt.ylabel('velocity [rad/s]')
+    plt.title('Joint Velocities')
+    plt.legend()
+    plt.grid(True)
+
+    # 加速度（只画 qdd*）
+    plt.figure()
+    for c in qdd_cols:
+        plt.plot(t, df[c].values, label=c)
+    plt.xlabel('time [s]')
+    plt.ylabel('acceleration [rad/s^2]')
+    plt.title('Joint Accelerations')
+    plt.legend()
+    plt.grid(True)
+
+    plt.show()
+
+if __name__ == '__main__':
+    main("/home/lgv/cmvr/cmvr-es/data/planner/traj.csv")

data/planner/traj.csv

@@ -0,0 +1,148 @@
+t,q1,q2,q3,q4,q5,q6,q7,qd1,qd2,qd3,qd4,qd5,qd6,qd7,qdd1,qdd2,qdd3,qdd4,qdd5,qdd6,qdd7
+0.0000000000,0.0000000000,-0.5000000000,0.8000000000,0.0000000000,0.2000000000,-0.3000000000,0.1000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000
+0.0100000000,0.0000303310,-0.4999823069,0.7999646138,0.0000176931,0.1999848345,-0.2999797793,0.0999924172,0.0118947055,0.0069385782,-0.0138771564,0.0069385782,-0.0059473527,0.0079298037,-0.0029736764,2.5705569358,1.4994915459,-2.9989830918,1.4994915459,-1.2852784679,1.7137046239,-0.6426392339
+0.0200000000,0.0002706117,-0.4998421432,0.7996842864,0.0001578568,0.1998646942,-0.2998195922,0.0999323471,0.0362504791,0.0211461128,-0.0422922256,0.0211461128,-0.0181252396,0.0241669861,-0.0090626198,2.5169090310,1.4681969348,-2.9363938695,1.4681969348,-1.2584545155,1.6779393540,-0.6292272578
+0.0300000000,0.0007555952,-0.4995592361,0.7991184723,0.0004407639,0.1996222024,-0.2994962699,0.0998111012,0.0607600493,0.0354433621,-0.0708867242,0.0354433621,-0.0303800246,0.0405066995,-0.0151900123,2.5596209442,1.4931122175,-2.9862244349,1.4931122175,-1.2798104721,1.7064139628,-0.6399052361
+0.0400000000,0.0014880673,-0.4991319607,0.7982639215,0.0008680393,0.1992559664,-0.2990079551,0.0996279832,0.0857190835,0.0500027987,-0.1000055974,0.0500027987,-0.0428595417,0.0571460556,-0.0214297709,2.4658651556,1.4384213408,-2.8768426816,1.4384213408,-1.2329325778,1.6439101038,-0.6164662889
+0.0500000000,0.0024696657,-0.4985593617,0.7971187233,0.0014406383,0.1987651671,-0.2983535562,0.0993825836,0.1106444598,0.0645426015,-0.1290852031,0.0645426015,-0.0553222299,0.0737629732,-0.0276611149,2.5568927194,1.4915207530,-2.9830415059,1.4915207530,-1.2784463597,1.7045951462,-0.6392231798
+0.0600000000,0.0037018685,-0.4978405767,0.7956811534,0.0021594233,0.1981490657,-0.2975320877,0.0990745329,0.1358094448,0.0792221761,-0.1584443523,0.0792221761,-0.0679047224,0.0905396299,-0.0339523612,2.5158582466,1.4675839772,-2.9351679544,1.4675839772,-1.2579291233,1.6772388311,-0.6289645616
+0.0700000000,0.0051853943,-0.4969751867,0.7939503733,0.0030248133,0.1974073029,-0.2965430705,0.0987036514,0.1609332845,0.0938777493,-0.1877554986,0.0938777493,-0.0804666423,0.1072888563,-0.0402333211,2.4840420418,1.4490245244,-2.8980490488,1.4490245244,-1.2420210209,1.6560280279,-0.6210105105
+0.0800000000,0.0069207211,-0.4959629127,0.7919258254,0.0040370873,0.1965396394,-0.2953861859,0.0982698197,0.1861077050,0.1085628279,-0.2171256559,0.1085628279,-0.0930538525,0.1240718034,-0.0465269263,2.5667165306,1.4972513095,-2.9945026190,1.4972513095,-1.2833582653,1.7111443537,-0.6416791326
+0.0900000000,0.0089085479,-0.4948033471,0.7896066941,0.0051966529,0.1955457261,-0.2940609681,0.0977728630,0.2114304418,0.1233344244,-0.2466688488,0.1233344244,-0.1057152209,0.1409536279,-0.0528576105,2.5507009528,1.4879088891,-2.9758177783,1.4879088891,-1.2753504764,1.7004673019,-0.6376752382
+0.1000000000,0.0111492914,-0.4934962467,0.7869924933,0.0065037533,0.1944253543,-0.2925671391,0.0972126771,0.2367541946,0.1381066135,-0.2762132270,0.1381066135,-0.1183770973,0.1578361297,-0.0591885486,2.5385172632,1.4808017369,-2.9616034738,1.4808017369,-1.2692586316,1.6923448422,-0.6346293158
+0.1100000000,0.0136432400,-0.4920414433,0.7840828866,0.0079585567,0.1931783800,-0.2909045066,0.0965891900,0.2620885162,0.1528849678,-0.3057699355,0.1528849678,-0.1310442581,0.1747256774,-0.0655221290,2.5296551441,1.4756321674,-2.9512643348,1.4756321674,-1.2648275720,1.6864367627,-0.6324137860
+0.1200000000,0.0163906632,-0.4904387798,0.7808775596,0.0095612202,0.1918046684,-0.2890728912,0.0959023342,0.2874390193,0.1676727613,-0.3353455225,0.1676727613,-0.1437195097,0.1916260129,-0.0718597548,2.5235281251,1.4720580730,-2.9441161460,1.4720580730,-1.2617640626,1.6823520834,-0.6308820313
+0.1300000000,0.0193918053,-0.4886881136,0.7773762272,0.0113118864,0.1903040974,-0.2870721298,0.0951520487,0.3128084890,0.1824716186,-0.3649432372,0.1824716186,-0.1564042445,0.2085389927,-0.0782021222,2.5195885283,1.4697599748,-2.9395199497,1.4697599748,-1.2597942641,1.6797256855,-0.6298971321
+0.1400000000,0.0226468840,-0.4867893176,0.7735786353,0.0132106824,0.1886765580,-0.2849020773,0.0943382790,0.3381978099,0.1972820558,-0.3945641116,0.1972820558,-0.1690989050,0.2254652066,-0.0845494525,2.5173649797,1.4684629048,-2.9369258096,1.4684629048,-1.2586824898,1.6782433198,-0.6293412449
+0.1500000000,0.0261560917,-0.4847422798,0.7694845596,0.0152577202,0.1869219541,-0.2825626055,0.0934609771,0.3636066556,0.2121038824,-0.4242077649,0.2121038824,-0.1818033278,0.2424044371,-0.0909016639,2.5164678157,1.4679395592,-2.9358791183,1.4679395592,-1.2582339079,1.6776452105,-0.6291169539
+0.1600000000,0.0299195974,-0.4825469015,0.7650938031,0.0174530985,0.1850402013,-0.2800536018,0.0925201007,0.3890339729,0.2269364842,-0.4538729683,0.2269364842,-0.1945169864,0.2593559819,-0.0972584932,2.5165820925,1.4680062206,-2.9360124412,1.4680062206,-1.2582910462,1.6777213950,-0.6291455231
+0.1700000000,0.0339375494,-0.4802030962,0.7604061924,0.0197969038,0.1830312253,-0.2773749671,0.0915156127,0.4144804821,0.2417802812,-0.4835605625,0.2417802812,-0.2072402411,0.2763203214,-0.1036201205,2.5712342052,1.4998866197,-2.9997732394,1.4998866197,-1.2856171026,1.7141561368,-0.6428085513
+0.1800000000,0.0382100796,-0.4777107869,0.7554215738,0.0222892131,0.1808949602,-0.2745266136,0.0904474801,0.4399496705,0.2566373078,-0.5132746156,0.2566373078,-0.2199748352,0.2932997803,-0.1099874176,2.5703909559,1.4993947243,-2.9987894485,1.4993947243,-1.2851954779,1.7135939706,-0.6425977390
+0.1900000000,0.0427373015,-0.4750699074,0.7501398149,0.0249300926,0.1786313492,-0.2715084657,0.0893156746,0.4654267687,0.2714989484,-0.5429978969,0.2714989484,-0.2327133844,0.3102845125,-0.1163566922,2.5700679475,1.4992063027,-2.9984126055,1.4992063027,-1.2850339738,1.7133786317,-0.6425169869
+0.2000000000,0.0475193158,-0.4722803991,0.7445607983,0.0277196009,0.1762403421,-0.2683204561,0.0881201711,0.4909112306,0.2863648845,-0.5727297690,0.2863648845,-0.2454556153,0.3272741537,-0.1227278076,2.5701631579,1.4992618421,-2.9985236842,1.4992618421,-1.2850815790,1.7134421053,-0.6425407895
+0.2100000000,0.0525562127,-0.4693422093,0.7386844185,0.0306577907,0.1737218937,-0.2649625249,0.0868609468,0.5164024132,0.3012347410,-0.6024694820,0.3012347410,-0.2582012066,0.3442682754,-0.1291006033,2.5705935321,1.4995128937,-2.9990257875,1.4995128937,-1.2852967661,1.7137290214,-0.6426483830
+0.2200000000,0.0578480733,-0.4662552906,0.7325105811,0.0337447094,0.1710759633,-0.2614346178,0.0855379817,0.5418996246,0.3161081144,-0.6322162287,0.3161081144,-0.2709498123,0.3612664164,-0.1354749062,2.5712921239,1.4999204056,-2.9998408112,1.4999204056,-1.2856460620,1.7141947493,-0.6428230310
+0.2300000000,0.0633949692,-0.4630196013,0.7260392026,0.0369803987,0.1683025154,-0.2577366872,0.0841512577,0.5674109491,0.3309897203,-0.6619794407,0.3309897203,-0.2837054746,0.3782739661,-0.1418527373,2.5301339481,1.4759114697,-2.9518229395,1.4759114697,-1.2650669741,1.6867559654,-0.6325334870
+0.2400000000,0.0691969624,-0.4596351052,0.7192702105,0.0403648948,0.1654015188,-0.2538686917,0.0827007594,0.5929303912,0.3458760615,-0.6917521231,0.3458760615,-0.2964651956,0.3952869275,-0.1482325978,2.5326648409,1.4773878239,-2.9547756477,1.4773878239,-1.2663324205,1.6884432273,-0.6331662102
+0.2500000000,0.0752541086,-0.4561017700,0.7122035400,0.0438982300,0.1623729457,-0.2498305943,0.0811864729,0.6184553299,0.3607656091,-0.7215312182,0.3607656091,-0.3092276649,0.4123035533,-0.1546138325,2.5351903243,1.4788610225,-2.9577220450,1.4788610225,-1.2675951621,1.6901268829,-0.6337975811
+0.2600000000,0.0815664568,-0.4524195669,0.7048391338,0.0475804331,0.1592167716,-0.2456223622,0.0796083858,0.6439847927,0.3756577957,-0.7513155914,0.3756577957,-0.3219923963,0.4293231951,-0.1609961982,2.5376890165,1.4803185930,-2.9606371859,1.4803185930,-1.2688445083,1.6917926777,-0.6344222541
+0.2700000000,0.0881340499,-0.4485884709,0.6971769418,0.0514115291,0.1559329750,-0.2412439667,0.0779664875,0.6695179008,0.3905521088,-0.7811042176,0.3905521088,-0.3347589504,0.4463452672,-0.1673794752,2.5401463450,1.4817520346,-2.9635040692,1.4817520346,-1.2700731725,1.6934308967,-0.6350365863
+0.2800000000,0.0949569253,-0.4446084602,0.6892169205,0.0553915398,0.1525215373,-0.2366953831,0.0762607687,0.6950538573,0.4054480834,-0.8108961669,0.4054480834,-0.3475269287,0.4633692382,-0.1737634643,2.5425531142,1.4831559833,-2.9663119666,1.4831559833,-1.2712765571,1.6950354095,-0.6356382786
+0.2900000000,0.1020351148,-0.4404795164,0.6809590328,0.0595204836,0.1489824426,-0.2319765902,0.0744912213,0.7205919350,0.4203452954,-0.8406905908,0.4203452954,-0.3602959675,0.4803946233,-0.1801479838,2.5449043618,1.4845275444,-2.9690550888,1.4845275444,-1.2724521809,1.6966029079,-0.6362260904
+0.3000000000,0.1093686449,-0.4362016238,0.6724032476,0.0637983762,0.1453156776,-0.2270875701,0.0726578388,0.7461314654,0.4352433548,-0.8704867096,0.4352433548,-0.3730657327,0.4974209769,-0.1865328663,2.5471984408,1.4858657571,-2.9717315143,1.4858657571,-1.2735992204,1.6981322939,-0.6367996102
+0.3100000000,0.1169575373,-0.4317747699,0.6635495398,0.0682252301,0.1415212313,-0.2220283085,0.0707606157,0.7716718282,0.4501418998,-0.9002837996,0.4501418998,-0.3858359141,0.5144478855,-0.1929179571,2.5494362796,1.4871711631,-2.9743423262,1.4871711631,-1.2747181398,1.6996241864,-0.6373590699
+0.3200000000,0.1248018088,-0.4271989449,0.6543978897,0.0728010551,0.1375990956,-0.2167987941,0.0687995478,0.7972124423,0.4650405913,-0.9300811826,0.4650405913,-0.3986062211,0.5314749615,-0.1993031106,2.5516207797,1.4884454548,-2.9768909096,1.4884454548,-1.2758103898,1.7010805198,-0.6379051949
+0.3300000000,0.1329014713,-0.4224741417,0.6449482835,0.0775258583,0.1335492643,-0.2113990191,0.0667746322,0.8227527569,0.4799391082,-0.9598782164,0.4799391082,-0.4113763785,0.5485018379,-0.2056881892,2.5537563207,1.4896911871,-2.9793823741,1.4896911871,-1.2768781603,1.7025042138,-0.6384390802
+0.3400000000,0.1412565321,-0.4176003563,0.6352007125,0.0823996437,0.1293717339,-0.2058289786,0.0646858670,0.8482922453,0.4948371431,-0.9896742862,0.4948371431,-0.4241461227,0.5655281636,-0.2120730613,2.5558483480,1.4909115363,-2.9818230726,1.4909115363,-1.2779241740,1.7038988986,-0.6389620870
+0.3500000000,0.1498669938,-0.4125775870,0.6251551739,0.0874224130,0.1250665031,-0.2000886708,0.0625332516,0.8738303983,0.5097343990,-1.0194687980,0.5097343990,-0.4369151991,0.5825535989,-0.2184575996,2.5579030214,1.4921100958,-2.9842201917,1.4921100958,-1.2789515107,1.7052686810,-0.6394757554
+0.3600000000,0.1587328541,-0.4074058351,0.6148116702,0.0925941649,0.1206335729,-0.1941780973,0.0603167865,0.8993667191,0.5246305861,-1.0492611723,0.5246305861,-0.4496833595,0.5995778127,-0.2248416798,2.5599269098,1.4932906974,-2.9865813948,1.4932906974,-1.2799634549,1.7066179399,-0.6399817275
+0.3700000000,0.1678541063,-0.4020851047,0.6041702093,0.0979148953,0.1160729469,-0.1880972625,0.0580364734,0.9249007197,0.5395254198,-1.0790508397,0.5395254198,-0.4624503599,0.6166004798,-0.2312251799,2.5619267159,1.4944572509,-2.9889145019,1.4944572509,-1.2809633580,1.7079511439,-0.6404816790
+0.3800000000,0.1772307386,-0.3966154025,0.5932308050,0.1033845975,0.1113846307,-0.1818461743,0.0556923153,0.9504319178,0.5544186187,-1.1088372375,0.5544186187,-0.4752159589,0.6336212785,-0.2376079795,2.5639090208,1.4956135955,-2.9912271910,1.4956135955,-1.2819545104,1.7092726806,-0.6409772552
+0.3900000000,0.1868627346,-0.3909967382,0.5819934763,0.1090032618,0.1065686327,-0.1754248436,0.0532843164,0.9759598343,0.5693099034,-1.1386198067,0.5693099034,-0.4879799172,0.6506398895,-0.2439899586,2.5658800374,1.4967633551,-2.9935267103,1.4967633551,-1.2829400187,1.7105866916,-0.6414700093
+0.4000000000,0.1967500727,-0.3852291242,0.5704582485,0.1147708758,0.1016249636,-0.1688332849,0.0508124818,1.0014839922,0.5841989955,-1.1683979909,0.5841989955,-0.5007419961,0.6676559948,-0.2503709981,2.5678453619,1.4979097944,-2.9958195888,1.4979097944,-1.2839226809,1.7118969079,-0.6419613405
+0.4100000000,0.2068927264,-0.3793125763,0.5586251526,0.1206874237,0.0965536368,-0.1620715158,0.0482768184,1.0270039158,0.5990856175,-1.1981712351,0.5990856175,-0.5135019579,0.6846692772,-0.2567509789,2.5698097166,1.4990556680,-2.9981113360,1.4990556680,-1.2849048583,1.7132064777,-0.6424524291
+0.4200000000,0.2172906634,-0.3732471130,0.5464942260,0.1267528870,0.0913546683,-0.1551395577,0.0456773341,1.0525228938,0.6139716881,-1.2279433761,0.6139716881,-0.5262614469,0.7016819292,-0.2631307235,2.5334478139,1.4778445581,-2.9556891162,1.4778445581,-1.2667239069,1.6889652093,-0.6333619535
+0.4300000000,0.2279438391,-0.3670327605,0.5340655210,0.1329672395,0.0860280804,-0.1480374406,0.0430140402,1.0780541229,0.6288649050,-1.2577298101,0.6288649050,-0.5390270615,0.7187027486,-0.2695135307,2.5346702833,1.4785576653,-2.9571153305,1.4785576653,-1.2673351417,1.6897801889,-0.6336675708
+0.4400000000,0.2388522072,-0.3606695458,0.5213390916,0.1393304542,0.0805738964,-0.1407651952,0.0402869482,1.1035795454,0.6437547348,-1.2875094697,0.6437547348,-0.5517897727,0.7357196970,-0.2758948864,2.5357608795,1.4791938464,-2.9583876928,1.4791938464,-1.2678804398,1.6905072530,-0.6339402199
+0.4500000000,0.2500157197,-0.3541574968,0.5083149936,0.1458425032,0.0749921401,-0.1333228535,0.0374960701,1.1290986643,0.6586408875,-1.3172817751,0.6586408875,-0.5645493322,0.7527324429,-0.2822746661,2.5367138257,1.4797497317,-2.9594994633,1.4797497317,-1.2683569129,1.6911425505,-0.6341784564
+0.4600000000,0.2614343228,-0.3474966450,0.4949932901,0.1525033550,0.0692828386,-0.1257104515,0.0346414193,1.1546109590,0.6735230594,-1.3470461188,0.6735230594,-0.5773054795,0.7697406393,-0.2886527397,2.5375199658,1.4802199800,-2.9604399601,1.4802199800,-1.2687599829,1.6916799772,-0.6343799914
+0.4700000000,0.2731079561,-0.3406870256,0.4813740513,0.1593129744,0.0634460220,-0.1179280293,0.0317230110,1.1801158814,0.6884009308,-1.3768018617,0.6884009308,-0.5900579407,0.7867439210,-0.2950289704,2.5381662432,1.4805969752,-2.9611939504,1.4805969752,-1.2690831216,1.6921108288,-0.6345415608
+0.4800000000,0.2850365524,-0.3337286778,0.4674573555,0.1662713222,0.0574817238,-0.1099756317,0.0287408619,1.2056128523,0.7032741638,-1.4065483277,0.7032741638,-0.6028064261,0.8037419015,-0.3014032131,2.5386350939,1.4808704715,-2.9617409429,1.4808704715,-1.2693175470,1.6924233960,-0.6346587735
+0.4900000000,0.2972200378,-0.3266216446,0.4532432892,0.1733783554,0.0513899811,-0.1018533081,0.0256949906,1.2311012549,0.7181423987,-1.4362847974,0.7181423987,-0.6155506275,0.8207341699,-0.3077753137,2.5389037380,1.4810271805,-2.9620543610,1.4810271805,-1.2694518690,1.6926024920,-0.6347259345
+0.5000000000,0.3096583306,-0.3193659738,0.4387319477,0.1806340262,0.0451708347,-0.0935611130,0.0225854174,1.2565804284,0.7330052499,-1.4660104998,0.7330052499,-0.6282902142,0.8377202856,-0.3141451071,2.5389433529,1.4810502892,-2.9621005783,1.4810502892,-1.2694716764,1.6926289019,-0.6347358382
+0.5100000000,0.3223398040,-0.3119684476,0.4239368953,0.1880315524,0.0388300980,-0.0851067973,0.0194150490,1.2785479637,0.7458196455,-1.4916392910,0.7458196455,-0.6392739819,0.8523653091,-0.3196369909,2.0023556732,1.1680408094,-2.3360816187,1.1680408094,-1.0011778366,1.3349037821,-0.5005889183
+0.5200000000,0.3351850019,-0.3044754155,0.4089508311,0.1955245845,0.0324074990,-0.0765433320,0.0162037495,1.2859026483,0.7501098782,-1.5002197564,0.7501098782,-0.6429513242,0.8572684322,-0.3214756621,-0.0191931251,-0.0111959897,0.0223919793,-0.0111959897,0.0095965626,-0.0127954167,0.0047982813
+0.5300000000,0.3480434171,-0.2969746734,0.3939493467,0.2030253266,0.0259782915,-0.0679710553,0.0129891457,1.2858989734,0.7501077345,-1.5002154690,0.7501077345,-0.6429494867,0.8572659823,-0.3214747434,-0.0127286032,-0.0074250186,0.0148500371,-0.0074250186,0.0063643016,-0.0084857355,0.0031821508
+0.5400000000,0.3609017634,-0.2894739713,0.3789479427,0.2105260287,0.0195491183,-0.0593988244,0.0097745591,1.2858943791,0.7501050544,-1.5002101089,0.7501050544,-0.6429471895,0.8572629194,-0.3214735948,-0.0053671710,-0.0031308497,0.0062616995,-0.0031308497,0.0026835855,-0.0035781140,0.0013417927
+0.5500000000,0.3737600481,-0.2819733053,0.3639466106,0.2180266947,0.0131199760,-0.0508266346,0.0065599880,1.2858866790,0.7501005628,-1.5002011255,0.7501005628,-0.6429433395,0.8572577860,-0.3214716698,0.0025824903,0.0015064527,-0.0030129053,0.0015064527,-0.0012912451,0.0017216602,-0.0006456226
+0.5600000000,0.3866182799,-0.2744726700,0.3489453401,0.2255273300,0.0066908600,-0.0422544800,0.0033454300,1.2858738787,0.7500930959,-1.5001861918,0.7500930959,-0.6429369394,0.8572492525,-0.3214684697,0.0108938667,0.0063547556,-0.0127095112,0.0063547556,-0.0054469334,0.0072625778,-0.0027234667
+0.5700000000,0.3994764703,-0.2669720590,0.3339441180,0.2330279410,0.0002617648,-0.0336823531,0.0001308824,1.2858542266,0.7500816322,-1.5001632644,0.7500816322,-0.6429271133,0.8572361511,-0.3214635567,0.0194052306,0.0113197179,-0.0226394357,0.0113197179,-0.0097026153,0.0129368204,-0.0048513077
+0.5800000000,0.4123346333,-0.2594714639,0.3189429278,0.2405285361,-0.0061673166,-0.0251102445,-0.0030836583,1.2858262209,0.7500652955,-1.5001305911,0.7500652955,-0.6429131105,0.8572174806,-0.3214565552,0.0280060331,0.0163368526,-0.0326737053,0.0163368526,-0.0140030165,0.0186706887,-0.0070015083
+0.5900000000,0.4251927857,-0.2519708750,0.3039417500,0.2480291250,-0.0125963928,-0.0165381429,-0.0062981964,1.2857885885,0.7500433433,-1.5000866865,0.7500433433,-0.6428942942,0.8571923923,-0.3214471471,0.0366262505,0.0213653128,-0.0427306256,0.0213653128,-0.0183131252,0.0244175003,-0.0091565626
+0.6000000000,0.4380509470,-0.2444702809,0.2889405618,0.2555297191,-0.0190254735,-0.0079660353,-0.0095127368,1.2857402461,0.7500151436,-1.5000302872,0.7500151436,-0.6428701231,0.8571601641,-0.3214350615,0.0452280513,0.0263830299,-0.0527660599,0.0263830299,-0.0226140257,0.0301520342,-0.0113070128
+0.6100000000,0.4509091177,-0.2369696813,0.2739393626,0.2630303187,-0.0254545589,0.0006060785,-0.0127272794,1.2857455136,0.7500182163,-1.5000364326,0.7500182163,-0.6428727568,0.8571636757,-0.3214363784,-0.0437967180,-0.0255480855,0.0510961709,-0.0255480855,0.0218983590,-0.0291978120,0.0109491795
+0.6200000000,0.4637672073,-0.2294691291,0.2589382582,0.2705308709,-0.0318836036,0.0091781382,-0.0159418018,1.2857916427,0.7500451249,-1.5000902498,0.7500451249,-0.6428958213,0.8571944285,-0.3214479107,-0.0311906849,-0.0181945662,0.0363891324,-0.0181945662,0.0155953425,-0.0207937899,0.0077976712
+0.6300000000,0.4766252180,-0.2219686228,0.2439372456,0.2780313772,-0.0383126090,0.0177501453,-0.0191563045,1.2858225124,0.7500631322,-1.5001262644,0.7500631322,-0.6429112562,0.8572150083,-0.3214556281,-0.0191112428,-0.0111482250,0.0222964499,-0.0111482250,0.0095556214,-0.0127408285,0.0047778107
+0.6400000000,0.4894831709,-0.2144681503,0.2289363006,0.2855318497,-0.0447415854,0.0263221139,-0.0223707927,1.2858377872,0.7500720425,-1.5001440850,0.7500720425,-0.6429188936,0.8572251914,-0.3214594468,-0.0074889372,-0.0043685467,0.0087370934,-0.0043685467,0.0037444686,-0.0049926248,0.0018722343
+0.6500000000,0.5023410897,-0.2069676977,0.2139353954,0.2930323023,-0.0511705448,0.0348940598,-0.0255852724,1.2858372226,0.7500717132,-1.5001434264,0.7500717132,-0.6429186113,0.8572248151,-0.3214593056,0.0037526389,0.0021890394,-0.0043780787,0.0021890394,-0.0018763195,0.0025017593,-0.0009381597
+0.6600000000,0.5151990005,-0.1994672497,0.1989344995,0.3005327503,-0.0575995002,0.0434660003,-0.0287997501,1.2858205995,0.7500620164,-1.5001240328,0.7500620164,-0.6429102998,0.8572137330,-0.3214551499,0.0146953785,0.0085723041,-0.0171446083,0.0085723041,-0.0073476893,0.0097969190,-0.0036738446
+0.6700000000,0.5280569313,-0.1919667901,0.1839335801,0.3080332099,-0.0640284657,0.0520379542,-0.0320142328,1.2857876672,0.7500428059,-1.5000856117,0.7500428059,-0.6428938336,0.8571917781,-0.3214469168,0.0254256344,0.0148316201,-0.0296632401,0.0148316201,-0.0127128172,0.0169504229,-0.0063564086
+0.6800000000,0.5409149122,-0.1844663012,0.1689326025,0.3155336988,-0.0704574561,0.0606099414,-0.0352287280,1.2857380953,0.7500138890,-1.5000277779,0.7500138890,-0.6428690477,0.8571587302,-0.3214345238,0.0360335417,0.0210195660,-0.0420391319,0.0210195660,-0.0180167708,0.0240223611,-0.0090083854
+0.6900000000,0.5537729351,-0.1769657879,0.1539315757,0.3230342121,-0.0768864675,0.0691819567,-0.0384432338,1.2857507896,0.7500212939,-1.5000425878,0.7500212939,-0.6428753948,0.8571671931,-0.3214376974,-0.0333854524,-0.0194748473,0.0389496945,-0.0194748473,0.0166927262,-0.0222569683,0.0083463631
+0.7000000000,0.5666308776,-0.1694653214,0.1389306428,0.3305346786,-0.0833154388,0.0777539184,-0.0416577194,1.2857951947,0.7500471969,-1.5000943938,0.7500471969,-0.6428975973,0.8571967965,-0.3214487987,-0.0217846478,-0.0127077112,0.0254154225,-0.0127077112,0.0108923239,-0.0145230985,0.0054461620
+0.7100000000,0.5794887561,-0.1619648923,0.1239297846,0.3380351077,-0.0897443780,0.0863258374,-0.0448721890,1.2858210399,0.7500622732,-1.5001245465,0.7500622732,-0.6429105199,0.8572140266,-0.3214552600,-0.0104249177,-0.0060812020,0.0121624040,-0.0060812020,0.0052124588,-0.0069499451,0.0026062294
+0.7200000000,0.5923466031,-0.1544644815,0.1089289631,0.3455355185,-0.0961733015,0.0948977354,-0.0480866508,1.2858285872,0.7500666758,-1.5001333517,0.7500666758,-0.6429142936,0.8572190581,-0.3214571468,0.0008015257,0.0004675566,-0.0009351133,0.0004675566,-0.0004007628,0.0005343505,-0.0002003814
+0.7300000000,0.6052044511,-0.1469640702,0.0939281404,0.3530359298,-0.1026022255,0.1034696341,-0.0513011128,1.2858179203,0.7500604535,-1.5001209070,0.7500604535,-0.6429089601,0.8572119468,-0.3214544801,0.0120022973,0.0070013401,-0.0140026802,0.0070013401,-0.0060011487,0.0080015315,-0.0030005743
+0.7400000000,0.6180623328,-0.1394636392,0.0789272783,0.3605363608,-0.1090311664,0.1120415552,-0.0545155832,1.2857889366,0.7500435463,-1.5000870926,0.7500435463,-0.6428944683,0.8571926244,-0.3214472341,0.0232851657,0.0135830133,-0.0271660267,0.0135830133,-0.0116425829,0.0155234438,-0.0058212914
+0.7500000000,0.6309202813,-0.1319631693,0.0639263385,0.3680368307,-0.1154601406,0.1206135208,-0.0577300703,1.2857413468,0.7500157856,-1.5000315712,0.7500157856,-0.6428706734,0.8571608978,-0.3214353367,0.0347583962,0.0202757311,-0.0405514623,0.0202757311,-0.0173791981,0.0231722642,-0.0086895991
+0.7600000000,0.6437782961,-0.1244626606,0.0489253212,0.3755373394,-0.1218891481,0.1291855307,-0.0609445740,1.2857477706,0.7500195328,-1.5000390656,0.7500195328,-0.6428738853,0.8571651804,-0.3214369426,-0.0335512026,-0.0195715349,0.0391430697,-0.0195715349,0.0167756013,-0.0223674684,0.0083878007
+0.7700000000,0.6566362507,-0.1169621871,0.0339243741,0.3830378129,-0.1283181254,0.1377575005,-0.0641590627,1.2857940819,0.7500465478,-1.5000930956,0.7500465478,-0.6428970410,0.8571960546,-0.3214485205,-0.0227620987,-0.0132778909,0.0265557818,-0.0132778909,0.0113810493,-0.0151747324,0.0056905247
+0.7800000000,0.6694941565,-0.1094617420,0.0189234840,0.3905382580,-0.1347470783,0.1463294377,-0.0673735391,1.2858231905,0.7500635278,-1.5001270556,0.7500635278,-0.6429115953,0.8572154604,-0.3214557976,-0.0118826836,-0.0069315654,0.0138631309,-0.0069315654,0.0059413418,-0.0079217891,0.0029706709
+0.7900000000,0.6823520444,-0.1019613075,0.0039226149,0.3980386925,-0.1411760222,0.1549013629,-0.0705880111,1.2858353651,0.7500706296,-1.5001412593,0.7500706296,-0.6429176825,0.8572235767,-0.3214588413,-0.0008163663,-0.0004762137,0.0009524274,-0.0004762137,0.0004081832,-0.0005442442,0.0002040916
+0.8000000000,0.6952099436,-0.0944608662,-0.0110782675,0.4055391338,-0.1476049718,0.1634732957,-0.0738024859,1.2858307604,0.7500679436,-1.5001358872,0.7500679436,-0.6429153802,0.8572205070,-0.3214576901,0.0105308698,0.0061430074,-0.0122860148,0.0061430074,-0.0052654349,0.0070205799,-0.0026327175
+0.8100000000,0.7080678820,-0.0869604022,-0.0260791956,0.4130395978,-0.1540339410,0.1720452546,-0.0770169705,1.2858094584,0.7500555174,-1.5001110348,0.7500555174,-0.6429047292,0.8572063056,-0.3214523646,0.0222501448,0.0129792511,-0.0259585022,0.0129792511,-0.0111250724,0.0148334299,-0.0055625362
+0.8200000000,0.7209258854,-0.0794599002,-0.0410801996,0.4205400998,-0.1604629427,0.1806172569,-0.0802314713,1.2857715166,0.7500333847,-1.5000667693,0.7500333847,-0.6428857583,0.8571810110,-0.3214428791,0.0344291026,0.0200836432,-0.0401672864,0.0200836432,-0.0172145513,0.0229527351,-0.0086072757
+0.8300000000,0.7337839772,-0.0719593466,-0.0560813067,0.4280406534,-0.1668919886,0.1891893181,-0.0834459943,1.2857170274,0.7500015993,-1.5000031986,0.7500015993,-0.6428585137,0.8571446849,-0.3214292568,0.0471510061,0.0275047535,-0.0550095071,0.0275047535,-0.0235755030,0.0314340041,-0.0117877515
+0.8400000000,0.7466421031,-0.0644587732,-0.0710824537,0.4355412268,-0.1733210516,0.1977614021,-0.0866605258,1.2857671470,0.7500308357,-1.5000616714,0.7500308357,-0.6428835735,0.8571780980,-0.3214417867,-0.0374598641,-0.0218515874,0.0437031747,-0.0218515874,0.0187299320,-0.0249732427,0.0093649660
+0.8500000000,0.7595001974,-0.0569582182,-0.0860835636,0.4430417818,-0.1797500987,0.2063334649,-0.0898750494,1.2858100610,0.7500558689,-1.5001117378,0.7500558689,-0.6429050305,0.8572067073,-0.3214525152,-0.0280984104,-0.0163907394,0.0327814788,-0.0163907394,0.0140492052,-0.0187322736,0.0070246026
+0.8600000000,0.7723582828,-0.0494576683,-0.1010846633,0.4505423317,-0.1861791414,0.2149055219,-0.0930895707,1.2858411637,0.7500740122,-1.5001480244,0.7500740122,-0.6429205819,0.8572274425,-0.3214602909,-0.0185986909,-0.0108492364,0.0216984727,-0.0108492364,0.0092993455,-0.0123991273,0.0046496727
+0.8700000000,0.7852163798,-0.0419571118,-0.1160857764,0.4580428882,-0.1926081899,0.2234775865,-0.0963040950,1.2858614075,0.7500858211,-1.5001716421,0.7500858211,-0.6429307038,0.8572409384,-0.3214653519,-0.0089609708,-0.0052272330,0.0104544660,-0.0052272330,0.0044804854,-0.0059739806,0.0022402427
+0.8800000000,0.7980745057,-0.0344565383,-0.1310869234,0.4655434617,-0.1990372529,0.2320496705,-0.0995186264,1.2858719343,0.7500919616,-1.5001839233,0.7500919616,-0.6429359671,0.8572479562,-0.3214679836,0.0007908084,0.0004613049,-0.0009226098,0.0004613049,-0.0003954042,0.0005272056,-0.0001977021
+0.8900000000,0.8109326749,-0.0269559396,-0.1460881208,0.4730440604,-0.2054663375,0.2406217833,-0.1027331687,1.2858741350,0.7500932454,-1.5001864908,0.7500932454,-0.6429370675,0.8572494233,-0.3214685337,0.0106028607,0.0061850021,-0.0123700041,0.0061850021,-0.0053014303,0.0070685738,-0.0026507152
+0.9000000000,0.8237908984,-0.0194553093,-0.1610893814,0.4805446907,-0.2118954492,0.2491939322,-0.1059477246,1.2858697039,0.7500906606,-1.5001813212,0.7500906606,-0.6429348519,0.8572464692,-0.3214674260,0.0203836296,0.0118904506,-0.0237809012,0.0118904506,-0.0101918148,0.0135890864,-0.0050959074
+0.9100000000,0.8366491838,-0.0119546428,-0.1760907144,0.4880453572,-0.2183245919,0.2577661225,-0.1091622959,1.2858606794,0.7500853963,-1.5001707926,0.7500853963,-0.6429303397,0.8572404529,-0.3214651698,0.0299936294,0.0174962838,-0.0349925677,0.0174962838,-0.0149968147,0.0199957530,-0.0074984074
+0.9200000000,0.8495075359,-0.0044539374,-0.1910921252,0.4955460626,-0.2247537679,0.2663383572,-0.1123768840,1.2858494625,0.7500788531,-1.5001577062,0.7500788531,-0.6429247312,0.8572329750,-0.3214623656,0.0392324386,0.0228855892,-0.0457711784,0.0228855892,-0.0196162193,0.0261549591,-0.0098081096
+0.9300000000,0.8623659570,0.0030468083,-0.2060936165,0.5030468083,-0.2311829785,0.2749106380,-0.1155914893,1.2858387981,0.7500726322,-1.5001452645,0.7500726322,-0.6429193991,0.8572258654,-0.3214596995,0.0478220500,0.0278961958,-0.0557923917,0.0278961958,-0.0239110250,0.0318813667,-0.0119555125
+0.9400000000,0.8752214871,0.0105458675,-0.2210917349,0.5105458675,-0.2376107435,0.2834809914,-0.1188053718,1.2823483150,0.7480365171,-1.4960730342,0.7480365171,-0.6411741575,0.8548988767,-0.3205870788,-1.9883869907,-1.1598924113,2.3197848225,-1.1598924113,0.9941934954,-1.3255913271,0.4970967477
+0.9500000000,0.8879438235,0.0179672304,-0.2359344608,0.5179672304,-0.2439719118,0.2919625490,-0.1219859559,1.2614071813,0.7358208557,-1.4716417115,0.7358208557,-0.6307035906,0.8409381209,-0.3153517953,-2.5295909024,-1.4755946930,2.9511893861,-1.4755946930,1.2647954512,-1.6863939349,0.6323977256
+0.9600000000,0.9004306434,0.0252512086,-0.2505024173,0.5252512086,-0.2502153217,0.3002870956,-0.1251076608,1.2359282906,0.7209581695,-1.4419163390,0.7209581695,-0.6179641453,0.8239521937,-0.3089820726,-2.5299285135,-1.4757916329,2.9515832658,-1.4757916329,1.2649642568,-1.6866190090,0.6324821284
+0.9700000000,0.9126626739,0.0323865598,-0.2647731195,0.5323865598,-0.2563313369,0.3084417826,-0.1281656685,1.2104396979,0.7060898238,-1.4121796475,0.7060898238,-0.6052198489,0.8069597986,-0.3026099245,-2.5299985479,-1.4758324863,2.9516649726,-1.4758324863,1.2649992740,-1.6866656986,0.6324996370
+0.9800000000,0.9246398321,0.0393732354,-0.2787464707,0.5393732354,-0.2623199160,0.3164265547,-0.1311599580,1.1849421216,0.6912162376,-1.3824324752,0.6912162376,-0.5924710608,0.7899614144,-0.2962355304,-2.5298324081,-1.4757355714,2.9514711428,-1.4757355714,1.2649162040,-1.6865549387,0.6324581020
+0.9900000000,0.9363620426,0.0462111915,-0.2924223831,0.5462111915,-0.2681810213,0.3242413617,-0.1340905107,1.1594362237,0.6763377971,-1.3526755943,0.6763377971,-0.5797181118,0.7729574824,-0.2898590559,-2.5294551323,-1.4755154939,2.9510309877,-1.4755154939,1.2647275662,-1.6863034216,0.6323637831
+1.0000000000,0.9478292357,0.0529003875,-0.3058007750,0.5529003875,-0.2739146179,0.3318861571,-0.1369573089,1.1339342694,0.6614616572,-1.3229233143,0.6614616572,-0.5669671347,0.7559561796,-0.2834835674,-2.5703310519,-1.4993597803,2.9987195605,-1.4993597803,1.2851655259,-1.7135540346,0.6425827630
+1.0100000000,0.9590413433,0.0594407836,-0.3188815672,0.5594407836,-0.2795206716,0.3393608955,-0.1397603358,1.1084324370,0.6465855883,-1.2931711765,0.6465855883,-0.5542162185,0.7389549580,-0.2771081093,-2.5685655748,-1.4983299187,2.9966598373,-1.4983299187,1.2842827874,-1.7123770499,0.6421413937
+1.0200000000,0.9699983086,0.0658323467,-0.3316646933,0.5658323467,-0.2849991543,0.3466655390,-0.1424995771,1.0829244674,0.6317059393,-1.2634118786,0.6317059393,-0.5414622337,0.7219496449,-0.2707311168,-2.5667757347,-1.4972858452,2.9945716905,-1.4972858452,1.2833878673,-1.7111838231,0.6416939337
+1.0300000000,0.9807000815,0.0720750476,-0.3441500951,0.5720750476,-0.2903500408,0.3538000544,-0.1451750204,1.0574108312,0.6168229849,-1.2336459698,0.6168229849,-0.5287054156,0.7049405542,-0.2643527078,-2.5649647508,-1.4962294380,2.9924588760,-1.4962294380,1.2824823754,-1.7099765006,0.6412411877
+1.0400000000,0.9911466182,0.0781688606,-0.3563377212,0.5781688606,-0.2955733091,0.3607644121,-0.1477866545,1.0318919990,0.6019369994,-1.2038739988,0.6019369994,-0.5159459995,0.6879279993,-0.2579729998,-2.5631334425,-1.4951611748,2.9903223496,-1.4951611748,1.2815667213,-1.7087556283,0.6407833606
+1.0500000000,1.0013378802,0.0841137635,-0.3682275269,0.5841137635,-0.3006689401,0.3675585868,-0.1503344701,1.0063684401,0.5870482567,-1.1740965135,0.5870482567,-0.5031842201,0.6709122934,-0.2515921100,-2.5612806272,-1.4940803659,2.9881607317,-1.4940803659,1.2806403136,-1.7075204181,0.6403201568
+1.0600000000,1.0112738348,0.0899097370,-0.3798194739,0.5899097370,-0.3056369174,0.3741825565,-0.1528184587,0.9808406229,0.5721570300,-1.1443140601,0.5721570300,-0.4904203115,0.6538937486,-0.2452101557,-2.5594034703,-1.4929853576,2.9859707153,-1.4929853576,1.2797017351,-1.7062689802,0.6398508676
+1.0700000000,1.0209544544,0.0955567651,-0.3911135302,0.5955567651,-0.3104772272,0.3806363029,-0.1552386136,0.9553090146,0.5572635919,-1.1145271837,0.5572635919,-0.4776545073,0.6368726764,-0.2388272537,-2.5574977952,-1.4918737138,2.9837474277,-1.4918737138,1.2787488976,-1.7049985301,0.6393744488
+1.0800000000,1.0303797167,0.1010548347,-0.4021096695,0.6010548347,-0.3151898583,0.3869198111,-0.1575949292,0.9297740819,0.5423682144,-1.0847364289,0.5423682144,-0.4648870410,0.6198493879,-0.2324435205,-2.5555583643,-1.4907423792,2.9814847584,-1.4907423792,1.2777791822,-1.7037055762,0.6388895911
+1.0900000000,1.0395496042,0.1064039358,-0.4128078715,0.6064039358,-0.3197748021,0.3930330694,-0.1598874010,0.9042362922,0.5274711704,-1.0549423409,0.5274711704,-0.4521181461,0.6028241948,-0.2260590730,-2.5535791389,-1.4895878310,2.9791756620,-1.4895878310,1.2767895694,-1.7023860926,0.6383947847
+1.1000000000,1.0484641043,0.1116040608,-0.4232081217,0.6116040608,-0.3242320521,0.3989760695,-0.1621160261,0.8786961153,0.5125727339,-1.0251454679,0.5125727339,-0.4393480577,0.5857974102,-0.2196740288,-2.5515535262,-1.4884062236,2.9768124472,-1.4884062236,1.2757767631,-1.7010356841,0.6378883815
+1.1100000000,1.0571232093,0.1166552054,-0.4333104108,0.6166552054,-0.3285616046,0.4047488062,-0.1642808023,0.8531540263,0.4976731820,-0.9953463640,0.4976731820,-0.4265770132,0.5687693509,-0.2132885066,-2.5494746240,-1.4871935307,2.9743870614,-1.4871935307,1.2747373120,-1.6996497494,0.6373686560
+1.1200000000,1.0655269161,0.1215573677,-0.4431147354,0.6215573677,-0.3327634580,0.4103512774,-0.1663817290,0.8276105090,0.4827727969,-0.9655455938,0.4827727969,-0.4138052545,0.5517403393,-0.2069026273,-2.5473354715,-1.4859456917,2.9718913835,-1.4859456917,1.2736677358,-1.6982236477,0.6368338679
+1.1300000000,1.0736752262,0.1263105486,-0.4526210973,0.6263105486,-0.3368376131,0.4157834841,-0.1684188066,0.8020660604,0.4678718685,-0.9357437371,0.4678718685,-0.4010330302,0.5347107069,-0.2005165151,-2.5451293159,-1.4846587676,2.9693175352,-1.4846587676,1.2725646579,-1.6967528772,0.6362823290
+1.1400000000,1.0815681460,0.1309147519,-0.4618295037,0.6309147519,-0.3407840730,0.4210454307,-0.1703920365,0.7765211961,0.4529706977,-0.9059413954,0.4529706977,-0.3882605980,0.5176807974,-0.1941302990,-2.5428499083,-1.4833291132,2.9666582264,-1.4833291132,1.2714249542,-1.6952332722,0.6357124771
+1.1500000000,1.0892056864,0.1353699837,-0.4707399675,0.6353699837,-0.3446028432,0.4261371243,-0.1723014216,0.7509764575,0.4380696002,-0.8761392004,0.4380696002,-0.3754882287,0.5006509716,-0.1877441144,-2.5404918423,-1.4819535747,2.9639071494,-1.4819535747,1.2702459212,-1.6936612282,0.6351229606
+1.1600000000,1.0965878630,0.1396762534,-0.4793525069,0.6396762534,-0.3482939315,0.4310585754,-0.1741469658,0.7254324190,0.4231689111,-0.8463378222,0.4231689111,-0.3627162095,0.4836216127,-0.1813581048,-2.5380509508,-1.4805297213,2.9610594425,-1.4805297213,1.2690254754,-1.6920339672,0.6345127377
+1.1700000000,1.1037146963,0.1438335729,-0.4876671457,0.6438335729,-0.3518573482,0.4358097975,-0.1759286741,0.6998896978,0.4082689904,-0.8165379808,0.4082689904,-0.3499448489,0.4665931319,-0.1749724245,-2.5355247826,-1.4790561232,2.9581122464,-1.4790561232,1.2677623913,-1.6903498551,0.6338811957
+1.1800000000,1.1105862113,0.1478419566,-0.4956839132,0.6478419566,-0.3552931056,0.4403908075,-0.1776465528,0.6743542226,0.3933732965,-0.7867465930,0.3933732965,-0.3371771113,0.4495694817,-0.1685885556,-2.5709641289,-1.4997290752,2.9994581504,-1.4997290752,1.2854820644,-1.7139760859,0.6427410322
+1.1900000000,1.1172024324,0.1517014189,-0.5034028378,0.6517014189,-0.3586012162,0.4448016216,-0.1793006081,0.6488344346,0.3784867535,-0.7569735070,0.3784867535,-0.3244172173,0.4325562897,-0.1622086087,-2.5693566763,-1.4987913945,2.9975827890,-1.4987913945,1.2846783381,-1.7129044508,0.6423391691
+1.2000000000,1.1235633946,0.1554119802,-0.5108239603,0.6554119802,-0.3617816973,0.4490422630,-0.1808908486,0.6233173954,0.3636018140,-0.7272036280,0.3636018140,-0.3116586977,0.4155449303,-0.1558293488,-2.5678204583,-1.4978952674,2.9957905347,-1.4978952674,1.2839102292,-1.7118803055,0.6419551146
+1.2100000000,1.1296691411,0.1589736657,-0.5179473313,0.6589736657,-0.3648345706,0.4531127608,-0.1824172853,0.5978037924,0.3487188789,-0.6974377578,0.3487188789,-0.2989018962,0.3985358616,-0.1494509481,-2.5663727128,-1.4970507491,2.9941014982,-1.4970507491,1.2831863564,-1.7109151418,0.6415931782
+1.2200000000,1.1355197216,0.1623865043,-0.5247730085,0.6623865043,-0.3677598608,0.4570131477,-0.1838799304,0.5722943469,0.3338383690,-0.6676767380,0.3338383690,-0.2861471734,0.3815295646,-0.1430735867,-2.5650370136,-1.4962715913,2.9925431826,-1.4962715913,1.2825185068,-1.7100246758,0.6412592534
+1.2300000000,1.1411151920,0.1656505287,-0.5313010574,0.6656505287,-0.3705575960,0.4607434614,-0.1852787980,0.5467898141,0.3189607249,-0.6379214498,0.3189607249,-0.2733949071,0.3645265427,-0.1366974535,-2.5638446591,-1.4955760511,2.9911521022,-1.4955760511,1.2819223295,-1.7092297727,0.6409611648
+1.2400000000,1.1464556158,0.1687657759,-0.5375315518,0.6687657759,-0.3732278079,0.4643037439,-0.1866139040,0.5212909808,0.3040864055,-0.6081728110,0.3040864055,-0.2606454904,0.3475273206,-0.1303227452,-2.5628363543,-1.4949878733,2.9899757467,-1.4949878733,1.2814181771,-1.7085575695,0.6407090886
+1.2500000000,1.1515410639,0.1717322873,-0.5434645746,0.6717322873,-0.3757705320,0.4676940426,-0.1878852660,0.4957986575,0.2892158835,-0.5784317670,0.2892158835,-0.2478993287,0.3305324383,-0.1239496644,-2.5620642437,-1.4945374755,2.9890749510,-1.4945374755,1.2810321218,-1.7080428291,0.6405160609
+1.2600000000,1.1563716160,0.1745501093,-0.5491002187,0.6745501093,-0.3781858080,0.4709144107,-0.1890929040,0.4703136626,0.2743496365,-0.5486992730,0.2743496365,-0.2351568313,0.3135424417,-0.1175784157,-2.5615943430,-1.4942633668,2.9885267336,-1.4942633668,1.2807971715,-1.7077295620,0.6403985858
+1.2700000000,1.1609473610,0.1772192939,-0.5544385879,0.6772192939,-0.3804736805,0.4739649073,-0.1902368403,0.4448367972,0.2594881317,-0.5189762634,0.2594881317,-0.2224183986,0.2965578648,-0.1112091993,-2.5615094112,-1.4942138232,2.9884276464,-1.4942138232,1.2807547056,-1.7076729408,0.6403773528
+1.2800000000,1.1652683983,0.1797398990,-0.5594797980,0.6797398990,-0.3826341992,0.4768455989,-0.1913170996,0.4193688029,0.2446318017,-0.4892636034,0.2446318017,-0.2096844014,0.2795792019,-0.1048422007,-2.5619122633,-1.4944488203,2.9888976406,-1.4944488203,1.2809561317,-1.7079415089,0.6404780658
+1.2900000000,1.1693348387,0.1821119892,-0.5642239785,0.6821119892,-0.3846674194,0.4795565591,-0.1923337097,0.3939102985,0.2297810075,-0.4595620149,0.2297810075,-0.1969551492,0.2626068657,-0.0984775746,-2.5629294456,-1.4950421766,2.9900843532,-1.4950421766,1.2814647228,-1.7086196304,0.6407323614
+1.3000000000,1.1731468056,0.1843356366,-0.5686712733,0.6843356366,-0.3865734028,0.4820978704,-0.1932867014,0.3684616868,0.2149359840,-0.4298719679,0.2149359840,-0.1842308434,0.2456411245,-0.0921154217,-2.5647150151,-1.4960837588,2.9921675177,-1.4960837588,1.2823575076,-1.7098100101,0.6411787538
+1.3100000000,1.1767044364,0.1864109213,-0.5728218425,0.6864109213,-0.3883522182,0.4844696243,-0.1941761091,0.3430230200,0.2000967617,-0.4001935233,0.2000967617,-0.1715115100,0.2286820133,-0.0857557550,-2.5674538230,-1.4976813968,2.9953627935,-1.4976813968,1.2837269115,-1.7116358820,0.6418634558
+1.3200000000,1.1800078833,0.1883379319,-0.5766758638,0.6883379319,-0.3900039416,0.4866719222,-0.1950019708,0.3175938120,0.1852630570,-0.3705261140,0.1852630570,-0.1587969060,0.2117292080,-0.0793984530,-2.5713630021,-1.4999617513,2.9999235025,-1.4999617513,1.2856815011,-1.7142420014,0.6428407505
+1.3300000000,1.1830573374,0.1901167802,-0.5802335603,0.6901167802,-0.3915286687,0.4887048916,-0.1957643344,0.2922301694,0.1704675988,-0.3409351976,0.1704675988,-0.1461150847,0.1948201129,-0.0730575423,-2.5074936059,-1.4627046034,2.9254092069,-1.4627046034,1.2537468030,-1.6716624039,0.6268734015
+1.3400000000,1.1858530327,0.1917476024,-0.5834952048,0.6917476024,-0.3929265163,0.4905686885,-0.1964632582,0.2668903566,0.1556860413,-0.3113720827,0.1556860413,-0.1334451783,0.1779269044,-0.0667225891,-2.5125593872,-1.4656596425,2.9313192851,-1.4656596425,1.2562796936,-1.6750395915,0.6281398468
+1.3500000000,1.1883952177,0.1932305437,-0.5864610873,0.6932305437,-0.3941976088,0.4922634785,-0.1970988044,0.2415719662,0.1409169803,-0.2818339606,0.1409169803,-0.1207859831,0.1610479775,-0.0603929916,-2.5203825534,-1.4702231562,2.9404463123,-1.4702231562,1.2601912767,-1.6802550356,0.6300956384
+1.3600000000,1.1906841715,0.1945657667,-0.5891315334,0.6945657667,-0.3953420858,0.4937894477,-0.1976710429,0.2162706052,0.1261578530,-0.2523157061,0.1261578530,-0.1081353026,0.1441804035,-0.0540676513,-2.5316526468,-1.4767973773,2.9535947546,-1.4767973773,1.2658263234,-1.6877684312,0.6329131617
+1.3700000000,1.1927201997,0.1957534498,-0.5915068996,0.6957534498,-0.3963600998,0.4951467998,-0.1981800499,0.1909778832,0.1114037652,-0.2228075304,0.1114037652,-0.0954889416,0.1273185888,-0.0477444708,-2.5470729502,-1.4857925543,2.9715851086,-1.4857925543,1.2735364751,-1.6980486335,0.6367682376
+1.3800000000,1.1945036227,0.1967937799,-0.5935875598,0.6967937799,-0.3972518113,0.4963357484,-0.1986259057,0.1656799741,0.0966466516,-0.1932933031,0.0966466516,-0.0828399870,0.1104533161,-0.0414199935,-2.5671424118,-1.4974997402,2.9949994805,-1.4974997402,1.2835712059,-1.7114282746,0.6417856030
+1.3900000000,1.1960349526,0.1976870557,-0.5953741114,0.6976870557,-0.3980174763,0.4973566351,-0.1990087381,0.1405702472,0.0819993109,-0.1639986217,0.0819993109,-0.0702851236,0.0937134981,-0.0351425618,-2.4826683511,-1.4482232048,2.8964464096,-1.4482232048,1.2413341755,-1.6551122341,0.6206670878
+1.4000000000,1.1973150119,0.1984337569,-0.5968675139,0.6984337569,-0.3986575059,0.4982100079,-0.1993287530,0.1154845751,0.0673660022,-0.1347320043,0.0673660022,-0.0577422876,0.0769897168,-0.0288711438,-2.5245291779,-1.4726420205,2.9452840409,-1.4726420205,1.2622645890,-1.6830194520,0.6311322945
+1.4100000000,1.1983442694,0.1990341572,-0.5980683143,0.6990341572,-0.3991721347,0.4988961796,-0.1995860674,0.0903598362,0.0527099045,-0.1054198089,0.0527099045,-0.0451799181,0.0602398908,-0.0225899591,-2.4050999542,-1.4029749733,2.8059499465,-1.4029749733,1.2025499771,-1.6033999695,0.6012749885
+1.4200000000,1.1991241143,0.1994890667,-0.5989781333,0.6994890667,-0.3995620571,0.4994160762,-0.1997810286,0.0655913792,0.0382616379,-0.0765232757,0.0382616379,-0.0327956896,0.0437275861,-0.0163978448,-2.5086534154,-1.4633811590,2.9267623180,-1.4633811590,1.2543267077,-1.6724356103,0.6271633539
+1.4300000000,1.1996560710,0.1997993748,-0.5995987496,0.6997993748,-0.3998280355,0.4997707140,-0.1999140178,0.0410822945,0.0239646718,-0.0479293436,0.0239646718,-0.0205411473,0.0273881964,-0.0102705736,-2.3550691592,-1.3737903429,2.7475806858,-1.3737903429,1.1775345796,-1.5700461062,0.5887672898
+1.4400000000,1.1999416939,0.1999659881,-0.5999319762,0.6999659881,-0.3999708469,0.4999611292,-0.1999854235,0.0165914466,0.0096783439,-0.0193566877,0.0096783439,-0.0082957233,0.0110609644,-0.0041478617,-2.2426221572,-1.3081962584,2.6163925167,-1.3081962584,1.1213110786,-1.4950814381,0.5606555393
+1.4500000000,1.1999999970,0.1999999983,-0.5999999965,0.6999999983,-0.3999999985,0.4999999980,-0.1999999993,0.0000091079,0.0000053129,-0.0000106259,0.0000053129,-0.0000045540,0.0000060719,-0.0000022770,-0.0232361933,-0.0135544461,0.0271088922,-0.0135544461,0.0116180966,-0.0154907955,0.0058090483
+1.4519588665,1.2000000000,0.2000000000,-0.6000000000,0.7000000000,-0.4000000000,0.5000000000,-0.2000000000,0.0000000000,-0.0000000000,0.0000000000,-0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,0.0000000000,-0.0000000000,0.0000000000,0.0000000000,-0.0000000000,0.0000000000

src/devices/robot/humanoid_robot/humanoid_robot.cpp

@@ -51,6 +51,11 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
 template<int DOF>
 void HumanoidRobot<DOF>::init() {
     motor_manager_ = MotorsInfo::getInstance()->getMotorManager();
+    joint_space_planner_ = JointSpacePlannerCreator::create(TOPPRA_BSPLINE);
+    joint_space_planner_->setPathType(PathType::Quintic);
+
+    joint_space_planner_->setSymmetricLimits(std::vector<double>(7, 1.0),
+                               std::vector<double>(7, 1.0));
     rsm_.store(ROBOT_ESTOP);
 }
 

src/devices/robot/humanoid_robot/humanoid_robot.h

@@ -31,6 +31,8 @@
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
 
+#include "planner/joint_space_planner/include/joint_space_planner_creator.h"
+#include "planner/joint_space_planner/include/joint_space_planner.h"
 
 namespace cmvr::device{
 
@@ -190,7 +192,7 @@ namespace cmvr::device{
 
         std::shared_ptr<MotorManager>  motor_manager_{nullptr};
 
-
+        std::shared_ptr<JointSpacePlanner> joint_space_planner_{nullptr};
 
 
     };

src/planner/CMakeLists.txt

@@ -10,15 +10,72 @@ include_directories(${TINYXML2_INCLUDE_DIRS})
 add_library(planner STATIC
     joint_space_planner/src/joint_space_planner.cpp
     joint_space_planner/src/joint_space_planner_creator.cpp
+        joint_space_planner/src/toppra_bspline.cpp
 )
 
 target_include_directories(planner PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
+include_directories(
+        ${CMAKE_SOURCE_DIR}/third_party/toppra/0.6.2/include
+)
+
+
+link_directories(
+        ${CMAKE_SOURCE_DIR}/third_party/toppra/0.6.2/lib
+)
+
 target_link_libraries(planner PRIVATE
         Eigen3::Eigen
         OsqpEigen::OsqpEigen
         ${TINYXML2_LIBRARIES}
         fcl
+        toppra
 )
 
-add_library(cmvr_es::planner ALIAS planner)
+add_library(cmvr_es::planner ALIAS planner)
+
+
+
+# --------------------------------------------------------
+# Unit test
+# --------------------------------------------------------
+find_package(glog REQUIRED)
+find_package(PkgConfig REQUIRED)
+find_package(fcl REQUIRED)
+find_package(OpenCV REQUIRED)
+
+include_directories(
+        ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
+)
+
+
+link_directories(
+        ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
+)
+
+
+
+
+add_executable(joint_space_planner_test
+        ${CMAKE_CURRENT_SOURCE_DIR}/joint_space_planner/src/joint_space_planner_test.cpp
+)
+
+
+target_link_libraries(joint_space_planner_test
+        PRIVATE
+        gtest
+        gtest_main
+        pthread
+        glog::glog
+        proto-objects
+        ccd
+        fcl
+        ${OpenCV_LIBS}
+        Eigen3::Eigen
+        OsqpEigen::OsqpEigen
+        cmvr_es::utils
+        cmvr_es::data_center
+        cmvr_es::planner
+)
+
+

src/planner/joint_space_planner/include/joint_space_planner.h

@@ -4,14 +4,70 @@
 
 #pragma once
 
-#include "joint_space_planner/include/joint_space_planner_creator.h"
+#include <vector>
+#include "common/consts/constant.h"
+#include <toppra/geometric_path/piecewise_poly_path.hpp>
+#include <toppra/parametrizer/const_accel.hpp>
+#include <toppra/parametrizer/spline.hpp>
+
 namespace cmvr {
+    // 统一轨迹接口
+    struct ITrajectory {
+        virtual ~ITrajectory() = default;
+        virtual toppra::Bound timeInterval() const = 0;
+        virtual Eigen::VectorXd q(double t)   const = 0;
+        virtual Eigen::VectorXd qd(double t)  const = 0;
+        virtual Eigen::VectorXd qdd(double t) const = 0;
+    };
+    using TrajPtr = std::shared_ptr<ITrajectory>;
+
+    struct TrajSample {
+        double t{};
+        Eigen::VectorXd q;    // DoF x 1
+        Eigen::VectorXd qd;   // DoF x 1
+        Eigen::VectorXd qdd;  // DoF x 1
+    };
+
+    // 三种几何路径
+    enum class PathType { Linear, CubicHermite, Quintic };
     class JointSpacePlanner {
     public:
 
-        JointSpacePlanner();
+        explicit JointSpacePlanner(PathType p):path_type_(p){};
+        JointSpacePlanner()=default;
+        virtual ~JointSpacePlanner() = default;
 
-        virtual ~JointSpacePlanner();
+        virtual bool plan(const std::vector<double>& start_joints,
+                         const std::vector<double>& goal_joints,
+                         TrajPtr& traj) {
+            UNUSED_VARIABLE(start_joints, goal_joints,traj);
+        }
+
+        // 采样函数：从 ITrajectory 生成采样序列 dt(s)
+        virtual std::vector<TrajSample> sampleTrajectory(const TrajPtr& traj, double dt) {
+            UNUSED_VARIABLE(traj,dt);
+            return {};
+        }
+
+        virtual bool writeTrajectoryCsv(const std::string& filename,const std::vector<TrajSample>& samples) {
+            UNUSED_VARIABLE(filename,samples);
+            return false;
+        }
+
+        // 对称限：[-v_max, v_max]、[-a_max, a_max]
+        virtual void setSymmetricLimits(const std::vector<double> &v_max,
+                                const std::vector<double> &a_max);
+
+        // 可调网格密度（默认 150/300）
+        virtual void setGridSizes(int N, int N_high);
+
+        // 切换几何路径类型
+        virtual void setPathType(PathType p);
+    protected:
+        PathType path_type_{PathType::Quintic};
+        int N_grid_ = 150;
+        int N_grid_high_ = 300;
+        std::vector<double> v_max_, a_max_;
 
     };
 }

src/planner/joint_space_planner/include/joint_space_planner_creator.h

@@ -7,6 +7,7 @@
 #include <memory>
 #include <unordered_map>
 #include <joint_space_planner/include/joint_space_planner.h>
+
 namespace cmvr
 {
     enum JointSpacePlannerType

src/planner/joint_space_planner/include/toppra_bspline.h

@@ -0,0 +1,81 @@
+#pragma once
+#include <memory>
+#include <vector>
+#include <Eigen/Dense>
+
+#include "planner/joint_space_planner/include/joint_space_planner.h"
+#include <toppra/geometric_path/piecewise_poly_path.hpp>
+#include <toppra/parametrizer/const_accel.hpp>
+#include <toppra/parametrizer/spline.hpp>
+
+namespace cmvr {
+
+
+    // 适配器：ConstAccel
+    class ConstAccelTraj : public ITrajectory {
+    public:
+        explicit ConstAccelTraj(std::shared_ptr<toppra::parametrizer::ConstAccel> p);
+
+        toppra::Bound timeInterval() const override;
+
+        Eigen::VectorXd q(double t) const override;
+
+        Eigen::VectorXd qd(double t) const override;
+
+        Eigen::VectorXd qdd(double t) const override;
+
+    private:
+        std::shared_ptr<toppra::parametrizer::ConstAccel> impl_;
+    };
+
+    // 适配器：Spline
+    class SplineTraj : public ITrajectory {
+    public:
+        SplineTraj(const std::shared_ptr<toppra::PiecewisePolyPath> &path,
+                   const toppra::Vector &grid,
+                   const toppra::Vector &vsq);
+
+        toppra::Bound timeInterval() const override;
+
+        Eigen::VectorXd q(double t) const override;
+
+        Eigen::VectorXd qd(double t) const override;
+
+        Eigen::VectorXd qdd(double t) const override;
+
+    private:
+        toppra::parametrizer::Spline impl_;
+    };
+
+    // 具体规划器：一次/三次/五次可切换；ConstAccel 校验失败自动回退 Spline
+    class ToppraBSpline : public JointSpacePlanner {
+    public:
+        explicit ToppraBSpline(PathType type = PathType::Quintic);
+
+        // 主入口：尽量保证返回轨迹（失败回退到 Spline）
+        bool plan(const std::vector<double> &start_joints,
+                  const std::vector<double> &goal_joints,
+                  TrajPtr &traj_out) override;
+
+        std::vector<TrajSample> sampleTrajectory(const TrajPtr& traj, double dt) override;
+        bool writeTrajectoryCsv(const std::string& filename,const std::vector<TrajSample>& samples) override;
+
+    private:
+        // 几何路径构造
+        static std::shared_ptr<toppra::PiecewisePolyPath>
+        buildLinear(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
+
+        static std::shared_ptr<toppra::PiecewisePolyPath>
+        buildCubicHermiteRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
+
+        static std::shared_ptr<toppra::PiecewisePolyPath>
+        buildQuinticRestToRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
+
+        static std::shared_ptr<toppra::PiecewisePolyPath>
+        buildPath(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1, PathType t);
+
+        static void sanitizeVsq(toppra::Vector &vsq);
+
+        bool ensureLimitsSized(std::size_t DoF);
+    };
+} // namespace cmvr

src/planner/joint_space_planner/src/joint_space_planner.cpp

@@ -1,3 +1,18 @@
 //
 // Created by lgv on 11/7/25.
-//
+//
+#include "planner/joint_space_planner/include/joint_space_planner.h"
+using namespace cmvr;
+
+void JointSpacePlanner::setSymmetricLimits(const std::vector<double> &v_max,
+                                       const std::vector<double> &a_max) {
+    v_max_ = v_max;
+    a_max_ = a_max;
+}
+
+void JointSpacePlanner::setGridSizes(int N, int N_high) {
+    N_grid_ = N;
+    N_grid_high_ = N_high;
+}
+
+void JointSpacePlanner::setPathType(PathType p) { path_type_ = p; }

src/planner/joint_space_planner/src/joint_space_planner_creator.cpp

@@ -3,6 +3,7 @@
 //
 
 #include "joint_space_planner/include/joint_space_planner_creator.h"
+#include "planner/joint_space_planner/include/toppra_bspline.h"
 
 namespace cmvr
 {
@@ -31,7 +32,7 @@ namespace cmvr
         case JOINT_SPACE_PLANNER_UNKNOWN:
             return nullptr;
         case TOPPRA_BSPLINE:
-            // return std::make_shared<HybridAstarParkIn>();
+            return std::make_shared<ToppraBSpline>();
         default:
             return nullptr;
         }

src/planner/joint_space_planner/src/joint_space_planner_test.cpp

@@ -0,0 +1,31 @@
+//
+// Created by lgv on 11/11/25.
+//
+
+#include "gtest/gtest.h"
+#include "planner/joint_space_planner/include/toppra_bspline.h"
+#include "planner/joint_space_planner/include/joint_space_planner_creator.h"
+using namespace cmvr;
+TEST(JOINT_SPACE_PLANNER_TEST,TOPPRA_TEST) {
+
+    auto planner = JointSpacePlannerCreator::create(JointSpacePlannerType::TOPPRA_BSPLINE);
+    planner->setPathType(PathType::Quintic);
+
+    planner->setSymmetricLimits(std::vector<double>(7, 1.5),
+                               std::vector<double>(7, 3.0));
+    TrajPtr traj;
+    std::vector<double> q0{0.0,-0.5,0.8,0.0,0.2,-0.3,0.1};
+    std::vector<double> q1{1.2,0.2,-0.6,0.7,-0.4,0.5,-0.2};
+    if (!planner->plan(q0, q1, traj)) { std::cerr << "plan failed\n"; }
+
+    // 2) 采样 0.01 s
+    auto samples = planner->sampleTrajectory(traj, 0.01);
+
+    // 3) 写 CSV
+    if (!planner->writeTrajectoryCsv("/home/lgv/cmvr/cmvr-es/data/planner/traj.csv", samples)) {
+        std::cerr << "write csv failed\n";
+    }
+
+    std::cout << "CSV saved: traj.csv\n";
+
+}

src/planner/joint_space_planner/src/toppra_bspline.cpp

@@ -0,0 +1,234 @@
+#include <toppra/algorithm/toppra.hpp>
+#include <toppra/constraint/linear_joint_velocity.hpp>
+#include <toppra/constraint/linear_joint_acceleration.hpp>
+#include <toppra/solver/seidel.hpp>
+#include <toppra/toppra.hpp>
+#include "planner/joint_space_planner/include/toppra_bspline.h"
+
+#include <fstream>
+#include <iomanip>
+
+namespace cmvr {
+    // ===== ConstAccelTraj =====
+    ConstAccelTraj::ConstAccelTraj(std::shared_ptr<toppra::parametrizer::ConstAccel> p)
+        : impl_(std::move(p)) {
+    }
+
+    toppra::Bound ConstAccelTraj::timeInterval() const { return impl_->pathInterval(); }
+    Eigen::VectorXd ConstAccelTraj::q(double t) const { return impl_->eval_single(t, 0); }
+    Eigen::VectorXd ConstAccelTraj::qd(double t) const { return impl_->eval_single(t, 1); }
+    Eigen::VectorXd ConstAccelTraj::qdd(double t) const { return impl_->eval_single(t, 2); }
+
+    // ===== SplineTraj =====
+    SplineTraj::SplineTraj(const std::shared_ptr<toppra::PiecewisePolyPath> &path,
+                           const toppra::Vector &grid,
+                           const toppra::Vector &vsq)
+        : impl_(path, grid, vsq) {
+    }
+
+    toppra::Bound SplineTraj::timeInterval() const { return impl_.pathInterval(); }
+    Eigen::VectorXd SplineTraj::q(double t) const { return impl_.eval_single(t, 0); }
+    Eigen::VectorXd SplineTraj::qd(double t) const { return impl_.eval_single(t, 1); }
+    Eigen::VectorXd SplineTraj::qdd(double t) const { return impl_.eval_single(t, 2); }
+
+    // ===== ToppraBSpline =====
+    ToppraBSpline::ToppraBSpline(PathType type) :JointSpacePlanner(type) {
+    }
+
+
+
+    bool ToppraBSpline::ensureLimitsSized(std::size_t DoF) {
+        if (v_max_.size() != DoF) v_max_.assign(DoF, 1.5);
+        if (a_max_.size() != DoF) a_max_.assign(DoF, 3.0);
+        return true;
+    }
+
+    void ToppraBSpline::sanitizeVsq(toppra::Vector &v) {
+        for (int i = 0; i < v.size(); ++i) if (v[i] < 0 && v[i] > -1e-10) v[i] = 0;
+        if (v.size() > 0) {
+            v[0] = 0;
+            v[v.size() - 1] = 0;
+        }
+    }
+
+    std::shared_ptr<toppra::PiecewisePolyPath>
+    ToppraBSpline::buildLinear(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1) {
+        // q(s) = b + a*s, s∈[0,1]。系数矩阵行0=一次项，行1=常数项
+        toppra::Matrix seg(2, q0.size());
+        seg.row(0) = (q1 - q0).transpose();
+        seg.row(1) = q0.transpose();
+        return std::make_shared<toppra::PiecewisePolyPath>(
+            toppra::Matrices{seg}, std::vector<double>{0.0, 1.0});
+    }
+
+    std::shared_ptr<toppra::PiecewisePolyPath>
+    ToppraBSpline::buildCubicHermiteRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1) {
+        // 起止速度 0 的三次 Hermite
+        toppra::Vectors pos{q0, q1};
+        toppra::Vectors vel{Eigen::VectorXd::Zero(q0.size()), Eigen::VectorXd::Zero(q1.size())};
+        std::vector<toppra::value_type> s{0.0, 1.0};
+        auto herm = toppra::PiecewisePolyPath::CubicHermiteSpline(pos, vel, s);
+        return std::make_shared<toppra::PiecewisePolyPath>(herm);
+    }
+
+    std::shared_ptr<toppra::PiecewisePolyPath>
+    ToppraBSpline::buildQuinticRestToRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1) {
+        // q(0)=q0, q'(0)=q''(0)=0; q(1)=q1, q'(1)=q''(1)=0 的闭式解
+        Eigen::VectorXd dq = q1 - q0;
+        toppra::Matrix seg(6, q0.size()); // x^5..x^0
+        seg.row(0) = (6.0 * dq).transpose(); // a5
+        seg.row(1) = (-15.0 * dq).transpose(); // a4
+        seg.row(2) = (10.0 * dq).transpose(); // a3
+        seg.row(3).setZero(); // a2
+        seg.row(4).setZero(); // a1
+        seg.row(5) = q0.transpose(); // a0
+        return std::make_shared<toppra::PiecewisePolyPath>(
+            toppra::Matrices{seg}, std::vector<double>{0.0, 1.0});
+    }
+
+    std::shared_ptr<toppra::PiecewisePolyPath>
+    ToppraBSpline::buildPath(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1, PathType t) {
+        switch (t) {
+            case PathType::Linear: return buildLinear(q0, q1);
+            case PathType::CubicHermite: return buildCubicHermiteRest(q0, q1);
+            case PathType::Quintic: return buildQuinticRestToRest(q0, q1);
+            default: return buildQuinticRestToRest(q0, q1);
+        }
+    }
+
+    bool ToppraBSpline::plan(const std::vector<double> &start_joints,
+                             const std::vector<double> &goal_joints,
+                             TrajPtr &traj_out) {
+        if (start_joints.empty() || start_joints.size() != goal_joints.size())
+            return false;
+
+        const std::size_t DoF = start_joints.size();
+        if (!ensureLimitsSized(DoF)) return false;
+
+        // 构造几何路径
+        Eigen::VectorXd q0 = Eigen::Map<const Eigen::VectorXd>(start_joints.data(), DoF);
+        Eigen::VectorXd q1 = Eigen::Map<const Eigen::VectorXd>(goal_joints.data(), DoF);
+        auto path = buildPath(q0, q1, path_type_);
+
+        // 约束
+        Eigen::VectorXd vmax(DoF), amax(DoF);
+        for (std::size_t i = 0; i < DoF; ++i) {
+            vmax[i] = v_max_[i];
+            amax[i] = a_max_[i];
+        }
+        auto vel = std::make_shared<toppra::constraint::LinearJointVelocity>(-vmax, vmax);
+        auto acc = std::make_shared<toppra::constraint::LinearJointAcceleration>(-amax, amax);
+        vel->discretizationType(toppra::DiscretizationType::Collocation);
+        acc->discretizationType(toppra::DiscretizationType::Interpolation);
+        toppra::LinearConstraintPtrs constraints{vel, acc};
+
+        // TOPPRA
+        toppra::algorithm::TOPPRA algo{constraints, path};
+        algo.setN(N_grid_);
+        algo.solver(std::make_shared<toppra::solver::Seidel>());
+
+        auto solve_once = [&](int N)-> bool {
+            algo.setN(N);
+            auto rc = algo.computePathParametrization(0.0, 0.0);
+            return rc == toppra::ReturnCode::OK;
+        };
+
+        if (!solve_once(N_grid_)) {
+            if (!solve_once(N_grid_high_)) return false;
+        }
+
+        const auto data = algo.getParameterizationData();
+        toppra::Vector grid = data.gridpoints;
+        toppra::Vector vsq = data.parametrization;
+
+        // 首选 ConstAccel
+        auto ca = std::make_shared<toppra::parametrizer::ConstAccel>(path, grid, vsq);
+        if (ca->validate()) {
+            traj_out = std::make_shared<ConstAccelTraj>(std::move(ca));
+            return true;
+        }
+
+        // 回退到 Spline（去毛刺）
+        sanitizeVsq(vsq);
+        try {
+            traj_out = std::make_shared<SplineTraj>(path, grid, vsq);
+            (void) traj_out->timeInterval();
+            return true;
+        } catch (...) {
+            return false;
+        }
+    }
+
+    std::vector<TrajSample> ToppraBSpline::sampleTrajectory(const TrajPtr &traj, double dt) {
+        std::vector<TrajSample> out;
+        if (!traj) return out;
+
+        auto I = traj->timeInterval();
+        const double t0 = I[0];
+        const double t1 = I[1];
+        if (!(t1 > t0)) return out;
+
+        // 2) 生成时间序列（含端点，考虑浮点误差）
+        constexpr double EPS = 1e-12;
+        std::vector<double> ts;
+        ts.reserve(static_cast<size_t>(std::max(2.0, std::ceil((t1 - t0) / dt) + 1.0)));
+
+        // 起点
+        ts.push_back(t0);
+
+        // 中间等间隔点
+        for (double t = t0 + dt; t <= t1 - EPS; t += dt) {
+            // 防守式：避免因累积误差越过 t1
+            ts.push_back((t > t1) ? t1 : t);
+        }
+
+        // 终点（若未加入）
+        if (ts.empty() || std::fabs(ts.back() - t1) > EPS) {
+            ts.push_back(t1);
+        } else {
+            // 若最后一个点因误差“很接近 t1”，直接覆盖为 t1
+            ts.back() = t1;
+        }
+
+        // 3) 采样
+        out.reserve(ts.size());
+        for (double t: ts) {
+            TrajSample s;
+            s.t = t;
+            s.q = traj->q(t);
+            s.qd = traj->qd(t);
+            s.qdd = traj->qdd(t);
+            out.emplace_back(std::move(s));
+        }
+        return out;
+    }
+
+
+    bool ToppraBSpline::writeTrajectoryCsv(const std::string &filename, const std::vector<TrajSample> &samples) {
+        if (samples.empty()) return false;
+
+        const int dof = static_cast<int>(samples.front().q.size());
+        std::ofstream ofs(filename);
+        if (!ofs) return false;
+
+        ofs.setf(std::ios::fixed);
+        ofs << std::setprecision(10);
+
+        // 表头
+        ofs << "t";
+        for (int j=1;j<=dof;++j) ofs << ",q"   << j;
+        for (int j=1;j<=dof;++j) ofs << ",qd"  << j;
+        for (int j=1;j<=dof;++j) ofs << ",qdd" << j;
+        ofs << "\n";
+
+        // 内容
+        for (const auto& s : samples) {
+            ofs << s.t;
+            for (int j=0;j<dof;++j) ofs << "," << s.q[j];
+            for (int j=0;j<dof;++j) ofs << "," << s.qd[j];
+            for (int j=0;j<dof;++j) ofs << "," << s.qdd[j];
+            ofs << "\n";
+        }
+        return true;
+    }
+} // namespace cmvr