feat: finsh CPS moveJ by joint planner

2025-11-12 09:58:22 +08:00 · 2025-11-12 09:58:22 +08:00 · 119e29e22d
commit 119e29e22d
parent d22fe1053e
10 changed files with 294 additions and 281 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -76,7 +76,7 @@ include_directories(
 ############################################################

 add_subdirectory(src)
-#add_subdirectory(example)
+add_subdirectory(example)
 #add_subdirectory(test)

 ############################################################
@ -102,4 +102,5 @@ target_link_libraries(cmvr_es PRIVATE
        cmvr_es::data_center
        cmvr_es::ik_solver
        cmvr_es::planner
+        cmvr_es::device::humanoid_robot
 )
--- a/data/planner/traj.csv
+++ b/data/planner/traj.csv
@ -1,148 +1,134 @@
-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
+t,q1,qd1,qdd1
+0.0000000000,0.0000000000,0.0000000000,0.0000000000
+0.0100000000,0.0000406839,0.0150597278,2.9544225473
+0.0200000000,0.0003318988,0.0434389689,2.8328411544
+0.0300000000,0.0009089804,0.0721925912,2.9085974602
+0.0400000000,0.0017753841,0.1011555878,2.9788370387
+0.0500000000,0.0029332621,0.1304223336,2.8974621431
+0.0600000000,0.0043835776,0.1596780532,2.9849342393
+0.0700000000,0.0061276028,0.1891399125,2.9436385788
+0.0800000000,0.0081659118,0.2185494408,2.9100229391
+0.0900000000,0.0104989150,0.2480391312,2.9903919563
+0.1000000000,0.0131273068,0.2776386486,2.9710319107
+0.1100000000,0.0160514180,0.3072295680,2.9553609553
+0.1200000000,0.0192715028,0.3368228958,2.9429784390
+0.1300000000,0.0227878085,0.3664259693,2.9333976058
+0.1400000000,0.0266005752,0.3960787191,2.9965532367
+0.1500000000,0.0307100762,0.4257870931,2.9893875915
+0.1600000000,0.0351164853,0.4555002329,2.9835420444
+0.1700000000,0.0398199454,0.4852198851,2.9788128547
+0.1800000000,0.0448205874,0.5149470456,2.9750155809
+0.1900000000,0.0501185300,0.5446821560,2.9719877241
+0.2000000000,0.0557138798,0.5744252584,2.9695884669
+0.2100000000,0.0616067323,0.6041761124,2.9676970254
+0.2200000000,0.0677971720,0.6339342832,2.9662104321
+0.2300000000,0.0742852736,0.6636992056,2.9650411835
+0.2400000000,0.0810711022,0.6934702307,2.9641149689
+0.2500000000,0.0881547141,0.7232466591,2.9633685823
+0.2600000000,0.0955361574,0.7530277630,2.9627480494
+0.2700000000,0.1032154722,0.7828128015,2.9622069731
+0.2800000000,0.1111926914,0.8126010294,2.9617050786
+0.2900000000,0.1194678407,0.8423917022,2.9612069361
+0.3000000000,0.1280409386,0.8721840779,2.9606808319
+0.3100000000,0.1369119973,0.9019774156,2.9600977635
+0.3200000000,0.1460810222,0.9317709736,2.9594305320
+0.3300000000,0.1555480123,0.9615640043,2.9586529089
+0.3400000000,0.1653129605,0.9913614506,2.9994436646
+0.3500000000,0.1753758533,1.0211579226,2.9987759136
+0.3600000000,0.1857366703,1.0509520895,2.9980466764
+0.3700000000,0.1963953845,1.0807433647,2.9972296032
+0.3800000000,0.2073519622,1.1105311329,2.9962959464
+0.3900000000,0.2186063626,1.1403147437,2.9952138015
+0.4000000000,0.2301585380,1.1700935028,2.9939472846
+0.4100000000,0.2420084329,1.1998666617,2.9924556275
+0.4200000000,0.2541559844,1.2296334033,2.9906921714
+0.4300000000,0.2666011211,1.2593928252,2.9886032370
+0.4400000000,0.2793437633,1.2891439183,2.9861268471
+0.4500000000,0.2923838217,1.3188855399,2.9831912740
+0.4600000000,0.3057211976,1.3486163807,2.9797133785
+0.4700000000,0.3193557814,1.3783349239,2.9755967043
+0.4800000000,0.3332874520,1.4080393945,2.9707292859
+0.4900000000,0.3475160759,1.4377276969,2.9649811176
+0.5000000000,0.3620415055,1.4673973379,2.9582012286
+0.5100000000,0.3768520933,1.4935024462,2.3994624602
+0.5200000000,0.3918443109,1.5001292329,-0.0473926375
+0.5300000000,0.4068455345,1.5000904478,-0.0558181019
+0.5400000000,0.4218466876,1.5000544796,-0.0617672439
+0.5500000000,0.4368477831,1.5000228970,-0.0657945138
+0.5600000000,0.4518488328,1.5000034924,0.0646808468
+0.5700000000,0.4668498542,1.5000225620,0.0580673577
+0.5800000000,0.4818508530,1.5000357501,0.0528981018
+0.5900000000,0.4968518295,1.5000446841,0.0489129125
+0.6000000000,0.5118527850,1.5000505493,0.0458984781
+0.6100000000,0.5268537219,1.5000542156,0.0436792658
+0.6200000000,0.5418546430,1.5000563308,0.0421099146
+0.6300000000,0.5568555514,1.5000573907,0.0410687802
+0.6400000000,0.5718564501,1.5000577910,0.0404523670
+0.6500000000,0.5868573425,1.5000578650,0.0401704239
+0.6600000000,0.6018582318,1.5000579132,0.0401415090
+0.6700000000,0.6168591213,1.5000582237,0.0402888495
+0.6800000000,0.6318600142,1.5000590879,0.0405363350
+0.6900000000,0.6468609138,1.5000608096,0.0408044882
+0.7000000000,0.6618618236,1.5000637088,0.0410062532
+0.7100000000,0.6768627471,1.5000681192,0.0410424371
+0.7200000000,0.6918636881,1.5000743763,0.0407966178
+0.7300000000,0.7068646509,1.5000827939,0.0401293113
+0.7400000000,0.7218656402,1.5000936245,0.0388711525
+0.7500000000,0.7368666615,1.5001069976,0.0368147988
+0.7600000000,0.7518677212,1.5001228260,0.0337052009
+0.7700000000,0.7668688267,1.5001406711,0.0292278068
+0.7800000000,0.7818699858,1.5001595500,0.0229941581
+0.7900000000,0.7968712072,1.5001776641,0.0145242063
+0.8000000000,0.8118724982,1.5001920227,0.0032245014
+0.8100000000,0.8268408076,1.4875275785,-2.4236229211
+0.8200000000,0.8415850092,1.4600455429,-2.9763843157
+0.8300000000,0.8560369100,1.4303603856,-2.9821334464
+0.8400000000,0.8701919621,1.4006590026,-2.9869477296
+0.8500000000,0.8840500235,1.3709435235,-2.9909698326
+0.8600000000,0.8976109653,1.3412157309,-2.9943223520
+0.8700000000,0.9108746707,1.3114771308,-2.9971106438
+0.8800000000,0.9238410335,1.2817290081,-2.9994252697
+0.8900000000,0.9365099596,1.2519856123,-2.9516661304
+0.9000000000,0.9488813661,1.2222373806,-2.9548394352
+0.9100000000,0.9609551735,1.1924800852,-2.9575790728
+0.9200000000,0.9727313111,1.1627149317,-2.9599417699
+0.9300000000,0.9842097169,1.1329430086,-2.9619771266
+0.9400000000,0.9953903367,1.1031653091,-2.9637287294
+0.9500000000,1.0062731238,1.0733827485,-2.9652351488
+0.9600000000,1.0168580392,1.0435961801,-2.9665308469
+0.9700000000,1.0271450505,1.0138064072,-2.9676470105
+0.9800000000,1.0371341323,0.9840141940,-2.9686123313
+0.9900000000,1.0468252659,0.9542202743,-2.9694537486
+1.0000000000,1.0562184391,0.9244253592,-2.9701971713
+1.0100000000,1.0653136460,0.8946301430,-2.9708681970
+1.0200000000,1.0741108876,0.8648353085,-2.9714928442
+1.0300000000,1.0826101708,0.8350415301,-2.9720983147
+1.0400000000,1.0908115096,0.8052494763,-2.9727138063
+1.0500000000,1.0987149244,0.7754598102,-2.9733713929
+1.0600000000,1.1063204427,0.7456731880,-2.9741069935
+1.0700000000,1.1136280987,0.7158902540,-2.9749614500
+1.0800000000,1.1206379346,0.6861116337,-2.9759817342
+1.0900000000,1.1273500000,0.6563379202,-2.9772223010
+1.1000000000,1.1337643527,0.6265696561,-2.9787465964
+1.1100000000,1.1398810593,0.5968073062,-2.9806287192
+1.1200000000,1.1457001954,0.5670512198,-2.9829552013
+1.1300000000,1.1512218466,0.5373015791,-2.9858268288
+1.1400000000,1.1564461083,0.5075583300,-2.9893603321
+1.1500000000,1.1613730868,0.4778210896,-2.9936896219
+1.1600000000,1.1660028991,0.4480890268,-2.9989659697
+1.1700000000,1.1703356966,0.4184158210,-2.9408728463
+1.1800000000,1.1743716794,0.3887671793,-2.9472841555
+1.1900000000,1.1781110347,0.3591305176,-2.9556079643
+1.2000000000,1.1815539662,0.3295023791,-2.9662283169
+1.2100000000,1.1847006925,0.2998771665,-2.9795424021
+1.2200000000,1.1875514414,0.2702465011,-2.9958998774
+1.2300000000,1.1901065598,0.2407521222,-2.9205021381
+1.2400000000,1.1923666133,0.2112921989,-2.9478052207
+1.2500000000,1.1943319553,0.1817988988,-2.9816509494
+1.2600000000,1.1960030174,0.1524407057,-2.8915887933
+1.2700000000,1.1973809412,0.1231678311,-2.9572485748
+1.2800000000,1.1984662906,0.0939921512,-2.8580629729
+1.2900000000,1.1992609116,0.0648770893,-2.9932552277
+1.3000000000,1.1997676425,0.0364479401,-2.9790115923
+1.3100000000,1.1999870932,0.0078713741,-2.8408663580
+1.3175726744,1.2000000000,-0.0000000000,-0.0000000000
--- a/include/devices/abstract_robot.h
+++ b/include/devices/abstract_robot.h
@ -68,7 +68,7 @@ namespace cmvr::device{

        virtual void eStop() { throw std::runtime_error("Not implemented"); }

-        virtual void moveJ(std::vector<double> &joints, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
+        virtual void moveJ(std::vector<double> &joints, double max_vel=0.5, double max_acc=0.1) { throw std::runtime_error("Not implemented"); }

        virtual void moveJ(std::vector<JointPoint> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
        virtual void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
--- a/src/devices/robot/humanoid_robot/CMakeLists.txt
+++ b/src/devices/robot/humanoid_robot/CMakeLists.txt
@ -2,7 +2,13 @@ find_package(protobuf REQUIRED)

 add_library(humanoid_robot SHARED humanoid_robot.cpp)

+
 target_include_directories(humanoid_robot PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_include_directories(humanoid_robot PUBLIC
+        ${CMAKE_SOURCE_DIR}/third_party/toppra/0.6.2/include)
+find_library(TOPPRA_LIB NAMES toppra
+        PATHS ${CMAKE_SOURCE_DIR}/third_party/toppra/0.6.2/lib
+        NO_DEFAULT_PATH)

 add_library(cmvr_es::device::humanoid_robot ALIAS humanoid_robot)

@ -11,9 +17,13 @@ target_link_libraries(humanoid_robot PRIVATE
        cmvr_es::data_center
        protobuf::libprotobuf
        cmvr_es::ik_solver
+        cmvr_es::planner
+        ${TOPPRA_LIB}
 )


+
+
 # --------------------------------------------------------
 # Unit test
 # --------------------------------------------------------
@ -30,14 +40,20 @@ link_directories(
        ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
 )

-
 add_executable(humanoid_robot_test
        ${CMAKE_CURRENT_SOURCE_DIR}/humanoid_robot_test.cpp
 )

+target_include_directories(humanoid_robot_test PUBLIC
+        ${CMAKE_SOURCE_DIR}/third_party/toppra/0.6.2/include)
+
+find_library(TOPPRA_LIB NAMES toppra
+        PATHS ${CMAKE_SOURCE_DIR}/third_party/toppra/0.6.2/lib
+        NO_DEFAULT_PATH)

 target_link_libraries(humanoid_robot_test
        PRIVATE
+        cmvr_es::device_manager
        cmvr_es::data_center
        cmvr_es::device::humanoid_robot
        gtest
@ -45,9 +61,7 @@ target_link_libraries(humanoid_robot_test
        pthread
        glog::glog
        proto-objects
+        ${OpenCV_LIBS}
        ccd
        fcl
-        cmvr_es::device_manager
-        ${OpenCV_LIBS}
-        cmvr_es::ik_solver
 )
--- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp
+++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp
@ -37,7 +37,6 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
        CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(cfg.getAttrDefault("bufferSize", 50));


-
        upd_timer_ = make_shared<FDTimer>();
        upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
                          [this] { update_state_(); });
@ -55,7 +54,7 @@ void HumanoidRobot<DOF>::init() {
    joint_space_planner_->setPathType(PathType::Quintic);

    joint_space_planner_->setSymmetricLimits(std::vector<double>(7, 1.0),
-                               std::vector<double>(7, 1.0));
+                                             std::vector<double>(7, 1.0));
    rsm_.store(ROBOT_ESTOP);
 }

@ -174,44 +173,135 @@ void HumanoidRobot<DOF>::eStop() {
    }
 }

+// template<int DOF>
+// void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
+//     try {
+//         for (const auto &j: cmd) {
+//             auto motor = motor_manager_->getMotor(j.joint_name);
+//             if (motor != nullptr) {
+//                 // PPM 模式下 这个实际速度会超30% 左右
+//                 motor->setQd(vel);
+//
+//                 if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
+//                     motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
+//                 }
+//                 motor->setQ(j.rad);
+//             }
+//         }
+//
+//         //3. wait for completion
+//         bool completion = true;
+//         do {
+//             completion = true;
+//             for (const auto &j: cmd) {
+//                 auto motor = motor_manager_->getMotor(j.joint_name);
+//                 if (motor != nullptr) {
+//                     if (!motor->reachedTargetQ()) {
+//                         completion = false;
+//                         break;
+//                     }
+//                 }
+//             }
+//             // 4. while waiting, check flash_cmd_, if it is true, set it false then exit
+//             if (flash_cmd_.load()) {
+//                 flash_cmd_.store(false);
+//                 return;
+//             }
+//             std::this_thread::sleep_for(std::chrono::milliseconds(2));
+//         } while (!completion);
+//     } catch (exception &e) {
+//         throw runtime_error(e.what());
+//     }
+// }
+
+
 template<int DOF>
 void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
-    try {
-        for (const auto &j: cmd) {
-            auto motor = motor_manager_->getMotor(j.joint_name);
-            if (motor != nullptr) {
-                // PPM 模式下 这个实际速度会超30% 左右
-                motor->setQd(vel);
+    if (cmd.empty()) return;

-                if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
-                    motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
-                }
-                motor->setQ(j.rad);
-            }
+    std::unordered_map<std::string, double> cur_joints_angle;
+    cur_joints_angle.reserve(cmd.size());
+
+    std::vector<std::shared_ptr<AbstractMotor> > motors;
+    motors.reserve(cmd.size());
+
+    for (auto &jp: cmd) {
+        auto motor = motor_manager_->getMotor(jp.joint_name);
+        motors.push_back(motor);
+        // 切 CSP
+        if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
+            motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
+        }
+        // 为 map 预填 key
+        cur_joints_angle.emplace(jp.joint_name, 0.0);
+    }
+
+    getJointQ(cur_joints_angle);
+
+    std::vector<double> q_start, q_goal;
+    q_start.reserve(cmd.size());
+    q_goal.reserve(cmd.size());
+    for (auto &jp: cmd) {
+        auto it = cur_joints_angle.find(jp.joint_name);
+        if (it == cur_joints_angle.end()) {
+            LOG(ERROR) << "missing current angle for " << jp.joint_name;
+            return;
+        }
+        q_start.push_back(it->second); // 当前读到的弧度
+        q_goal.push_back(jp.rad); // 目标弧度
+    }
+    if (q_start.size() != q_goal.size()) return;
+
+
+    TrajPtr traj;
+    joint_space_planner_->setPathType(PathType::Quintic);
+    joint_space_planner_->setSymmetricLimits(std::vector<double>(cmd.size(), vel),
+                                             std::vector<double>(cmd.size(), acc));
+
+
+    bool ok = joint_space_planner_->plan(q_start, q_goal, traj);
+    if (!ok) {
+        LOG(ERROR) << "planner plan() failed";
+        return;
+    }
+
+
+    // 10ms
+    const double dt = 0.01;
+    auto samples = joint_space_planner_->sampleTrajectory(traj, dt);
+    if (samples.size() < 2) {
+        LOG(ERROR) << "trajectory too short";
+        return;
+    }
+
+    const auto t0 = std::chrono::steady_clock::now();
+
+    //  k=1 开始
+    size_t k = 0;
+    const size_t K = samples.size();
+
+    while (k < K) {
+        // ——根据墙钟计算“应当在的拍次”，用于追赶——
+        auto now = std::chrono::steady_clock::now();
+        double elapsed = std::chrono::duration<double>(now - t0).count();
+        size_t k_should = static_cast<size_t>(std::floor(elapsed / dt)); // 向下取整更保守
+        if (k_should >= K) k_should = K - 1;
+        if (k < k_should) k = k_should;  // 追赶，避免越拖越晚
+
+        // ——下发第 k 个采样点——
+        const auto &s = samples[k];
+        // 这里最好在进入循环前做过维度检查：s.q/qd 与 motors.size() 相等
+        for (size_t i = 0; i < motors.size(); ++i) {
+            motors[i]->setQd(s.qd[i]);
+            motors[i]->setQ(s.q[i]);
        }

-        //3. wait for completion
-        bool completion = true;
-        do {
-            completion = true;
-            for (const auto &j: cmd) {
-                auto motor = motor_manager_->getMotor(j.joint_name);
-                if (motor != nullptr) {
-                    if (!motor->reachedTargetQ()) {
-                        completion = false;
-                        break;
-                    }
-                }
-            }
-            // 4. while waiting, check flash_cmd_, if it is true, set it false then exit
-            if (flash_cmd_.load()) {
-                flash_cmd_.store(false);
-                return;
-            }
-            std::this_thread::sleep_for(std::chrono::milliseconds(2));
-        } while (!completion);
-    } catch (exception &e) {
-        throw runtime_error(e.what());
+        // ——睡到下一拍 (k+1)*dt——
+        ++k; // 先递增，下一拍时间点明确
+        if (k < K) {
+            auto next_t = t0 + std::chrono::duration<double>(k * dt);
+            std::this_thread::sleep_until(next_t);
+        }
    }
 }

--- a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp
+++ b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp
@ -65,85 +65,10 @@ TEST(HumanoidRobotTest,MyRobotTest) {
    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);

    auto robot = dmgr.getDevice<AbstractRobot>("hc01");
+    robot->calibrateZeroQ("R_WRIST_R");

-    std::vector<JointPoint> cmd{};
+    // std::vector<JointPoint> cmd{};

-    std::vector<std::vector<JointPoint>>  traj;
-
-//    robot->calibrateZeroQ("R_WRIST_P");
-    // robot->calibrateZeroQ("R_WRIST_Y");
-//    robot->calibrateZeroQ("R_WRIST_R");
-     //
-     cmd = {
-         {"L_SHOULDER_P", 0.0},
-         {"L_SHOULDER_R", 0.0},
-         {"L_SHOULDER_Y", 0.0},
-         {"L_ELBOW_R",    0.0},
-         {"L_WRIST_P",    0.0},
-         {"L_WRIST_Y",    0.0},
-         {"L_WRIST_R",    0.0},
-         //
-         // {"R_SHOULDER_P", 0.0}
-         // {"R_SHOULDER_R", 0.0},
-         // {"R_SHOULDER_Y", 0.0},
-         // {"R_ELBOW_R",    0.0},
-         // {"R_WRIST_P",    0.0},
-         // {"R_WRIST_Y",    0.0},
-         // {"R_WRIST_R",    0.0},
-
-         // {"WAIST_P" ,0.0},
-         // {"WAIST_Y" ,0.0},
-
-         {"HEAD_P",0.0},
-         {"HEAD_Y",0.0},
-         {"HEAD_R",0.0},
-
-     };
-     robot->moveJ(cmd,0.8);
-
-
-    // cmd = {
-    //     {"L_SHOULDER_P", 0.0},
-    //     {"L_SHOULDER_R", -1.31873},
-    //     {"L_SHOULDER_Y", 0.0},
-    //     {"L_ELBOW_R",    -0.537621},
-    //     {"L_WRIST_P",    0.0},
-    //     {"L_WRIST_Y",    0.000183204},
-    //     {"L_WRIST_R",    0.0225797},
-    //
-    //     {"R_SHOULDER_P", 0.00203898},
-    //     {"R_SHOULDER_R", 1.34062},
-    //     {"R_SHOULDER_Y", 0.0},
-    //     {"R_ELBOW_R",    0.522261},
-    //     {"R_WRIST_P",    0.0},
-    //     {"R_WRIST_Y",    -0.000210733},
-    //     {"R_WRIST_R",    -0.0942364}
-    // };
-
-    // robot->moveJ(cmd,0.8);
-
-    // std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-    // robot->eStop();
-
-    // robot->torqueOff();
-
-
-    while (true) {
-
-        // cmd = {{"L_SHOULDER_P",3.14},{"L_SHOULDER_R",3.14},{"L_SHOULDER_Y",3.14}};
-        // robot->moveJ(cmd,1.0);
-        // std::this_thread::sleep_for(std::chrono::milliseconds(8000));
-        // cmd = {{"L_SHOULDER_P",-3.14},{"L_SHOULDER_R",-3.14},{"L_SHOULDER_Y",-3.14}};
-        // robot->moveJ(cmd,1.0);
-        // std::this_thread::sleep_for(std::chrono::milliseconds(8000));
-
-        // cmd = {{"L_SHOULDER_R",3.14}};
-        // robot->moveJ(cmd,0.7);
-        // // std::this_thread::sleep_for(std::chrono::milliseconds(10000));
-        // cmd = {{"L_SHOULDER_R",-3.14}};
-        // robot->moveJ(cmd,0.7);
-        std::this_thread::sleep_for(std::chrono::milliseconds(10000));
-    }
 }


--- a/src/planner/CMakeLists.txt
+++ b/src/planner/CMakeLists.txt
@ -14,22 +14,18 @@ add_library(planner STATIC
 )

 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_include_directories(planner PUBLIC
+        ${CMAKE_SOURCE_DIR}/third_party/toppra/0.6.2/include)
+find_library(TOPPRA_LIB NAMES toppra
+        PATHS ${CMAKE_SOURCE_DIR}/third_party/toppra/0.6.2/lib
+        NO_DEFAULT_PATH)

 target_link_libraries(planner PRIVATE
        Eigen3::Eigen
        OsqpEigen::OsqpEigen
        ${TINYXML2_LIBRARIES}
        fcl
-        toppra
+        ${TOPPRA_LIB}
 )

 add_library(cmvr_es::planner ALIAS planner)
--- a/src/planner/joint_space_planner/include/joint_space_planner.h
+++ b/src/planner/joint_space_planner/include/joint_space_planner.h
@ -41,6 +41,7 @@ namespace cmvr {
                         const std::vector<double>& goal_joints,
                         TrajPtr& traj) {
            UNUSED_VARIABLE(start_joints, goal_joints,traj);
+            return false;
        }

        // 采样函数：从 ITrajectory 生成采样序列 dt(s)
--- a/src/planner/joint_space_planner/include/joint_space_planner_creator.h
+++ b/src/planner/joint_space_planner/include/joint_space_planner_creator.h
@ -6,7 +6,7 @@

 #include <memory>
 #include <unordered_map>
-#include <joint_space_planner/include/joint_space_planner.h>
+#include <planner/joint_space_planner/include/joint_space_planner.h>

 namespace cmvr
 {
--- a/src/planner/joint_space_planner/src/joint_space_planner_test.cpp
+++ b/src/planner/joint_space_planner/src/joint_space_planner_test.cpp
@ -11,11 +11,11 @@ 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));
+    planner->setSymmetricLimits(std::vector<double>(1, 1.5),
+                               std::vector<double>(1, 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};
+    std::vector<double> q0{0.0};
+    std::vector<double> q1{1.2};
    if (!planner->plan(q0, q1, traj)) { std::cerr << "plan failed\n"; }

    // 2) 采样 0.01 s