From 9aa74f39adc69624fa523f3e86b8bc4a82526039 Mon Sep 17 00:00:00 2001 From: lgv Date: Fri, 7 Nov 2025 14:28:41 +0800 Subject: [PATCH] feat: add best psi select --- data/ik_psi_sweep.csv | 512 +++++------------------ data/plot_joint_data.py | 73 ++++ data/{plot_data.py => plot_psi_data.py} | 0 src/utils/srs_ik/ik_hybird_optimizer.cpp | 5 + src/utils/srs_ik/ik_hybird_optimizer.h | 23 + src/utils/srs_ik/ik_limit_analyzer.cpp | 295 +++++++++++++ src/utils/srs_ik/ik_limit_analyzer.h | 44 ++ src/utils/srs_ik/srs_ik_slover.cpp | 7 +- src/utils/srs_ik/srs_ik_slover.h | 20 +- src/utils/srs_ik/srs_ik_test.cpp | 327 ++++++++++++--- 10 files changed, 821 insertions(+), 485 deletions(-) create mode 100644 data/plot_joint_data.py rename data/{plot_data.py => plot_psi_data.py} (100%) create mode 100644 src/utils/srs_ik/ik_hybird_optimizer.cpp create mode 100644 src/utils/srs_ik/ik_hybird_optimizer.h diff --git a/data/ik_psi_sweep.csv b/data/ik_psi_sweep.csv index 1bff2cfc..ac43cf17 100644 --- a/data/ik_psi_sweep.csv +++ b/data/ik_psi_sweep.csv @@ -1,410 +1,104 @@ 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.579899673,0.143167639,0.494149973,1.326455520,1.570796327,-1.548029451,1.262701768,-0.567664028 -0.580899673,0.142808658,0.494834787,1.327480021,1.570796327,-1.546509357,1.262685100,-0.565328255 -0.581899673,0.142451607,0.495519776,1.328502707,1.570796327,-1.544989477,1.262667355,-0.562992694 -0.582899673,0.142096480,0.496204938,1.329523584,1.570796327,-1.543469825,1.262648534,-0.560657360 -0.583899673,0.141743270,0.496890271,1.330542661,1.570796327,-1.541950415,1.262628637,-0.558322264 -0.584899673,0.141391968,0.497575776,1.331559944,1.570796327,-1.540431260,1.262607664,-0.555987420 -0.585899673,0.141042569,0.498261450,1.332575441,1.570796327,-1.538912372,1.262585615,-0.553652842 -0.586899673,0.140695065,0.498947294,1.333589160,1.570796327,-1.537393767,1.262562491,-0.551318543 -0.587899673,0.140349449,0.499633305,1.334601106,1.570796327,-1.535875456,1.262538291,-0.548984536 -0.588899673,0.140005714,0.500319484,1.335611288,1.570796327,-1.534357453,1.262513017,-0.546650834 -0.589899673,0.139663853,0.501005828,1.336619713,1.570796327,-1.532839772,1.262486667,-0.544317450 -0.590899673,0.139323860,0.501692338,1.337626388,1.570796327,-1.531322426,1.262459244,-0.541984397 -0.591899673,0.138985727,0.502379011,1.338631319,1.570796327,-1.529805428,1.262430746,-0.539651689 -0.592899673,0.138649447,0.503065847,1.339634514,1.570796327,-1.528288792,1.262401174,-0.537319339 -0.593899673,0.138315015,0.503752846,1.340635981,1.570796327,-1.526772530,1.262370529,-0.534987360 -0.594899673,0.137982422,0.504440005,1.341635725,1.570796327,-1.525256656,1.262338811,-0.532655764 -0.595899673,0.137651662,0.505127325,1.342633755,1.570796327,-1.523741183,1.262306020,-0.530324566 -0.596899673,0.137322729,0.505814804,1.343630076,1.570796327,-1.522226124,1.262272156,-0.527993778 -0.597899673,0.136995616,0.506502441,1.344624696,1.570796327,-1.520711493,1.262237221,-0.525663413 -0.598899673,0.136670316,0.507190236,1.345617622,1.570796327,-1.519197303,1.262201213,-0.523333484 -0.599899673,0.136346823,0.507878186,1.346608860,1.570796327,-1.517683566,1.262164135,-0.521004004 -0.600899673,0.136025129,0.508566292,1.347598417,1.570796327,-1.516170297,1.262125986,-0.518674987 -0.601899673,0.135705229,0.509254553,1.348586301,1.570796327,-1.514657507,1.262086766,-0.516346444 -0.602899673,0.135387116,0.509942967,1.349572517,1.570796327,-1.513145211,1.262046476,-0.514018390 -0.603899673,0.135070783,0.510631533,1.350557073,1.570796327,-1.511633421,1.262005117,-0.511690837 -0.604899673,0.134756224,0.511320252,1.351539975,1.570796327,-1.510122151,1.261962689,-0.509363798 -0.605899673,0.134443432,0.512009120,1.352521230,1.570796327,-1.508611412,1.261919192,-0.507037286 -0.606899673,0.134132402,0.512698139,1.353500845,1.570796327,-1.507101220,1.261874627,-0.504711314 -0.607899673,0.133823126,0.513387307,1.354478825,1.570796327,-1.505591585,1.261828995,-0.502385894 -0.608899673,0.133515598,0.514076623,1.355455179,1.570796327,-1.504082522,1.261782295,-0.500061040 -0.609899673,0.133209813,0.514766086,1.356429911,1.570796327,-1.502574043,1.261734528,-0.497736764 -0.610899673,0.132905763,0.515455695,1.357403029,1.570796327,-1.501066161,1.261685696,-0.495413079 -0.611899673,0.132603443,0.516145449,1.358374539,1.570796327,-1.499558890,1.261635798,-0.493089998 -0.612899673,0.132302846,0.516835348,1.359344448,1.570796327,-1.498052241,1.261584835,-0.490767533 -0.613899673,0.132003966,0.517525390,1.360312762,1.570796327,-1.496546228,1.261532807,-0.488445698 -0.614899673,0.131706797,0.518215575,1.361279487,1.570796327,-1.495040863,1.261479716,-0.486124504 -0.615899673,0.131411333,0.518905902,1.362244630,1.570796327,-1.493536160,1.261425561,-0.483803965 -0.616899673,0.131117567,0.519596369,1.363208197,1.570796327,-1.492032132,1.261370344,-0.481484094 -0.617899673,0.130825494,0.520286977,1.364170195,1.570796327,-1.490528789,1.261314064,-0.479164901 -0.618899673,0.130535108,0.520977724,1.365130629,1.570796327,-1.489026147,1.261256723,-0.476846402 -0.619899673,0.130246402,0.521668609,1.366089506,1.570796327,-1.487524217,1.261198322,-0.474528607 -0.620899673,0.129959370,0.522359631,1.367046832,1.570796327,-1.486023011,1.261138860,-0.472211530 -0.621899673,0.129674007,0.523050791,1.368002613,1.570796327,-1.484522544,1.261078338,-0.469895182 -0.622899673,0.129390307,0.523742086,1.368956856,1.570796327,-1.483022826,1.261016758,-0.467579577 -0.623899673,0.129108264,0.524433515,1.369909567,1.570796327,-1.481523871,1.260954119,-0.465264726 -0.624899673,0.128827871,0.525125079,1.370860751,1.570796327,-1.480025691,1.260890423,-0.462950643 -0.625899673,0.128549123,0.525816776,1.371810414,1.570796327,-1.478528298,1.260825670,-0.460637339 -0.626899673,0.128272014,0.526508606,1.372758564,1.570796327,-1.477031706,1.260759861,-0.458324826 -0.627899673,0.127996539,0.527200566,1.373705206,1.570796327,-1.475535926,1.260692996,-0.456013118 -0.628899673,0.127722691,0.527892658,1.374650345,1.570796327,-1.474040972,1.260625077,-0.453702227 -0.629899673,0.127450465,0.528584880,1.375593988,1.570796327,-1.472546854,1.260556103,-0.451392163 -0.630899673,0.127179855,0.529277230,1.376536141,1.570796327,-1.471053586,1.260486077,-0.449082941 -0.631899673,0.126910855,0.529969709,1.377476809,1.570796327,-1.469561180,1.260414998,-0.446774571 -0.632899673,0.126643460,0.530662315,1.378415999,1.570796327,-1.468069648,1.260342867,-0.444467067 -0.633899673,0.126377664,0.531355048,1.379353716,1.570796327,-1.466579003,1.260269686,-0.442160439 -0.634899673,0.126113462,0.532047907,1.380289967,1.570796327,-1.465089256,1.260195454,-0.439854701 -0.635899673,0.125850848,0.532740891,1.381224756,1.570796327,-1.463600420,1.260120173,-0.437549864 -0.636899673,0.125589815,0.533433999,1.382158091,1.570796327,-1.462112507,1.260043844,-0.435245940 -0.637899673,0.125330360,0.534127230,1.383089976,1.570796327,-1.460625528,1.259966467,-0.432942941 -0.638899673,0.125072476,0.534820584,1.384020417,1.570796327,-1.459139497,1.259888043,-0.430640879 -0.639899673,0.124816158,0.535514060,1.384949420,1.570796327,-1.457654424,1.259808573,-0.428339765 -0.640899673,0.124561400,0.536207656,1.385876991,1.570796327,-1.456170322,1.259728058,-0.426039613 -0.641899673,0.124308197,0.536901374,1.386803135,1.570796327,-1.454687203,1.259646498,-0.423740433 -0.642899673,0.124056543,0.537595210,1.387727858,1.570796327,-1.453205079,1.259563896,-0.421442237 -0.643899673,0.123806434,0.538289165,1.388651165,1.570796327,-1.451723962,1.259480250,-0.419145037 -0.644899673,0.123557863,0.538983239,1.389573063,1.570796327,-1.450243862,1.259395563,-0.416848845 -0.645899673,0.123310826,0.539677429,1.390493556,1.570796327,-1.448764793,1.259309836,-0.414553672 -0.646899673,0.123065317,0.540371736,1.391412651,1.570796327,-1.447286766,1.259223068,-0.412259529 -0.647899673,0.122821331,0.541066158,1.392330352,1.570796327,-1.445809792,1.259135262,-0.409966429 -0.648899673,0.122578862,0.541760696,1.393246666,1.570796327,-1.444333883,1.259046418,-0.407674383 -0.649899673,0.122337906,0.542455347,1.394161597,1.570796327,-1.442859051,1.258956537,-0.405383402 -0.650899673,0.122098457,0.543150112,1.395075151,1.570796327,-1.441385308,1.258865620,-0.403093497 -0.651899673,0.121860510,0.543844989,1.395987334,1.570796327,-1.439912664,1.258773667,-0.400804681 -0.652899673,0.121624059,0.544539978,1.396898151,1.570796327,-1.438441131,1.258680681,-0.398516964 -0.653899673,0.121389101,0.545235079,1.397807606,1.570796327,-1.436970721,1.258586662,-0.396230358 -0.654899673,0.121155628,0.545930290,1.398715707,1.570796327,-1.435501446,1.258491610,-0.393944874 -0.655899673,0.120923638,0.546625610,1.399622457,1.570796327,-1.434033315,1.258395527,-0.391660523 -0.656899673,0.120693123,0.547321039,1.400527862,1.570796327,-1.432566341,1.258298415,-0.389377316 -0.657899673,0.120464080,0.548016577,1.401431928,1.570796327,-1.431100536,1.258200273,-0.387095265 -0.658899673,0.120236504,0.548712222,1.402334659,1.570796327,-1.429635909,1.258101103,-0.384814380 -0.659899673,0.120010388,0.549407974,1.403236062,1.570796327,-1.428172473,1.258000906,-0.382534672 -0.660899673,0.119785729,0.550103832,1.404136140,1.570796327,-1.426710238,1.257899683,-0.380256154 -0.661899673,0.119562522,0.550799795,1.405034900,1.570796327,-1.425249216,1.257797436,-0.377978834 -0.662899673,0.119340761,0.551495862,1.405932346,1.570796327,-1.423789417,1.257694164,-0.375702725 -0.663899673,0.119120441,0.552192034,1.406828483,1.570796327,-1.422330853,1.257589870,-0.373427837 -0.664899673,0.118901558,0.552888308,1.407723318,1.570796327,-1.420873534,1.257484555,-0.371154180 -0.665899673,0.118684107,0.553584685,1.408616853,1.570796327,-1.419417471,1.257378218,-0.368881767 -0.666899673,0.118468083,0.554281164,1.409509096,1.570796327,-1.417962675,1.257270863,-0.366610606 -0.667899673,0.118253482,0.554977744,1.410400051,1.570796327,-1.416509158,1.257162489,-0.364340710 -0.668899673,0.118040297,0.555674424,1.411289722,1.570796327,-1.415056929,1.257053098,-0.362072088 -0.669899673,0.117828525,0.556371204,1.412178115,1.570796327,-1.413605999,1.256942690,-0.359804751 -0.670899673,0.117618162,0.557068083,1.413065235,1.570796327,-1.412156379,1.256831268,-0.357538710 -0.671899673,0.117409201,0.557765060,1.413951087,1.570796327,-1.410708080,1.256718833,-0.355273975 -0.672899673,0.117201638,0.558462135,1.414835675,1.570796327,-1.409261111,1.256605384,-0.353010556 -0.673899673,0.116995470,0.559159306,1.415719005,1.570796327,-1.407815485,1.256490925,-0.350748465 -0.674899673,0.116790690,0.559856574,1.416601081,1.570796327,-1.406371210,1.256375455,-0.348487710 -0.675899673,0.116587295,0.560553937,1.417481909,1.570796327,-1.404928297,1.256258976,-0.346228302 -0.676899673,0.116385280,0.561251395,1.418361493,1.570796327,-1.403486757,1.256141489,-0.343970252 -0.677899673,0.116184640,0.561948948,1.419239838,1.570796327,-1.402046601,1.256022995,-0.341713570 -0.678899673,0.115985371,0.562646594,1.420116948,1.570796327,-1.400607837,1.255903497,-0.339458266 -0.679899673,0.115787468,0.563344332,1.420992830,1.570796327,-1.399170476,1.255782994,-0.337204349 -0.680899673,0.115590927,0.564042163,1.421867486,1.570796327,-1.397734530,1.255661488,-0.334951830 -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 +-0.000000048,0.002038980,1.340620000,0.000000000,0.522261000,0.000000000,-0.000210733,-0.094236400 +0.556521960,0.152127806,0.478192710,1.301969310,1.570796327,1.347703459,0.291064331,0.230347615 +0.771261416,0.102931374,0.627402558,1.496396670,1.570796327,1.156280065,0.243329923,0.080698976 +0.888086437,0.097502506,0.710527703,1.578136435,1.566414799,1.057630893,0.204332305,0.012141046 +0.955163562,0.098163871,0.758400761,1.620123715,1.561968928,1.003011363,0.178048729,-0.021862524 +0.993973315,0.099039484,0.786243811,1.642081749,1.557458620,0.972273899,0.161127367,-0.037608954 +1.016138882,0.099188037,0.802349007,1.652836912,1.552883773,0.955182732,0.150393132,-0.043134232 +1.028376055,0.098680005,0.811490304,1.657044755,1.548244277,0.946082745,0.143560899,-0.042763131 +1.034654767,0.097742852,0.816476052,1.657338477,1.543540016,0.941724228,0.139129811,-0.038932566 +1.037340138,0.096569737,0.818972515,1.655254344,1.538770861,0.940203623,0.136155821,-0.033050626 +1.037844614,0.095293395,0.819971733,1.651701648,1.533936676,0.940395189,0.134058820,-0.025942410 +1.037011999,0.093997863,0.820065093,1.647222208,1.529037317,0.941630081,0.132487097,-0.018095168 +1.035346032,0.092734315,0.819605210,1.642139686,1.524072627,0.943510217,0.131229133,-0.009797785 +1.033146936,0.091533355,0.818802211,1.636647318,1.519042442,0.945798773,0.130158390,-0.001221774 +1.030593077,0.090413216,0.817781131,1.630860032,1.513946584,0.948355264,0.129199456,0.007531135 +1.027789823,0.089384875,0.816616155,1.624845592,1.508784868,0.951096878,0.128307556,0.016400826 +1.024798765,0.088455137,0.815351066,1.618643268,1.503557094,0.953975399,0.127456222,0.025352060 +1.021655201,0.087628445,0.814011443,1.612275049,1.498263053,0.956963419,0.126629922,0.034364513 +1.018378581,0.086907950,0.812611949,1.605752380,1.492902522,0.960046092,0.125819652,0.043426829 +1.014978750,0.086296115,0.811160674,1.599080207,1.487475267,0.963216206,0.125020313,0.052533074 +1.011459667,0.085795082,0.809661725,1.592259399,1.481981040,0.966471247,0.124229149,0.061680634 +1.007821617,0.085406871,0.808116761,1.585288189,1.476419579,0.969811645,0.123444822,0.070868952 +1.004062519,0.085133511,0.806525907,1.578163029,1.470790610,0.973239752,0.122666862,0.080098793 +1.000178682,0.084977117,0.804888290,1.570879075,1.465093842,0.976759230,0.121895345,0.089371810 +0.996165246,0.084939944,0.803202344,1.563430461,1.459328970,0.980374719,0.121130705,0.098690296 +0.992016408,0.085024426,0.801465983,1.555810431,1.453495675,0.984091652,0.120373623,0.108057049 +0.987725533,0.085233217,0.799676691,1.548011384,1.447593619,0.987916174,0.119624973,0.117475311 +0.983285184,0.085569222,0.797831557,1.540024862,1.441622451,0.991855129,0.118885793,0.126948746 +0.978687099,0.086035639,0.795927274,1.531841493,1.435581798,0.995916078,0.118157269,0.136481449 +0.973922120,0.086636002,0.793960118,1.523450901,1.429471273,1.000107368,0.117440743,0.146077982 +0.968980104,0.087374231,0.791925907,1.514841585,1.423290469,1.004438211,0.116737724,0.155743419 +0.963849790,0.088254693,0.789819940,1.506000764,1.417038960,1.008918807,0.116049904,0.165483418 +0.958518636,0.089282271,0.787636922,1.496914187,1.410716299,1.013560480,0.115379184,0.175304304 +0.952972617,0.090462458,0.785370868,1.487565896,1.404322019,1.018375862,0.114727709,0.185213177 +0.947195982,0.091801455,0.783014983,1.477937944,1.397855632,1.023379109,0.114097905,0.195218041 +0.941170943,0.093306311,0.780561518,1.468010035,1.391316626,1.028586170,0.113492532,0.205327972 +0.934877298,0.094985081,0.778001581,1.457759082,1.384704468,1.034015127,0.112914748,0.215553318 +0.928291946,0.096847040,0.775324904,1.447158654,1.378018599,1.039686614,0.112368188,0.225905960 +0.921388283,0.098902942,0.772519552,1.436178264,1.371258435,1.045624359,0.111857062,0.236399637 +0.914135413,0.101165367,0.769571531,1.424782455,1.364423366,1.051855874,0.111386292,0.247050377 +0.906497117,0.103649173,0.766464284,1.412929601,1.357512757,1.058413369,0.110961679,0.257877053 +0.898430484,0.106372101,0.763178018,1.400570304,1.350525941,1.065334958,0.110590132,0.268902135 +0.889884036,0.109355604,0.759688780,1.387645214,1.343462225,1.072666314,0.110279980,0.280152709 +0.880795125,0.112626005,0.755967161,1.374081980,1.336320882,1.080462969,0.110041405,0.291661910 +0.871086195,0.116216166,0.751976445,1.359790886,1.329101156,1.088793604,0.109887062,0.303470980 +0.860659244,0.120167969,0.747669842,1.344658367,1.321802257,1.097744946,0.109833006,0.315632328 +0.849387272,0.124536152,0.742986207,1.328537001,1.314423358,1.107429304,0.109900118,0.328214268 +0.837100390,0.129394552,0.737843062,1.311229259,1.306963599,1.117996822,0.110116435,0.341308739 +0.823561762,0.134846923,0.732124450,1.292459357,1.299422079,1.129656687,0.110521197,0.355044715 +0.808422172,0.141047319,0.725657901,1.271820173,1.291797860,1.142717173,0.111172547,0.369613603 +0.791123135,0.148243395,0.718165031,1.248660255,1.284089961,1.157670987,0.112164100,0.385323525 +0.770647516,0.156886915,0.709133577,1.221794006,1.276297358,1.175414791,0.113668148,0.402739386 +0.744611022,0.168030005,0.697347187,1.188452194,1.268418982,1.198048128,0.116096349,0.423194067 +0.637032782,0.214836584,0.643082607,1.060955797,1.260453716,1.292372114,0.132983463,0.489751204 +0.574933616,0.250105672,0.615452371,0.978234894,1.252400393,1.347436583,0.138454646,0.530947518 +0.539590877,0.273860359,0.602337244,0.926155015,1.244257794,1.378928777,0.139036854,0.556650107 +0.520199795,0.288907482,0.597319871,0.894082499,1.236024645,1.396237711,0.137352783,0.572992332 +0.510376439,0.298087117,0.596923614,0.874644737,1.227699615,1.405002708,0.134526008,0.583706286 +0.506318609,0.303535716,0.599218948,0.863032726,1.219281310,1.408608023,0.131072654,0.591039128 +0.505756372,0.306684673,0.603111992,0.856227231,1.210768276,1.409081276,0.127246986,0.596336295 +0.507334409,0.308440165,0.607972681,0.852376019,1.202158989,1.407635104,0.123182132,0.600400663 +0.510244464,0.309358677,0.613434335,0.850358903,1.193451854,1.404993161,0.118951022,0.603710250 +0.514005804,0.309776105,0.619282387,0.849504927,1.184645201,1.401585936,0.114594682,0.606549100 +0.518333935,0.309893131,0.625391162,0.849415421,1.175737284,1.397668163,0.110136351,0.609085599 +0.523062081,0.309828855,0.631687352,0.849855669,1.166726271,1.393389138,0.105588962,0.611419214 +0.528094236,0.309653596,0.638128685,0.850689406,1.157610241,1.388834772,0.100959309,0.613608326 +0.533377116,0.309408629,0.644691392,0.851839519,1.148387181,1.384052732,0.096250437,0.615686797 +0.538883420,0.309117971,0.651362822,0.853264591,1.139054979,1.379067420,0.091463047,0.617673798 +0.544601886,0.308795367,0.658137078,0.854944960,1.129611417,1.373888888,0.086596320,0.619579648 +0.550531398,0.308448411,0.665012457,0.856874494,1.120054164,1.368518104,0.081648403,0.621409255 +0.556677562,0.308080974,0.671989950,0.859055780,1.110380773,1.362950021,0.076616687,0.623164139 +0.563050749,0.307694607,0.679072396,0.861497372,1.100588667,1.357175319,0.071497952,0.624843603 +0.569665063,0.307289357,0.686264006,0.864212284,1.090675137,1.351181335,0.066288440,0.626445390 +0.576537873,0.306864210,0.693570141,0.867217268,1.080637328,1.344952472,0.060983861,0.627966025 +0.583689740,0.306417315,0.700997243,0.870532584,1.070472230,1.338470272,0.055579362,0.629400956 +0.591144612,0.305946073,0.708552871,0.874182131,1.060176671,1.331713236,0.050069468,0.630744563 +0.598930270,0.305447119,0.716245832,0.878193853,1.049747299,1.324656431,0.044447975,0.631990064 +0.607079000,0.304916236,0.724086399,0.882600426,1.039180573,1.317270885,0.038707816,0.633129321 +0.615628554,0.304348188,0.732086615,0.887440237,1.028472749,1.309522742,0.032840873,0.634152555 +0.624623451,0.303736469,0.740260735,0.892758742,1.017619861,1.301372098,0.026837723,0.635047936 +0.634116783,0.303072945,0.748625825,0.898610354,1.006617707,1.292771403,0.020687289,0.635800997 +0.644172727,0.302347335,0.757202622,0.905061077,0.995461828,1.283663231,0.014376362,0.636393815 +0.654870154,0.301546450,0.766016762,0.912192288,0.984147488,1.273977087,0.007888899,0.636803812 +0.666307957,0.300653050,0.775100628,0.920106350,0.972669650,1.263624695,0.001204997,0.637001953 +0.678613267,0.299644050,0.784496219,0.928935253,0.961022948,1.252492730,-0.005700698,0.636949951 +0.691954782,0.298487563,0.794259834,0.938854664,0.949201660,1.240431065,-0.012861580,0.636595655 +0.706565844,0.297137684,0.804470277,0.950108280,0.937199677,1.227232411,-0.020323575,0.635864986 +0.722788111,0.295524457,0.815244499,0.963053989,0.925010463,1.212593839,-0.028153656,0.634646465 +0.741165210,0.293531924,0.826771435,0.978263035,0.912627013,1.196034335,-0.036458117,0.632757639 +0.762686891,0.290939626,0.839400906,0.996779156,0.900041812,1.176680004,-0.045430129,0.629856366 +0.789708560,0.287196010,0.853980910,1.021097833,0.887246777,1.152455561,-0.055527958,0.625101724 +0.837468810,0.278494864,0.876119896,1.067475321,0.874233200,1.109963552,-0.069695803,0.612835819 +0.866728664,0.275899768,0.891364369,1.093244121,0.860991679,1.083726355,-0.080812562,0.608306115 +0.884955480,0.276549441,0.902432818,1.107041943,0.847512040,1.067136594,-0.089940632,0.608256886 +0.896594602,0.279027152,0.911023238,1.113793159,0.833783247,1.056284776,-0.097855475,0.610838734 +0.904301301,0.282588985,0.918169223,1.116366559,0.819793300,1.048842654,-0.105068975,0.614987391 +0.909663781,0.286828624,0.924492301,1.116453168,0.805529113,1.043418780,-0.111904440,0.620085967 +0.913632713,0.291519190,0.930364254,1.115056164,0.790976372,1.039179568,-0.118562216,0.625776036 +0.916777563,0.296533258,0.936007401,1.112772667,0.776119374,1.035624457,-0.125166014,0.631849885 +0.919439819,0.301800196,0.941555314,1.109958612,0.760940832,1.032451887,-0.131793012,0.638188256 +0.921824652,0.307282553,0.947089206,1.106826149,0.745421641,1.029479207,-0.138492662,0.644724121 +0.924055763,0.312962691,0.952659616,1.103501520,0.729540608,1.026594731,-0.145298210,0.651421508 +0.926208216,0.318835136,0.958299305,1.100059525,0.713274113,1.023729006,-0.152233712,0.658263129 diff --git a/data/plot_joint_data.py b/data/plot_joint_data.py new file mode 100644 index 00000000..1c8a7ba7 --- /dev/null +++ b/data/plot_joint_data.py @@ -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() diff --git a/data/plot_data.py b/data/plot_psi_data.py similarity index 100% rename from data/plot_data.py rename to data/plot_psi_data.py diff --git a/src/utils/srs_ik/ik_hybird_optimizer.cpp b/src/utils/srs_ik/ik_hybird_optimizer.cpp new file mode 100644 index 00000000..69435106 --- /dev/null +++ b/src/utils/srs_ik/ik_hybird_optimizer.cpp @@ -0,0 +1,5 @@ +// +// Created by lgv on 11/7/25. +// + +#include "ik_hybird_optimizer.h" diff --git a/src/utils/srs_ik/ik_hybird_optimizer.h b/src/utils/srs_ik/ik_hybird_optimizer.h new file mode 100644 index 00000000..124c16ec --- /dev/null +++ b/src/utils/srs_ik/ik_hybird_optimizer.h @@ -0,0 +1,23 @@ +// +// Created by lgv on 11/7/25. +// + +#ifndef CMVR_ES_HYBIRD_OPTIMIZER_H +#define CMVR_ES_HYBIRD_OPTIMIZER_H + + +#include + +namespace cmvr { + namespace utils { + class IkHybirdOptimizer { + + public: + private: + + }; + } +} + + +#endif //CMVR_ES_HYBIRD_OPTIMIZER_H \ No newline at end of file diff --git a/src/utils/srs_ik/ik_limit_analyzer.cpp b/src/utils/srs_ik/ik_limit_analyzer.cpp index 1f9d2420..5e9a5d52 100644 --- a/src/utils/srs_ik/ik_limit_analyzer.cpp +++ b/src/utils/srs_ik/ik_limit_analyzer.cpp @@ -447,3 +447,298 @@ IkLimitAnalyzer::union_intervals(const std::vector > & out.push_back({L, R}); 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(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& 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 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::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>& 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= 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::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); +} diff --git a/src/utils/srs_ik/ik_limit_analyzer.h b/src/utils/srs_ik/ik_limit_analyzer.h index 2de5a7bc..1eeaf429 100644 --- a/src/utils/srs_ik/ik_limit_analyzer.h +++ b/src/utils/srs_ik/ik_limit_analyzer.h @@ -8,11 +8,13 @@ #include #include #include +#include namespace cmvr { namespace utils { class IkLimitAnalyzer { public: + // Tan 型关节：给定 (an,ad,bn,bd,cn,cd) 与关节极限 // 返回 psi 的允许区间边界： [L1,R1,L2,R2,...] static std::vector > @@ -48,6 +50,45 @@ namespace cmvr { 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> 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& theta_c, // 当前 7 关节角 + int s_conf, int e_conf, int w_conf, + double prefer_psi = std::numeric_limits::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>& psi_all, // merged ∪-intervals, each L > union_intervals(const std::vector > &in); + + + }; }; } diff --git a/src/utils/srs_ik/srs_ik_slover.cpp b/src/utils/srs_ik/srs_ik_slover.cpp index 4d1c35ef..78a61379 100644 --- a/src/utils/srs_ik/srs_ik_slover.cpp +++ b/src/utils/srs_ik/srs_ik_slover.cpp @@ -31,7 +31,7 @@ SRSIkSlover::SRSIkSlover() { joints_limits_ = { {-0.26, 1.57}, - {-0.78, 0.78}, + {-0.78, 1.57}, {-M_PI, M_PI}, {0, 2.05}, {-3.00, 3.0}, @@ -170,10 +170,6 @@ std::vector SRSIkSlover::inverse_kinematics(const Eigen::MatrixXd &pose, joints[5] = normalize_angle(theta_y - M_PI / 2); joints[6] = normalize_angle(psi_z); - for (double q1: joints) { - std::cout << q1 << " , "; - } - std::cout << std::endl; return joints; @@ -343,7 +339,6 @@ std::vector > SRSIkSlover::calc_arm_angle_limits( const Eigen::Matrix3d Cw = w_mat.block<3, 3>(0, 6); auto s = static_cast(shoulder_config_); - auto e = static_cast(elbow_config_); auto w = static_cast(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), diff --git a/src/utils/srs_ik/srs_ik_slover.h b/src/utils/srs_ik/srs_ik_slover.h index ca671027..1e6f1379 100644 --- a/src/utils/srs_ik/srs_ik_slover.h +++ b/src/utils/srs_ik/srs_ik_slover.h @@ -43,6 +43,24 @@ namespace utils { + int get_shoulder_config() { + return static_cast(shoulder_config_); + } + + int get_elbow_config() { + return static_cast(elbow_config_); + } + + int get_wrist_config() { + return static_cast(wrist_config_); + } + + std::vector> get_joints_limits() { + return joints_limits_; + } + + + private: @@ -63,8 +81,6 @@ namespace utils { - - // 计算参考平面相对于基坐标系的旋转矩阵 Eigen::Matrix3d reference_plane(const Eigen::Vector3d& S, const Eigen::Vector3d& W); diff --git a/src/utils/srs_ik/srs_ik_test.cpp b/src/utils/srs_ik/srs_ik_test.cpp index 6cde6d91..9e48b6fb 100644 --- a/src/utils/srs_ik/srs_ik_test.cpp +++ b/src/utils/srs_ik/srs_ik_test.cpp @@ -15,33 +15,31 @@ using namespace manif; using namespace cmvr::utils; struct IkSample { - double psi; - std::array q; // q1..q7 + double psi; + std::array q; // q1..q7 }; - -bool write_ik_samples_csv(const std::string& filepath, - const std::vector& samples, +bool write_ik_samples_csv(const std::string &filepath, + const std::vector &samples, bool write_header, - int precision) -{ - std::ofstream ofs(filepath, std::ios::out | std::ios::trunc); - if (!ofs.is_open()) return false; + int precision) { + std::ofstream ofs(filepath, std::ios::out | std::ios::trunc); + if (!ofs.is_open()) return false; - // 固定小数点（避免本地化成逗号） - ofs.imbue(std::locale::classic()); - ofs << std::fixed << std::setprecision(precision); + // 固定小数点（避免本地化成逗号） + ofs.imbue(std::locale::classic()); + ofs << std::fixed << std::setprecision(precision); - if (write_header) { - ofs << "psi,q1,q2,q3,q4,q5,q6,q7\n"; - } - for (const auto& s : samples) { - ofs << s.psi; - for (int i = 0; i < 7; ++i) ofs << ',' << s.q[i]; - ofs << '\n'; - } - return true; + if (write_header) { + ofs << "psi,q1,q2,q3,q4,q5,q6,q7\n"; + } + for (const auto &s: samples) { + ofs << s.psi; + for (int i = 0; i < 7; ++i) ofs << ',' << s.q[i]; + ofs << '\n'; + } + return true; } TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) { @@ -55,7 +53,9 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) { samples.reserve(4096); std::vector 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) 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); 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); // 误差统计 - const double kPosTol = 1e-4; // 位置容差（m） - const double kRotTol = 1e-3; // 姿态容差（rad）≈ 0.0573° + 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); + 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 { + 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] @@ -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"; // ψ 扫描 - for (const auto& limit : limits) { + for (const auto &limit: limits) { const double psi_lo = limit.first; const double psi_hi = limit.second; @@ -99,7 +106,7 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) { // IK 解 auto q = slover.inverse_kinematics(target_pose, psi); if (q.size() != 7 || std::any_of(q.begin(), q.end(), - [](double v){ return !std::isfinite(v); })) { + [](double v) { return !std::isfinite(v); })) { ++bad; ++total; cout << psi << ", nan, nan, nan\n"; @@ -108,8 +115,8 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) { // 用 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 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); @@ -137,13 +144,13 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) { // 摘要打印 cout << "\nSummary:\n" - << " total=" << total - << " bad=" << bad - << " pos_err_max=" << max_pos_err << " m" - << " 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" - << " 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=" << total + << " bad=" << bad + << " pos_err_max=" << max_pos_err << " m" + << " 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" + << " rot_err_mean=" << (total ? (sum_rot_err / total) : 0.0) << " rad (" + << (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); @@ -153,41 +160,225 @@ TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) { ASSERT_LT(max_rot_err, 10 * kRotTol) << "Max rotation error too large."; } -TEST(SRS_IK_TEST,INTERSECT_TEST) { - // 测试 1: 有交集的区间 - std::vector> A = {{-3.0, -1.0}, {1.0, 4.0}}; - std::vector> 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] +TEST(SRS_IK_TEST, BEST_PSI_SLOVER_TEST) { + using std::cout; + using std::endl; - // 测试 2: 相邻但不重叠的区间 - std::vector> C = {{-3.0, -1.0}, {2.0, 4.0}}; - std::vector> D = {{-1.0, 0.0}, {1.0, 3.0}}; + // std::cout << std::fixed << std::setprecision(7); - std::cout << "Test 2: Adjacent intervals" << std::endl; - auto result2 = IkLimitAnalyzer::intersect(C, D); - IkLimitAnalyzer::print_intervals(result2); - // 预期输出: [2.0, 3.0] + SRSIkSlover slover; + std::vector samples; + samples.reserve(4096); - // 测试 3: 无交集的区间 - std::vector> E = {{-5.0, -3.0}, {2.0, 4.0}}; - std::vector> F = {{5.0, 6.0}, {7.0, 8.0}}; + // 1: 当前位姿 + std::vector 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()); - std::cout << "Test 3: Non-intersecting intervals" << std::endl; - auto result3 = IkLimitAnalyzer::intersect(E, F); - IkLimitAnalyzer::print_intervals(result3); - // 预期输出: (无输出) + // 2: 目标位姿 + joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 1.99, 1.56}; + const auto target_pose = slover.calc_total_transform(joint_angles); + slover.cal_coefficient_matrix(target_pose, s_mat, w_mat); - // 测试 4: 一个空的区间集 - std::vector> G = {}; - std::vector> H = {{1.0, 2.0}, {3.0, 4.0}}; + // 3； 计算limit + auto limits = slover.calc_arm_angle_limits(s_mat, w_mat); - std::cout << "Test 4: Empty intervals" << std::endl; - auto result4 = IkLimitAnalyzer::intersect(G, H); - IkLimitAnalyzer::print_intervals(result4); - // 预期输出: (无输出) + // 4； 计算best + auto psi = IkLimitAnalyzer::update_psi(res.psi, limits, IkLimitAnalyzer::PsiUpdateParams()); -} \ No newline at end of file + // 误差统计 + 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 > A = {{-3.0, -1.0}, {1.0, 4.0}}; + std::vector > 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 > C = {{-3.0, -1.0}, {2.0, 4.0}}; + std::vector > 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 > E = {{-5.0, -3.0}, {2.0, 4.0}}; + std::vector > 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 > G = {}; + std::vector > 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 samples; + samples.reserve(4096); + + // 1) 当前位姿（估计上一时刻 ψ 用） + std::vector 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(k) / static_cast(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); +}