feat: add best psi select
This commit is contained in:
parent
bad9b6cc10
commit
9aa74f39ad
@ -1,410 +1,104 @@
|
|||||||
psi,q1,q2,q3,q4,q5,q6,q7
|
psi,q1,q2,q3,q4,q5,q6,q7
|
||||||
0.578899673,0.143528558,0.493465333,1.325429196,1.570796327,-1.549549747,1.262717359,-0.570000000
|
-0.000000048,0.002038980,1.340620000,0.000000000,0.522261000,0.000000000,-0.000210733,-0.094236400
|
||||||
0.579899673,0.143167639,0.494149973,1.326455520,1.570796327,-1.548029451,1.262701768,-0.567664028
|
0.556521960,0.152127806,0.478192710,1.301969310,1.570796327,1.347703459,0.291064331,0.230347615
|
||||||
0.580899673,0.142808658,0.494834787,1.327480021,1.570796327,-1.546509357,1.262685100,-0.565328255
|
0.771261416,0.102931374,0.627402558,1.496396670,1.570796327,1.156280065,0.243329923,0.080698976
|
||||||
0.581899673,0.142451607,0.495519776,1.328502707,1.570796327,-1.544989477,1.262667355,-0.562992694
|
0.888086437,0.097502506,0.710527703,1.578136435,1.566414799,1.057630893,0.204332305,0.012141046
|
||||||
0.582899673,0.142096480,0.496204938,1.329523584,1.570796327,-1.543469825,1.262648534,-0.560657360
|
0.955163562,0.098163871,0.758400761,1.620123715,1.561968928,1.003011363,0.178048729,-0.021862524
|
||||||
0.583899673,0.141743270,0.496890271,1.330542661,1.570796327,-1.541950415,1.262628637,-0.558322264
|
0.993973315,0.099039484,0.786243811,1.642081749,1.557458620,0.972273899,0.161127367,-0.037608954
|
||||||
0.584899673,0.141391968,0.497575776,1.331559944,1.570796327,-1.540431260,1.262607664,-0.555987420
|
1.016138882,0.099188037,0.802349007,1.652836912,1.552883773,0.955182732,0.150393132,-0.043134232
|
||||||
0.585899673,0.141042569,0.498261450,1.332575441,1.570796327,-1.538912372,1.262585615,-0.553652842
|
1.028376055,0.098680005,0.811490304,1.657044755,1.548244277,0.946082745,0.143560899,-0.042763131
|
||||||
0.586899673,0.140695065,0.498947294,1.333589160,1.570796327,-1.537393767,1.262562491,-0.551318543
|
1.034654767,0.097742852,0.816476052,1.657338477,1.543540016,0.941724228,0.139129811,-0.038932566
|
||||||
0.587899673,0.140349449,0.499633305,1.334601106,1.570796327,-1.535875456,1.262538291,-0.548984536
|
1.037340138,0.096569737,0.818972515,1.655254344,1.538770861,0.940203623,0.136155821,-0.033050626
|
||||||
0.588899673,0.140005714,0.500319484,1.335611288,1.570796327,-1.534357453,1.262513017,-0.546650834
|
1.037844614,0.095293395,0.819971733,1.651701648,1.533936676,0.940395189,0.134058820,-0.025942410
|
||||||
0.589899673,0.139663853,0.501005828,1.336619713,1.570796327,-1.532839772,1.262486667,-0.544317450
|
1.037011999,0.093997863,0.820065093,1.647222208,1.529037317,0.941630081,0.132487097,-0.018095168
|
||||||
0.590899673,0.139323860,0.501692338,1.337626388,1.570796327,-1.531322426,1.262459244,-0.541984397
|
1.035346032,0.092734315,0.819605210,1.642139686,1.524072627,0.943510217,0.131229133,-0.009797785
|
||||||
0.591899673,0.138985727,0.502379011,1.338631319,1.570796327,-1.529805428,1.262430746,-0.539651689
|
1.033146936,0.091533355,0.818802211,1.636647318,1.519042442,0.945798773,0.130158390,-0.001221774
|
||||||
0.592899673,0.138649447,0.503065847,1.339634514,1.570796327,-1.528288792,1.262401174,-0.537319339
|
1.030593077,0.090413216,0.817781131,1.630860032,1.513946584,0.948355264,0.129199456,0.007531135
|
||||||
0.593899673,0.138315015,0.503752846,1.340635981,1.570796327,-1.526772530,1.262370529,-0.534987360
|
1.027789823,0.089384875,0.816616155,1.624845592,1.508784868,0.951096878,0.128307556,0.016400826
|
||||||
0.594899673,0.137982422,0.504440005,1.341635725,1.570796327,-1.525256656,1.262338811,-0.532655764
|
1.024798765,0.088455137,0.815351066,1.618643268,1.503557094,0.953975399,0.127456222,0.025352060
|
||||||
0.595899673,0.137651662,0.505127325,1.342633755,1.570796327,-1.523741183,1.262306020,-0.530324566
|
1.021655201,0.087628445,0.814011443,1.612275049,1.498263053,0.956963419,0.126629922,0.034364513
|
||||||
0.596899673,0.137322729,0.505814804,1.343630076,1.570796327,-1.522226124,1.262272156,-0.527993778
|
1.018378581,0.086907950,0.812611949,1.605752380,1.492902522,0.960046092,0.125819652,0.043426829
|
||||||
0.597899673,0.136995616,0.506502441,1.344624696,1.570796327,-1.520711493,1.262237221,-0.525663413
|
1.014978750,0.086296115,0.811160674,1.599080207,1.487475267,0.963216206,0.125020313,0.052533074
|
||||||
0.598899673,0.136670316,0.507190236,1.345617622,1.570796327,-1.519197303,1.262201213,-0.523333484
|
1.011459667,0.085795082,0.809661725,1.592259399,1.481981040,0.966471247,0.124229149,0.061680634
|
||||||
0.599899673,0.136346823,0.507878186,1.346608860,1.570796327,-1.517683566,1.262164135,-0.521004004
|
1.007821617,0.085406871,0.808116761,1.585288189,1.476419579,0.969811645,0.123444822,0.070868952
|
||||||
0.600899673,0.136025129,0.508566292,1.347598417,1.570796327,-1.516170297,1.262125986,-0.518674987
|
1.004062519,0.085133511,0.806525907,1.578163029,1.470790610,0.973239752,0.122666862,0.080098793
|
||||||
0.601899673,0.135705229,0.509254553,1.348586301,1.570796327,-1.514657507,1.262086766,-0.516346444
|
1.000178682,0.084977117,0.804888290,1.570879075,1.465093842,0.976759230,0.121895345,0.089371810
|
||||||
0.602899673,0.135387116,0.509942967,1.349572517,1.570796327,-1.513145211,1.262046476,-0.514018390
|
0.996165246,0.084939944,0.803202344,1.563430461,1.459328970,0.980374719,0.121130705,0.098690296
|
||||||
0.603899673,0.135070783,0.510631533,1.350557073,1.570796327,-1.511633421,1.262005117,-0.511690837
|
0.992016408,0.085024426,0.801465983,1.555810431,1.453495675,0.984091652,0.120373623,0.108057049
|
||||||
0.604899673,0.134756224,0.511320252,1.351539975,1.570796327,-1.510122151,1.261962689,-0.509363798
|
0.987725533,0.085233217,0.799676691,1.548011384,1.447593619,0.987916174,0.119624973,0.117475311
|
||||||
0.605899673,0.134443432,0.512009120,1.352521230,1.570796327,-1.508611412,1.261919192,-0.507037286
|
0.983285184,0.085569222,0.797831557,1.540024862,1.441622451,0.991855129,0.118885793,0.126948746
|
||||||
0.606899673,0.134132402,0.512698139,1.353500845,1.570796327,-1.507101220,1.261874627,-0.504711314
|
0.978687099,0.086035639,0.795927274,1.531841493,1.435581798,0.995916078,0.118157269,0.136481449
|
||||||
0.607899673,0.133823126,0.513387307,1.354478825,1.570796327,-1.505591585,1.261828995,-0.502385894
|
0.973922120,0.086636002,0.793960118,1.523450901,1.429471273,1.000107368,0.117440743,0.146077982
|
||||||
0.608899673,0.133515598,0.514076623,1.355455179,1.570796327,-1.504082522,1.261782295,-0.500061040
|
0.968980104,0.087374231,0.791925907,1.514841585,1.423290469,1.004438211,0.116737724,0.155743419
|
||||||
0.609899673,0.133209813,0.514766086,1.356429911,1.570796327,-1.502574043,1.261734528,-0.497736764
|
0.963849790,0.088254693,0.789819940,1.506000764,1.417038960,1.008918807,0.116049904,0.165483418
|
||||||
0.610899673,0.132905763,0.515455695,1.357403029,1.570796327,-1.501066161,1.261685696,-0.495413079
|
0.958518636,0.089282271,0.787636922,1.496914187,1.410716299,1.013560480,0.115379184,0.175304304
|
||||||
0.611899673,0.132603443,0.516145449,1.358374539,1.570796327,-1.499558890,1.261635798,-0.493089998
|
0.952972617,0.090462458,0.785370868,1.487565896,1.404322019,1.018375862,0.114727709,0.185213177
|
||||||
0.612899673,0.132302846,0.516835348,1.359344448,1.570796327,-1.498052241,1.261584835,-0.490767533
|
0.947195982,0.091801455,0.783014983,1.477937944,1.397855632,1.023379109,0.114097905,0.195218041
|
||||||
0.613899673,0.132003966,0.517525390,1.360312762,1.570796327,-1.496546228,1.261532807,-0.488445698
|
0.941170943,0.093306311,0.780561518,1.468010035,1.391316626,1.028586170,0.113492532,0.205327972
|
||||||
0.614899673,0.131706797,0.518215575,1.361279487,1.570796327,-1.495040863,1.261479716,-0.486124504
|
0.934877298,0.094985081,0.778001581,1.457759082,1.384704468,1.034015127,0.112914748,0.215553318
|
||||||
0.615899673,0.131411333,0.518905902,1.362244630,1.570796327,-1.493536160,1.261425561,-0.483803965
|
0.928291946,0.096847040,0.775324904,1.447158654,1.378018599,1.039686614,0.112368188,0.225905960
|
||||||
0.616899673,0.131117567,0.519596369,1.363208197,1.570796327,-1.492032132,1.261370344,-0.481484094
|
0.921388283,0.098902942,0.772519552,1.436178264,1.371258435,1.045624359,0.111857062,0.236399637
|
||||||
0.617899673,0.130825494,0.520286977,1.364170195,1.570796327,-1.490528789,1.261314064,-0.479164901
|
0.914135413,0.101165367,0.769571531,1.424782455,1.364423366,1.051855874,0.111386292,0.247050377
|
||||||
0.618899673,0.130535108,0.520977724,1.365130629,1.570796327,-1.489026147,1.261256723,-0.476846402
|
0.906497117,0.103649173,0.766464284,1.412929601,1.357512757,1.058413369,0.110961679,0.257877053
|
||||||
0.619899673,0.130246402,0.521668609,1.366089506,1.570796327,-1.487524217,1.261198322,-0.474528607
|
0.898430484,0.106372101,0.763178018,1.400570304,1.350525941,1.065334958,0.110590132,0.268902135
|
||||||
0.620899673,0.129959370,0.522359631,1.367046832,1.570796327,-1.486023011,1.261138860,-0.472211530
|
0.889884036,0.109355604,0.759688780,1.387645214,1.343462225,1.072666314,0.110279980,0.280152709
|
||||||
0.621899673,0.129674007,0.523050791,1.368002613,1.570796327,-1.484522544,1.261078338,-0.469895182
|
0.880795125,0.112626005,0.755967161,1.374081980,1.336320882,1.080462969,0.110041405,0.291661910
|
||||||
0.622899673,0.129390307,0.523742086,1.368956856,1.570796327,-1.483022826,1.261016758,-0.467579577
|
0.871086195,0.116216166,0.751976445,1.359790886,1.329101156,1.088793604,0.109887062,0.303470980
|
||||||
0.623899673,0.129108264,0.524433515,1.369909567,1.570796327,-1.481523871,1.260954119,-0.465264726
|
0.860659244,0.120167969,0.747669842,1.344658367,1.321802257,1.097744946,0.109833006,0.315632328
|
||||||
0.624899673,0.128827871,0.525125079,1.370860751,1.570796327,-1.480025691,1.260890423,-0.462950643
|
0.849387272,0.124536152,0.742986207,1.328537001,1.314423358,1.107429304,0.109900118,0.328214268
|
||||||
0.625899673,0.128549123,0.525816776,1.371810414,1.570796327,-1.478528298,1.260825670,-0.460637339
|
0.837100390,0.129394552,0.737843062,1.311229259,1.306963599,1.117996822,0.110116435,0.341308739
|
||||||
0.626899673,0.128272014,0.526508606,1.372758564,1.570796327,-1.477031706,1.260759861,-0.458324826
|
0.823561762,0.134846923,0.732124450,1.292459357,1.299422079,1.129656687,0.110521197,0.355044715
|
||||||
0.627899673,0.127996539,0.527200566,1.373705206,1.570796327,-1.475535926,1.260692996,-0.456013118
|
0.808422172,0.141047319,0.725657901,1.271820173,1.291797860,1.142717173,0.111172547,0.369613603
|
||||||
0.628899673,0.127722691,0.527892658,1.374650345,1.570796327,-1.474040972,1.260625077,-0.453702227
|
0.791123135,0.148243395,0.718165031,1.248660255,1.284089961,1.157670987,0.112164100,0.385323525
|
||||||
0.629899673,0.127450465,0.528584880,1.375593988,1.570796327,-1.472546854,1.260556103,-0.451392163
|
0.770647516,0.156886915,0.709133577,1.221794006,1.276297358,1.175414791,0.113668148,0.402739386
|
||||||
0.630899673,0.127179855,0.529277230,1.376536141,1.570796327,-1.471053586,1.260486077,-0.449082941
|
0.744611022,0.168030005,0.697347187,1.188452194,1.268418982,1.198048128,0.116096349,0.423194067
|
||||||
0.631899673,0.126910855,0.529969709,1.377476809,1.570796327,-1.469561180,1.260414998,-0.446774571
|
0.637032782,0.214836584,0.643082607,1.060955797,1.260453716,1.292372114,0.132983463,0.489751204
|
||||||
0.632899673,0.126643460,0.530662315,1.378415999,1.570796327,-1.468069648,1.260342867,-0.444467067
|
0.574933616,0.250105672,0.615452371,0.978234894,1.252400393,1.347436583,0.138454646,0.530947518
|
||||||
0.633899673,0.126377664,0.531355048,1.379353716,1.570796327,-1.466579003,1.260269686,-0.442160439
|
0.539590877,0.273860359,0.602337244,0.926155015,1.244257794,1.378928777,0.139036854,0.556650107
|
||||||
0.634899673,0.126113462,0.532047907,1.380289967,1.570796327,-1.465089256,1.260195454,-0.439854701
|
0.520199795,0.288907482,0.597319871,0.894082499,1.236024645,1.396237711,0.137352783,0.572992332
|
||||||
0.635899673,0.125850848,0.532740891,1.381224756,1.570796327,-1.463600420,1.260120173,-0.437549864
|
0.510376439,0.298087117,0.596923614,0.874644737,1.227699615,1.405002708,0.134526008,0.583706286
|
||||||
0.636899673,0.125589815,0.533433999,1.382158091,1.570796327,-1.462112507,1.260043844,-0.435245940
|
0.506318609,0.303535716,0.599218948,0.863032726,1.219281310,1.408608023,0.131072654,0.591039128
|
||||||
0.637899673,0.125330360,0.534127230,1.383089976,1.570796327,-1.460625528,1.259966467,-0.432942941
|
0.505756372,0.306684673,0.603111992,0.856227231,1.210768276,1.409081276,0.127246986,0.596336295
|
||||||
0.638899673,0.125072476,0.534820584,1.384020417,1.570796327,-1.459139497,1.259888043,-0.430640879
|
0.507334409,0.308440165,0.607972681,0.852376019,1.202158989,1.407635104,0.123182132,0.600400663
|
||||||
0.639899673,0.124816158,0.535514060,1.384949420,1.570796327,-1.457654424,1.259808573,-0.428339765
|
0.510244464,0.309358677,0.613434335,0.850358903,1.193451854,1.404993161,0.118951022,0.603710250
|
||||||
0.640899673,0.124561400,0.536207656,1.385876991,1.570796327,-1.456170322,1.259728058,-0.426039613
|
0.514005804,0.309776105,0.619282387,0.849504927,1.184645201,1.401585936,0.114594682,0.606549100
|
||||||
0.641899673,0.124308197,0.536901374,1.386803135,1.570796327,-1.454687203,1.259646498,-0.423740433
|
0.518333935,0.309893131,0.625391162,0.849415421,1.175737284,1.397668163,0.110136351,0.609085599
|
||||||
0.642899673,0.124056543,0.537595210,1.387727858,1.570796327,-1.453205079,1.259563896,-0.421442237
|
0.523062081,0.309828855,0.631687352,0.849855669,1.166726271,1.393389138,0.105588962,0.611419214
|
||||||
0.643899673,0.123806434,0.538289165,1.388651165,1.570796327,-1.451723962,1.259480250,-0.419145037
|
0.528094236,0.309653596,0.638128685,0.850689406,1.157610241,1.388834772,0.100959309,0.613608326
|
||||||
0.644899673,0.123557863,0.538983239,1.389573063,1.570796327,-1.450243862,1.259395563,-0.416848845
|
0.533377116,0.309408629,0.644691392,0.851839519,1.148387181,1.384052732,0.096250437,0.615686797
|
||||||
0.645899673,0.123310826,0.539677429,1.390493556,1.570796327,-1.448764793,1.259309836,-0.414553672
|
0.538883420,0.309117971,0.651362822,0.853264591,1.139054979,1.379067420,0.091463047,0.617673798
|
||||||
0.646899673,0.123065317,0.540371736,1.391412651,1.570796327,-1.447286766,1.259223068,-0.412259529
|
0.544601886,0.308795367,0.658137078,0.854944960,1.129611417,1.373888888,0.086596320,0.619579648
|
||||||
0.647899673,0.122821331,0.541066158,1.392330352,1.570796327,-1.445809792,1.259135262,-0.409966429
|
0.550531398,0.308448411,0.665012457,0.856874494,1.120054164,1.368518104,0.081648403,0.621409255
|
||||||
0.648899673,0.122578862,0.541760696,1.393246666,1.570796327,-1.444333883,1.259046418,-0.407674383
|
0.556677562,0.308080974,0.671989950,0.859055780,1.110380773,1.362950021,0.076616687,0.623164139
|
||||||
0.649899673,0.122337906,0.542455347,1.394161597,1.570796327,-1.442859051,1.258956537,-0.405383402
|
0.563050749,0.307694607,0.679072396,0.861497372,1.100588667,1.357175319,0.071497952,0.624843603
|
||||||
0.650899673,0.122098457,0.543150112,1.395075151,1.570796327,-1.441385308,1.258865620,-0.403093497
|
0.569665063,0.307289357,0.686264006,0.864212284,1.090675137,1.351181335,0.066288440,0.626445390
|
||||||
0.651899673,0.121860510,0.543844989,1.395987334,1.570796327,-1.439912664,1.258773667,-0.400804681
|
0.576537873,0.306864210,0.693570141,0.867217268,1.080637328,1.344952472,0.060983861,0.627966025
|
||||||
0.652899673,0.121624059,0.544539978,1.396898151,1.570796327,-1.438441131,1.258680681,-0.398516964
|
0.583689740,0.306417315,0.700997243,0.870532584,1.070472230,1.338470272,0.055579362,0.629400956
|
||||||
0.653899673,0.121389101,0.545235079,1.397807606,1.570796327,-1.436970721,1.258586662,-0.396230358
|
0.591144612,0.305946073,0.708552871,0.874182131,1.060176671,1.331713236,0.050069468,0.630744563
|
||||||
0.654899673,0.121155628,0.545930290,1.398715707,1.570796327,-1.435501446,1.258491610,-0.393944874
|
0.598930270,0.305447119,0.716245832,0.878193853,1.049747299,1.324656431,0.044447975,0.631990064
|
||||||
0.655899673,0.120923638,0.546625610,1.399622457,1.570796327,-1.434033315,1.258395527,-0.391660523
|
0.607079000,0.304916236,0.724086399,0.882600426,1.039180573,1.317270885,0.038707816,0.633129321
|
||||||
0.656899673,0.120693123,0.547321039,1.400527862,1.570796327,-1.432566341,1.258298415,-0.389377316
|
0.615628554,0.304348188,0.732086615,0.887440237,1.028472749,1.309522742,0.032840873,0.634152555
|
||||||
0.657899673,0.120464080,0.548016577,1.401431928,1.570796327,-1.431100536,1.258200273,-0.387095265
|
0.624623451,0.303736469,0.740260735,0.892758742,1.017619861,1.301372098,0.026837723,0.635047936
|
||||||
0.658899673,0.120236504,0.548712222,1.402334659,1.570796327,-1.429635909,1.258101103,-0.384814380
|
0.634116783,0.303072945,0.748625825,0.898610354,1.006617707,1.292771403,0.020687289,0.635800997
|
||||||
0.659899673,0.120010388,0.549407974,1.403236062,1.570796327,-1.428172473,1.258000906,-0.382534672
|
0.644172727,0.302347335,0.757202622,0.905061077,0.995461828,1.283663231,0.014376362,0.636393815
|
||||||
0.660899673,0.119785729,0.550103832,1.404136140,1.570796327,-1.426710238,1.257899683,-0.380256154
|
0.654870154,0.301546450,0.766016762,0.912192288,0.984147488,1.273977087,0.007888899,0.636803812
|
||||||
0.661899673,0.119562522,0.550799795,1.405034900,1.570796327,-1.425249216,1.257797436,-0.377978834
|
0.666307957,0.300653050,0.775100628,0.920106350,0.972669650,1.263624695,0.001204997,0.637001953
|
||||||
0.662899673,0.119340761,0.551495862,1.405932346,1.570796327,-1.423789417,1.257694164,-0.375702725
|
0.678613267,0.299644050,0.784496219,0.928935253,0.961022948,1.252492730,-0.005700698,0.636949951
|
||||||
0.663899673,0.119120441,0.552192034,1.406828483,1.570796327,-1.422330853,1.257589870,-0.373427837
|
0.691954782,0.298487563,0.794259834,0.938854664,0.949201660,1.240431065,-0.012861580,0.636595655
|
||||||
0.664899673,0.118901558,0.552888308,1.407723318,1.570796327,-1.420873534,1.257484555,-0.371154180
|
0.706565844,0.297137684,0.804470277,0.950108280,0.937199677,1.227232411,-0.020323575,0.635864986
|
||||||
0.665899673,0.118684107,0.553584685,1.408616853,1.570796327,-1.419417471,1.257378218,-0.368881767
|
0.722788111,0.295524457,0.815244499,0.963053989,0.925010463,1.212593839,-0.028153656,0.634646465
|
||||||
0.666899673,0.118468083,0.554281164,1.409509096,1.570796327,-1.417962675,1.257270863,-0.366610606
|
0.741165210,0.293531924,0.826771435,0.978263035,0.912627013,1.196034335,-0.036458117,0.632757639
|
||||||
0.667899673,0.118253482,0.554977744,1.410400051,1.570796327,-1.416509158,1.257162489,-0.364340710
|
0.762686891,0.290939626,0.839400906,0.996779156,0.900041812,1.176680004,-0.045430129,0.629856366
|
||||||
0.668899673,0.118040297,0.555674424,1.411289722,1.570796327,-1.415056929,1.257053098,-0.362072088
|
0.789708560,0.287196010,0.853980910,1.021097833,0.887246777,1.152455561,-0.055527958,0.625101724
|
||||||
0.669899673,0.117828525,0.556371204,1.412178115,1.570796327,-1.413605999,1.256942690,-0.359804751
|
0.837468810,0.278494864,0.876119896,1.067475321,0.874233200,1.109963552,-0.069695803,0.612835819
|
||||||
0.670899673,0.117618162,0.557068083,1.413065235,1.570796327,-1.412156379,1.256831268,-0.357538710
|
0.866728664,0.275899768,0.891364369,1.093244121,0.860991679,1.083726355,-0.080812562,0.608306115
|
||||||
0.671899673,0.117409201,0.557765060,1.413951087,1.570796327,-1.410708080,1.256718833,-0.355273975
|
0.884955480,0.276549441,0.902432818,1.107041943,0.847512040,1.067136594,-0.089940632,0.608256886
|
||||||
0.672899673,0.117201638,0.558462135,1.414835675,1.570796327,-1.409261111,1.256605384,-0.353010556
|
0.896594602,0.279027152,0.911023238,1.113793159,0.833783247,1.056284776,-0.097855475,0.610838734
|
||||||
0.673899673,0.116995470,0.559159306,1.415719005,1.570796327,-1.407815485,1.256490925,-0.350748465
|
0.904301301,0.282588985,0.918169223,1.116366559,0.819793300,1.048842654,-0.105068975,0.614987391
|
||||||
0.674899673,0.116790690,0.559856574,1.416601081,1.570796327,-1.406371210,1.256375455,-0.348487710
|
0.909663781,0.286828624,0.924492301,1.116453168,0.805529113,1.043418780,-0.111904440,0.620085967
|
||||||
0.675899673,0.116587295,0.560553937,1.417481909,1.570796327,-1.404928297,1.256258976,-0.346228302
|
0.913632713,0.291519190,0.930364254,1.115056164,0.790976372,1.039179568,-0.118562216,0.625776036
|
||||||
0.676899673,0.116385280,0.561251395,1.418361493,1.570796327,-1.403486757,1.256141489,-0.343970252
|
0.916777563,0.296533258,0.936007401,1.112772667,0.776119374,1.035624457,-0.125166014,0.631849885
|
||||||
0.677899673,0.116184640,0.561948948,1.419239838,1.570796327,-1.402046601,1.256022995,-0.341713570
|
0.919439819,0.301800196,0.941555314,1.109958612,0.760940832,1.032451887,-0.131793012,0.638188256
|
||||||
0.678899673,0.115985371,0.562646594,1.420116948,1.570796327,-1.400607837,1.255903497,-0.339458266
|
0.921824652,0.307282553,0.947089206,1.106826149,0.745421641,1.029479207,-0.138492662,0.644724121
|
||||||
0.679899673,0.115787468,0.563344332,1.420992830,1.570796327,-1.399170476,1.255782994,-0.337204349
|
0.924055763,0.312962691,0.952659616,1.103501520,0.729540608,1.026594731,-0.145298210,0.651421508
|
||||||
0.680899673,0.115590927,0.564042163,1.421867486,1.570796327,-1.397734530,1.255661488,-0.334951830
|
0.926208216,0.318835136,0.958299305,1.100059525,0.713274113,1.023729006,-0.152233712,0.658263129
|
||||||
0.681899673,0.115395742,0.564740086,1.422740923,1.570796327,-1.396300006,1.255538980,-0.332700719
|
|
||||||
0.682899673,0.115201910,0.565438099,1.423613144,1.570796327,-1.394866916,1.255415472,-0.330451025
|
|
||||||
0.683899673,0.115009426,0.566136202,1.424484155,1.570796327,-1.393435270,1.255290965,-0.328202759
|
|
||||||
0.684899673,0.114818286,0.566834395,1.425353960,1.570796327,-1.392005077,1.255165459,-0.325955930
|
|
||||||
0.685899673,0.114628485,0.567532677,1.426222564,1.570796327,-1.390576348,1.255038958,-0.323710548
|
|
||||||
0.686899673,0.114440019,0.568231048,1.427089972,1.570796327,-1.389149092,1.254911460,-0.321466622
|
|
||||||
0.687899673,0.114252883,0.568929505,1.427956187,1.570796327,-1.387723318,1.254782969,-0.319224163
|
|
||||||
0.688899673,0.114067073,0.569628050,1.428821215,1.570796327,-1.386299037,1.254653485,-0.316983180
|
|
||||||
0.689899673,0.113882585,0.570326681,1.429685061,1.570796327,-1.384876259,1.254523009,-0.314743682
|
|
||||||
0.690899673,0.113699414,0.571025398,1.430547728,1.570796327,-1.383454992,1.254391543,-0.312505678
|
|
||||||
0.691899673,0.113517557,0.571724200,1.431409222,1.570796327,-1.382035246,1.254259089,-0.310269179
|
|
||||||
0.692899673,0.113337008,0.572423086,1.432269546,1.570796327,-1.380617032,1.254125646,-0.308034194
|
|
||||||
0.693899673,0.113157763,0.573122056,1.433128706,1.570796327,-1.379200357,1.253991218,-0.305800731
|
|
||||||
0.694899673,0.112979819,0.573821109,1.433986706,1.570796327,-1.377785233,1.253855805,-0.303568800
|
|
||||||
0.695899673,0.112803170,0.574520244,1.434843550,1.570796327,-1.376371667,1.253719408,-0.301338410
|
|
||||||
0.696899673,0.112627814,0.575219462,1.435699242,1.570796327,-1.374959669,1.253582029,-0.299109571
|
|
||||||
0.697899673,0.112453745,0.575918761,1.436553788,1.570796327,-1.373549249,1.253443670,-0.296882291
|
|
||||||
0.698899673,0.112280959,0.576618140,1.437407192,1.570796327,-1.372140416,1.253304331,-0.294656580
|
|
||||||
0.699899673,0.112109453,0.577317600,1.438259457,1.570796327,-1.370733178,1.253164014,-0.292432446
|
|
||||||
0.700899673,0.111939221,0.578017139,1.439110589,1.570796327,-1.369327545,1.253022720,-0.290209898
|
|
||||||
0.701899673,0.111770261,0.578716756,1.439960592,1.570796327,-1.367923525,1.252880451,-0.287988946
|
|
||||||
0.702899673,0.111602568,0.579416452,1.440809469,1.570796327,-1.366521129,1.252737209,-0.285769597
|
|
||||||
0.703899673,0.111436137,0.580116226,1.441657226,1.570796327,-1.365120363,1.252592994,-0.283551861
|
|
||||||
0.704899673,0.111270965,0.580816076,1.442503866,1.570796327,-1.363721238,1.252447808,-0.281335747
|
|
||||||
0.705899673,0.111107047,0.581516003,1.443349395,1.570796327,-1.362323763,1.252301652,-0.279121262
|
|
||||||
0.706899673,0.110944381,0.582216006,1.444193815,1.570796327,-1.360927945,1.252154528,-0.276908416
|
|
||||||
0.707899673,0.110782961,0.582916084,1.445037132,1.570796327,-1.359533793,1.252006438,-0.274697217
|
|
||||||
0.708899673,0.110622783,0.583616236,1.445879350,1.570796327,-1.358141316,1.251857382,-0.272487674
|
|
||||||
0.709899673,0.110463844,0.584316463,1.446720472,1.570796327,-1.356750523,1.251707363,-0.270279794
|
|
||||||
0.710899673,0.110306140,0.585016763,1.447560503,1.570796327,-1.355361422,1.251556381,-0.268073587
|
|
||||||
0.711899673,0.110149667,0.585717136,1.448399447,1.570796327,-1.353974022,1.251404438,-0.265869060
|
|
||||||
0.712899673,0.109994421,0.586417581,1.449237309,1.570796327,-1.352588330,1.251251536,-0.263666222
|
|
||||||
0.713899673,0.109840397,0.587118097,1.450074092,1.570796327,-1.351204355,1.251097676,-0.261465081
|
|
||||||
0.714899673,0.109687592,0.587818685,1.450909801,1.570796327,-1.349822106,1.250942860,-0.259265645
|
|
||||||
0.715899673,0.109536003,0.588519343,1.451744440,1.570796327,-1.348441590,1.250787089,-0.257067922
|
|
||||||
0.716899673,0.109385625,0.589220071,1.452578012,1.570796327,-1.347062816,1.250630364,-0.254871919
|
|
||||||
0.717899673,0.109236455,0.589920869,1.453410522,1.570796327,-1.345685791,1.250472687,-0.252677646
|
|
||||||
0.718899673,0.109088488,0.590621735,1.454241973,1.570796327,-1.344310525,1.250314060,-0.250485110
|
|
||||||
0.719899673,0.108941721,0.591322669,1.455072371,1.570796327,-1.342937024,1.250154484,-0.248294318
|
|
||||||
0.720899673,0.108796150,0.592023671,1.455901718,1.570796327,-1.341565296,1.249993960,-0.246105279
|
|
||||||
0.721899673,0.108651772,0.592724740,1.456730020,1.570796327,-1.340195350,1.249832491,-0.243918000
|
|
||||||
0.722899673,0.108508582,0.593425876,1.457557279,1.570796327,-1.338827193,1.249670077,-0.241732489
|
|
||||||
0.723899673,0.108366577,0.594127077,1.458383500,1.570796327,-1.337460833,1.249506721,-0.239548753
|
|
||||||
0.724899673,0.108225753,0.594828344,1.459208687,1.570796327,-1.336096277,1.249342423,-0.237366799
|
|
||||||
0.725899673,0.108086106,0.595529676,1.460032843,1.570796327,-1.334733534,1.249177186,-0.235186637
|
|
||||||
0.726899673,0.107947634,0.596231072,1.460855973,1.570796327,-1.333372610,1.249011010,-0.233008271
|
|
||||||
0.727899673,0.107810331,0.596932531,1.461678081,1.570796327,-1.332013514,1.248843898,-0.230831711
|
|
||||||
0.728899673,0.107674195,0.597634054,1.462499170,1.570796327,-1.330656252,1.248675851,-0.228656964
|
|
||||||
0.729899673,0.107539221,0.598335639,1.463319244,1.570796327,-1.329300832,1.248506871,-0.226484036
|
|
||||||
0.730899673,0.107405407,0.599037287,1.464138307,1.570796327,-1.327947262,1.248336958,-0.224312934
|
|
||||||
0.731899673,0.107272749,0.599738995,1.464956363,1.570796327,-1.326595547,1.248166116,-0.222143667
|
|
||||||
0.732899673,0.107141242,0.600440765,1.465773416,1.570796327,-1.325245697,1.247994345,-0.219976240
|
|
||||||
0.733899673,0.107010884,0.601142595,1.466589469,1.570796327,-1.323897718,1.247821646,-0.217810661
|
|
||||||
0.734899673,0.106881671,0.601844486,1.467404527,1.570796327,-1.322551616,1.247648023,-0.215646938
|
|
||||||
0.735899673,0.106753599,0.602546435,1.468218593,1.570796327,-1.321207399,1.247473475,-0.213485076
|
|
||||||
0.736899673,0.106626664,0.603248443,1.469031670,1.570796327,-1.319865074,1.247298005,-0.211325082
|
|
||||||
0.737899673,0.106500865,0.603950510,1.469843763,1.570796327,-1.318524648,1.247121614,-0.209166964
|
|
||||||
0.738899673,0.106376196,0.604652634,1.470654876,1.570796327,-1.317186128,1.246944305,-0.207010727
|
|
||||||
0.739899673,0.106252654,0.605354815,1.471465011,1.570796327,-1.315849520,1.246766078,-0.204856380
|
|
||||||
0.740899673,0.106130236,0.606057053,1.472274173,1.570796327,-1.314514831,1.246586935,-0.202703927
|
|
||||||
0.741899673,0.106008939,0.606759347,1.473082366,1.570796327,-1.313182067,1.246406879,-0.200553377
|
|
||||||
0.742899673,0.105888759,0.607461697,1.473889592,1.570796327,-1.311851236,1.246225909,-0.198404734
|
|
||||||
0.743899673,0.105769692,0.608164102,1.474695857,1.570796327,-1.310522344,1.246044029,-0.196258006
|
|
||||||
0.744899673,0.105651736,0.608866561,1.475501162,1.570796327,-1.309195398,1.245861240,-0.194113199
|
|
||||||
0.745899673,0.105534886,0.609569075,1.476305512,1.570796327,-1.307870403,1.245677543,-0.191970320
|
|
||||||
0.746899673,0.105419140,0.610271642,1.477108911,1.570796327,-1.306547366,1.245492941,-0.189829373
|
|
||||||
0.747899673,0.105304494,0.610974262,1.477911362,1.570796327,-1.305226294,1.245307434,-0.187690367
|
|
||||||
0.748899673,0.105190944,0.611676934,1.478712869,1.570796327,-1.303907193,1.245121025,-0.185553306
|
|
||||||
0.749899673,0.105078489,0.612379658,1.479513435,1.570796327,-1.302590068,1.244933714,-0.183418197
|
|
||||||
0.750899673,0.104967123,0.613082434,1.480313063,1.570796327,-1.301274926,1.244745505,-0.181285045
|
|
||||||
0.751899673,0.104856844,0.613785261,1.481111758,1.570796327,-1.299961774,1.244556398,-0.179153857
|
|
||||||
0.752899673,0.104747649,0.614488139,1.481909523,1.570796327,-1.298650616,1.244366396,-0.177024639
|
|
||||||
0.753899673,0.104639533,0.615191066,1.482706361,1.570796327,-1.297341460,1.244175499,-0.174897397
|
|
||||||
0.754899673,0.104532495,0.615894043,1.483502275,1.570796327,-1.296034310,1.243983710,-0.172772135
|
|
||||||
0.755899673,0.104426531,0.616597068,1.484297270,1.570796327,-1.294729174,1.243791030,-0.170648860
|
|
||||||
0.756899673,0.104321637,0.617300142,1.485091349,1.570796327,-1.293426056,1.243597461,-0.168527578
|
|
||||||
0.757899673,0.104217811,0.618003264,1.485884515,1.570796327,-1.292124962,1.243403005,-0.166408294
|
|
||||||
0.758899673,0.104115049,0.618706434,1.486676771,1.570796327,-1.290825898,1.243207663,-0.164291013
|
|
||||||
0.759899673,0.104013348,0.619409650,1.487468122,1.570796327,-1.289528869,1.243011437,-0.162175742
|
|
||||||
0.760899673,0.103912704,0.620112913,1.488258570,1.570796327,-1.288233882,1.242814329,-0.160062485
|
|
||||||
0.761899673,0.103813116,0.620816222,1.489048118,1.570796327,-1.286940941,1.242616341,-0.157951247
|
|
||||||
0.762899673,0.103714579,0.621519576,1.489836771,1.570796327,-1.285650053,1.242417474,-0.155842035
|
|
||||||
0.763899673,0.103617090,0.622222975,1.490624532,1.570796327,-1.284361221,1.242217730,-0.153734853
|
|
||||||
0.764899673,0.103520646,0.622926419,1.491411403,1.570796327,-1.283074452,1.242017111,-0.151629707
|
|
||||||
0.765899673,0.103425245,0.623629907,1.492197388,1.570796327,-1.281789752,1.241815618,-0.149526601
|
|
||||||
0.766899673,0.103330883,0.624333438,1.492982491,1.570796327,-1.280507124,1.241613253,-0.147425541
|
|
||||||
0.767899673,0.103237557,0.625037013,1.493766715,1.570796327,-1.279226574,1.241410019,-0.145326532
|
|
||||||
0.768899673,0.103145264,0.625740630,1.494550062,1.570796327,-1.277948108,1.241205916,-0.143229578
|
|
||||||
0.769899673,0.103054000,0.626444289,1.495332538,1.570796327,-1.276671730,1.241000946,-0.141134685
|
|
||||||
0.770899673,0.102963764,0.627147989,1.496114143,1.570796327,-1.275397446,1.240795112,-0.139041857
|
|
||||||
0.771899673,0.102874552,0.627851731,1.496894883,1.570796327,-1.274125260,1.240588415,-0.136951100
|
|
||||||
0.772899673,0.102786360,0.628555513,1.497674760,1.570796327,-1.272855176,1.240380856,-0.134862417
|
|
||||||
0.773899673,0.102699186,0.629259336,1.498453777,1.570796327,-1.271587201,1.240172438,-0.132775813
|
|
||||||
0.774899673,0.102613028,0.629963198,1.499231938,1.570796327,-1.270321339,1.239963162,-0.130691294
|
|
||||||
0.775899673,0.102527881,0.630667100,1.500009246,1.570796327,-1.269057593,1.239753030,-0.128608863
|
|
||||||
0.776899673,0.102443743,0.631371040,1.500785703,1.570796327,-1.267795970,1.239542044,-0.126528526
|
|
||||||
0.777899673,0.102360611,0.632075019,1.501561314,1.570796327,-1.266536473,1.239330206,-0.124450286
|
|
||||||
0.778899673,0.102278483,0.632779035,1.502336081,1.570796327,-1.265279107,1.239117516,-0.122374148
|
|
||||||
0.779899673,0.102197354,0.633483089,1.503110007,1.570796327,-1.264023876,1.238903978,-0.120300116
|
|
||||||
0.780899673,0.102117223,0.634187180,1.503883096,1.570796327,-1.262770786,1.238689593,-0.118228194
|
|
||||||
0.781899673,0.102038086,0.634891307,1.504655351,1.570796327,-1.261519839,1.238474363,-0.116158388
|
|
||||||
0.782899673,0.101959941,0.635595471,1.505426775,1.570796327,-1.260271041,1.238258288,-0.114090700
|
|
||||||
0.783899673,0.101882785,0.636299669,1.506197371,1.570796327,-1.259024396,1.238041373,-0.112025135
|
|
||||||
0.784899673,0.101806614,0.637003903,1.506967141,1.570796327,-1.257779908,1.237823617,-0.109961698
|
|
||||||
0.785899673,0.101731426,0.637708172,1.507736090,1.570796327,-1.256537580,1.237605023,-0.107900391
|
|
||||||
0.786899673,0.101657219,0.638412474,1.508504220,1.570796327,-1.255297418,1.237385592,-0.105841219
|
|
||||||
0.787899673,0.101583988,0.639116811,1.509271534,1.570796327,-1.254059425,1.237165327,-0.103784186
|
|
||||||
0.788899673,0.101511733,0.639821180,1.510038035,1.570796327,-1.252823605,1.236944230,-0.101729296
|
|
||||||
0.789899673,0.101440449,0.640525583,1.510803727,1.570796327,-1.251589962,1.236722301,-0.099676552
|
|
||||||
0.790899673,0.101370134,0.641230018,1.511568612,1.570796327,-1.250358499,1.236499543,-0.097625958
|
|
||||||
0.791899673,0.101300785,0.641934485,1.512332693,1.570796327,-1.249129222,1.236275958,-0.095577517
|
|
||||||
0.792899673,0.101232400,0.642638983,1.513095973,1.570796327,-1.247902132,1.236051547,-0.093531235
|
|
||||||
0.793899673,0.101164975,0.643343512,1.513858456,1.570796327,-1.246677235,1.235826312,-0.091487113
|
|
||||||
0.794899673,0.101098508,0.644048072,1.514620144,1.570796327,-1.245454533,1.235600256,-0.089445155
|
|
||||||
0.795899673,0.101032997,0.644752662,1.515381040,1.570796327,-1.244234031,1.235373379,-0.087405365
|
|
||||||
0.796899673,0.100968438,0.645457282,1.516141148,1.570796327,-1.243015731,1.235145684,-0.085367747
|
|
||||||
0.797899673,0.100904829,0.646161931,1.516900469,1.570796327,-1.241799637,1.234917173,-0.083332303
|
|
||||||
0.798899673,0.100842167,0.646866609,1.517659008,1.570796327,-1.240585754,1.234687846,-0.081299037
|
|
||||||
0.799899673,0.100780450,0.647571316,1.518416766,1.570796327,-1.239374083,1.234457707,-0.079267952
|
|
||||||
0.800899673,0.100719674,0.648276050,1.519173747,1.570796327,-1.238164628,1.234226757,-0.077239051
|
|
||||||
0.801899673,0.100659838,0.648980812,1.519929954,1.570796327,-1.236957393,1.233994998,-0.075212338
|
|
||||||
0.802899673,0.100600938,0.649685601,1.520685389,1.570796327,-1.235752381,1.233762431,-0.073187815
|
|
||||||
0.803899673,0.100542972,0.650390417,1.521440056,1.570796327,-1.234549594,1.233529059,-0.071165485
|
|
||||||
0.804899673,0.100485937,0.651095259,1.522193957,1.570796327,-1.233349037,1.233294883,-0.069145352
|
|
||||||
0.805899673,0.100429831,0.651800127,1.522947096,1.570796327,-1.232150712,1.233059905,-0.067127418
|
|
||||||
0.806899673,0.100374651,0.652505020,1.523699474,1.570796327,-1.230954622,1.232824128,-0.065111687
|
|
||||||
0.807899673,0.100320395,0.653209938,1.524451095,1.570796327,-1.229760770,1.232587551,-0.063098160
|
|
||||||
0.808899673,0.100267060,0.653914881,1.525201962,1.570796327,-1.228569158,1.232350179,-0.061086841
|
|
||||||
0.809899673,0.100214643,0.654619848,1.525952077,1.570796327,-1.227379791,1.232112012,-0.059077732
|
|
||||||
0.810899673,0.100163141,0.655324839,1.526701443,1.570796327,-1.226192670,1.231873052,-0.057070837
|
|
||||||
0.811899673,0.100112553,0.656029853,1.527450063,1.570796327,-1.225007798,1.231633301,-0.055066157
|
|
||||||
0.812899673,0.100062876,0.656734889,1.528197940,1.570796327,-1.223825178,1.231392761,-0.053063696
|
|
||||||
0.813899673,0.100014107,0.657439949,1.528945076,1.570796327,-1.222644813,1.231151434,-0.051063455
|
|
||||||
0.814899673,0.099966244,0.658145030,1.529691475,1.570796327,-1.221466705,1.230909322,-0.049065438
|
|
||||||
0.815899673,0.099919284,0.658850133,1.530437139,1.570796327,-1.220290857,1.230666426,-0.047069646
|
|
||||||
0.816899673,0.099873225,0.659555257,1.531182070,1.570796327,-1.219117271,1.230422748,-0.045076082
|
|
||||||
0.817899673,0.099828064,0.660260402,1.531926272,1.570796327,-1.217945950,1.230178290,-0.043084748
|
|
||||||
0.818899673,0.099783799,0.660965568,1.532669747,1.570796327,-1.216776896,1.229933055,-0.041095647
|
|
||||||
0.819899673,0.099740427,0.661670753,1.533412497,1.570796327,-1.215610111,1.229687043,-0.039108781
|
|
||||||
0.820899673,0.099697946,0.662375958,1.534154527,1.570796327,-1.214445598,1.229440257,-0.037124151
|
|
||||||
0.821899673,0.099656354,0.663081182,1.534895837,1.570796327,-1.213283358,1.229192698,-0.035141761
|
|
||||||
0.822899673,0.099615648,0.663786425,1.535636431,1.570796327,-1.212123395,1.228944369,-0.033161611
|
|
||||||
0.823899673,0.099575825,0.664491686,1.536376312,1.570796327,-1.210965710,1.228695271,-0.031183705
|
|
||||||
0.824899673,0.099536884,0.665196966,1.537115481,1.570796327,-1.209810306,1.228445406,-0.029208044
|
|
||||||
0.825899673,0.099498821,0.665902262,1.537853943,1.570796327,-1.208657184,1.228194776,-0.027234629
|
|
||||||
0.826899673,0.099461635,0.666607576,1.538591699,1.570796327,-1.207506346,1.227943382,-0.025263464
|
|
||||||
0.827899673,0.099425324,0.667312907,1.539328752,1.570796327,-1.206357795,1.227691227,-0.023294548
|
|
||||||
0.828899673,0.099389883,0.668018254,1.540065104,1.570796327,-1.205211532,1.227438313,-0.021327886
|
|
||||||
0.829899673,0.099355313,0.668723617,1.540800759,1.570796327,-1.204067559,1.227184641,-0.019363477
|
|
||||||
0.830899673,0.099321609,0.669428996,1.541535719,1.570796327,-1.202925878,1.226930213,-0.017401324
|
|
||||||
0.831899673,0.099288770,0.670134390,1.542269985,1.570796327,-1.201786491,1.226675031,-0.015441429
|
|
||||||
0.832899673,0.099256793,0.670839798,1.543003562,1.570796327,-1.200649399,1.226419097,-0.013483793
|
|
||||||
0.833899673,0.099225677,0.671545221,1.543736451,1.570796327,-1.199514604,1.226162412,-0.011528417
|
|
||||||
0.834899673,0.099195418,0.672250658,1.544468655,1.570796327,-1.198382108,1.225904979,-0.009575303
|
|
||||||
0.835899673,0.099166015,0.672956108,1.545200177,1.570796327,-1.197251913,1.225646799,-0.007624453
|
|
||||||
0.836899673,0.099137465,0.673661572,1.545931019,1.570796327,-1.196124019,1.225387875,-0.005675867
|
|
||||||
0.837899673,0.099109765,0.674367049,1.546661183,1.570796327,-1.194998429,1.225128207,-0.003729548
|
|
||||||
0.838899673,0.099082915,0.675072538,1.547390673,1.570796327,-1.193875143,1.224867799,-0.001785496
|
|
||||||
0.839899673,0.099056911,0.675778038,1.548119490,1.570796327,-1.192754163,1.224606651,0.000156287
|
|
||||||
0.840899673,0.099031751,0.676483551,1.548847637,1.570796327,-1.191635492,1.224344766,0.002095799
|
|
||||||
0.841899673,0.099007433,0.677189075,1.549575117,1.570796327,-1.190519129,1.224082145,0.004033041
|
|
||||||
0.842899673,0.098983955,0.677894610,1.550301932,1.570796327,-1.189405076,1.223818791,0.005968010
|
|
||||||
0.843899673,0.098961314,0.678600155,1.551028084,1.570796327,-1.188293334,1.223554705,0.007900706
|
|
||||||
0.844899673,0.098939509,0.679305711,1.551753576,1.570796327,-1.187183905,1.223289889,0.009831128
|
|
||||||
0.845899673,0.098918536,0.680011276,1.552478410,1.570796327,-1.186076790,1.223024344,0.011759274
|
|
||||||
0.846899673,0.098898395,0.680716850,1.553202590,1.570796327,-1.184971989,1.222758073,0.013685143
|
|
||||||
0.847899673,0.098879082,0.681422434,1.553926116,1.570796327,-1.183869505,1.222491078,0.015608736
|
|
||||||
0.848899673,0.098860596,0.682128026,1.554648992,1.570796327,-1.182769337,1.222223360,0.017530050
|
|
||||||
0.849899673,0.098842934,0.682833626,1.555371221,1.570796327,-1.181671487,1.221954922,0.019449085
|
|
||||||
0.850899673,0.098826094,0.683539235,1.556092803,1.570796327,-1.180575955,1.221685764,0.021365840
|
|
||||||
0.851899673,0.098810075,0.684244850,1.556813743,1.570796327,-1.179482744,1.221415890,0.023280315
|
|
||||||
0.852899673,0.098794873,0.684950473,1.557534041,1.570796327,-1.178391852,1.221145300,0.025192508
|
|
||||||
0.853899673,0.098780487,0.685656103,1.558253702,1.570796327,-1.177303282,1.220873996,0.027102419
|
|
||||||
0.854899673,0.098766915,0.686361739,1.558972726,1.570796327,-1.176217034,1.220601981,0.029010048
|
|
||||||
0.855899673,0.098754155,0.687067381,1.559691116,1.570796327,-1.175133109,1.220329257,0.030915394
|
|
||||||
0.856899673,0.098742204,0.687773028,1.560408876,1.570796327,-1.174051507,1.220055824,0.032818456
|
|
||||||
0.857899673,0.098731060,0.688478681,1.561126006,1.570796327,-1.172972229,1.219781686,0.034719234
|
|
||||||
0.858899673,0.098720722,0.689184339,1.561842509,1.570796327,-1.171895276,1.219506843,0.036617727
|
|
||||||
0.859899673,0.098711187,0.689890001,1.562558388,1.570796327,-1.170820647,1.219231298,0.038513936
|
|
||||||
0.860899673,0.098702453,0.690595667,1.563273645,1.570796327,-1.169748345,1.218955052,0.040407858
|
|
||||||
0.861899673,0.098694519,0.691301337,1.563988282,1.570796327,-1.168678368,1.218678108,0.042299495
|
|
||||||
0.862899673,0.098687381,0.692007011,1.564702302,1.570796327,-1.167610719,1.218400466,0.044188846
|
|
||||||
0.863899673,0.098681039,0.692712688,1.565415706,1.570796327,-1.166545396,1.218122130,0.046075911
|
|
||||||
0.864899673,0.098675489,0.693418367,1.566128498,1.570796327,-1.165482401,1.217843100,0.047960689
|
|
||||||
0.865899673,0.098670731,0.694124049,1.566840679,1.570796327,-1.164421733,1.217563379,0.049843181
|
|
||||||
0.866899673,0.098666762,0.694829733,1.567552251,1.570796327,-1.163363393,1.217282969,0.051723385
|
|
||||||
0.867899673,0.098663579,0.695535418,1.568263218,1.570796327,-1.162307381,1.217001871,0.053601303
|
|
||||||
0.868899673,0.098661182,0.696241105,1.568973580,1.570796327,-1.161253698,1.216720087,0.055476934
|
|
||||||
0.869899673,0.098659567,0.696946792,1.569683341,1.570796327,-1.160202343,1.216437619,0.057350277
|
|
||||||
0.870899673,0.098658734,0.697652480,1.570392502,1.570796327,-1.159153317,1.216154468,0.059221334
|
|
||||||
0.871899673,0.098658679,0.698358169,1.571101066,1.570796327,-1.158106620,1.215870637,0.061090103
|
|
||||||
0.872899673,0.098659402,0.699063857,1.571809036,1.570796327,-1.157062251,1.215586128,0.062956586
|
|
||||||
0.873899673,0.098660900,0.699769544,1.572516412,1.570796327,-1.156020212,1.215300942,0.064820781
|
|
||||||
0.874899673,0.098663171,0.700475231,1.573223198,1.570796327,-1.154980501,1.215015081,0.066682690
|
|
||||||
0.875899673,0.098666213,0.701180917,1.573929395,1.570796327,-1.153943119,1.214728547,0.068542313
|
|
||||||
0.876899673,0.098670025,0.701886601,1.574635006,1.570796327,-1.152908066,1.214441341,0.070399649
|
|
||||||
0.877899673,0.098674604,0.702592283,1.575340033,1.570796327,-1.151875341,1.214153466,0.072254700
|
|
||||||
0.878899673,0.098679948,0.703297963,1.576044478,1.570796327,-1.150844945,1.213864923,0.074107465
|
|
||||||
0.879899673,0.098686056,0.704003640,1.576748343,1.570796327,-1.149816876,1.213575715,0.075957944
|
|
||||||
0.880899673,0.098692926,0.704709314,1.577451630,1.570796327,-1.148791136,1.213285842,0.077806138
|
|
||||||
0.881899673,0.098700555,0.705414985,1.578154342,1.570796327,-1.147767723,1.212995307,0.079652048
|
|
||||||
0.882899673,0.098708942,0.706120652,1.578856481,1.570796327,-1.146746638,1.212704112,0.081495674
|
|
||||||
0.883899673,0.098718086,0.706826316,1.579558049,1.570796327,-1.145727880,1.212412258,0.083337016
|
|
||||||
0.884899673,0.098727983,0.707531975,1.580259047,1.570796327,-1.144711449,1.212119747,0.085176075
|
|
||||||
0.885899673,0.098738633,0.708237629,1.580959479,1.570796327,-1.143697343,1.211826581,0.087012852
|
|
||||||
0.886899673,0.098750033,0.708943278,1.581659345,1.570796327,-1.142685564,1.211532762,0.088847346
|
|
||||||
0.887899673,0.098762182,0.709648922,1.582358649,1.570796327,-1.141676110,1.211238292,0.090679559
|
|
||||||
0.888899673,0.098775077,0.710354561,1.583057392,1.570796327,-1.140668981,1.210943171,0.092509491
|
|
||||||
0.889899673,0.098788718,0.711060193,1.583755576,1.570796327,-1.139664176,1.210647404,0.094337144
|
|
||||||
0.890899673,0.098803101,0.711765819,1.584453204,1.570796327,-1.138661695,1.210350990,0.096162516
|
|
||||||
0.891899673,0.098818226,0.712471438,1.585150277,1.570796327,-1.137661537,1.210053931,0.097985610
|
|
||||||
0.892899673,0.098834090,0.713177050,1.585846798,1.570796327,-1.136663701,1.209756231,0.099806427
|
|
||||||
0.893899673,0.098850692,0.713882654,1.586542768,1.570796327,-1.135668187,1.209457889,0.101624966
|
|
||||||
0.894899673,0.098868030,0.714588251,1.587238190,1.570796327,-1.134674995,1.209158909,0.103441229
|
|
||||||
0.895899673,0.098886102,0.715293840,1.587933066,1.570796327,-1.133684123,1.208859292,0.105255216
|
|
||||||
0.896899673,0.098904906,0.715999421,1.588627398,1.570796327,-1.132695570,1.208559039,0.107066929
|
|
||||||
0.897899673,0.098924441,0.716704992,1.589321187,1.570796327,-1.131709337,1.208258153,0.108876369
|
|
||||||
0.898899673,0.098944704,0.717410555,1.590014436,1.570796327,-1.130725421,1.207956635,0.110683536
|
|
||||||
0.899899673,0.098965695,0.718116108,1.590707147,1.570796327,-1.129743823,1.207654488,0.112488431
|
|
||||||
0.900899673,0.098987411,0.718821652,1.591399321,1.570796327,-1.128764540,1.207351712,0.114291056
|
|
||||||
0.901899673,0.099009850,0.719527185,1.592090961,1.570796327,-1.127787574,1.207048309,0.116091411
|
|
||||||
0.902899673,0.099033011,0.720232708,1.592782069,1.570796327,-1.126812921,1.206744282,0.117889498
|
|
||||||
0.903899673,0.099056893,0.720938221,1.593472647,1.570796327,-1.125840582,1.206439632,0.119685318
|
|
||||||
0.904899673,0.099081492,0.721643722,1.594162696,1.570796327,-1.124870556,1.206134361,0.121478871
|
|
||||||
0.905899673,0.099106809,0.722349212,1.594852219,1.570796327,-1.123902841,1.205828471,0.123270159
|
|
||||||
0.906899673,0.099132840,0.723054690,1.595541217,1.570796327,-1.122937436,1.205521963,0.125059184
|
|
||||||
0.907899673,0.099159584,0.723760156,1.596229693,1.570796327,-1.121974341,1.205214839,0.126845946
|
|
||||||
0.908899673,0.099187040,0.724465610,1.596917649,1.570796327,-1.121013554,1.204907102,0.128630446
|
|
||||||
0.909899673,0.099215206,0.725171051,1.597605086,1.570796327,-1.120055073,1.204598751,0.130412687
|
|
||||||
0.910899673,0.099244080,0.725876480,1.598292006,1.570796327,-1.119098899,1.204289791,0.132192669
|
|
||||||
0.911899673,0.099273660,0.726581894,1.598978412,1.570796327,-1.118145029,1.203980221,0.133970393
|
|
||||||
0.912899673,0.099303945,0.727287296,1.599664305,1.570796327,-1.117193463,1.203670045,0.135745861
|
|
||||||
0.913899673,0.099334934,0.727992683,1.600349687,1.570796327,-1.116244198,1.203359263,0.137519075
|
|
||||||
0.914899673,0.099366624,0.728698056,1.601034560,1.570796327,-1.115297235,1.203047878,0.139290035
|
|
||||||
0.915899673,0.099399014,0.729403414,1.601718925,1.570796327,-1.114352571,1.202735891,0.141058744
|
|
||||||
0.916899673,0.099432102,0.730108758,1.602402786,1.570796327,-1.113410205,1.202423304,0.142825203
|
|
||||||
0.917899673,0.099465886,0.730814086,1.603086143,1.570796327,-1.112470136,1.202110118,0.144589412
|
|
||||||
0.918899673,0.099500366,0.731519398,1.603768999,1.570796327,-1.111532363,1.201796336,0.146351374
|
|
||||||
0.919899673,0.099535539,0.732224695,1.604451356,1.570796327,-1.110596884,1.201481959,0.148111091
|
|
||||||
0.920899673,0.099571404,0.732929976,1.605133214,1.570796327,-1.109663697,1.201166989,0.149868564
|
|
||||||
0.921899673,0.099607959,0.733635240,1.605814577,1.570796327,-1.108732801,1.200851428,0.151623793
|
|
||||||
0.922899673,0.099645202,0.734340487,1.606495446,1.570796327,-1.107804195,1.200535276,0.153376782
|
|
||||||
0.923899673,0.099683133,0.735045717,1.607175823,1.570796327,-1.106877877,1.200218537,0.155127532
|
|
||||||
0.924899673,0.099721748,0.735750930,1.607855709,1.570796327,-1.105953846,1.199901212,0.156876044
|
|
||||||
0.925899673,0.099761048,0.736456125,1.608535107,1.570796327,-1.105032100,1.199583302,0.158622321
|
|
||||||
0.926899673,0.099801030,0.737161301,1.609214018,1.570796327,-1.104112637,1.199264810,0.160366363
|
|
||||||
0.927899673,0.099841692,0.737866460,1.609892445,1.570796327,-1.103195455,1.198945736,0.162108173
|
|
||||||
0.928899673,0.099883034,0.738571599,1.610570388,1.570796327,-1.102280554,1.198626083,0.163847752
|
|
||||||
0.929899673,0.099925053,0.739276720,1.611247850,1.570796327,-1.101367932,1.198305852,0.165585102
|
|
||||||
0.930899673,0.099967748,0.739981821,1.611924833,1.570796327,-1.100457586,1.197985046,0.167320226
|
|
||||||
0.931899673,0.100011117,0.740686903,1.612601338,1.570796327,-1.099549515,1.197663665,0.169053124
|
|
||||||
0.932899673,0.100055160,0.741391965,1.613277368,1.570796327,-1.098643718,1.197341711,0.170783799
|
|
||||||
0.933899673,0.100099873,0.742097006,1.613952923,1.570796327,-1.097740192,1.197019187,0.172512253
|
|
||||||
0.934899673,0.100145257,0.742802027,1.614628006,1.570796327,-1.096838936,1.196696094,0.174238487
|
|
||||||
0.935899673,0.100191309,0.743507027,1.615302619,1.570796327,-1.095939949,1.196372433,0.175962503
|
|
||||||
0.936899673,0.100238028,0.744212006,1.615976763,1.570796327,-1.095043227,1.196048206,0.177684304
|
|
||||||
0.937899673,0.100285412,0.744916963,1.616650440,1.570796327,-1.094148770,1.195723415,0.179403892
|
|
||||||
0.938899673,0.100333460,0.745621899,1.617323652,1.570796327,-1.093256575,1.195398062,0.181121268
|
|
||||||
0.939899673,0.100382170,0.746326813,1.617996400,1.570796327,-1.092366640,1.195072148,0.182836434
|
|
||||||
0.940899673,0.100431541,0.747031704,1.618668686,1.570796327,-1.091478965,1.194745676,0.184549393
|
|
||||||
0.941899673,0.100481572,0.747736572,1.619340513,1.570796327,-1.090593546,1.194418645,0.186260146
|
|
||||||
0.942899673,0.100532260,0.748441418,1.620011881,1.570796327,-1.089710382,1.194091059,0.187968696
|
|
||||||
0.943899673,0.100583605,0.749146240,1.620682793,1.570796327,-1.088829471,1.193762920,0.189675045
|
|
||||||
0.944899673,0.100635605,0.749851038,1.621353250,1.570796327,-1.087950810,1.193434227,0.191379195
|
|
||||||
0.945899673,0.100688258,0.750555813,1.622023254,1.570796327,-1.087074399,1.193104984,0.193081148
|
|
||||||
0.946899673,0.100741563,0.751260563,1.622692807,1.570796327,-1.086200234,1.192775193,0.194780906
|
|
||||||
0.947899673,0.100795519,0.751965289,1.623361910,1.570796327,-1.085328314,1.192444853,0.196478471
|
|
||||||
0.948899673,0.100850124,0.752669990,1.624030564,1.570796327,-1.084458636,1.192113968,0.198173846
|
|
||||||
0.949899673,0.100905377,0.753374666,1.624698773,1.570796327,-1.083591199,1.191782539,0.199867033
|
|
||||||
0.950899673,0.100961276,0.754079317,1.625366537,1.570796327,-1.082726001,1.191450568,0.201558034
|
|
||||||
0.951899673,0.101017820,0.754783942,1.626033858,1.570796327,-1.081863039,1.191118056,0.203246851
|
|
||||||
0.952899673,0.101075008,0.755488541,1.626700738,1.570796327,-1.081002311,1.190785005,0.204933487
|
|
||||||
0.953899673,0.101132837,0.756193114,1.627367178,1.570796327,-1.080143815,1.190451416,0.206617944
|
|
||||||
0.954899673,0.101191307,0.756897660,1.628033180,1.570796327,-1.079287549,1.190117291,0.208300225
|
|
||||||
0.955899673,0.101250417,0.757602179,1.628698746,1.570796327,-1.078433510,1.189782633,0.209980330
|
|
||||||
0.956899673,0.101310164,0.758306671,1.629363877,1.570796327,-1.077581697,1.189447442,0.211658264
|
|
||||||
0.957899673,0.101370548,0.759011135,1.630028576,1.570796327,-1.076732106,1.189111720,0.213334029
|
|
||||||
0.958899673,0.101431567,0.759715572,1.630692843,1.570796327,-1.075884737,1.188775468,0.215007626
|
|
||||||
0.959899673,0.101493219,0.760419981,1.631356680,1.570796327,-1.075039586,1.188438689,0.216679058
|
|
||||||
0.960899673,0.101555504,0.761124361,1.632020089,1.570796327,-1.074196652,1.188101384,0.218348328
|
|
||||||
0.961899673,0.101618420,0.761828713,1.632683072,1.570796327,-1.073355931,1.187763555,0.220015438
|
|
||||||
0.962899673,0.101681966,0.762533036,1.633345630,1.570796327,-1.072517423,1.187425203,0.221680390
|
|
||||||
0.963899673,0.101746139,0.763237330,1.634007765,1.570796327,-1.071681123,1.187086329,0.223343188
|
|
||||||
0.964899673,0.101810940,0.763941594,1.634669478,1.570796327,-1.070847030,1.186746936,0.225003833
|
|
||||||
0.965899673,0.101876366,0.764645828,1.635330771,1.570796327,-1.070015142,1.186407025,0.226662328
|
|
||||||
0.966899673,0.101942417,0.765350032,1.635991646,1.570796327,-1.069185456,1.186066598,0.228318675
|
|
||||||
0.967899673,0.102009090,0.766054206,1.636652104,1.570796327,-1.068357970,1.185725656,0.229972878
|
|
||||||
0.968899673,0.102076385,0.766758350,1.637312146,1.570796327,-1.067532681,1.185384201,0.231624938
|
|
||||||
0.969899673,0.102144300,0.767462462,1.637971775,1.570796327,-1.066709587,1.185042234,0.233274859
|
|
||||||
0.970899673,0.102212834,0.768166543,1.638630992,1.570796327,-1.065888685,1.184699757,0.234922643
|
|
||||||
0.971899673,0.102281986,0.768870592,1.639289799,1.570796327,-1.065069974,1.184356772,0.236568292
|
|
||||||
0.972899673,0.102351754,0.769574610,1.639948196,1.570796327,-1.064253449,1.184013280,0.238211809
|
|
||||||
0.973899673,0.102422137,0.770278596,1.640606186,1.570796327,-1.063439110,1.183669283,0.239853197
|
|
||||||
0.974899673,0.102493134,0.770982549,1.641263771,1.570796327,-1.062626953,1.183324783,0.241492458
|
|
||||||
0.975899673,0.102564743,0.771686469,1.641920951,1.570796327,-1.061816976,1.182979780,0.243129596
|
|
||||||
0.976899673,0.102636963,0.772390357,1.642577728,1.570796327,-1.061009176,1.182634277,0.244764612
|
|
||||||
0.977899673,0.102709793,0.773094211,1.643234104,1.570796327,-1.060203551,1.182288275,0.246397509
|
|
||||||
0.978899673,0.102783232,0.773798032,1.643890081,1.570796327,-1.059400098,1.181941775,0.248028291
|
|
||||||
0.979899673,0.102857278,0.774501819,1.644545659,1.570796327,-1.058598815,1.181594780,0.249656960
|
|
||||||
0.980899673,0.102931930,0.775205572,1.645200841,1.570796327,-1.057799698,1.181247291,0.251283518
|
|
||||||
0.981899673,0.103007187,0.775909291,1.645855627,1.570796327,-1.057002746,1.180899309,0.252907969
|
|
||||||
0.982899673,0.103083047,0.776612975,1.646510021,1.570796327,-1.056207956,1.180550836,0.254530315
|
|
||||||
0.983899673,0.103159510,0.777316624,1.647164022,1.570796327,-1.055415325,1.180201873,0.256150559
|
|
||||||
0.984899673,0.103236573,0.778020238,1.647817632,1.570796327,-1.054624850,1.179852422,0.257768704
|
|
||||||
0.985899673,0.103314237,0.778723816,1.648470854,1.570796327,-1.053836529,1.179502485,0.259384752
|
|
||||||
0.986899673,0.103392499,0.779427359,1.649123688,1.570796327,-1.053050359,1.179152063,0.260998707
|
|
||||||
|
|||||||
|
73
data/plot_joint_data.py
Normal file
73
data/plot_joint_data.py
Normal file
@ -0,0 +1,73 @@
|
|||||||
|
import pandas as pd
|
||||||
|
import numpy as np
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
|
CSV = '/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv'
|
||||||
|
df = pd.read_csv(CSV)
|
||||||
|
|
||||||
|
# ---- 曲线图：q1~q7 (+ 可选 psi) vs index ----
|
||||||
|
joint_cols_expect = ['q1','q2','q3','q4','q5','q6','q7']
|
||||||
|
joint_cols = [c for c in joint_cols_expect if c in df.columns]
|
||||||
|
assert len(joint_cols) == 7, f"CSV 缺少关节列，期望 {joint_cols_expect}，实际 {list(df.columns)}"
|
||||||
|
|
||||||
|
x = np.arange(len(df))
|
||||||
|
plt.figure(figsize=(12, 5))
|
||||||
|
for c in joint_cols:
|
||||||
|
plt.plot(x, df[c].to_numpy(), label=c, linewidth=1.2)
|
||||||
|
|
||||||
|
if 'psi' in df.columns:
|
||||||
|
plt.plot(x, df['psi'].to_numpy(), '--', label='psi', linewidth=1.2)
|
||||||
|
|
||||||
|
plt.xlabel('index')
|
||||||
|
plt.ylabel('angle (rad)')
|
||||||
|
plt.title('q1..q7 (and psi) vs index')
|
||||||
|
plt.grid(True, alpha=0.35)
|
||||||
|
plt.legend(ncol=4, fontsize=9)
|
||||||
|
plt.tight_layout()
|
||||||
|
plt.show()
|
||||||
|
|
||||||
|
# ---- 柱状图：最近限位距离（选“最危险帧”） ----
|
||||||
|
limits = np.array([
|
||||||
|
[-0.26, 1.57],
|
||||||
|
[-0.78, 1.57],
|
||||||
|
[-np.pi, np.pi],
|
||||||
|
[ 0.00, 2.05],
|
||||||
|
[-3.00, 3.00],
|
||||||
|
[-2.00, 2.00],
|
||||||
|
[-0.57, 1.57],
|
||||||
|
])
|
||||||
|
|
||||||
|
Q = df[joint_cols].to_numpy() # [N,7]
|
||||||
|
lo = limits[:, 0][None, :] # [1,7]
|
||||||
|
hi = limits[:, 1][None, :]
|
||||||
|
|
||||||
|
dist_low = Q - lo # 到下限的距离
|
||||||
|
dist_high = hi - Q # 到上限的距离
|
||||||
|
nearest = np.minimum(dist_low, dist_high) # 最近限位（可为负，负值=超限）
|
||||||
|
|
||||||
|
# 找“最危险”的 index（全关节最小裕度最小）
|
||||||
|
min_margin_per_row = nearest.min(axis=1) # 每帧的最小关节裕度
|
||||||
|
worst_idx = int(np.argmin(min_margin_per_row))
|
||||||
|
vals = nearest[worst_idx, :]
|
||||||
|
|
||||||
|
psi_text = f", psi={df['psi'].iloc[worst_idx]:.4f}" if 'psi' in df.columns else ""
|
||||||
|
|
||||||
|
plt.figure(figsize=(9, 4.5))
|
||||||
|
plt.bar(joint_cols, vals)
|
||||||
|
plt.axhline(0.0, linewidth=1, color='k')
|
||||||
|
plt.ylabel('Nearest distance to limit (rad)')
|
||||||
|
plt.title(f'Per-joint margin at worst frame (index={worst_idx}{psi_text})')
|
||||||
|
plt.grid(True, axis='y', alpha=0.35)
|
||||||
|
plt.tight_layout()
|
||||||
|
plt.show()
|
||||||
|
|
||||||
|
# ---- 可选：整段最小裕度曲线（帮助定位危险段）----
|
||||||
|
plt.figure(figsize=(12, 3.2))
|
||||||
|
plt.plot(min_margin_per_row, linewidth=1.2)
|
||||||
|
plt.axhline(0.0, linewidth=1, color='k')
|
||||||
|
plt.xlabel('index')
|
||||||
|
plt.ylabel('min margin (rad)')
|
||||||
|
plt.title('Minimum per-frame joint margin over sweep (rad)')
|
||||||
|
plt.grid(True, alpha=0.35)
|
||||||
|
plt.tight_layout()
|
||||||
|
plt.show()
|
||||||
5
src/utils/srs_ik/ik_hybird_optimizer.cpp
Normal file
5
src/utils/srs_ik/ik_hybird_optimizer.cpp
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
//
|
||||||
|
// Created by lgv on 11/7/25.
|
||||||
|
//
|
||||||
|
|
||||||
|
#include "ik_hybird_optimizer.h"
|
||||||
23
src/utils/srs_ik/ik_hybird_optimizer.h
Normal file
23
src/utils/srs_ik/ik_hybird_optimizer.h
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
//
|
||||||
|
// Created by lgv on 11/7/25.
|
||||||
|
//
|
||||||
|
|
||||||
|
#ifndef CMVR_ES_HYBIRD_OPTIMIZER_H
|
||||||
|
#define CMVR_ES_HYBIRD_OPTIMIZER_H
|
||||||
|
|
||||||
|
|
||||||
|
#include <math>
|
||||||
|
|
||||||
|
namespace cmvr {
|
||||||
|
namespace utils {
|
||||||
|
class IkHybirdOptimizer {
|
||||||
|
|
||||||
|
public:
|
||||||
|
private:
|
||||||
|
|
||||||
|
};
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endif //CMVR_ES_HYBIRD_OPTIMIZER_H
|
||||||
@ -447,3 +447,298 @@ IkLimitAnalyzer::union_intervals(const std::vector<std::pair<double, double> > &
|
|||||||
out.push_back({L, R});
|
out.push_back({L, R});
|
||||||
return out;
|
return out;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// --- 前向预测（给 ψ 预测 θ̂，用于候选打分） ---
|
||||||
|
inline double IkLimitAnalyzer::predict_theta_tan(
|
||||||
|
double an,double ad,double bn,double bd,double cn,double cd,double psi,double offset)
|
||||||
|
{
|
||||||
|
double N = an*std::sin(psi) + bn*std::cos(psi) + cn;
|
||||||
|
double D = ad*std::sin(psi) + bd*std::cos(psi) + cd;
|
||||||
|
double phi = std::atan2(N, D);
|
||||||
|
return normalize_angle(phi - offset); // θ̂ = φ - offset
|
||||||
|
}
|
||||||
|
|
||||||
|
inline double IkLimitAnalyzer::predict_theta_cos(
|
||||||
|
double a,double b,double c,int conf,double psi,double offset)
|
||||||
|
{
|
||||||
|
double ct = a*std::sin(psi) + b*std::cos(psi) + c;
|
||||||
|
ct = clamp(ct, -1.0, 1.0);
|
||||||
|
double phi = static_cast<double>(conf) * std::acos(ct);
|
||||||
|
return normalize_angle(phi - offset); // θ̂ = φ - offset
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
IkLimitAnalyzer::PsiEstimateResult
|
||||||
|
IkLimitAnalyzer::estimate_psi_from_joints(
|
||||||
|
const Eigen::MatrixXd& s_mat, const Eigen::MatrixXd& w_mat,
|
||||||
|
const std::vector<double>& theta_c,
|
||||||
|
int s_conf, int /*e_conf*/, int w_conf,
|
||||||
|
double prefer_psi)
|
||||||
|
{
|
||||||
|
PsiEstimateResult res;
|
||||||
|
|
||||||
|
// 尺寸检查
|
||||||
|
if (s_mat.rows()!=3 || s_mat.cols()!=9 || w_mat.rows()!=3 || w_mat.cols()!=9 || theta_c.size() != 7) {
|
||||||
|
res.ok = false; return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 拆块
|
||||||
|
const Eigen::Matrix3d As = s_mat.block<3,3>(0,0);
|
||||||
|
const Eigen::Matrix3d Bs = s_mat.block<3,3>(0,3);
|
||||||
|
const Eigen::Matrix3d Cs = s_mat.block<3,3>(0,6);
|
||||||
|
|
||||||
|
const Eigen::Matrix3d Aw = w_mat.block<3,3>(0,0);
|
||||||
|
const Eigen::Matrix3d Bw = w_mat.block<3,3>(0,3);
|
||||||
|
const Eigen::Matrix3d Cw = w_mat.block<3,3>(0,6);
|
||||||
|
|
||||||
|
const int s = s_conf;
|
||||||
|
const int w = w_conf;
|
||||||
|
|
||||||
|
std::vector<double> cands;
|
||||||
|
|
||||||
|
// —— 用每个关节各自反算 ψ 候选（与限位时的系数完全一致）——
|
||||||
|
|
||||||
|
// J1: tan
|
||||||
|
{
|
||||||
|
double an = -s * As(1,1), ad = -s * As(0,1);
|
||||||
|
double bn = -s * Bs(1,1), bd = -s * Bs(0,1);
|
||||||
|
double cn = -s * Cs(1,1), cd = -s * Cs(0,1);
|
||||||
|
auto v = calc_tan_solution(an,ad,bn,bd,cn,cd, theta_c[0]);
|
||||||
|
cands.insert(cands.end(), v.begin(), v.end());
|
||||||
|
}
|
||||||
|
|
||||||
|
// J2: cos (同 limit_2；反算时不需要 conf 放入 cos() 里)
|
||||||
|
{
|
||||||
|
double a = -As(2,1), b = -Bs(2,1), c = -Cs(2,1);
|
||||||
|
auto v = calc_cos_solution(a,b,c, theta_c[1]);
|
||||||
|
cands.insert(cands.end(), v.begin(), v.end());
|
||||||
|
}
|
||||||
|
|
||||||
|
// J3: tan (同 limit_3)
|
||||||
|
{
|
||||||
|
double an = s * As(2,2), ad = -s * As(2,0);
|
||||||
|
double bn = s * Bs(2,2), bd = -s * Bs(2,0);
|
||||||
|
double cn = s * Cs(2,2), cd = -s * Cs(2,0);
|
||||||
|
auto v = calc_tan_solution(an,ad,bn,bd,cn,cd, theta_c[2]);
|
||||||
|
cands.insert(cands.end(), v.begin(), v.end());
|
||||||
|
}
|
||||||
|
|
||||||
|
// J5: tan + offset(+π/2) (同 limit_5)
|
||||||
|
{
|
||||||
|
double an = w * Aw(1,2), ad = w * Aw(0,2);
|
||||||
|
double bn = w * Bw(1,2), bd = w * Bw(0,2);
|
||||||
|
double cn = w * Cw(1,2), cd = w * Cw(0,2);
|
||||||
|
double theta_adj = theta_c[4] + M_PI/2.0;
|
||||||
|
auto v = calc_tan_solution(an,ad,bn,bd,cn,cd, theta_adj);
|
||||||
|
cands.insert(cands.end(), v.begin(), v.end());
|
||||||
|
}
|
||||||
|
|
||||||
|
// J6: cos + offset(+π/2) (同 limit_6)
|
||||||
|
{
|
||||||
|
double a = Aw(2,2), b = Bw(2,2), c = Cw(2,2);
|
||||||
|
double theta_adj = theta_c[5] + M_PI/2.0;
|
||||||
|
auto v = calc_cos_solution(a,b,c, theta_adj);
|
||||||
|
cands.insert(cands.end(), v.begin(), v.end());
|
||||||
|
}
|
||||||
|
|
||||||
|
// J7: tan (同 limit_7)
|
||||||
|
{
|
||||||
|
double an = w * Aw(2,1), ad = -w * Aw(2,0);
|
||||||
|
double bn = w * Bw(2,1), bd = -w * Bw(2,0);
|
||||||
|
double cn = w * Cw(2,1), cd = -w * Cw(2,0);
|
||||||
|
auto v = calc_tan_solution(an,ad,bn,bd,cn,cd, theta_c[6]);
|
||||||
|
cands.insert(cands.end(), v.begin(), v.end());
|
||||||
|
}
|
||||||
|
|
||||||
|
// 候选去重
|
||||||
|
std::sort(cands.begin(), cands.end());
|
||||||
|
cands.erase(std::unique(cands.begin(), cands.end(),
|
||||||
|
[&](double a,double b){ return std::abs(normalize_angle(a - b)) < 1e-9; }), cands.end());
|
||||||
|
|
||||||
|
// if (cands.empty()) { res.ok=false; return res; }
|
||||||
|
// 认为设置
|
||||||
|
if (cands.empty()) {
|
||||||
|
const int K = 720; // 0.5° 网格
|
||||||
|
cands.reserve(K);
|
||||||
|
for (int i=0;i<K;++i){
|
||||||
|
cands.push_back(-M_PI + (2.0*M_PI)*(i+0.5)/K);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// —— 用所有关节的前向解析 θ̂(ψ) 对每个候选打分，选总残差最小者 ——
|
||||||
|
auto wrap_diff = [&](double x){ return normalize_angle(x); };
|
||||||
|
|
||||||
|
auto score_of = [&](double psi){
|
||||||
|
double sc = 0.0;
|
||||||
|
|
||||||
|
// J1 tan
|
||||||
|
{
|
||||||
|
double an = -s * As(1,1), ad = -s * As(0,1);
|
||||||
|
double bn = -s * Bs(1,1), bd = -s * Bs(0,1);
|
||||||
|
double cn = -s * Cs(1,1), cd = -s * Cs(0,1);
|
||||||
|
double th = predict_theta_tan(an,ad,bn,bd,cn,cd, psi, 0.0);
|
||||||
|
sc += std::pow(wrap_diff(th - theta_c[0]), 2);
|
||||||
|
}
|
||||||
|
// J2 cos
|
||||||
|
{
|
||||||
|
double a = -As(2,1), b = -Bs(2,1), c = -Cs(2,1);
|
||||||
|
double th = predict_theta_cos(a,b,c, s, psi, 0.0);
|
||||||
|
sc += std::pow(wrap_diff(th - theta_c[1]), 2);
|
||||||
|
}
|
||||||
|
// J3 tan
|
||||||
|
{
|
||||||
|
double an = s * As(2,2), ad = -s * As(2,0);
|
||||||
|
double bn = s * Bs(2,2), bd = -s * Bs(2,0);
|
||||||
|
double cn = s * Cs(2,2), cd = -s * Cs(2,0);
|
||||||
|
double th = predict_theta_tan(an,ad,bn,bd,cn,cd, psi, 0.0);
|
||||||
|
sc += std::pow(wrap_diff(th - theta_c[2]), 2);
|
||||||
|
}
|
||||||
|
// J5 tan + offset
|
||||||
|
{
|
||||||
|
double an = w * Aw(1,2), ad = w * Aw(0,2);
|
||||||
|
double bn = w * Bw(1,2), bd = w * Bw(0,2);
|
||||||
|
double cn = w * Cw(1,2), cd = w * Cw(0,2);
|
||||||
|
double th = predict_theta_tan(an,ad,bn,bd,cn,cd, psi, M_PI/2.0);
|
||||||
|
sc += std::pow(wrap_diff(th - theta_c[4]), 2);
|
||||||
|
}
|
||||||
|
// J6 cos + offset
|
||||||
|
{
|
||||||
|
double a = Aw(2,2), b = Bw(2,2), c = Cw(2,2);
|
||||||
|
double th = predict_theta_cos(a,b,c, w, psi, M_PI/2.0);
|
||||||
|
sc += std::pow(wrap_diff(th - theta_c[5]), 2);
|
||||||
|
}
|
||||||
|
// J7 tan
|
||||||
|
{
|
||||||
|
double an = w * Aw(2,1), ad = -w * Aw(2,0);
|
||||||
|
double bn = w * Bw(2,1), bd = -w * Bw(2,0);
|
||||||
|
double cn = w * Cw(2,1), cd = -w * Cw(2,0);
|
||||||
|
double th = predict_theta_tan(an,ad,bn,bd,cn,cd, psi, 0.0);
|
||||||
|
sc += std::pow(wrap_diff(th - theta_c[6]), 2);
|
||||||
|
}
|
||||||
|
return sc;
|
||||||
|
};
|
||||||
|
|
||||||
|
double best = std::numeric_limits<double>::infinity();
|
||||||
|
double bestpsi = 0.0;
|
||||||
|
|
||||||
|
for (double psi : cands) {
|
||||||
|
double sc = score_of(psi);
|
||||||
|
res.candidates.emplace_back(psi,sc);
|
||||||
|
if (sc < best - 1e-12) {
|
||||||
|
best = sc; bestpsi = psi;
|
||||||
|
} else if (std::abs(sc - best) <= 1e-12 && std::isfinite(prefer_psi)) {
|
||||||
|
// 并列时选更靠近 prefer_psi 的
|
||||||
|
if (std::abs(normalize_angle(psi - prefer_psi)) <
|
||||||
|
std::abs(normalize_angle(bestpsi - prefer_psi))) {
|
||||||
|
best = sc; bestpsi = psi;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
res.ok = true;
|
||||||
|
res.psi = bestpsi;
|
||||||
|
res.score = best;
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
double IkLimitAnalyzer::update_psi(
|
||||||
|
double psi_prev,
|
||||||
|
const std::vector<std::pair<double,double>>& psi_all,
|
||||||
|
const PsiUpdateParams& p
|
||||||
|
){
|
||||||
|
// 没有可行区间：按需返回原值或报错；这里返回原值
|
||||||
|
if (psi_all.empty()) {
|
||||||
|
return psi_prev;
|
||||||
|
}
|
||||||
|
|
||||||
|
const double EPS = 1e-12;
|
||||||
|
|
||||||
|
// 2) 找到包含 ψ_{t-1} 的可行段 Ψ_all,m = [L, U]
|
||||||
|
int hit = -1;
|
||||||
|
for (int i = 0; i < (int)psi_all.size(); ++i) {
|
||||||
|
double L = psi_all[i].first;
|
||||||
|
double U = psi_all[i].second;
|
||||||
|
if (psi_prev >= L - EPS && psi_prev <= U + EPS) {
|
||||||
|
hit = i; break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
auto clamp_step = [&](double d){
|
||||||
|
if (p.step_cap > 0.0)
|
||||||
|
return std::max(-p.step_cap, std::min(p.step_cap, d));
|
||||||
|
return d;
|
||||||
|
};
|
||||||
|
|
||||||
|
// 工具：到区间的环形距离（若在区间内则为 0）
|
||||||
|
auto ang_dist_to_interval = [&](double x, double L, double U){
|
||||||
|
x = normalize_angle(x);
|
||||||
|
// 不跨界，且 L<U
|
||||||
|
if (x >= L - EPS && x <= U + EPS) return 0.0;
|
||||||
|
auto wrap_abs = [&](double d){ return std::abs(normalize_angle(d)); };
|
||||||
|
return std::min(wrap_abs(x - L), wrap_abs(x - U));
|
||||||
|
};
|
||||||
|
|
||||||
|
// 3) 命中某个可行段 → 用论文的指数排斥公式
|
||||||
|
if (hit >= 0) {
|
||||||
|
double L = psi_all[hit].first;
|
||||||
|
double U = psi_all[hit].second;
|
||||||
|
double W = U - L;
|
||||||
|
if (W <= EPS) {
|
||||||
|
return normalize_angle(0.5 * (L + U));
|
||||||
|
}
|
||||||
|
|
||||||
|
// 归一化到 [0,1] 的左右边界距离
|
||||||
|
double sL = clamp((psi_prev - L) / W,0.0,1.0); // ∈[0,1]
|
||||||
|
double sU = clamp((U - psi_prev) / W,0.0,1.0); // ∈[0,1]
|
||||||
|
|
||||||
|
// 公式：
|
||||||
|
// ψ_t = ψ_{t-1} + K*(W/2) * [ exp(-α * sL) - exp(-α * sU) ]
|
||||||
|
double kick = p.K * (0.5 * W) * ( std::exp(-p.alpha * sL) - std::exp(-p.alpha * sU) );
|
||||||
|
kick = clamp_step(kick);
|
||||||
|
|
||||||
|
double psi_new = normalize_angle(psi_prev + kick);
|
||||||
|
double margin = std::max(p.edge_margin, 0.02 * W);
|
||||||
|
margin = std::min(margin, 0.25 * W);
|
||||||
|
if (psi_new < L + 1e-9 || psi_new > U - 1e-9) {
|
||||||
|
psi_new = std::min(U - margin, std::max(L + margin, psi_new));
|
||||||
|
}
|
||||||
|
return psi_new;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 4) 没命中的情况：把肘（ψ）“移入最近的可行段”
|
||||||
|
int best = -1;
|
||||||
|
double best_d = std::numeric_limits<double>::infinity();
|
||||||
|
for (int i = 0; i < (int)psi_all.size(); ++i) {
|
||||||
|
double L = psi_all[i].first;
|
||||||
|
double U = psi_all[i].second;
|
||||||
|
double d = ang_dist_to_interval(psi_prev, L, U);
|
||||||
|
if (d < best_d) { best_d = d; best = i; }
|
||||||
|
}
|
||||||
|
// 理论上 best 必定存在
|
||||||
|
double L = psi_all[best].first;
|
||||||
|
double U = psi_all[best].second;
|
||||||
|
double W = U - L;
|
||||||
|
|
||||||
|
// 选择最近的边界，并向内缩 margin
|
||||||
|
// 根据 ψ_{t-1} 与 [L,U] 的相对位置，决定吸向 L+δ 还是 U-δ
|
||||||
|
auto wrap = [&](double x){ return normalize_angle(x); };
|
||||||
|
|
||||||
|
// 判断离哪个端点近：用环形距离
|
||||||
|
auto wrap_abs = [&](double d){ return std::abs(normalize_angle(d)); };
|
||||||
|
bool closer_to_L = (wrap_abs(psi_prev - L) <= wrap_abs(psi_prev - U));
|
||||||
|
|
||||||
|
double margin = std::max(p.edge_margin, 0.02 * W); // 至少内缩一点，或用 2% 的区间宽
|
||||||
|
margin = std::min(margin, 0.25 * W); // 别缩太多
|
||||||
|
|
||||||
|
double target = closer_to_L ? (L + margin) : (U - margin);
|
||||||
|
|
||||||
|
// 如需要平滑，可按 step_cap 限制一步走到 target 的幅度
|
||||||
|
double delta = normalize_angle(target - psi_prev);
|
||||||
|
delta = clamp_step(delta);
|
||||||
|
|
||||||
|
return wrap(psi_prev + delta);
|
||||||
|
}
|
||||||
|
|||||||
@ -8,11 +8,13 @@
|
|||||||
#include <vector>
|
#include <vector>
|
||||||
#include <array>
|
#include <array>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
#include <Eigen/Core>
|
||||||
|
|
||||||
namespace cmvr {
|
namespace cmvr {
|
||||||
namespace utils {
|
namespace utils {
|
||||||
class IkLimitAnalyzer {
|
class IkLimitAnalyzer {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Tan 型关节：给定 (an,ad,bn,bd,cn,cd) 与关节极限
|
// Tan 型关节：给定 (an,ad,bn,bd,cn,cd) 与关节极限
|
||||||
// 返回 psi 的允许区间边界： [L1,R1,L2,R2,...]
|
// 返回 psi 的允许区间边界： [L1,R1,L2,R2,...]
|
||||||
static std::vector<std::pair<double, double> >
|
static std::vector<std::pair<double, double> >
|
||||||
@ -48,6 +50,45 @@ namespace cmvr {
|
|||||||
|
|
||||||
static void set_sing_avid(double value_deg){psi_sing_avid_ = deg2rad(value_deg);}
|
static void set_sing_avid(double value_deg){psi_sing_avid_ = deg2rad(value_deg);}
|
||||||
|
|
||||||
|
public:
|
||||||
|
// 反算 ψ 的返回结果
|
||||||
|
struct PsiEstimateResult {
|
||||||
|
bool ok{false};
|
||||||
|
double psi{0.0}; // 估计出来的 ψ
|
||||||
|
double score{0.0}; // 总残差（越小越好）
|
||||||
|
std::vector<std::pair<double, double>> candidates{}; // 候选 ψ ，以及分数
|
||||||
|
};
|
||||||
|
|
||||||
|
// 用当前关节角反算“上一时刻/当前估计”的臂角 ψ
|
||||||
|
// s_conf/e_conf/w_conf = {+1, -1}；prefer_psi NaN 表示无偏好
|
||||||
|
static PsiEstimateResult estimate_psi_from_joints(
|
||||||
|
const Eigen::MatrixXd& s_mat, // 3x9 [As|Bs|Cs]
|
||||||
|
const Eigen::MatrixXd& w_mat, // 3x9 [Aw|Bw|Cw]
|
||||||
|
const std::vector<double>& theta_c, // 当前 7 关节角
|
||||||
|
int s_conf, int e_conf, int w_conf,
|
||||||
|
double prefer_psi = std::numeric_limits<double>::quiet_NaN()
|
||||||
|
);
|
||||||
|
|
||||||
|
private:
|
||||||
|
// 打分时的前向预测（方程里的“φ”减去 offset 得 θ̂）
|
||||||
|
static inline double predict_theta_tan(double an,double ad,double bn,double bd,double cn,double cd,double psi,double offset);
|
||||||
|
static inline double predict_theta_cos(double a,double b,double c,int conf,double psi,double offset);
|
||||||
|
|
||||||
|
public:
|
||||||
|
// 论文 Fig.10 的“Calculate New ψ”一步更新所需参数
|
||||||
|
struct PsiUpdateParams {
|
||||||
|
double K = 0.6; // [0,1] 排斥强度
|
||||||
|
double alpha = 5.0; // >0 开始排斥的“敏感度”
|
||||||
|
double step_cap = 0.0; // psi 与上一次psi 的单步最大变化（rad），<=0 表示不限制
|
||||||
|
double edge_margin = 1e-4; // 落到区间边界时的内缩量
|
||||||
|
};
|
||||||
|
|
||||||
|
// 根据全局可行区间 Ψ_all（并集），按论文公式从 ψ_{t-1} 计算 ψ_{t}
|
||||||
|
static double update_psi(
|
||||||
|
double psi_prev,
|
||||||
|
const std::vector<std::pair<double,double>>& psi_all, // merged ∪-intervals, each L<R in [-π,π]
|
||||||
|
const PsiUpdateParams& p
|
||||||
|
);
|
||||||
private:
|
private:
|
||||||
static constexpr double EPS = 1e-9;
|
static constexpr double EPS = 1e-9;
|
||||||
|
|
||||||
@ -117,6 +158,9 @@ namespace cmvr {
|
|||||||
// 线性并集（输入输出都在 [-π,π] 且 L<=U；跨界已在上游拆分）
|
// 线性并集（输入输出都在 [-π,π] 且 L<=U；跨界已在上游拆分）
|
||||||
static std::vector<std::pair<double, double> >
|
static std::vector<std::pair<double, double> >
|
||||||
union_intervals(const std::vector<std::pair<double, double> > &in);
|
union_intervals(const std::vector<std::pair<double, double> > &in);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
};
|
};
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|||||||
@ -31,7 +31,7 @@ SRSIkSlover::SRSIkSlover() {
|
|||||||
|
|
||||||
joints_limits_ = {
|
joints_limits_ = {
|
||||||
{-0.26, 1.57},
|
{-0.26, 1.57},
|
||||||
{-0.78, 0.78},
|
{-0.78, 1.57},
|
||||||
{-M_PI, M_PI},
|
{-M_PI, M_PI},
|
||||||
{0, 2.05},
|
{0, 2.05},
|
||||||
{-3.00, 3.0},
|
{-3.00, 3.0},
|
||||||
@ -170,10 +170,6 @@ std::vector<double> SRSIkSlover::inverse_kinematics(const Eigen::MatrixXd &pose,
|
|||||||
joints[5] = normalize_angle(theta_y - M_PI / 2);
|
joints[5] = normalize_angle(theta_y - M_PI / 2);
|
||||||
joints[6] = normalize_angle(psi_z);
|
joints[6] = normalize_angle(psi_z);
|
||||||
|
|
||||||
for (double q1: joints) {
|
|
||||||
std::cout << q1 << " , ";
|
|
||||||
}
|
|
||||||
std::cout << std::endl;
|
|
||||||
|
|
||||||
|
|
||||||
return joints;
|
return joints;
|
||||||
@ -343,7 +339,6 @@ std::vector<std::pair<double, double> > SRSIkSlover::calc_arm_angle_limits(
|
|||||||
const Eigen::Matrix3d Cw = w_mat.block<3, 3>(0, 6);
|
const Eigen::Matrix3d Cw = w_mat.block<3, 3>(0, 6);
|
||||||
|
|
||||||
auto s = static_cast<int>(shoulder_config_);
|
auto s = static_cast<int>(shoulder_config_);
|
||||||
auto e = static_cast<int>(elbow_config_);
|
|
||||||
auto w = static_cast<int>(wrist_config_);
|
auto w = static_cast<int>(wrist_config_);
|
||||||
|
|
||||||
auto limit_1 = ik_limit_analyzer_.calc_tan_limits(-s * As(1, 1), -s * As(0, 1), -s * Bs(1, 1), -s * Bs(0, 1),
|
auto limit_1 = ik_limit_analyzer_.calc_tan_limits(-s * As(1, 1), -s * As(0, 1), -s * Bs(1, 1), -s * Bs(0, 1),
|
||||||
|
|||||||
@ -43,6 +43,24 @@ namespace utils {
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
int get_shoulder_config() {
|
||||||
|
return static_cast<int>(shoulder_config_);
|
||||||
|
}
|
||||||
|
|
||||||
|
int get_elbow_config() {
|
||||||
|
return static_cast<int>(elbow_config_);
|
||||||
|
}
|
||||||
|
|
||||||
|
int get_wrist_config() {
|
||||||
|
return static_cast<int>(wrist_config_);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<std::pair<double, double>> get_joints_limits() {
|
||||||
|
return joints_limits_;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
|
||||||
@ -63,8 +81,6 @@ namespace utils {
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 计算参考平面相对于基坐标系的旋转矩阵
|
// 计算参考平面相对于基坐标系的旋转矩阵
|
||||||
Eigen::Matrix3d reference_plane(const Eigen::Vector3d& S, const Eigen::Vector3d& W);
|
Eigen::Matrix3d reference_plane(const Eigen::Vector3d& S, const Eigen::Vector3d& W);
|
||||||
|
|
||||||
|
|||||||
@ -15,33 +15,31 @@ using namespace manif;
|
|||||||
using namespace cmvr::utils;
|
using namespace cmvr::utils;
|
||||||
|
|
||||||
struct IkSample {
|
struct IkSample {
|
||||||
double psi;
|
double psi;
|
||||||
std::array<double,7> q; // q1..q7
|
std::array<double, 7> q; // q1..q7
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
bool write_ik_samples_csv(const std::string &filepath,
|
||||||
bool write_ik_samples_csv(const std::string& filepath,
|
const std::vector<IkSample> &samples,
|
||||||
const std::vector<IkSample>& samples,
|
|
||||||
bool write_header,
|
bool write_header,
|
||||||
int precision)
|
int precision) {
|
||||||
{
|
std::ofstream ofs(filepath, std::ios::out | std::ios::trunc);
|
||||||
std::ofstream ofs(filepath, std::ios::out | std::ios::trunc);
|
if (!ofs.is_open()) return false;
|
||||||
if (!ofs.is_open()) return false;
|
|
||||||
|
|
||||||
// 固定小数点（避免本地化成逗号）
|
// 固定小数点（避免本地化成逗号）
|
||||||
ofs.imbue(std::locale::classic());
|
ofs.imbue(std::locale::classic());
|
||||||
ofs << std::fixed << std::setprecision(precision);
|
ofs << std::fixed << std::setprecision(precision);
|
||||||
|
|
||||||
if (write_header) {
|
if (write_header) {
|
||||||
ofs << "psi,q1,q2,q3,q4,q5,q6,q7\n";
|
ofs << "psi,q1,q2,q3,q4,q5,q6,q7\n";
|
||||||
}
|
}
|
||||||
for (const auto& s : samples) {
|
for (const auto &s: samples) {
|
||||||
ofs << s.psi;
|
ofs << s.psi;
|
||||||
for (int i = 0; i < 7; ++i) ofs << ',' << s.q[i];
|
for (int i = 0; i < 7; ++i) ofs << ',' << s.q[i];
|
||||||
ofs << '\n';
|
ofs << '\n';
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
|
TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
|
||||||
@ -55,7 +53,9 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
|
|||||||
samples.reserve(4096);
|
samples.reserve(4096);
|
||||||
|
|
||||||
std::vector<double> joint_angles(7, 0);
|
std::vector<double> joint_angles(7, 0);
|
||||||
joint_angles = { 0.875, 0.22, 0.2644, M_PI / 2, 1.0, 1.99, 1.56 };
|
joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 1.99, 1.56};
|
||||||
|
joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
|
||||||
|
|
||||||
|
|
||||||
// 目标位姿：FK(joint_angles)
|
// 目标位姿：FK(joint_angles)
|
||||||
const auto target_pose = slover.calc_total_transform(joint_angles);
|
const auto target_pose = slover.calc_total_transform(joint_angles);
|
||||||
@ -64,24 +64,31 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
|
|||||||
// 系数矩阵 & ψ 扫描区间
|
// 系数矩阵 & ψ 扫描区间
|
||||||
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||||
slover.cal_coefficient_matrix(target_pose, s_mat, w_mat);
|
slover.cal_coefficient_matrix(target_pose, s_mat, w_mat);
|
||||||
|
|
||||||
|
auto res = IkLimitAnalyzer::estimate_psi_from_joints(s_mat, w_mat, joint_angles, slover.get_shoulder_config(),
|
||||||
|
slover.get_elbow_config(), slover.get_wrist_config());
|
||||||
|
|
||||||
|
if (res.ok) {
|
||||||
|
std::cout << "res.psi" << res.psi << std::endl;
|
||||||
|
}
|
||||||
auto limits = slover.calc_arm_angle_limits(s_mat, w_mat);
|
auto limits = slover.calc_arm_angle_limits(s_mat, w_mat);
|
||||||
|
|
||||||
// 误差统计
|
// 误差统计
|
||||||
const double kPosTol = 1e-4; // 位置容差（m）
|
const double kPosTol = 1e-4; // 位置容差（m）
|
||||||
const double kRotTol = 1e-3; // 姿态容差（rad）≈ 0.0573°
|
const double kRotTol = 1e-3; // 姿态容差（rad）≈ 0.0573°
|
||||||
double max_pos_err = 0.0, max_rot_err = 0.0;
|
double max_pos_err = 0.0, max_rot_err = 0.0;
|
||||||
double sum_pos_err = 0.0, sum_rot_err = 0.0;
|
double sum_pos_err = 0.0, sum_rot_err = 0.0;
|
||||||
size_t total = 0, bad = 0;
|
size_t total = 0, bad = 0;
|
||||||
|
|
||||||
// 便捷引用
|
// 便捷引用
|
||||||
const Eigen::Vector3d p_target = target_pose.block<3,1>(0,3);
|
const Eigen::Vector3d p_target = target_pose.block<3, 1>(0, 3);
|
||||||
const Eigen::Matrix3d R_target = target_pose.block<3,3>(0,0);
|
const Eigen::Matrix3d R_target = target_pose.block<3, 3>(0, 0);
|
||||||
|
|
||||||
auto clamp = [](double x, double lo, double hi) {
|
auto clamp = [](double x, double lo, double hi) {
|
||||||
return std::max(lo, std::min(hi, x));
|
return std::max(lo, std::min(hi, x));
|
||||||
};
|
};
|
||||||
|
|
||||||
auto rot_err_rad = [&](const Eigen::Matrix3d& R) -> double {
|
auto rot_err_rad = [&](const Eigen::Matrix3d &R) -> double {
|
||||||
Eigen::Matrix3d dR = R_target.transpose() * R;
|
Eigen::Matrix3d dR = R_target.transpose() * R;
|
||||||
double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0);
|
double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0);
|
||||||
return std::acos(c); // [0, pi]
|
return std::acos(c); // [0, pi]
|
||||||
@ -91,7 +98,7 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
|
|||||||
cout << "psi(rad), pos_err(m), rot_err(rad), rot_err(deg)\n";
|
cout << "psi(rad), pos_err(m), rot_err(rad), rot_err(deg)\n";
|
||||||
|
|
||||||
// ψ 扫描
|
// ψ 扫描
|
||||||
for (const auto& limit : limits) {
|
for (const auto &limit: limits) {
|
||||||
const double psi_lo = limit.first;
|
const double psi_lo = limit.first;
|
||||||
const double psi_hi = limit.second;
|
const double psi_hi = limit.second;
|
||||||
|
|
||||||
@ -99,7 +106,7 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
|
|||||||
// IK 解
|
// IK 解
|
||||||
auto q = slover.inverse_kinematics(target_pose, psi);
|
auto q = slover.inverse_kinematics(target_pose, psi);
|
||||||
if (q.size() != 7 || std::any_of(q.begin(), q.end(),
|
if (q.size() != 7 || std::any_of(q.begin(), q.end(),
|
||||||
[](double v){ return !std::isfinite(v); })) {
|
[](double v) { return !std::isfinite(v); })) {
|
||||||
++bad;
|
++bad;
|
||||||
++total;
|
++total;
|
||||||
cout << psi << ", nan, nan, nan\n";
|
cout << psi << ", nan, nan, nan\n";
|
||||||
@ -108,8 +115,8 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
|
|||||||
|
|
||||||
// 用 IK 解做 FK，计算误差
|
// 用 IK 解做 FK，计算误差
|
||||||
const auto T_fk = slover.calc_total_transform(q);
|
const auto T_fk = slover.calc_total_transform(q);
|
||||||
const Eigen::Vector3d p_fk = T_fk.block<3,1>(0,3);
|
const Eigen::Vector3d p_fk = T_fk.block<3, 1>(0, 3);
|
||||||
const Eigen::Matrix3d R_fk = T_fk.block<3,3>(0,0);
|
const Eigen::Matrix3d R_fk = T_fk.block<3, 3>(0, 0);
|
||||||
|
|
||||||
const double pos_err = (p_fk - p_target).norm();
|
const double pos_err = (p_fk - p_target).norm();
|
||||||
const double rot_err = rot_err_rad(R_fk);
|
const double rot_err = rot_err_rad(R_fk);
|
||||||
@ -137,13 +144,13 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
|
|||||||
|
|
||||||
// 摘要打印
|
// 摘要打印
|
||||||
cout << "\nSummary:\n"
|
cout << "\nSummary:\n"
|
||||||
<< " total=" << total
|
<< " total=" << total
|
||||||
<< " bad=" << bad
|
<< " bad=" << bad
|
||||||
<< " pos_err_max=" << max_pos_err << " m"
|
<< " pos_err_max=" << max_pos_err << " m"
|
||||||
<< " rot_err_max=" << max_rot_err << " rad (" << max_rot_err * 180.0 / M_PI << " deg)\n"
|
<< " rot_err_max=" << max_rot_err << " rad (" << max_rot_err * 180.0 / M_PI << " deg)\n"
|
||||||
<< " pos_err_mean=" << (total ? (sum_pos_err / total) : 0.0) << " m"
|
<< " pos_err_mean=" << (total ? (sum_pos_err / total) : 0.0) << " m"
|
||||||
<< " rot_err_mean=" << (total ? (sum_rot_err / total) : 0.0) << " rad ("
|
<< " rot_err_mean=" << (total ? (sum_rot_err / total) : 0.0) << " rad ("
|
||||||
<< (total ? (sum_rot_err / total) * 180.0 / M_PI : 0.0) << " deg)\n";
|
<< (total ? (sum_rot_err / total) * 180.0 / M_PI : 0.0) << " deg)\n";
|
||||||
|
|
||||||
// 文件输出（与原逻辑一致）
|
// 文件输出（与原逻辑一致）
|
||||||
write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9);
|
write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9);
|
||||||
@ -153,41 +160,225 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) {
|
|||||||
ASSERT_LT(max_rot_err, 10 * kRotTol) << "Max rotation error too large.";
|
ASSERT_LT(max_rot_err, 10 * kRotTol) << "Max rotation error too large.";
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(SRS_IK_TEST,INTERSECT_TEST) {
|
|
||||||
// 测试 1: 有交集的区间
|
|
||||||
std::vector<std::pair<double, double>> A = {{-3.0, -1.0}, {1.0, 4.0}};
|
|
||||||
std::vector<std::pair<double, double>> B = {{-2.0, 0.5}, {2.5, 5.0}};
|
|
||||||
|
|
||||||
std::cout << "Test 1: Intersecting intervals" << std::endl;
|
TEST(SRS_IK_TEST, BEST_PSI_SLOVER_TEST) {
|
||||||
auto result1 = IkLimitAnalyzer::intersect(A, B);
|
using std::cout;
|
||||||
IkLimitAnalyzer::print_intervals(result1);
|
using std::endl;
|
||||||
// 预期输出: [-2.0, -1.0] [2.5, 4.0]
|
|
||||||
|
|
||||||
// 测试 2: 相邻但不重叠的区间
|
// std::cout << std::fixed << std::setprecision(7);
|
||||||
std::vector<std::pair<double, double>> C = {{-3.0, -1.0}, {2.0, 4.0}};
|
|
||||||
std::vector<std::pair<double, double>> D = {{-1.0, 0.0}, {1.0, 3.0}};
|
|
||||||
|
|
||||||
std::cout << "Test 2: Adjacent intervals" << std::endl;
|
SRSIkSlover slover;
|
||||||
auto result2 = IkLimitAnalyzer::intersect(C, D);
|
std::vector<IkSample> samples;
|
||||||
IkLimitAnalyzer::print_intervals(result2);
|
samples.reserve(4096);
|
||||||
// 预期输出: [2.0, 3.0]
|
|
||||||
|
|
||||||
// 测试 3: 无交集的区间
|
// 1: 当前位姿
|
||||||
std::vector<std::pair<double, double>> E = {{-5.0, -3.0}, {2.0, 4.0}};
|
std::vector<double> joint_angles(7, 0);
|
||||||
std::vector<std::pair<double, double>> F = {{5.0, 6.0}, {7.0, 8.0}};
|
joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
|
||||||
|
const auto cur_pose = slover.calc_total_transform(joint_angles);
|
||||||
|
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||||
|
slover.cal_coefficient_matrix(cur_pose, s_mat, w_mat);
|
||||||
|
auto res = IkLimitAnalyzer::estimate_psi_from_joints(s_mat, w_mat, joint_angles, slover.get_shoulder_config(),
|
||||||
|
slover.get_elbow_config(), slover.get_wrist_config());
|
||||||
|
|
||||||
std::cout << "Test 3: Non-intersecting intervals" << std::endl;
|
// 2: 目标位姿
|
||||||
auto result3 = IkLimitAnalyzer::intersect(E, F);
|
joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 1.99, 1.56};
|
||||||
IkLimitAnalyzer::print_intervals(result3);
|
const auto target_pose = slover.calc_total_transform(joint_angles);
|
||||||
// 预期输出: (无输出)
|
slover.cal_coefficient_matrix(target_pose, s_mat, w_mat);
|
||||||
|
|
||||||
// 测试 4: 一个空的区间集
|
// 3； 计算limit
|
||||||
std::vector<std::pair<double, double>> G = {};
|
auto limits = slover.calc_arm_angle_limits(s_mat, w_mat);
|
||||||
std::vector<std::pair<double, double>> H = {{1.0, 2.0}, {3.0, 4.0}};
|
|
||||||
|
|
||||||
std::cout << "Test 4: Empty intervals" << std::endl;
|
// 4； 计算best
|
||||||
auto result4 = IkLimitAnalyzer::intersect(G, H);
|
auto psi = IkLimitAnalyzer::update_psi(res.psi, limits, IkLimitAnalyzer::PsiUpdateParams());
|
||||||
IkLimitAnalyzer::print_intervals(result4);
|
|
||||||
// 预期输出: (无输出)
|
|
||||||
|
|
||||||
}
|
// 误差统计
|
||||||
|
const double kPosTol = 1e-4; // 位置容差（m）
|
||||||
|
const double kRotTol = 1e-3; // 姿态容差（rad）≈ 0.0573°
|
||||||
|
double max_pos_err = 0.0, max_rot_err = 0.0;
|
||||||
|
double sum_pos_err = 0.0, sum_rot_err = 0.0;
|
||||||
|
size_t total = 0, bad = 0;
|
||||||
|
|
||||||
|
// 便捷引用
|
||||||
|
const Eigen::Vector3d p_target = target_pose.block<3, 1>(0, 3);
|
||||||
|
const Eigen::Matrix3d R_target = target_pose.block<3, 3>(0, 0);
|
||||||
|
|
||||||
|
auto clamp = [](double x, double lo, double hi) {
|
||||||
|
return std::max(lo, std::min(hi, x));
|
||||||
|
};
|
||||||
|
|
||||||
|
auto rot_err_rad = [&](const Eigen::Matrix3d &R) -> double {
|
||||||
|
Eigen::Matrix3d dR = R_target.transpose() * R;
|
||||||
|
double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0);
|
||||||
|
return std::acos(c); // [0, pi]
|
||||||
|
};
|
||||||
|
|
||||||
|
// IK 解
|
||||||
|
auto q = slover.inverse_kinematics(target_pose, psi);
|
||||||
|
|
||||||
|
for (double q1: q) {
|
||||||
|
std::cout << q1 << " , ";
|
||||||
|
}
|
||||||
|
std::cout << std::endl;
|
||||||
|
|
||||||
|
// 用 IK 解做 FK，计算误差
|
||||||
|
const auto T_fk = slover.calc_total_transform(q);
|
||||||
|
const Eigen::Vector3d p_fk = T_fk.block<3, 1>(0, 3);
|
||||||
|
const Eigen::Matrix3d R_fk = T_fk.block<3, 3>(0, 0);
|
||||||
|
|
||||||
|
const double pos_err = (p_fk - p_target).norm();
|
||||||
|
const double rot_err = rot_err_rad(R_fk);
|
||||||
|
const double rot_err_deg = rot_err * 180.0 / M_PI;
|
||||||
|
|
||||||
|
|
||||||
|
// 表头
|
||||||
|
cout << "psi(rad), pos_err(m), rot_err(rad), rot_err(deg)\n";
|
||||||
|
cout << psi << ", " << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
TEST(SRS_IK_TEST, INTERSECT_TEST) {
|
||||||
|
// 测试 1: 有交集的区间
|
||||||
|
std::vector<std::pair<double, double> > A = {{-3.0, -1.0}, {1.0, 4.0}};
|
||||||
|
std::vector<std::pair<double, double> > B = {{-2.0, 0.5}, {2.5, 5.0}};
|
||||||
|
|
||||||
|
std::cout << "Test 1: Intersecting intervals" << std::endl;
|
||||||
|
auto result1 = IkLimitAnalyzer::intersect(A, B);
|
||||||
|
IkLimitAnalyzer::print_intervals(result1);
|
||||||
|
// 预期输出: [-2.0, -1.0] [2.5, 4.0]
|
||||||
|
|
||||||
|
// 测试 2: 相邻但不重叠的区间
|
||||||
|
std::vector<std::pair<double, double> > C = {{-3.0, -1.0}, {2.0, 4.0}};
|
||||||
|
std::vector<std::pair<double, double> > D = {{-1.0, 0.0}, {1.0, 3.0}};
|
||||||
|
|
||||||
|
std::cout << "Test 2: Adjacent intervals" << std::endl;
|
||||||
|
auto result2 = IkLimitAnalyzer::intersect(C, D);
|
||||||
|
IkLimitAnalyzer::print_intervals(result2);
|
||||||
|
// 预期输出: [2.0, 3.0]
|
||||||
|
|
||||||
|
// 测试 3: 无交集的区间
|
||||||
|
std::vector<std::pair<double, double> > E = {{-5.0, -3.0}, {2.0, 4.0}};
|
||||||
|
std::vector<std::pair<double, double> > F = {{5.0, 6.0}, {7.0, 8.0}};
|
||||||
|
|
||||||
|
std::cout << "Test 3: Non-intersecting intervals" << std::endl;
|
||||||
|
auto result3 = IkLimitAnalyzer::intersect(E, F);
|
||||||
|
IkLimitAnalyzer::print_intervals(result3);
|
||||||
|
// 预期输出: (无输出)
|
||||||
|
|
||||||
|
// 测试 4: 一个空的区间集
|
||||||
|
std::vector<std::pair<double, double> > G = {};
|
||||||
|
std::vector<std::pair<double, double> > H = {{1.0, 2.0}, {3.0, 4.0}};
|
||||||
|
|
||||||
|
std::cout << "Test 4: Empty intervals" << std::endl;
|
||||||
|
auto result4 = IkLimitAnalyzer::intersect(G, H);
|
||||||
|
IkLimitAnalyzer::print_intervals(result4);
|
||||||
|
// 预期输出: (无输出)
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
TEST(SRS_IK_TEST, MOVE_L_SLOVER_TEST) {
|
||||||
|
using std::cout;
|
||||||
|
using std::endl;
|
||||||
|
|
||||||
|
SRSIkSlover slover;
|
||||||
|
std::vector<IkSample> samples;
|
||||||
|
samples.reserve(4096);
|
||||||
|
|
||||||
|
// 1) 当前位姿（估计上一时刻 ψ 用）
|
||||||
|
std::vector<double> joint_angles(7, 0);
|
||||||
|
joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
|
||||||
|
const auto cur_pose = slover.calc_total_transform(joint_angles);
|
||||||
|
|
||||||
|
Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
|
||||||
|
slover.cal_coefficient_matrix(cur_pose, s_mat, w_mat);
|
||||||
|
|
||||||
|
auto res = IkLimitAnalyzer::estimate_psi_from_joints(
|
||||||
|
s_mat, w_mat,joint_angles ,
|
||||||
|
slover.get_shoulder_config(),
|
||||||
|
slover.get_elbow_config(),
|
||||||
|
slover.get_wrist_config()
|
||||||
|
);
|
||||||
|
samples.push_back(IkSample{res.psi, {joint_angles[0], joint_angles[1],
|
||||||
|
joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5], joint_angles[6]}});
|
||||||
|
|
||||||
|
// 2) 目标位姿（作为直线的起点）
|
||||||
|
joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 0.29, 0.59};
|
||||||
|
const auto target_pose = slover.calc_total_transform(joint_angles);
|
||||||
|
|
||||||
|
// 直线插补参数 —— 从 target_pose 出发沿 X 方向 L 米，共 N 段（N+1 个点，包含起点）
|
||||||
|
const int N = 100; // 采样点数（间隔均匀）
|
||||||
|
const double L = 0.20; // 直线长度 0.20 m
|
||||||
|
Eigen::Vector3d dir = Eigen::Vector3d::UnitX();
|
||||||
|
dir.normalize();
|
||||||
|
|
||||||
|
// 固定姿态（也可以改成对姿态做 Slerp）
|
||||||
|
const Eigen::Matrix3d R_fixed = target_pose.block<3,3>(0,0);
|
||||||
|
const Eigen::Vector3d p0 = target_pose.block<3,1>(0,3);
|
||||||
|
|
||||||
|
// 3) 初始 ψ：用估计得到的 ψ，再根据 target_pose 的可行区间做一次更新
|
||||||
|
slover.cal_coefficient_matrix(target_pose, s_mat, w_mat);
|
||||||
|
auto limits0 = slover.calc_arm_angle_limits(s_mat, w_mat);
|
||||||
|
|
||||||
|
IkLimitAnalyzer::PsiUpdateParams up;
|
||||||
|
up.K = 0.6; // 排斥强度
|
||||||
|
up.alpha = 3.0; // 靠边越强
|
||||||
|
up.step_cap = -1; // 单步最大变化
|
||||||
|
up.edge_margin = 1e-4; // 吸附到段里时的内缩
|
||||||
|
|
||||||
|
double psi_curr = IkLimitAnalyzer::update_psi(res.psi, limits0, up);
|
||||||
|
auto q = slover.inverse_kinematics(target_pose, psi_curr);
|
||||||
|
|
||||||
|
// 4) 误差评估工具
|
||||||
|
const auto clamp = [](double x, double lo, double hi) {
|
||||||
|
return std::max(lo, std::min(hi, x));
|
||||||
|
};
|
||||||
|
auto rot_err_rad = [&](const Eigen::Matrix3d &R_goal, const Eigen::Matrix3d &R_fk) -> double {
|
||||||
|
Eigen::Matrix3d dR = R_goal.transpose() * R_fk;
|
||||||
|
double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0);
|
||||||
|
return std::acos(c);
|
||||||
|
};
|
||||||
|
|
||||||
|
samples.push_back(IkSample{psi_curr, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
|
||||||
|
cout << "idx, s(0..1), psi(rad), q1..q7, pos_err(m), rot_err(rad), rot_err(deg)\n";
|
||||||
|
|
||||||
|
// 5) 直线采样 & 每点求 IK（带 ψ 更新）
|
||||||
|
for (int k = 0; k <= N; ++k) {
|
||||||
|
const double s = static_cast<double>(k) / static_cast<double>(N); // [0,1]
|
||||||
|
Eigen::Vector3d p = p0 + s * L * dir;
|
||||||
|
|
||||||
|
Eigen::Matrix4d T_goal = Eigen::Matrix4d::Identity();
|
||||||
|
T_goal.block<3,3>(0,0) = R_fixed;
|
||||||
|
T_goal.block<3,1>(0,3) = p;
|
||||||
|
|
||||||
|
// 计算当前点的 arm-angle 可行区间，并基于上一时刻 psi_curr 更新一次
|
||||||
|
slover.cal_coefficient_matrix(T_goal, s_mat, w_mat);
|
||||||
|
auto limits = slover.calc_arm_angle_limits(s_mat, w_mat);
|
||||||
|
psi_curr = IkLimitAnalyzer::update_psi(psi_curr, limits, up);
|
||||||
|
|
||||||
|
// 逆解（带 ψ）
|
||||||
|
auto q = slover.inverse_kinematics(T_goal, psi_curr);
|
||||||
|
|
||||||
|
// 容错：若 IK 失败（大小不为 7），跳过但打印提示
|
||||||
|
if (q.size() != 7) {
|
||||||
|
cout << k << ", " << s << ", " << psi_curr
|
||||||
|
<< ", IK_FAIL, , , , , , , , ,\n";
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 前向校验
|
||||||
|
const auto T_fk = slover.calc_total_transform(q);
|
||||||
|
const Eigen::Vector3d p_fk = T_fk.block<3,1>(0,3);
|
||||||
|
const Eigen::Matrix3d R_fk = T_fk.block<3,3>(0,0);
|
||||||
|
|
||||||
|
const double pos_err = (p_fk - p).norm();
|
||||||
|
const double rot_err = rot_err_rad(R_fixed, R_fk);
|
||||||
|
const double rot_err_deg = rot_err * 180.0 / M_PI;
|
||||||
|
|
||||||
|
cout << k << ", " << s << ", " << psi_curr << ", "
|
||||||
|
<< q[0] << ", " << q[1] << ", " << q[2] << ", "
|
||||||
|
<< q[3] << ", " << q[4] << ", " << q[5] << ", " << q[6] << ", "
|
||||||
|
<< pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
|
||||||
|
|
||||||
|
samples.push_back(IkSample{psi_curr, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
|
||||||
|
}
|
||||||
|
write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9);
|
||||||
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user