Revert "feat:add joint waypoints plan"

This reverts commit 058ea0f505.
2025-12-18 16:08:08 +08:00 · 2025-12-18 16:08:08 +08:00 · 60e1b565a4
commit 60e1b565a4
parent 058ea0f505
17 changed files with 645 additions and 3298 deletions
--- a/config/cabin_robot.xml
+++ b/config/cabin_robot.xml
@ -20,9 +20,9 @@
            </Camera>
            <DexHand>
-<!--                <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000">-->
+                <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000">
-<!--                    <Freedom order="01" default_force="500" default_speed="500" />-->
+                    <Freedom order="01" default_force="500" default_speed="500" />
-<!--                </RH56DFTP>-->
+                </RH56DFTP>
 <!--                <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">-->
 <!--                    <Freedom order="01" default_force="500" default_speed="500" />-->
 <!--                </RH56DFTP>-->
@ -40,7 +40,7 @@
                          bufferSize="50"
                          verbose="false">
                    <CanManger id="" devId="">
-                        <LeftArmCan id = " " devId = " " channelId ="0" enable="false" toolFrame="L_FINGER_TIP">
+                        <LeftArmCan id = " " devId = " " channelId ="0" enable="true" toolFrame="L_FINGER_TIP">
                            <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                            <Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                            <Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
@ -49,14 +49,14 @@
                            <Motor id="21" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                            <Motor id="22" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                        </LeftArmCan>
-                        <RightArmCan id = " " devId = " " channelId ="1"  enable="true" toolFrame="R_FINGER_TIP">
+                        <RightArmCan id = " " devId = " " channelId ="1"  enable="false" toolFrame="R_FINGER_TIP">
-<!--                            <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
+                            <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
 <!--                            <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
-<!--                            <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
+                            <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-<!--                            <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
+                            <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
-<!--                            <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>-->
+                            <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
                            <Motor id="28" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
-<!--                            <Motor id="1" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>-->
+                            <Motor id="1" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
                        </RightArmCan>
                        <HeadCan id = " " devId = " " channelId ="2" enable="false">
                            <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
--- a/config/robot_description/hc_description/dual_arm.urdf
+++ b/config/robot_description/hc_description/dual_arm.urdf
@ -1,51 +1,51 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot name="dual_arm">
-<!--   <mujoco>-->
+   <mujoco>
-<!--    <compiler-->
+    <compiler
-<!--    meshdir="meshes"-->
+    meshdir="meshes"
-<!--    balanceinertia="true"-->
+    balanceinertia="true"
-<!--    discardvisual="false" />-->
+    discardvisual="false" />
-<!--   </mujoco>-->
+   </mujoco>
-<!--    <link name="base_link">-->
+    <link name="base_link">
-<!--      <visual>-->
+      <visual>
-<!--        <origin xyz="0 0 0.6" rpy="0 0 0"/>-->
+        <origin xyz="0 0 0.6" rpy="0 0 0"/>
-<!--        <geometry>-->
+        <geometry>
-<!--          <cylinder radius="0.05" length="1.2"/>-->
+          <cylinder radius="0.05" length="1.2"/>
-<!--        </geometry>-->
+        </geometry>
-<!--        <material name="gray">-->
+        <material name="gray">
-<!--          <color rgba="0.5 0.5 0.5 1.0"/>-->
+          <color rgba="0.5 0.5 0.5 1.0"/>
-<!--        </material>-->
+        </material>
-<!--      </visual>-->
+      </visual>
-<!--      <collision>-->
+      <collision>
-<!--        <origin xyz="0 0 0.6" rpy="0 0 0"/>-->
+        <origin xyz="0 0 0.6" rpy="0 0 0"/>
-<!--        <geometry>-->
+        <geometry>
-<!--          <cylinder radius="0.05" length="1.2"/>-->
+          <cylinder radius="0.05" length="1.2"/>
-<!--        </geometry>-->
+        </geometry>
-<!--      </collision>-->
+      </collision>
-<!--        <inertial>-->
+        <inertial>
-<!--          <origin xyz="0 0 0" rpy="0 0 0"/>-->
+          <origin xyz="0 0 0" rpy="0 0 0"/>
-<!--          <mass value="25.4469"/>-->
+          <mass value="25.4469"/>
-<!--          <inertia-->
+          <inertia
-<!--            ixx="3.06953"-->
+            ixx="3.06953"
-<!--            ixy="0.0"-->
+            ixy="0.0"
-<!--            ixz="0.0"-->
+            ixz="0.0"
-<!--            iyy="3.06953"-->
+            iyy="3.06953"
-<!--            iyz="0.0"-->
+            iyz="0.0"
-<!--            izz="0.03181"/>-->
+            izz="0.03181"/>
-<!--        </inertial>-->
+        </inertial>
-<!--    </link>-->
+    </link>
-<!--    <joint name="base_fixed" type="fixed">-->
+    <joint name="base_fixed" type="fixed">
-<!--      <origin rpy="0 0 0" xyz="0 0 1.2"/>-->
+      <origin rpy="0 0 0" xyz="0 0 1.2"/>
-<!--      <parent link="base_link"/>-->
+      <parent link="base_link"/>
-<!--      <child link="PELVIS_S"/>-->
+      <child link="PELVIS_S"/>
-<!--    </joint>-->
+    </joint>
  <link name="PELVIS_S">
    <inertial>
--- a/data/ik_psi_sweep.csv
+++ b/data/ik_psi_sweep.csv
@ -1,177 +1,128 @@
 psi,q1,q2,q3,q4,q5,q6,q7
-0.000000000,0.293251835,0.758274585,1.296200413,1.570796327,-1.328603785,0.029738090,-0.243184799
+0.000000000,0.002038980,1.340620000,0.000000000,0.522261000,0.000000000,-0.000210733,-0.094236400
-0.000000000,0.250086584,0.987999944,1.558700421,1.570796327,-1.558796269,0.000072292,-0.012048253
+0.000000000,0.578202939,0.486316887,0.740005579,1.570796327,0.625727922,0.531578664,0.175673351
-0.000000000,0.254718830,1.039656166,1.614136195,1.577578275,-1.610593775,-0.009025684,0.046125314
+0.000000000,0.440761487,0.565335165,0.964336466,1.570796327,0.491292823,0.442355133,0.112628007
-0.000000000,0.261036192,1.072261700,1.649251612,1.584215943,-1.643600103,-0.016981472,0.085189632
+0.000000000,0.371238784,0.622365950,1.090264075,1.575500039,0.409159634,0.381149325,0.079289637
-0.000000000,0.267611406,1.094382460,1.673588191,1.590709595,-1.666295983,-0.024886645,0.113748291
+0.000000000,0.330463834,0.665306397,1.171413265,1.580187790,0.353118626,0.336100996,0.059064268
-0.000000000,0.274059516,1.109714288,1.691108731,1.597059459,-1.682328199,-0.033042438,0.135510724
+0.000000000,0.303629491,0.698061413,1.228156884,1.584859682,0.312714061,0.302027645,0.045779819
-0.000000000,0.280282638,1.120309738,1.703952262,1.603265727,-1.693717627,-0.041520863,0.152536084
+0.000000000,0.284432039,0.723308206,1.269901344,1.589515814,0.282624592,0.275821335,0.036565409
-0.000000000,0.286283897,1.127473061,1.713447991,1.609328561,-1.701747164,-0.050310820,0.166132315
+0.000000000,0.269759165,0.742935173,1.301736480,1.594156283,0.259722475,0.255419601,0.029896996
-0.000000000,0.292103210,1.132099435,1.720491885,1.615248090,-1.707297496,-0.059370845,0.177196824
+0.000000000,0.257917625,0.758312994,1.326716496,1.598781185,0.241997632,0.239372235,0.024895220
-0.000000000,0.297791027,1.134830956,1.725716897,1.621024417,-1.711001029,-0.068651392,0.186373132
+0.000000000,0.247921267,0.770456663,1.346806717,1.603390613,0.228082340,0.226624470,0.021022719
-0.000000000,0.303396968,1.136139730,1.729582201,1.626657617,-1.713323590,-0.078104630,0.194134291
+0.000000000,0.239166095,0.780127414,1.363333864,1.607984658,0.217010934,0.216393752,0.017936958
-0.000000000,0.308965157,1.136377620,1.732425797,1.632147741,-1.714613160,-0.087688428,0.200832821
+0.000000000,0.231267578,0.787900642,1.377223781,1.612563410,0.208084987,0.208092599,0.015412337
-0.000000000,0.314532769,1.135808781,1.734498413,1.637494815,-1.715131682,-0.097367501,0.206733349
+0.000000000,0.223973270,0.794213370,1.389137788,1.617126957,0.200791220,0.201276831,0.013296323
-0.000000000,0.320130097,1.134632261,1.735986728,1.642698847,-1.715077120,-0.107113203,0.212035277
+0.000000000,0.217113255,0.799398540,1.399556180,1.621675385,0.194748299,0.195609113,0.011483536
-0.000000000,0.325781276,1.132998327,1.737029929,1.647759821,-1.714599391,-0.116902735,0.216889133
+0.000000000,0.210570785,0.803710343,1.408832069,1.626208778,0.189670728,0.190832474,0.009899847
-0.000000000,0.331505226,1.131020502,1.737731782,1.652677706,-1.713812131,-0.126718186,0.221408639
+0.000000000,0.204264237,0.807343170,1.417227480,1.630727217,0.185343528,0.186750724,0.008492307
-0.000000000,0.337316601,1.128784578,1.738169564,1.657452454,-1.712801504,-0.136545589,0.225679720
+0.000000000,0.198135690,0.810445914,1.424938288,1.635230785,0.181604070,0.183213775,0.007222605
-0.000000000,0.343226634,1.126355382,1.738400710,1.662083998,-1.711632845,-0.146374089,0.229767287
+0.000000000,0.192143504,0.813132804,1.432111826,1.639719559,0.178328830,0.180106596,0.006062701
-0.000000000,0.349243858,1.123781896,1.738467780,1.666572263,-1.710355678,-0.156195244,0.233720347
+0.000000000,0.186257388,0.815491634,1.438859529,1.644193617,0.175423635,0.177340836,0.004991872
-0.000000000,0.355374700,1.121101117,1.738402183,1.670917156,-1.709007533,-0.166002460,0.237575874
+0.000000000,0.180455066,0.817590024,1.445266121,1.648653034,0.172816415,0.174848488,0.003994663
-0.000000000,0.361623948,1.118340983,1.738226968,1.675118577,-1.707616827,-0.175790547,0.241361730
+0.000000000,0.174719980,0.819480182,1.451396347,1.653097884,0.170451797,0.172577060,0.003059449
-0.000000000,0.367995123,1.115522584,1.737958914,1.679176414,-1.706205056,-0.185555370,0.245098877
+0.000000000,0.169039695,0.821202537,1.457299956,1.657528240,0.168287051,0.170485916,0.002177423
-0.000000000,0.374490772,1.112661836,1.737610104,1.683090548,-1.704788463,-0.195293580,0.248803052
+0.000000000,0.163404770,0.822788501,1.463015392,1.661944171,0.166289047,0.168543487,0.001341855
-0.000000000,0.381112689,1.109770746,1.737189098,1.686860851,-1.703379291,-0.205002413,0.252486032
+0.000000000,0.157807954,0.824262581,1.468572550,1.666345748,0.164431955,0.166725144,0.000547564
-0.000000000,0.387862085,1.106858365,1.736701822,1.690487191,-1.701986748,-0.214679533,0.256156589
+0.000000000,0.152243613,0.825643979,1.473994836,1.670733038,0.162695519,0.165011583,-0.000209470
-0.000000000,0.394739723,1.103931514,1.736152226,1.693969430,-1.700617722,-0.224322917,0.259821217
+0.000000000,0.146707313,0.826947825,1.479300719,1.675106107,0.161063745,0.163387594,-0.000932394
-0.000000000,0.401746015,1.100995333,1.735542782,1.697307426,-1.699277342,-0.233930769,0.263484676
+0.000000000,0.141195517,0.828186109,1.484504889,1.679465019,0.159523905,0.161841116,-0.001623691
-0.000000000,0.408881100,1.098053688,1.734874862,1.700501036,-1.697969382,-0.243501452,0.267150407
+0.000000000,0.135705364,0.829368399,1.489619141,1.683809837,0.158065790,0.160362521,-0.002285340
-0.000000000,0.416144903,1.095109485,1.734149011,1.703550113,-1.696696586,-0.253033443,0.270820844
+0.000000000,0.130234505,0.830502387,1.494653037,1.688140624,0.156681132,0.158944065,-0.002918932
-0.000000000,0.423537174,1.092164908,1.733365164,1.706454512,-1.695460901,-0.262525293,0.274497650
+0.000000000,0.124780985,0.831594308,1.499614410,1.692457439,0.155363175,0.157579468,-0.003525759
-0.000000000,0.431057527,1.089221592,1.732522797,1.709214087,-1.694263660,-0.271975604,0.278181894
+0.000000000,0.119343153,0.832649258,1.504509746,1.696760341,0.154106335,0.156263589,-0.004106883
-0.000000000,0.438705462,1.086280764,1.731621052,1.711828693,-1.693105713,-0.281383007,0.281874188
+0.000000000,0.113919592,0.833671449,1.509344478,1.701049387,0.152905953,0.154992182,-0.004663182
-0.000000000,0.446480386,1.083343336,1.730658821,1.714298189,-1.691987535,-0.290746150,0.285574784
+0.000000000,0.108509076,0.834664389,1.514123204,1.705324634,0.151758094,0.153761706,-0.005195391
-0.000000000,0.454381625,1.080409987,1.729634815,1.716622436,-1.690909301,-0.300063689,0.289283655
+0.000000000,0.103110526,0.835631035,1.518849866,1.709586135,0.150659401,0.152569174,-0.005704133
-0.000000000,0.462408439,1.077481219,1.728547612,1.718801298,-1.689870942,-0.309334276,0.293000547
+0.000000000,0.097722982,0.836573903,1.523527872,1.713833945,0.149606979,0.151412044,-0.006189938
-0.000000000,0.470560028,1.074557400,1.727395700,1.720834646,-1.688872188,-0.318556559,0.296725028
+0.000000000,0.092345584,0.837495157,1.528160207,1.718068116,0.148598304,0.150288131,-0.006653263
-0.000000000,0.478835538,1.071638796,1.726177497,1.722722354,-1.687912602,-0.327729179,0.300456517
+0.000000000,0.086977554,0.838396676,1.532749505,1.722288697,0.147631153,0.149195535,-0.007094505
-0.000000000,0.487234070,1.068725597,1.724891381,1.724464304,-1.686991600,-0.336850765,0.304194305
+0.000000000,0.081618181,0.839280108,1.537298113,1.726495738,0.146703554,0.148132591,-0.007514010
-0.000000000,0.495754680,1.065817932,1.723535697,1.726060384,-1.686108472,-0.345919933,0.307937579
+0.000000000,0.076266813,0.840146913,1.541808142,1.730689288,0.145813738,0.147097827,-0.007912084
-0.000000000,0.504396387,1.062915888,1.722108778,1.727510489,-1.685262390,-0.354935290,0.311685431
+0.000000000,0.070922846,0.840998392,1.546281501,1.734869392,0.144960109,0.146089928,-0.008288996
-0.000000000,0.513158172,1.060019512,1.720608945,1.728814522,-1.684452417,-0.363895433,0.315436869
+0.000000000,0.065585720,0.841835716,1.550719930,1.739036098,0.144141215,0.145107715,-0.008644986
-0.000000000,0.522038981,1.057128827,1.719034519,1.729972396,-1.683677515,-0.372798945,0.319190822
+0.000000000,0.060254914,0.842659947,1.555125023,1.743189448,0.143355729,0.144150120,-0.008980270
-0.000000000,0.531037733,1.054243830,1.717383824,1.730984029,-1.682936542,-0.381644403,0.322946147
+0.000000000,0.054929937,0.843472051,1.559498249,1.747329486,0.142602429,0.143216170,-0.009295039
-0.000000000,0.540153313,1.051364501,1.715655189,1.731849352,-1.682228263,-0.390430375,0.326701630
+0.000000000,0.049610331,0.844272918,1.563840968,1.751456254,0.141880187,0.142304975,-0.009589466
-0.000000000,0.549384581,1.048490801,1.713846952,1.732568304,-1.681551341,-0.399155422,0.330455992
+0.000000000,0.044295663,0.845063363,1.568154441,1.755569792,0.141187955,0.141415716,-0.009863707
-0.000000000,0.558730372,1.045622678,1.711957460,1.733140834,-1.680904341,-0.407818104,0.334207883
+0.000000000,0.038985523,0.845844147,1.572439847,1.759670141,0.140524756,0.140547637,-0.010117902
-0.000000000,0.568189497,1.042760064,1.709985069,1.733566901,-1.680285729,-0.416416973,0.337955889
+0.000000000,0.033679525,0.846615971,1.576698286,1.763757337,0.139889677,0.139700037,-0.010352177
-0.000000000,0.577760749,1.039902877,1.707928148,1.733846474,-1.679693866,-0.424950583,0.341698527
+0.000000000,0.028377300,0.847379495,1.580930790,1.767831419,0.139281864,0.138872263,-0.010566647
-0.000000000,0.587442898,1.037051021,1.705785070,1.733979534,-1.679127008,-0.433417490,0.345434249
+0.000000000,0.023078501,0.848135333,1.585138328,1.771892423,0.138700512,0.138063706,-0.010761414
-0.000000000,0.597234700,1.034204385,1.703554221,1.733966070,-1.678583306,-0.441816248,0.349161436
+0.000000000,0.017782795,0.848884063,1.589321815,1.775940383,0.138144864,0.137273797,-0.010936570
-0.000000000,0.607134897,1.031362843,1.701233991,1.733806084,-1.678060795,-0.450145421,0.352878400
+0.000000000,0.012489864,0.849626229,1.593482110,1.779975333,0.137614205,0.136502000,-0.011092198
-0.000000000,0.617142217,1.028526250,1.698822775,1.733499587,-1.677557397,-0.458403575,0.356583379
+0.000000000,0.007199406,0.850362343,1.597620030,1.783997306,0.137107858,0.135747816,-0.011228373
-0.000000000,0.627255381,1.025694446,1.696318972,1.733046601,-1.677070913,-0.466589287,0.360274540
+0.000000000,0.001911132,0.851092890,1.601736342,1.788006333,0.136625184,0.135010771,-0.011345161
-0.000000000,0.637473100,1.022867248,1.693720982,1.732447159,-1.676599022,-0.474701143,0.363949972
+0.000000000,-0.003375237,0.851818328,1.605831779,1.792002444,0.136165576,0.134290419,-0.011442619
-0.000000000,0.647794085,1.020044453,1.691027199,1.731701303,-1.676139274,-0.482737740,0.367607687
+0.000000000,-0.008659966,0.852539094,1.609907031,1.795985670,0.135728457,0.133586342,-0.011520800
-0.000000000,0.658217042,1.017225835,1.688236014,1.730809087,-1.675689085,-0.490697690,0.371245616
+0.000000000,-0.013943309,0.853255600,1.613962757,1.799956038,0.135313278,0.132898140,-0.011579750
-0.000000000,0.668740680,1.014411142,1.685345808,1.729770573,-1.675245734,-0.498579618,0.374861606
+0.000000000,-0.019225512,0.853968239,1.617999581,1.803913576,0.134919519,0.132225440,-0.011619505
-0.000000000,0.679363713,1.011600094,1.682354949,1.728585833,-1.674806356,-0.506382168,0.378453418
+0.000000000,-0.024506812,0.854677386,1.622018097,1.807858309,0.134546681,0.131567884,-0.011640101
-0.000000000,0.690084861,1.008792381,1.679261785,1.727254948,-1.674367937,-0.514104000,0.382018723
+0.000000000,-0.029787434,0.855383399,1.626018873,1.811790264,0.134194290,0.130925134,-0.011641565
-0.000000000,0.700902857,1.005987660,1.676064643,1.725778009,-1.673927308,-0.521743795,0.385555100
+0.000000000,-0.035067598,0.856086617,1.630002447,1.815709463,0.133861895,0.130296872,-0.011623919
-0.000000000,0.711816448,1.003185553,1.672761822,1.724155114,-1.673481136,-0.529300255,0.389060032
+0.000000000,-0.040347515,0.856787368,1.633969333,1.819615930,0.133549062,0.129682792,-0.011587182
-0.000000000,0.722824400,1.000385642,1.669351585,1.722386368,-1.673025921,-0.536772102,0.392530898
+0.000000000,-0.045627388,0.857485962,1.637920022,1.823509687,0.133255378,0.129082605,-0.011531367
-0.000000000,0.733925499,0.997587468,1.665832156,1.720471886,-1.672557987,-0.544158082,0.395964977
+0.000000000,-0.050907414,0.858182698,1.641854983,1.827390756,0.132980448,0.128496037,-0.011456482
-0.000000000,0.745118558,0.994790524,1.662201712,1.718411787,-1.672073470,-0.551456965,0.399359435
+0.000000000,-0.056187785,0.858877862,1.645774662,1.831259155,0.132723892,0.127922826,-0.011362533
-0.000000000,0.756402423,0.991994257,1.658458371,1.716206196,-1.671568317,-0.558667547,0.402711326
+0.000000000,-0.061468683,0.859571728,1.649679488,1.835114905,0.132485349,0.127362723,-0.011249521
-0.000000000,0.767775970,0.989198057,1.654600193,1.713855246,-1.671038269,-0.565788646,0.406017582
+0.000000000,-0.066750288,0.860264561,1.653569869,1.838958023,0.132264471,0.126815490,-0.011117442
-0.000000000,0.779238118,0.986401256,1.650625158,1.711359072,-1.670478855,-0.572819109,0.409275011
+0.000000000,-0.072032772,0.860956612,1.657446195,1.842788526,0.132060923,0.126280903,-0.010966291
-0.000000000,0.790787829,0.983603125,1.646531169,1.708717813,-1.669885378,-0.579757807,0.412480287
+0.000000000,-0.077316303,0.861648126,1.661308841,1.846606431,0.131874386,0.125758746,-0.010796058
-0.000000000,0.802424115,0.980802862,1.642316031,1.705931612,-1.669252905,-0.586603636,0.415629945
+0.000000000,-0.082601044,0.862339336,1.665158164,1.850411752,0.131704553,0.125248813,-0.010606730
-0.000000000,0.814146043,0.977999594,1.637977442,1.703000613,-1.668576251,-0.593355521,0.418720372
+0.000000000,-0.087887153,0.863030468,1.668994507,1.854204504,0.131551128,0.124750909,-0.010398291
-0.000000000,0.825952742,0.975192365,1.633512981,1.699924964,-1.667849966,-0.600012409,0.421747798
+0.000000000,-0.093174784,0.863721739,1.672818198,1.857984700,0.131413827,0.124264846,-0.010170721
-0.000000000,0.837843406,0.972380129,1.628920093,1.696704808,-1.667068313,-0.606573273,0.424708286
+0.000000000,-0.098464085,0.864413358,1.676629550,1.861752352,0.131292378,0.123790447,-0.009923998
-0.000000000,0.849817307,0.969561745,1.624196069,1.693340293,-1.666225257,-0.613037111,0.427597723
+0.000000000,-0.103755203,0.865105528,1.680428865,1.865507472,0.131186518,0.123327542,-0.009658099
-0.000000000,0.861873796,0.966735967,1.619338032,1.689831562,-1.665314438,-0.619402942,0.430411804
+0.000000000,-0.109048278,0.865798443,1.684216430,1.869250069,0.131095995,0.122875967,-0.009372995
-0.000000000,0.874012317,0.963901431,1.614342913,1.686178754,-1.664329149,-0.625669805,0.433146019
+0.000000000,-0.114343449,0.866492291,1.687992522,1.872980154,0.131020565,0.122435568,-0.009068657
-0.000000000,0.886232411,0.961056649,1.609207434,1.682382009,-1.663262309,-0.631836760,0.435795641
+0.000000000,-0.119640850,0.867187256,1.691757406,1.876697735,0.130959994,0.122006196,-0.008745051
-0.000000000,0.898533727,0.958199994,1.603928076,1.678441457,-1.662106435,-0.637902883,0.438355702
+0.000000000,-0.124940612,0.867883512,1.695511334,1.880402820,0.130914057,0.121587711,-0.008402144
-0.000000000,0.910916037,0.955329684,1.598501056,1.674357224,-1.660853605,-0.643867262,0.440820977
+0.000000000,-0.130242862,0.868581230,1.699254550,1.884095415,0.130882535,0.121179977,-0.008039898
-0.000000000,0.923379242,0.952443770,1.592922291,1.670129428,-1.659495422,-0.649728996,0.443185960
+0.000000000,-0.135547726,0.869280574,1.702987286,1.887775526,0.130865219,0.120782865,-0.007658274
-0.000000000,0.935923388,0.949540115,1.587187367,1.665758179,-1.658022969,-0.655487192,0.445444838
+0.000000000,-0.140855324,0.869981705,1.706709765,1.891443158,0.130861907,0.120396252,-0.007257229
-0.000000000,0.948548681,0.946616377,1.581291495,1.661243577,-1.656426756,-0.661140956,0.447591458
+0.000000000,-0.146165776,0.870684776,1.710422202,1.895098315,0.130872404,0.120020021,-0.006836721
-0.000000000,0.961255505,0.943669979,1.575229462,1.656585710,-1.654696668,-0.666689390,0.449619294
+0.000000000,-0.151479199,0.871389938,1.714124799,1.898741001,0.130896520,0.119654058,-0.006396704
-0.000000000,0.974044439,0.940698091,1.568995586,1.651784653,-1.652821891,-0.672131588,0.451521408
+0.000000000,-0.156795705,0.872097336,1.717817754,1.902371216,0.130934076,0.119298259,-0.005937130
-0.000000000,0.986916283,0.937697588,1.562583646,1.646840468,-1.650790840,-0.677466623,0.453290397
+0.000000000,-0.162115406,0.872807111,1.721501253,1.905988964,0.130984894,0.118952519,-0.005457950
-0.000000000,0.999872076,0.934665021,1.555986816,1.641753201,-1.648591064,-0.682693542,0.454918344
+0.000000000,-0.167438410,0.873519398,1.725175477,1.909594244,0.131048805,0.118616741,-0.004959112
-0.000000000,1.012913137,0.931596572,1.549197580,1.636522881,-1.646209141,-0.687811353,0.456396747
+0.000000000,-0.172764825,0.874234332,1.728840596,1.913187056,0.131125646,0.118290833,-0.004440562
-0.000000000,1.026041088,0.928488000,1.542207639,1.631149519,-1.643630554,-0.692819017,0.457716449
+0.000000000,-0.178094754,0.874952041,1.732496776,1.916767399,0.131215259,0.117974706,-0.003902247
-0.000000000,1.039257901,0.925334586,1.535007792,1.625633104,-1.640839540,-0.697715426,0.458867536
+0.000000000,-0.183428299,0.875672650,1.736144172,1.920335270,0.131317489,0.117668276,-0.003344109
-0.000000000,1.052565948,0.922131054,1.527587804,1.619973606,-1.637818914,-0.702499392,0.459839238
+0.000000000,-0.188765561,0.876396281,1.739782936,1.923890667,0.131432190,0.117371461,-0.002766090
-0.000000000,1.065968052,0.918871490,1.519936244,1.614170968,-1.634549856,-0.707169622,0.460619788
+0.000000000,-0.194106637,0.877123052,1.743413209,1.927433586,0.131559218,0.117084186,-0.002168129
-0.000000000,1.079467564,0.915549233,1.512040289,1.608225111,-1.631011650,-0.711724691,0.461196265
+0.000000000,-0.199451624,0.877853078,1.747035130,1.930964022,0.131698435,0.116806378,-0.001550166
-0.000000000,1.093068444,0.912156749,1.503885486,1.602135929,-1.627181371,-0.716163015,0.461554398
+0.000000000,-0.204800616,0.878586471,1.750648827,1.934481969,0.131849707,0.116537967,-0.000912137
-0.000000000,1.106775364,0.908685463,1.495455457,1.595903284,-1.623033493,-0.720482806,0.461678316
+0.000000000,-0.210153705,0.879323339,1.754254425,1.937987422,0.132012905,0.116278888,-0.000253978
-0.000000000,1.120593846,0.905125569,1.486731541,1.589527011,-1.618539405,-0.724682026,0.461550249
+0.000000000,-0.215510983,0.880063789,1.757852042,1.941480372,0.132187903,0.116029078,0.000424378
-0.000000000,1.134530416,0.901465767,1.477692331,1.583006910,-1.613666797,-0.728758326,0.461150136
+0.000000000,-0.220872538,0.880807923,1.761441791,1.944960813,0.132374581,0.115788479,0.001122999
-0.000000000,1.148592822,0.897692941,1.468313092,1.576342747,-1.608378875,-0.732708960,0.460455133
+0.000000000,-0.226238458,0.881555841,1.765023777,1.948428734,0.132572820,0.115557034,0.001841952
-0.000000000,1.162790299,0.893791736,1.458565003,1.569534253,-1.602633350,-0.736530688,0.459438966
+0.000000000,-0.231608829,0.882307642,1.768598103,1.951884127,0.132782508,0.115334690,0.002581309
-0.000000000,1.177133934,0.889743992,1.448414163,1.562581117,-1.596381091,-0.740219633,0.458071082
+0.000000000,-0.236983736,0.883063421,1.772164864,1.955326981,0.133003534,0.115121397,0.003341141
-0.000000000,1.191637142,0.885527982,1.437820252,1.555482988,-1.589564322,-0.743771097,0.456315492
+0.000000000,-0.242363261,0.883823269,1.775724150,1.958757284,0.133235792,0.114917109,0.004121521
-0.000000000,1.206316331,0.881117365,1.426734686,1.548239473,-1.582114142,-0.747179303,0.454129186
+0.000000000,-0.247747485,0.884587277,1.779276047,1.962175024,0.133479179,0.114721780,0.004922524
-0.000000000,1.221191834,0.876479684,1.415098026,1.540850132,-1.573947010,-0.750437029,0.451459882
+0.000000000,-0.253136490,0.885355532,1.782820636,1.965580188,0.133733595,0.114535369,0.005744225
-0.000000000,1.236289270,0.871574171,1.402836188,1.533314474,-1.564959620,-0.753535076,0.448242723
+0.000000000,-0.258530353,0.886128121,1.786357992,1.968972763,0.133998943,0.114357837,0.006586701
-0.000000000,1.251641599,0.866348404,1.389854711,1.525631961,-1.555021132,-0.756461457,0.444395270
+0.000000000,-0.263929153,0.886905126,1.789888186,1.972352734,0.134275129,0.114189147,0.007450029
-0.000000000,1.267292374,0.860732939,1.376029659,1.517801999,-1.543960797,-0.759200110,0.439809491
+0.000000000,-0.269332966,0.887686629,1.793411286,1.975720085,0.134562062,0.114029264,0.008334290
-0.000000000,1.283301206,0.854632183,1.361192256,1.509823937,-1.531547018,-0.761728735,0.434338172
+0.000000000,-0.274741865,0.888472709,1.796927352,1.979074800,0.134859656,0.113878158,0.009239563
-0.000000000,1.299753711,0.847907518,1.345100768,1.501697063,-1.517448891,-0.764014853,0.427769876
+0.000000000,-0.280155926,0.889263443,1.800436443,1.982416862,0.135167823,0.113735798,0.010165930
-0.000000000,1.316781719,0.840342278,1.327382852,1.493420604,-1.501156940,-0.766007816,0.419777156
+0.000000000,-0.285575221,0.890058905,1.803938611,1.985746253,0.135486482,0.113602158,0.011113473
-0.000000000,1.334612022,0.831555008,1.307394739,1.484993719,-1.481788251,-0.767619644,0.409788756
+0.000000000,-0.290999820,0.890859169,1.807433906,1.989062955,0.135815553,0.113477212,0.012082277
-0.000000000,1.353726657,0.820703382,1.283749263,1.476415499,-1.457426845,-0.768662883,0.396555090
+0.000000000,-0.296429795,0.891664305,1.810922373,1.992366949,0.136154957,0.113360936,0.013072425
-0.000000000,1.376358759,0.803517157,1.249732350,1.467684957,-1.418592456,-0.768291811,0.373816610
+0.000000000,-0.301865215,0.892474384,1.814404052,1.995658213,0.136504620,0.113253312,0.014084004
-0.000000000,1.494881519,0.662922654,0.987221696,1.458801032,-1.102236486,-0.721806346,0.172010222
+0.000000000,-0.307306147,0.893289473,1.817878980,1.998936727,0.136864469,0.113154318,0.015117101
-0.000000000,1.622376425,0.587008625,0.771885834,1.449762579,-0.914161382,-0.669279429,0.063293625
+0.000000000,-0.312752658,0.894109637,1.821347191,2.002202469,0.137234433,0.113063939,0.016171803
-0.000000000,1.741132088,0.546022020,0.594563695,1.440568367,-0.792024842,-0.624694251,0.000642426
+0.000000000,-0.318204815,0.894934939,1.824808713,2.005455417,0.137614443,0.112982160,0.017248199
-0.000000000,2.155554266,0.490079021,-0.001316571,1.431217073,-0.478295544,-0.460208109,-0.137582524
+0.000000000,-0.323662683,0.895765444,1.828263571,2.008695547,0.138004434,0.112908967,0.018346379
-0.000000000,2.391871198,0.515238742,-0.333359890,1.421707279,-0.328461722,-0.360786644,-0.179375072
+0.000000000,-0.329126324,0.896601210,1.831711788,2.011922835,0.138404340,0.112844349,0.019466434
-0.000000000,2.528829630,0.552905712,-0.533538771,1.412037462,-0.232793556,-0.293087092,-0.194688060
+0.000000000,-0.334595804,0.897442297,1.835153381,2.015137257,0.138814099,0.112788297,0.020608455
-0.000000000,2.612745002,0.588985362,-0.662201311,1.402205995,-0.165213887,-0.245115316,-0.198850488
+0.000000000,-0.340071182,0.898288761,1.838588365,2.018338785,0.139233651,0.112740803,0.021772535
-0.000000000,2.667045396,0.620413529,-0.749106592,1.392211136,-0.114999025,-0.210595521,-0.197528729
+0.000000000,-0.345552521,0.899140659,1.842016750,2.021527395,0.139662937,0.112701861,0.022958766
-0.000000000,2.703747443,0.647017619,-0.809843425,1.382051020,-0.076558182,-0.185693838,-0.193294787
+0.000000000,-0.351039881,0.899998044,1.845438544,2.024703059,0.140101899,0.112671467,0.024167244
-0.000000000,2.729389595,0.669394033,-0.853235127,1.371723658,-0.046538833,-0.167858916,-0.187447747
+0.000000000,-0.356533320,0.900860968,1.848853751,2.027865749,0.140550484,0.112649617,0.025398064
-0.000000000,2.747750395,0.688279875,-0.884634202,1.361226923,-0.022745180,-0.155305438,-0.180683929
+0.000000000,-0.362032897,0.901729483,1.852262372,2.031015436,0.141008637,0.112636311,0.026651320
-0.000000000,2.761129762,0.704359440,-0.907473113,1.350558545,-0.003656540,-0.146746205,-0.173389055
+0.000000000,-0.367538669,0.902603637,1.855664402,2.034152091,0.141476307,0.112631550,0.027927111
-0.000000000,2.770988545,0.718211663,-0.924048133,1.339716101,0.011817232,-0.141236559,-0.165780362
+0.000000000,-0.373050693,0.903483479,1.859059837,2.037275683,0.141953442,0.112635335,0.029225533
-0.000000000,2.778286951,0.730305848,-0.935943933,1.328697007,0.024473831,-0.138076114,-0.157980813
+0.000000000,-0.378569025,0.904369054,1.862448667,2.040386181,0.142439995,0.112647669,0.030546685
-0.000000000,2.783673311,0.741013373,-0.944278258,1.317498505,0.034903359,-0.136742868,-0.150059680
+0.000000000,-0.384093719,0.905260407,1.865830879,2.043483554,0.142935918,0.112668558,0.031890665
-0.000000000,2.787594299,0.750623415,-0.949850584,1.306117653,0.043543912,-0.136847223,-0.142055389
+0.000000000,-0.389624828,0.906157581,1.869206457,2.046567770,0.143441165,0.112698008,0.033257574
-0.000000000,2.790361811,0.759358134,-0.953236370,1.294551311,0.050719591,-0.138099142,-0.133988634
+0.000000000,-0.395162407,0.907060618,1.872575383,2.049638794,0.143955692,0.112736027,0.034647511
 0.000000000,2.792194807,0.767385723,-0.954848631,1.282796132,0.056667081,-0.140284435,-0.125869987
 0.000000000,2.793246005,0.774830859,-0.954978712,1.270848539,0.061554189,-0.143247747,-0.117704371
 0.000000000,2.793618880,0.781782506,-0.953822773,1.258704719,0.065492182,-0.146880728,-0.109493720
 0.000000000,2.793377888,0.788298984,-0.951497274,1.246360597,0.068542630,-0.151114669,-0.101238624
 0.000000000,2.792553065,0.794409851,-0.948043939,1.233811819,0.070718173,-0.155917751,-0.092939465
 0.000000000,2.791138131,0.800112985,-0.943420684,1.221053733,0.071974061,-0.161298746,-0.084597436
 0.000000000,2.789076767,0.805361545,-0.937464366,1.208081362,0.072179962,-0.167323900,-0.076216037
 0.000000000,2.786213997,0.810019508,-0.929767547,1.194889379,0.071030267,-0.174174302,-0.067804764
 0.000000000,2.782045634,0.813632132,-0.919055368,1.181472077,0.067596045,-0.182440317,-0.059397000
 0.000000000,2.683125328,0.748345095,-0.695197764,1.167823341,-0.081906538,-0.287025470,-0.049989218
 0.000000000,2.598912603,0.716533226,-0.523961361,1.153936604,-0.190185378,-0.359171246,-0.031394777
 0.000000000,2.532313043,0.702803463,-0.393366612,1.139804812,-0.272040321,-0.410518215,-0.010367593
 0.000000000,2.481979254,0.699113155,-0.294529717,1.125420380,-0.335108223,-0.447731562,0.010406772
 0.000000000,2.445129419,0.701057849,-0.220173261,1.110775136,-0.384181898,-0.475031777,0.029884491
 0.000000000,2.418865920,0.706196441,-0.164442504,1.095860267,-0.422628299,-0.495240487,0.047772522
 0.000000000,2.400675015,0.713173393,-0.122760938,1.080666253,-0.452954226,-0.510304566,0.064124766
 0.000000000,2.388540928,0.721236959,-0.091622985,1.065182792,-0.477075284,-0.521593998,0.079140965
 0.000000000,2.380908688,0.729971636,-0.068375633,1.049398718,-0.496469043,-0.530085467,0.093064949
 0.000000000,2.376602854,0.739149258,-0.051024596,1.033301901,-0.512276914,-0.536481553,0.106135332
 0.000000000,2.374744548,0.748646148,-0.038076206,1.016879140,-0.525379100,-0.541290771,0.118564276
 0.000000000,2.374680600,0.758397041,-0.028413895,1.000116028,-0.536452517,-0.544882602,0.130530926
 0.000000000,2.375927260,0.768369489,-0.021203819,0.982996810,-0.546016334,-0.547525972,0.142182035
 0.000000000,2.378127003,0.778549676,-0.015823579,0.965504206,-0.554467853,-0.549416511,0.153635719
 0.000000000,2.381015951,0.788934577,-0.011808714,0.947619202,-0.562110640,-0.550696059,0.164986212
 0.000000000,2.384399540,0.799527674,-0.008812662,0.929320818,-0.569176466,-0.551466723,0.176308573
 0.000000000,2.388134441,0.810336640,-0.006576838,0.910585815,-0.575842314,-0.551801066,0.187662891
 0.000000000,2.392115153,0.821372150,-0.004908303,0.891388353,-0.582243502,-0.551749506,0.199097855
 0.000000000,2.396264095,0.832647331,-0.003663095,0.871699580,-0.588483767,-0.551345694,0.210653707
 0.000000000,2.400524277,0.844177594,-0.002733793,0.851487130,-0.594643013,-0.550610403,0.222364642
 0.000000000,2.404853890,0.855980705,-0.002040243,0.830714509,-0.600783243,-0.549554308,0.234260763
 0.000000000,2.409222314,0.868077034,-0.001522631,0.809340332,-0.606953119,-0.548179937,0.246369706
 0.000000000,2.413607169,0.880489962,-0.001136320,0.787317374,-0.613191476,-0.546482967,0.258718019
 0.000000000,2.417992142,0.893246440,-0.000848003,0.764591364,-0.619530059,-0.544453018,0.271332386
 0.000000000,2.422365388,0.906377737,-0.000632822,0.741099445,-0.625995685,-0.542073991,0.284240801
 0.000000000,2.426718343,0.919920431,-0.000472224,0.716768158,-0.632611991,-0.539324023,0.297473761
 0.000000000,2.431044859,0.933917728,-0.000352364,0.691510791,-0.639400923,-0.536175022,0.311065600
 0.000000000,2.435340562,0.948421247,-0.000262911,0.665223790,-0.646384071,-0.532591751,0.325056080
 0.000000000,2.439602380,0.963493479,-0.000196150,0.637781830,-0.653583987,-0.528530329,0.339492426
 0.000000000,2.443828192,0.979211266,-0.000146328,0.609030839,-0.661025623,-0.523935931,0.354432083
 0.000000000,2.448016581,0.995670854,-0.000109146,0.578777870,-0.668738085,-0.518739309,0.369946615
 0.000000000,2.452166640,1.012995500,-0.000081399,0.546775855,-0.676756989,-0.512851445,0.386127510
 0.000000000,2.456277833,1.031347417,-0.000060694,0.512699669,-0.685127935,-0.506155073,0.403095221
 0.000000000,2.460349899,1.050947510,-0.000045243,0.476106583,-0.693912048,-0.498490618,0.421014086
 0.000000000,2.464382775,1.072110203,-0.000033714,0.436366460,-0.703195579,-0.489631332,0.440118625
 0.000000000,2.468376540,1.095310457,-0.000025111,0.392527463,-0.713108213,-0.479235477,0.460764164
 0.000000000,2.472331374,1.121329088,-0.000018691,0.343024831,-0.723862558,-0.466742698,0.483536697
 0.000000000,2.476247532,1.151629728,-0.000013896,0.284925459,-0.735856247,-0.451105524,0.509538150
 0.000000000,2.480125307,1.189691780,-0.000010311,0.211256648,-0.750032017,-0.429841129,0.541396289
 0.000000000,2.483964957,1.251746362,-0.000007600,0.089491200,-0.770768674,-0.391406950,0.591410694
--- a/data/planner/traj.csv
+++ b/data/planner/traj.csv
--- a/src/common/utils/math/support_functions.h
+++ b/src/common/utils/math/support_functions.h
@ -13,9 +13,6 @@ class SupportFunctions {
 private:
    static constexpr double EPS = 1e-9;
 public:
    static  std::vector<double> eigen_to_vector(const Eigen::VectorXd &v) {
        return std::vector<double>(v.data(), v.data() + v.size());
    }
    static double normalize_angle(double angle) {
        double a = std::fmod(angle, 2.0 * M_PI);
        if (a < -M_PI) a += 2.0 * M_PI;
--- a/src/ik_solver/CMakeLists.txt
+++ b/src/ik_solver/CMakeLists.txt
@ -65,7 +65,6 @@ target_link_libraries(srs_ik_test
        OsqpEigen::OsqpEigen
        cmvr_es::utils
        cmvr_es::ik_solver
        cmvr_es::planner
 )
--- a/src/ik_solver/include/bias_srs_ik_slover.h
+++ b/src/ik_solver/include/bias_srs_ik_slover.h
@ -21,8 +21,7 @@ namespace cmvr {
        BiasSRSIkSolver();
        ~BiasSRSIkSolver(){};
-        // std::vector<double>  inverse_kinematics(const Eigen::MatrixXd& pose, double psi);
+        std::vector<double>  inverse_kinematics(const Eigen::MatrixXd& pose, double psi);
        bool  inverse_kinematics(const Eigen::MatrixXd& pose, std::vector<double> &joints,double psi);
        Eigen::Matrix4d calc_total_transform(const std::vector<double>& joint_angles);
--- a/src/ik_solver/src/bias_srs_ik_slover.cpp
+++ b/src/ik_solver/src/bias_srs_ik_slover.cpp
@ -95,9 +95,11 @@ Eigen::Matrix3d BiasSRSIkSolver::reference_plane(const Eigen::Vector3d &S, const
    return R30;
 }
 bool BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &pose, std::vector<double> &joints, double psi) {
-        joints.resize(7,0);
+std::vector<double> BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &pose, double psi) {
    try {
        // Eigen::VectorXd joints(7);
        std::vector<double> joints(7, 0);
        // 目标位置
        Eigen::Vector3d P_target = pose.block<3, 1>(0, 3);
@ -124,8 +126,7 @@ bool BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &pose, std::vecto
        // - diff_max > EPS  → 太远
        // - diff_min > EPS  → 太近
        if (diff_max > EPS || diff_min > EPS) {
-           std::cerr << "Pose outside reachable workspace, IK solve failed";
+            throw std::runtime_error("Pose outside reachable workspace, IK solve failed");
            return false;
        }
        // 计算肘部角度  关节3
@ -180,6 +181,27 @@ bool BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &pose, std::vecto
        Eigen::Matrix3d R47 = R04.transpose() * pose.block<3, 3>(0, 0);
        // 提取腕部欧拉角
        // double phi_z = std::atan2(R47(1, 2), R47(0, 2));
        // double theta_y = std::atan2(std::sqrt(R47(2, 0) * R47(2, 0) + R47(2, 1) * R47(2, 1)), R47(2, 2));
        // double psi_z = std::atan2(R47(2, 1), -R47(2, 0));
        //
        // // 处理奇异情况
        // if (std::sin(theta_y) < 1e-12) {
        //     phi_z = std::atan2(R47(1, 0), R47(0, 0));
        //     psi_z = 0.0;
        // }
        // if (std::sin(M_PI - theta_y) < 1e-12) {
        //     phi_z = std::atan2(-R47(1, 0), -R47(0, 0));
        //     psi_z = 0.0;
        // }
        //
        // // 腕部分支调整
        // if (wrist_config_ == INWARD) {
        //     phi_z += M_PI;
        //     theta_y = -theta_y;
        //     psi_z += M_PI;
        // }
        k = wrist_config_;   // +1 / -1
        // ===== 1. 通用 ZYZ 提取=====
@ -212,11 +234,12 @@ bool BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &pose, std::vecto
        joints[5] = SupportFunctions::normalize_angle(theta_y - M_PI / 2);
        joints[6] = SupportFunctions::normalize_angle(psi_z);
-        return true;
+        return joints;
-
+    } catch (const std::exception &e) {
        throw std::runtime_error(e.what());
    }
 }
 Eigen::Matrix3d BiasSRSIkSolver::calc_rotation_matrix(const Eigen::Vector3d &rotation_axis, double rotation_angle) {
    // 归一化旋转轴
    Eigen::Vector3d normalized_axis = rotation_axis.normalized();
@ -235,10 +258,6 @@ Eigen::Matrix3d BiasSRSIkSolver::calc_rotation_matrix(const Eigen::Vector3d &rot
    return rotation_matrix;
 }
 Eigen::Matrix4d BiasSRSIkSolver::calc_dh(double d, double alpha, double a, double theta) {
    double ca = std::cos(alpha);
    double sa = std::sin(alpha);
@ -284,7 +303,7 @@ bool BiasSRSIkSolver::cal_coefficient_matrix(const Eigen::MatrixXd &pose, Eigen:
    if (s_mat.rows() != 3 || s_mat.cols() != 9) s_mat.setZero(3, 9);
    if (w_mat.rows() != 3 || w_mat.cols() != 9) w_mat.setZero(3, 9);
-
+    try {
        // Eigen::VectorXd joints(7);
        std::vector<double> joints(7, 0);
@ -313,8 +332,7 @@ bool BiasSRSIkSolver::cal_coefficient_matrix(const Eigen::MatrixXd &pose, Eigen:
        // - diff_max > EPS  → 太远
        // - diff_min > EPS  → 太近
        if (diff_max > EPS || diff_min > EPS) {
-            std::cerr << "Pose outside reachable workspace, IK solve failed" << std::endl;
+            throw std::runtime_error("Pose outside reachable workspace, IK solve failed");
            return false;
        }
        // 计算肘部角度  关节3
@ -357,4 +375,8 @@ bool BiasSRSIkSolver::cal_coefficient_matrix(const Eigen::MatrixXd &pose, Eigen:
        w_mat.block<3, 3>(0, 6) = C_w;
        return true;
    } catch (const std::exception &e) {
        throw std::runtime_error(e.what());
        return false;
    }
 }
--- a/src/ik_solver/src/opt_psi_limit_bias_solver.cpp
+++ b/src/ik_solver/src/opt_psi_limit_bias_solver.cpp
@ -8,9 +8,9 @@
 using namespace cmvr;
 OptPsiLimitBiasSolver::OptPsiLimitBiasSolver() : IKSolver() {
-    bias_srs_ik_solver_ = std::make_shared<BiasSRSIkSolver>();
+    bias_srs_ik_solver_    = std::make_shared<BiasSRSIkSolver>();
    joints_limit_analyzer_ = std::make_shared<JointsLimitAnalyzer>();
-    opt_psi_selector_ = std::make_shared<OptPsiSelector>();
+    opt_psi_selector_      = std::make_shared<OptPsiSelector>();
    this->init();
 }
@ -19,19 +19,19 @@ bool OptPsiLimitBiasSolver::init() {
    Eigen::Matrix4d T_tool_flange, T_arm_robot;
    T_tool_flange << 0, 1, 0, -0.284077,
-            0, 0, 1, 0.00801525,
+                     0, 0, 1, 0.00801525,
-            1, 0, 0, 0.00684256,
+                     1, 0, 0, 0.00684256,
-            0, 0, 0, 1;
+                     0, 0, 0, 1;
    T_arm_robot << 0, 1, 0, 0,
-            0, 0, -1, 0,
+                   0, 0, -1, 0,
-            -1, 0, 0, 0.042,
+                  -1, 0, 0, 0.042,
-            0, 0, 0, 1;
+                   0, 0, 0, 1;
    T_flange_urdf_mdh_ << 0, 1, 0, 0,
-            0, 0, 1, 0,
+                          0, 0, 1, 0,
-            1, 0, 0, 0,
+                          1, 0, 0, 0,
-            0, 0, 0, 1;
+                          0, 0, 0, 1;
    setTcpTransform(T_tool_flange);
    setArmBaseTransform(T_arm_robot);
@ -41,8 +41,8 @@ bool OptPsiLimitBiasSolver::init() {
    // 代价参数（可之后再通过 set_cost_params 调整）
    set_cost_params(
-        5.5, // lambda_q_
+        5.5,   // lambda_q_
-        1e-3 // cur_branch_cost_threshold_
+        1e-3   // cur_branch_cost_threshold_
    );
    return true;
@ -67,7 +67,7 @@ bool OptPsiLimitBiasSolver::estimate_state_from_current_joints() {
    // 1) 当前位姿 & 系数矩阵
    Eigen::Matrix4d cur_pose =
-            bias_srs_ik_solver_->calc_total_transform(cur_joints_angle_);
+        bias_srs_ik_solver_->calc_total_transform(cur_joints_angle_);
    Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9);
    bias_srs_ik_solver_->cal_coefficient_matrix(cur_pose, s_mat, w_mat);
@ -88,9 +88,10 @@ bool OptPsiLimitBiasSolver::estimate_state_from_current_joints() {
        BiasSRSIkSolver::ConfigDirection::INWARD
    };
-    for (BiasSRSIkSolver::ConfigDirection s_dir: dirs) {
+    for (BiasSRSIkSolver::ConfigDirection s_dir : dirs) {
-        for (BiasSRSIkSolver::ConfigDirection e_dir: dirs) {
+        for (BiasSRSIkSolver::ConfigDirection e_dir : dirs) {
-            for (BiasSRSIkSolver::ConfigDirection w_dir: dirs) {
+            for (BiasSRSIkSolver::ConfigDirection w_dir : dirs) {
                int s = sign_from_dir(s_dir);
                int e = sign_from_dir(e_dir);
                int w = sign_from_dir(w_dir);
@ -108,20 +109,20 @@ bool OptPsiLimitBiasSolver::estimate_state_from_current_joints() {
                int ie = idx_from_sign(e);
                int iw = idx_from_sign(w);
                BranchPsiState &slot = branch_init_[is][ie][iw];
-                slot.valid = true;
+                slot.valid  = true;
-                slot.psi = res.psi;
+                slot.psi    = res.psi;
                slot.s_conf = s;
                slot.e_conf = e;
                slot.w_conf = w;
                // 同时更新“最佳分支”作为 branch_state_
                if (!best.valid || res.score < best_score) {
-                    best.valid = true;
+                    best.valid   = true;
-                    best.psi = res.psi;
+                    best.psi     = res.psi;
-                    best.s_conf = s;
+                    best.s_conf  = s;
-                    best.e_conf = e;
+                    best.e_conf  = e;
-                    best.w_conf = w;
+                    best.w_conf  = w;
-                    best_score = res.score;
+                    best_score   = res.score;
                }
            }
        }
@ -133,8 +134,8 @@ bool OptPsiLimitBiasSolver::estimate_state_from_current_joints() {
    // 4) 把求得的“最佳分支”应用到 bias_srs_ik_solver_，保持一致
    bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(best.s_conf));
-    bias_srs_ik_solver_->set_elbow_config(dir_from_sign(best.e_conf));
+    bias_srs_ik_solver_->set_elbow_config   (dir_from_sign(best.e_conf));
-    bias_srs_ik_solver_->set_wrist_config(dir_from_sign(best.w_conf));
+    bias_srs_ik_solver_->set_wrist_config   (dir_from_sign(best.w_conf));
    branch_state_ = best;
    return true;
@ -146,7 +147,8 @@ bool OptPsiLimitBiasSolver::solve_on_branch(const Eigen::Matrix4d &target_cal_po
                                            const BranchPsiState &branch,
                                            std::vector<double> &q_out,
                                            double &psi_out,
-                                            double &cost_out) {
+                                            double &cost_out)
 {
    auto joints_limits = bias_srs_ik_solver_->get_joints_limits();
    // 1) 这一分支下的 ψ 可行区间
@ -169,16 +171,17 @@ bool OptPsiLimitBiasSolver::solve_on_branch(const Eigen::Matrix4d &target_cal_po
    // 3) 把 bias_srs_ik_solver_ 的分支设置为当前 branch
    bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch.s_conf));
-    bias_srs_ik_solver_->set_elbow_config(dir_from_sign(branch.e_conf));
+    bias_srs_ik_solver_->set_elbow_config   (dir_from_sign(branch.e_conf));
-    bias_srs_ik_solver_->set_wrist_config(dir_from_sign(branch.w_conf));
+    bias_srs_ik_solver_->set_wrist_config   (dir_from_sign(branch.w_conf));
    // 4) 解析 IK
-    std::vector<double> q;
+    std::vector<double> q =
-    if (!bias_srs_ik_solver_->inverse_kinematics(target_cal_pose, q, best_psi)) {
+        bias_srs_ik_solver_->inverse_kinematics(target_cal_pose, best_psi);
    if (q.size() != joints_limits.size()) {
        return false;
    }
    // 5) 检查关节限位
    for (int i = 0; i < static_cast<int>(q.size()); ++i) {
        if (q[i] < joints_limits[i].first || q[i] > joints_limits[i].second) {
@ -196,17 +199,18 @@ bool OptPsiLimitBiasSolver::solve_on_branch(const Eigen::Matrix4d &target_cal_po
    }
    cost_out = dpsi * dpsi + lambda_q_ * q_cost;
-    q_out = std::move(q);
+    q_out   = std::move(q);
    psi_out = best_psi;
    return true;
 }
 bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
                               std::vector<double> &joints_angle,
-                               bool is_tcp) {
+                               bool is_tcp)
 {
    // --- 0) 预处理 target_cal_pose ---
    Eigen::Matrix4d target_cal_pose =
-            SupportFunctions::invertHomogeneous(T_arm_robot_) * target_pose;
+        SupportFunctions::invertHomogeneous(T_arm_robot_) * target_pose;
    if (is_tcp) {
        target_cal_pose *= SupportFunctions::invertHomogeneous(T_tool_flange_);
@ -217,17 +221,18 @@ bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
    // --- 1) 校验当前缓存的 branch_state_ 是否仍然对应 cur_joints_angle_ ---
    if (branch_state_.valid) {
        Eigen::Matrix4d cur_pose =
-                bias_srs_ik_solver_->calc_total_transform(cur_joints_angle_);
+            bias_srs_ik_solver_->calc_total_transform(cur_joints_angle_);
        // 确保 solver 内部分支与 branch_state_ 一致
        bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf));
-        bias_srs_ik_solver_->set_elbow_config(dir_from_sign(branch_state_.e_conf));
+        bias_srs_ik_solver_->set_elbow_config   (dir_from_sign(branch_state_.e_conf));
-        bias_srs_ik_solver_->set_wrist_config(dir_from_sign(branch_state_.w_conf));
+        bias_srs_ik_solver_->set_wrist_config   (dir_from_sign(branch_state_.w_conf));
-        std::vector<double> cur_joints_angle;
+        auto cur_joints_angle =
            bias_srs_ik_solver_->inverse_kinematics(cur_pose,
                                                    branch_state_.psi);
-        if (!bias_srs_ik_solver_->inverse_kinematics(cur_pose, cur_joints_angle,
+        if (cur_joints_angle.size() != cur_joints_angle_.size()) {
                                                     branch_state_.psi)) {
            branch_state_.valid = false;
        } else {
            for (int i = 0; i < static_cast<int>(cur_joints_angle_.size()); ++i) {
@ -242,7 +247,7 @@ bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
    // --- 2) 如果还没有“上一次 ψ + 分支”，用当前关节估一次 ---
    if (!branch_state_.valid) {
        if (!estimate_state_from_current_joints()) {
-            return false; // 当前姿态都反推不了分支，直接失败
+            return false;   // 当前姿态都反推不了分支，直接失败
        }
    }
@ -252,16 +257,16 @@ bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
    // --- 4) 先在“当前分支”上试一次 ---
    std::vector<double> q_cur;
-    double psi_cur = 0.0;
+    double psi_cur  = 0.0;
    double cost_cur = 0.0;
-    bool cur_ok = solve_on_branch(target_cal_pose, s_mat, w_mat,
+    bool   cur_ok   = solve_on_branch(target_cal_pose, s_mat, w_mat,
-                                  branch_state_, q_cur, psi_cur, cost_cur);
+                                      branch_state_, q_cur, psi_cur, cost_cur);
    // 若当前分支有解且 cost 足够小，直接用当前分支，不再搜索其它分支
    if (cur_ok && cost_cur < cur_branch_cost_threshold_) {
-        joints_angle = q_cur;
+        joints_angle        = q_cur;
-        cur_joints_angle_ = joints_angle;
+        cur_joints_angle_   = joints_angle;
-        branch_state_.psi = psi_cur;
+        branch_state_.psi   = psi_cur;
        branch_state_.valid = true;
        // s_conf/e_conf/w_conf 不变
        return true;
@ -269,18 +274,18 @@ bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
    // --- 5) 当前分支不合适：在 8 个分支中全局搜索最小 cost ---
    BranchPsiState best_state{};
-    bool have_candidate = false;
+    bool   have_candidate = false;
-    double best_cost = std::numeric_limits<double>::infinity();
+    double best_cost      = std::numeric_limits<double>::infinity();
    std::vector<double> best_q;
    // 先把“当前分支的候选”也纳入考虑，避免再算一遍
    if (cur_ok) {
        have_candidate = true;
-        best_cost = cost_cur;
+        best_cost      = cost_cur;
-        best_q = q_cur;
+        best_q         = q_cur;
-        best_state = branch_state_;
+        best_state       = branch_state_;
-        best_state.psi = psi_cur;
+        best_state.psi   = psi_cur;
        best_state.valid = true;
    }
@ -289,9 +294,10 @@ bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
        BiasSRSIkSolver::ConfigDirection::INWARD
    };
-    for (BiasSRSIkSolver::ConfigDirection s_dir: dirs) {
+    for (BiasSRSIkSolver::ConfigDirection s_dir : dirs) {
-        for (BiasSRSIkSolver::ConfigDirection e_dir: dirs) {
+        for (BiasSRSIkSolver::ConfigDirection e_dir : dirs) {
-            for (BiasSRSIkSolver::ConfigDirection w_dir: dirs) {
+            for (BiasSRSIkSolver::ConfigDirection w_dir : dirs) {
                int s = sign_from_dir(s_dir);
                int e = sign_from_dir(e_dir);
                int w = sign_from_dir(w_dir);
@ -309,36 +315,37 @@ bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
                cand.w_conf = w;
                // 为这一分支选择它自己的 prefer_psi：
-                if (const BranchPsiState *init_st = get_branch_init_state(s, e, w)) {
+                if (const BranchPsiState* init_st = get_branch_init_state(s, e, w)) {
-                    cand.psi = init_st->psi; // 用该分支自己的 ψ 估计
+                    cand.psi = init_st->psi;      // 用该分支自己的 ψ 估计
                } else {
                    cand.psi = branch_state_.psi; // 兜底：用当前主分支的 ψ
                }
                std::vector<double> q_cand;
-                double psi_cand = 0.0;
+                double psi_cand  = 0.0;
                double cost_cand = 0.0;
                if (!solve_on_branch(target_cal_pose, s_mat, w_mat,
                                     cand, q_cand, psi_cand, cost_cand)) {
-                    continue; // 这一分支无解
+                    continue;   // 这一分支无解
                }
                // 是否与上一帧处于同一分支，用来做 tie-break
                bool same_branch =
-                (s == branch_state_.s_conf &&
+                    (s == branch_state_.s_conf &&
-                 e == branch_state_.e_conf &&
+                     e == branch_state_.e_conf &&
-                 w == branch_state_.w_conf);
+                     w == branch_state_.w_conf);
                if (!have_candidate ||
                    cost_cand < best_cost - 1e-12 ||
-                    (std::abs(cost_cand - best_cost) <= 1e-12 && same_branch)) {
+                    (std::abs(cost_cand - best_cost) <= 1e-12 && same_branch))
                {
                    have_candidate = true;
-                    best_cost = cost_cand;
+                    best_cost      = cost_cand;
-                    best_q = std::move(q_cand);
+                    best_q         = std::move(q_cand);
-                    best_state = cand;
+                    best_state       = cand;
-                    best_state.psi = psi_cand;
+                    best_state.psi   = psi_cand;
                    best_state.valid = true;
                }
            }
@ -354,11 +361,11 @@ bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
    branch_state_ = best_state;
    bias_srs_ik_solver_->set_shoulder_config(dir_from_sign(branch_state_.s_conf));
-    bias_srs_ik_solver_->set_elbow_config(dir_from_sign(branch_state_.e_conf));
+    bias_srs_ik_solver_->set_elbow_config   (dir_from_sign(branch_state_.e_conf));
-    bias_srs_ik_solver_->set_wrist_config(dir_from_sign(branch_state_.w_conf));
+    bias_srs_ik_solver_->set_wrist_config   (dir_from_sign(branch_state_.w_conf));
-    joints_angle = best_q;
+    joints_angle      = best_q;
-    cur_joints_angle_ = joints_angle; // 作为下一次的“当前姿态”
+    cur_joints_angle_ = joints_angle;   // 作为下一次的“当前姿态”
    return true;
 }
--- a/src/ik_solver/src/srs_ik_test.cpp
+++ b/src/ik_solver/src/srs_ik_test.cpp
@ -14,9 +14,6 @@
 #include "ik_solver/include/opt_psi_limit_bias_solver.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
 #include  "common/utils/math/support_functions.h"
 #include "planner/joint_space_planner/include/toppra_bspline.h"
 #include "planner/joint_space_planner/include/joint_space_planner_creator.h"
 #include "common/utils/math/support_functions.h"
 using namespace cmvr;
@ -521,291 +518,124 @@ private:
 // }
-// TEST(SRS_IK_TEST, MOVE_L_SLOVER_TEST) {
+TEST(SRS_IK_TEST, MOVE_L_SLOVER_TEST) {
 //     using std::cout;
 //     using std::endl;
 //
 //     const char *model_path =
 //             "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.xml";
 //
 //     DualArmViewer viewer(model_path);
 //
 //
 //     // int viewer = 0;
 //     // 把所有 IK 运算 + moveJ 循环放到控制线程里
 //     std::thread ctrl_thread([&viewer]() {
 //            std::vector<IkSample> samples;
 //         std::this_thread::sleep_for(std::chrono::seconds(3));
 //     samples.reserve(4096);
 //
 //     OptPsiLimitBiasSolver solver;
 //
 //
 //     // 1) 当前位姿（估计上一时刻 ψ 用）
 //     std::vector<double> joint_angles(7, 0);
 //     joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
 //     solver.update_joints_state(joint_angles);
 //     samples.push_back(IkSample{
 //         0.0, {
 //             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.25, 1.00, M_PI / 2, M_PI / 2, 0, 0, 0};
 //     Eigen::Matrix4d target_pose;
 //
 //     solver.fk(joint_angles,target_pose,true);
 //
 //     // 直线插补参数 —— 从 target_pose 出发沿 X 方向 L 米，共 N 段（N+1 个点，包含起点）
 //     const int N = 400; // 采样点数（间隔均匀）
 //     const double L = -0.40; // 直线长度 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 的可行区间做一次更新
 //     std::vector<double> q;
 //     solver.ik(target_pose,q);
 //
 //     // 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{0.0, {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 更新一次
 //
 //
 //         // 逆解（带 ψ）
 //         bool ok = solver.ik(T_goal, q);
 //         if (!ok) {
 //             cout << k << ", " << s << ", IK_FAIL\n";
 //             break; // 直接跳出循环，看看是在哪个 k 失败的
 //         }
 //
 //         solver.update_joints_state(q);
 //
 //         viewer.moveJ(q); // 更新目标角
 //         std::this_thread::sleep_for(std::chrono::duration<double>(0.1));
 //
 //         // 前向校验
 //         Eigen::Matrix4d T_fk;
 //         solver.fk(q,T_fk,true);
 //         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 << ", " << 0.0 << ", "
 //                 << 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{0.0, {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);
 //     });
 //
 //     // ★ MuJoCo / OpenGL 一定在主线程跑
 //     viewer.run(); // 阻塞，直到你关掉窗口
 //     ctrl_thread.join(); // 控制线程结束
 //
 //     // 这里不用再 sleep / join sim_thread 了
 // }
 //
 TEST(SRS_IK_TEST, MOVE_L_PLANNER_TEST) {
    using std::cout;
    using std::endl;
    const char *model_path =
-        "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.xml";
+            "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.xml";
    DualArmViewer viewer(model_path);
-    // ★ 把 IK + planner + moveJ 放在控制线程
+
    // int viewer = 0;
    // 把所有 IK 运算 + moveJ 循环放到控制线程里
    std::thread ctrl_thread([&viewer]() {
-        using namespace std::chrono_literals;
+           std::vector<IkSample> samples;
        std::this_thread::sleep_for(std::chrono::seconds(3));
    samples.reserve(4096);
-        // 等 MuJoCo / OpenGL 初始化好
+    OptPsiLimitBiasSolver solver;
        std::this_thread::sleep_for(3s);
        OptPsiLimitBiasSolver solver;
-        // =============== 1) 设置初始关节状态 ===============
+    // 1) 当前位姿（估计上一时刻 ψ 用）
-        std::vector<double> joint_angles(7, 0.0);
+    std::vector<double> joint_angles(7, 0);
-        joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
+    joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
-
+    solver.update_joints_state(joint_angles);
-        solver.update_joints_state(joint_angles);
+    samples.push_back(IkSample{
-
+        0.0, {
-        // =============== 2) 目标位姿 (直线起点) ===============
+            joint_angles[0], joint_angles[1],
-        joint_angles = {0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0};
+            joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5], joint_angles[6]
        Eigen::Matrix4d target_pose;
        solver.fk(joint_angles, target_pose, true);
        // 直线插补：从 target_pose 出发沿 X 方向 L 米，共 N 段
        const int    N = 200;
        const double L = 0.60;
        Eigen::Vector3d dir = Eigen::Vector3d::UnitZ();
        dir.normalize();
        const Eigen::Matrix3d R_fixed = target_pose.block<3, 3>(0, 0);
        const Eigen::Vector3d p0      = target_pose.block<3, 1>(0, 3);
        // =============== 3) 准备容器：IK 路点 + CSV 样本 ===============
        std::vector<std::vector<double>> waypoints;
        waypoints.reserve(N + 1);
        std::vector<IkSample> ik_samples;
        ik_samples.reserve(N + 1);
        // 用一次 IK 作为起点的 q
        std::vector<double> q;
        bool ok0 = solver.ik(target_pose, q);
        if (!ok0 || q.size() != 7) {
            std::cerr << "IK at target_pose failed\n";
            return;
        }
    });
    // 2) 目标位姿（作为直线的起点）
    joint_angles = {0.25, 1.00, M_PI / 2, M_PI / 2, 0, 0, 0};
    Eigen::Matrix4d target_pose;
    solver.fk(joint_angles,target_pose,true);
    // 直线插补参数 —— 从 target_pose 出发沿 X 方向 L 米，共 N 段（N+1 个点，包含起点）
    const int N = 400; // 采样点数（间隔均匀）
    const double L = -0.40; // 直线长度 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 的可行区间做一次更新
    std::vector<double> q;
    solver.ik(target_pose,q);
    // 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{0.0, {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 更新一次
        // 逆解（带 ψ）
        bool ok = solver.ik(T_goal, q);
        if (!ok) {
            cout << k << ", " << s << ", IK_FAIL\n";
            break; // 直接跳出循环，看看是在哪个 k 失败的
        }
        solver.update_joints_state(q);
-        waypoints.push_back(q);
+        viewer.moveJ(q); // 更新目标角
-        ik_samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
+        std::this_thread::sleep_for(std::chrono::duration<double>(0.1));
-        // 辅助函数：计算旋转误差
+        // 前向校验
-        auto clamp = [](double x, double lo, double hi) {
+        Eigen::Matrix4d T_fk;
-            return std::max(lo, std::min(hi, x));
+        solver.fk(q,T_fk,true);
-        };
+        const Eigen::Vector3d p_fk = T_fk.block < 3,
-        auto rot_err_rad = [&](const Eigen::Matrix3d &R_goal,
+        1 > (0, 3);
-                               const Eigen::Matrix3d &R_fk) -> double {
+        const Eigen::Matrix3d R_fk = T_fk.block < 3,
-            Eigen::Matrix3d dR = R_goal.transpose() * R_fk;
+        3 > (0, 0);
            double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0);
            return std::acos(c);
        };
-        cout << "=== IK path along straight line ===\n";
+        const double pos_err = (p_fk - p).norm();
-        cout << "idx, s(0..1), q1..q7, pos_err(m), rot_err(rad), rot_err(deg)\n";
+        const double rot_err = rot_err_rad(R_fixed, R_fk);
        const double rot_err_deg = rot_err * 180.0 / M_PI;
-        // =============== 4) 直线采样 & 求 IK（只收集，不 moveJ） ===============
+        cout << k << ", " << s << ", " << 0.0 << ", "
-        for (int k = 0; k <= N; ++k) {
+                << q[0] << ", " << q[1] << ", " << q[2] << ", "
-            const double s = static_cast<double>(k) / static_cast<double>(N);  // [0,1]
+                << q[3] << ", " << q[4] << ", " << q[5] << ", " << q[6] << ", "
-            Eigen::Vector3d p = p0 + s * L * dir;
+                << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
-            Eigen::Matrix4d T_goal = Eigen::Matrix4d::Identity();
+        samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
-            T_goal.block<3, 3>(0, 0) = R_fixed;
+    }
-            T_goal.block<3, 1>(0, 3) = p;
+    write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9);
            bool ok = solver.ik(T_goal, q);
            if (!ok) {
                cout << k << ", " << s << ", IK_FAIL\n";
                break;
            }
            solver.update_joints_state(q);
            // 前向校验一下 IK 误差（方便你确认 IK 本身没问题）
            Eigen::Matrix4d T_fk;
            solver.fk(q, T_fk, true);
            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 << ", "
                 << q[0] << ", " << q[1] << ", " << q[2] << ", "
                 << q[3] << ", " << q[4] << ", " << q[5] << ", " << q[6] << ", "
                 << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n";
            waypoints.push_back(q);
            ik_samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}});
        }
        // 原始 IK 轨迹先写一份 CSV，方便对比
        write_ik_samples_csv(
            "/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv",
            ik_samples, true, 9);
        if (waypoints.size() < 2) {
            std::cerr << "Not enough IK waypoints for planner\n";
            return;
        }
        // =============== 5) 使用 JointSpacePlanner 对 IK 路点做时间参数化 ===============
        auto planner = JointSpacePlannerCreator::create(JointSpacePlannerType::TOPPRA_BSPLINE);
        planner->setPathType(PathType::Natural);
        // 按自己实际的关节约束改
        planner->setSymmetricLimits(
            std::vector<double>(7, 1.5),  // vmax
            std::vector<double>(7, 3.0)   // amax
        );
        TrajPtr traj;
        if (!planner->plan(waypoints, traj)) {
            std::cerr << "planner.plan(waypoints) failed\n";
            return;
        }
        // 采样规划后的轨迹（这里用 0.01 s）
        auto plan_samples = planner->sampleTrajectory(traj, 0.01);
        if (plan_samples.empty()) {
            std::cerr << "planner.sampleTrajectory returned empty\n";
            return;
        }
        // 写一份规划后轨迹的 CSV
        planner->writeTrajectoryCsv(
            "/home/lgv/cmvr/cmvr-es/data/planner/traj.csv",
            plan_samples);
        cout << "=== Start executing planned trajectory ===\n";
        // =============== 6) 播放规划后的轨迹到 MuJoCo ===============
        for (const auto &smp : plan_samples) {
            const std::vector<double> &q_plan = SupportFunctions::eigen_to_vector(smp.q);
            viewer.moveJ(q_plan);
            std::this_thread::sleep_for(10ms);
        }
        cout << "=== Planned trajectory finished ===\n";
    });
    // ★ MuJoCo / OpenGL 一定在主线程跑
-    viewer.run();       // 阻塞，直到你关掉窗口
+    viewer.run(); // 阻塞，直到你关掉窗口
    ctrl_thread.join(); // 控制线程结束
 }
    // 这里不用再 sleep / join sim_thread 了
 }
 //
 //
--- a/src/planner/CMakeLists.txt
+++ b/src/planner/CMakeLists.txt
@ -20,7 +20,7 @@ find_library(TOPPRA_LIB NAMES toppra
        PATHS ${CMAKE_SOURCE_DIR}/third_party/toppra/0.6.2/lib
        NO_DEFAULT_PATH)
-target_link_libraries(planner PUBLIC
+target_link_libraries(planner PRIVATE
        Eigen3::Eigen
        OsqpEigen::OsqpEigen
        ${TINYXML2_LIBRARIES}
--- a/src/planner/joint_space_planner/include/joint_space_planner.h
+++ b/src/planner/joint_space_planner/include/joint_space_planner.h
@ -5,6 +5,7 @@
 #pragma once
 #include <vector>
 #include "common/consts/constant.h"
 #include <toppra/geometric_path/piecewise_poly_path.hpp>
 #include <toppra/parametrizer/const_accel.hpp>
 #include <toppra/parametrizer/spline.hpp>
@ -28,11 +29,7 @@ namespace cmvr {
    };
    // 三种几何路径
-    enum class PathType { Linear,
+    enum class PathType { Linear, CubicHermite, Quintic };
            CubicHermite,
            Quintic,
            // 三次B样条
            Natural };
    class JointSpacePlanner {
    public:
@ -43,21 +40,18 @@ namespace cmvr {
        virtual bool plan(const std::vector<double>& start_joints,
                         const std::vector<double>& goal_joints,
                         TrajPtr& traj) {
-            return false;
+            UNUSED_VARIABLE(start_joints, goal_joints,traj);
        }
        virtual bool plan(const std::vector<std::vector<double>>& waypoints, TrajPtr& traj){
            return false;
        }
        // 采样函数：从 ITrajectory 生成采样序列 dt(s)
        virtual std::vector<TrajSample> sampleTrajectory(const TrajPtr& traj, double dt) {
-
+            UNUSED_VARIABLE(traj,dt);
            return {};
        }
        virtual bool writeTrajectoryCsv(const std::string& filename,const std::vector<TrajSample>& samples) {
-
+            UNUSED_VARIABLE(filename,samples);
            return false;
        }
--- a/src/planner/joint_space_planner/include/toppra_bspline.h
+++ b/src/planner/joint_space_planner/include/toppra_bspline.h
@ -1,5 +1,4 @@
 #pragma once
 #include <memory>
 #include <vector>
 #include <Eigen/Dense>
@ -10,6 +9,8 @@
 #include <toppra/parametrizer/spline.hpp>
 namespace cmvr {
    // 适配器：ConstAccel
    class ConstAccelTraj : public ITrajectory {
    public:
@ -51,176 +52,30 @@ namespace cmvr {
    public:
        explicit ToppraBSpline(PathType type = PathType::Quintic);
-
+        // 主入口：尽量保证返回轨迹（失败回退到 Spline）
        // 统一入口：两点/多点皆可
        bool plan(const std::vector<std::vector<double> > &waypoints, TrajPtr &traj_out) override;
        // 兼容旧 API（可选）：转发为两点的统一入口
        bool plan(const std::vector<double> &start_joints,
                  const std::vector<double> &goal_joints,
                  TrajPtr &traj_out) override;
-        std::vector<TrajSample> sampleTrajectory(const TrajPtr &traj, double dt) override;
+        std::vector<TrajSample> sampleTrajectory(const TrajPtr& traj, double dt) override;
-
+        bool writeTrajectoryCsv(const std::string& filename,const std::vector<TrajSample>& samples) override;
        bool writeTrajectoryCsv(const std::string &filename, const std::vector<TrajSample> &samples) override;
    private:
-        // —— 几何路径统一分发 ——
+        // 几何路径构造
-        std::shared_ptr<toppra::PiecewisePolyPath>
+        static std::shared_ptr<toppra::PiecewisePolyPath>
-        buildPathUnified(const std::vector<Eigen::VectorXd> &q,
+        buildLinear(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
                         const std::vector<toppra::value_type> &S);
-        //  二点专用
+        static std::shared_ptr<toppra::PiecewisePolyPath>
-        std::shared_ptr<toppra::PiecewisePolyPath>
+        buildCubicHermiteRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
        buildTwoPointPath(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
-        std::shared_ptr<toppra::PiecewisePolyPath>
+        static std::shared_ptr<toppra::PiecewisePolyPath>
-        buildLinearTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
+        buildQuinticRestToRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
-        std::shared_ptr<toppra::PiecewisePolyPath>
+        static std::shared_ptr<toppra::PiecewisePolyPath>
-        buildCubicHermiteTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
+        buildPath(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1, PathType t);
-        std::shared_ptr<toppra::PiecewisePolyPath>
+        static void sanitizeVsq(toppra::Vector &vsq);
        buildQuinticRestToRestTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
        std::shared_ptr<toppra::PiecewisePolyPath>
        buildNaturalTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
        // 多点
        std::shared_ptr<toppra::PiecewisePolyPath>
        buildLinearMulti(const std::vector<Eigen::VectorXd> &q,
                         const std::vector<toppra::value_type> &S);
        std::shared_ptr<toppra::PiecewisePolyPath>
        buildCubicHermiteMulti(const std::vector<Eigen::VectorXd> &q,
                               const std::vector<toppra::value_type> &S);
        std::shared_ptr<toppra::PiecewisePolyPath>
        buildQuinticC2Multi(const std::vector<Eigen::VectorXd> &q,
                            const std::vector<toppra::value_type> &S);
        std::shared_ptr<toppra::PiecewisePolyPath>
        buildNaturalMulti(const std::vector<Eigen::VectorXd> &q,
                          const std::vector<toppra::value_type> &S);
        // —— 工具：限幅/参数/估计 ——
        bool ensureLimitsSized(std::size_t DoF);
        static void sanitizeVsq(toppra::Vector &v);
        // centripetal 弦长（alpha=0.5），生成严格递增 S
        static std::vector<toppra::value_type>
        makeS_centripetal(const std::vector<Eigen::VectorXd> &q) {
            const size_t M = q.size();
            std::vector<toppra::value_type> S(M, 0.0);
            auto chord = [](const Eigen::VectorXd &a, const Eigen::VectorXd &b) {
                double d = (a - b).norm();
                return std::pow(std::max(d, 1e-16), 0.5);
            };
            for (size_t i = 1; i < M; ++i) {
                S[i] = S[i - 1] + chord(q[i], q[i - 1]);
                if (S[i] <= S[i - 1]) S[i] = S[i - 1] + 1e-12;
            }
            return S;
        }
        // 等距参数（简单稳妥）
        static inline std::vector<toppra::value_type> makeS_equal(size_t M) {
            std::vector<toppra::value_type> S(M);
            for (size_t i = 0; i < M; ++i) S[i] = static_cast<toppra::value_type>(i);
            return S;
        }
        // 或：先用centripetal，再整体归一化到跨度≈(M-1)，并设置每段最小ds
        static inline void normalize_and_floor_S(std::vector<toppra::value_type> &S, double ds_min = 0.2) {
            for (size_t i = 1; i < S.size(); ++i) S[i] -= S[0];
            double L = S.back();
            if (L > 0) for (auto &x: S) x *= (S.size() - 1) / L;
            for (size_t i = 1; i < S.size(); ++i) if (S[i] - S[i - 1] < ds_min) S[i] = S[i - 1] + ds_min;
        }
        // Catmull–Rom（centripetal）估计结点几何速度 v（端点=0）
        static std::vector<Eigen::VectorXd>
        estimateVelsCatmull(const std::vector<Eigen::VectorXd> &q,
                            const std::vector<toppra::value_type> &S) {
            const size_t M = q.size();
            const int DoF = static_cast<int>(q[0].size());
            std::vector<Eigen::VectorXd> v(M, Eigen::VectorXd::Zero(DoF));
            if (M <= 2) return v;
            for (size_t i = 1; i + 1 < M; ++i) {
                double ds0 = std::max<double>(S[i] - S[i - 1], 1e-12);
                double ds1 = std::max<double>(S[i + 1] - S[i], 1e-12);
                v[i] = ((q[i + 1] - q[i]) / ds1 * ds0 + (q[i] - q[i - 1]) / ds0 * ds1) / (ds0 + ds1);
            }
            return v;
        }
        // 对内点几何速度限幅，抑制过冲（k∈[0.5,1.0]）
        static void clampNodeVels(std::vector<Eigen::VectorXd> &v,
                                  const std::vector<Eigen::VectorXd> &q,
                                  double k = 1.0) {
            const size_t M = q.size();
            if (M <= 2) return;
            for (size_t i = 1; i + 1 < M; ++i) {
                double d0 = (q[i] - q[i - 1]).norm();
                double d1 = (q[i + 1] - q[i]).norm();
                double d = std::max(std::min(d0, d1), 1e-12);
                double vmax = k * d;
                double n = v[i].norm();
                if (n > vmax) v[i] *= (vmax / n);
            }
        }
        // 估计结点几何加速度 a（端点=0；中点二阶差分，按 s 尺度）
        static std::vector<Eigen::VectorXd>
        estimateAccelsSecondDiff(const std::vector<Eigen::VectorXd> &q,
                                 const std::vector<toppra::value_type> &S) {
            const size_t M = q.size();
            const int DoF = static_cast<int>(q[0].size());
            std::vector<Eigen::VectorXd> a(M, Eigen::VectorXd::Zero(DoF));
            if (M <= 2) return a;
            for (size_t i = 1; i + 1 < M; ++i) {
                double h0 = std::max<double>(S[i] - S[i - 1], 1e-12); // 左间距
                double h1 = std::max<double>(S[i + 1] - S[i], 1e-12); // 右间距
                double denom = 0.5 * (h0 + h1); // 局部尺度
                // 非均匀中心二阶差分（更精确）：
                // a ≈ 2 * [ (q_{i+1}-q_i)/h1 - (q_i - q_{i-1})/h0 ] / (h0 + h1)
                a[i] = 2.0 * ((q[i + 1] - q[i]) / h1 - (q[i] - q[i - 1]) / h0) / (h0 + h1);
            }
            return a;
        }
        // τ→s 变元：把局部 Quintic(τ) 的系数 c_tau[0..5]（τ^0..τ^5）
        // 变成全局 s 的系数 alpha[0..5]（s^0..s^5），其中 τ = (s - S_k) / ds
        static inline void localQuinticToGlobalCoeffs(
            const std::array<Eigen::VectorXd, 6> &c_tau, // c0..c5（DoF维向量）
            double Sk, double ds,
            std::array<Eigen::VectorXd, 6> &alpha // α0..α5（DoF维向量）
        ) {
            static const double C[6][6] = {
                // binomial(n,m)
                {1, 0, 0, 0, 0, 0},
                {1, 1, 0, 0, 0, 0},
                {1, 2, 1, 0, 0, 0},
                {1, 3, 3, 1, 0, 0},
                {1, 4, 6, 4, 1, 0},
                {1, 5, 10, 10, 5, 1}
            };
            const double eps = 1e-12;
            ds = std::max(ds, eps);
            for (int m = 0; m <= 5; ++m) alpha[m].setZero(c_tau[0].size());
            // α_m = Σ_{n=m..5} c_n * C(n,m) * (-S_k)^{n-m} / ds^{n}
            for (int n = 0; n <= 5; ++n) {
                double invdsn = std::pow(ds, -n);
                for (int m = 0; m <= n; ++m) {
                    double factor = C[n][m] * std::pow(-Sk, n - m) * invdsn;
                    alpha[m].noalias() += factor * c_tau[n];
                }
            }
        }
    };
 } // namespace cmvr
--- a/src/planner/joint_space_planner/src/joint_space_planner_creator.cpp
+++ b/src/planner/joint_space_planner/src/joint_space_planner_creator.cpp
@ -29,12 +29,12 @@ namespace cmvr
        std::shared_ptr<JointSpacePlanner> result;
        switch (type)
        {
-            case JOINT_SPACE_PLANNER_UNKNOWN:
+        case JOINT_SPACE_PLANNER_UNKNOWN:
-                return nullptr;
+            return nullptr;
-            case TOPPRA_BSPLINE:
+        case TOPPRA_BSPLINE:
-                return std::make_shared<ToppraBSpline>();
+            return std::make_shared<ToppraBSpline>();
-            default:
+        default:
-                return nullptr;
+            return nullptr;
        }
    }
 }
--- a/src/planner/joint_space_planner/src/joint_space_planner_test.cpp
+++ b/src/planner/joint_space_planner/src/joint_space_planner_test.cpp
@ -5,21 +5,58 @@
 #include "gtest/gtest.h"
 #include "planner/joint_space_planner/include/toppra_bspline.h"
 #include "planner/joint_space_planner/include/joint_space_planner_creator.h"
 #include "common/utils/math/support_functions.h"
 #include <thread>
 using namespace cmvr;
 TEST(JOINT_SPACE_PLANNER_TEST,TOPPRA_TEST) {
    auto planner = JointSpacePlannerCreator::create(JointSpacePlannerType::TOPPRA_BSPLINE);
-    planner->setPathType(PathType::Quintic);
+    planner->setPathType(PathType::CubicHermite);
-    planner->setSymmetricLimits(std::vector<double>(7, 1.5),
+    planner->setSymmetricLimits(std::vector<double>(1, 1.5),
-                               std::vector<double>(7, 3.0));
+                               std::vector<double>(1, 6.0));
    TrajPtr traj;
-    std::vector<double> q0{0.0,-0.5,0.8,0.0,0.2,-0.3,0.1};
+    std::vector<double> q0{0.0};
-    std::vector<double> q1{1.2,0.2,-0.6,0.7,-0.4,0.5,-0.2};
+    std::vector<double> q1{-1.2};
    if (!planner->plan(q0, q1, traj)) { std::cerr << "plan failed\n"; }
    // 2) 采样 0.01 s
-    auto samples = planner->sampleTrajectory(traj, 0.01);
+    double dt = 0.01;
    auto samples = planner->sampleTrajectory(traj, dt);
    const auto t0 = std::chrono::steady_clock::now();
    size_t k = 1;
    const size_t K = samples.size();
    const int lookahead_steps = 1; // 看前 1 个点，或者直接设 0 就是你的原版
    while (k < K) {
        const auto &s_now = samples[k];
        int global_lookahead = 1;
        for (size_t i = 0; i < q0.size(); ++i) {
            int la_i = SupportFunctions::calcLookahead(
                s_now.qd[i],
                s_now.qdd[i],
                1.5,
                dt,1,10);
            global_lookahead = std::max(global_lookahead, la_i);
        }
        size_t k_la = std::min(k + (size_t) global_lookahead, K - 1);
        const auto &s_cmd = samples[k];
        std::cout << s_cmd.q << std::endl;
        ++k;
        if (k < K) {
            auto next_t = t0 + std::chrono::duration<double>(k * dt);
            std::this_thread::sleep_until(next_t);
        }
    }
    // 3) 写 CSV
    if (!planner->writeTrajectoryCsv("/home/lgv/cmvr/cmvr-es/data/planner/traj.csv", samples)) {
@ -29,58 +66,3 @@ TEST(JOINT_SPACE_PLANNER_TEST,TOPPRA_TEST) {
    std::cout << "CSV saved: traj.csv\n";
 }
 TEST(JOINT_SPACE_PLANNER_TEST, TOPPRA_WAYPOINTS_TEST) {
    auto planner = JointSpacePlannerCreator::create(JointSpacePlannerType::TOPPRA_BSPLINE);
    planner->setPathType(PathType::Quintic);
    // 7 自由度对称速度 / 加速度约束
    planner->setSymmetricLimits(std::vector<double>(7, 1.5),
                                std::vector<double>(7, 3.0));
    // ------- 1) 构造多个 q 路点 -------
    std::vector<double> q0 { 0.0, -0.5,  0.8,  0.0,  0.2, -0.3,  0.1};
    std::vector<double> q1 { 0.5, -0.2,  0.4,  0.3, -0.1,  0.1,  0.0};
    std::vector<double> q2 { 0.9,  0.1, -0.3,  0.5, -0.3,  0.3, -0.1};
    std::vector<double> q3 { 1.2,  0.2, -0.6,  0.7, -0.4,  0.5, -0.2}; // 终点
    std::vector<std::vector<double>> waypoints;
    waypoints.push_back(q0);
    waypoints.push_back(q1);
    waypoints.push_back(q2);
    waypoints.push_back(q3);
    // ------- 2) 调多路点 plan -------
    TrajPtr traj;
    if (!planner->plan(waypoints, traj)) {
        std::cerr << "multi-waypoints plan failed\n";
        FAIL();  // GTest 标记失败
    }
    // ------- 3) 采样并简单校验 -------
    // 0.01 s 采样
    auto samples = planner->sampleTrajectory(traj, 0.01);
    ASSERT_FALSE(samples.empty());
    // （下面假设 TrajSample 里有 q / pos 这样的关节角向量字段，
    //  你按自己的结构名改一下就行）
    const auto &q_start = samples.front().q;
    const auto &q_end   = samples.back().q;
    ASSERT_EQ(q_start.size(), q0.size());
    ASSERT_EQ(q_end.size(),   q3.size());
    for (size_t i = 0; i < q0.size(); ++i) {
        EXPECT_NEAR(q_start[i], q0[i], 1e-4);
        EXPECT_NEAR(q_end[i],   q3[i], 1e-4);
    }
    // ------- 4) 写 CSV 看一下轨迹 -------
    if (!planner->writeTrajectoryCsv(
            "/home/lgv/cmvr/cmvr-es/data/planner/traj.csv", samples)) {
        std::cerr << "write csv failed\n";
            } else {
                std::cout << "CSV saved: traj.csv\n";
            }
 }
--- a/src/planner/joint_space_planner/src/toppra_bspline.cpp
+++ b/src/planner/joint_space_planner/src/toppra_bspline.cpp
@ -50,34 +50,72 @@ namespace cmvr {
            v[v.size() - 1] = 0;
        }
    }
-    // ===== 统一入口：两点/多点 =====
+
-    bool ToppraBSpline::plan(const std::vector<std::vector<double>>& waypoints,
+    std::shared_ptr<toppra::PiecewisePolyPath>
-                             TrajPtr& traj_out) {
+    ToppraBSpline::buildLinear(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1) {
-        traj_out.reset();
+        // q(s) = b + a*s, s∈[0,1]。系数矩阵行0=一次项，行1=常数项
-        const size_t M = waypoints.size();
+        toppra::Matrix seg(2, q0.size());
-        if (M < 2) return false;
+        seg.row(0) = (q1 - q0).transpose();
-        const size_t DoF = waypoints.front().size();
+        seg.row(1) = q0.transpose();
-        for (const auto& w : waypoints) if (w.size()!=DoF) return false;
+        return std::make_shared<toppra::PiecewisePolyPath>(
            toppra::Matrices{seg}, std::vector<double>{0.0, 1.0});
    }
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildCubicHermiteRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1) {
        // 起止速度 0 的三次 Hermite
        toppra::Vectors pos{q0, q1};
        toppra::Vectors vel{Eigen::VectorXd::Zero(q0.size()), Eigen::VectorXd::Zero(q1.size())};
        std::vector<toppra::value_type> s{0.0, 1.0};
        auto herm = toppra::PiecewisePolyPath::CubicHermiteSpline(pos, vel, s);
        return std::make_shared<toppra::PiecewisePolyPath>(herm);
    }
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildQuinticRestToRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1) {
        // q(0)=q0, q'(0)=q''(0)=0; q(1)=q1, q'(1)=q''(1)=0 的闭式解
        Eigen::VectorXd dq = q1 - q0;
        toppra::Matrix seg(6, q0.size()); // x^5..x^0
        seg.row(0) = (6.0 * dq).transpose(); // a5
        seg.row(1) = (-15.0 * dq).transpose(); // a4
        seg.row(2) = (10.0 * dq).transpose(); // a3
        seg.row(3).setZero(); // a2
        seg.row(4).setZero(); // a1
        seg.row(5) = q0.transpose(); // a0
        return std::make_shared<toppra::PiecewisePolyPath>(
            toppra::Matrices{seg}, std::vector<double>{0.0, 1.0});
    }
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildPath(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1, PathType t) {
        switch (t) {
            case PathType::Linear: return buildLinear(q0, q1);
            case PathType::CubicHermite: return buildCubicHermiteRest(q0, q1);
            case PathType::Quintic: return buildQuinticRestToRest(q0, q1);
            default: return buildQuinticRestToRest(q0, q1);
        }
    }
    bool ToppraBSpline::plan(const std::vector<double> &start_joints,
                             const std::vector<double> &goal_joints,
                             TrajPtr &traj_out) {
        if (start_joints.empty() || start_joints.size() != goal_joints.size())
            return false;
        const std::size_t DoF = start_joints.size();
        if (!ensureLimitsSized(DoF)) return false;
-        // 组装
+        // 构造几何路径
-        std::vector<Eigen::VectorXd> q; q.reserve(M);
+        Eigen::VectorXd q0 = Eigen::Map<const Eigen::VectorXd>(start_joints.data(), DoF);
-        for (const auto& w : waypoints)
+        Eigen::VectorXd q1 = Eigen::Map<const Eigen::VectorXd>(goal_joints.data(), DoF);
-            q.emplace_back(Eigen::Map<const Eigen::VectorXd>(w.data(), DoF));
+        auto path = buildPath(q0, q1, path_type_);
        // 生成 S
 //        std::vector<toppra::value_type> S = (M==2) ? std::vector<toppra::value_type>{0.0,1.0}
 //                                                   : makeS_centripetal(q);
        std::vector<toppra::value_type> S = (M==2) ? std::vector<toppra::value_type>{0.0,1.0}
                                                   : makeS_equal(M);
        // 几何路径
        auto path = buildPathUnified(q, S);
        if (!path) return false;
        // 约束
        Eigen::VectorXd vmax(DoF), amax(DoF);
-        for (size_t i=0;i<DoF;++i) { vmax[i]=v_max_[i]; amax[i]=a_max_[i]; }
+        for (std::size_t i = 0; i < DoF; ++i) {
            vmax[i] = v_max_[i];
            amax[i] = a_max_[i];
        }
        auto vel = std::make_shared<toppra::constraint::LinearJointVelocity>(-vmax, vmax);
        auto acc = std::make_shared<toppra::constraint::LinearJointAcceleration>(-amax, amax);
        vel->discretizationType(toppra::DiscretizationType::Collocation);
@ -86,24 +124,31 @@ namespace cmvr {
        // TOPPRA
        toppra::algorithm::TOPPRA algo{constraints, path};
-        auto solve_once = [&](int N)->bool{
+        algo.setN(N_grid_);
        algo.solver(std::make_shared<toppra::solver::Seidel>());
        auto solve_once = [&](int N)-> bool {
            algo.setN(N);
-            algo.solver(std::make_shared<toppra::solver::Seidel>());
+            auto rc = algo.computePathParametrization(0.0, 0.0);
-            return algo.computePathParametrization(0.0, 0.0) == toppra::ReturnCode::OK;
+            return rc == toppra::ReturnCode::OK;
        };
        if (!solve_once(N_grid_)) {
            if (!solve_once(N_grid_high_)) return false;
        }
        const auto data = algo.getParameterizationData();
        toppra::Vector grid = data.gridpoints;
-        toppra::Vector vsq  = data.parametrization;
+        toppra::Vector vsq = data.parametrization;
        // 首选 ConstAccel
        auto ca = std::make_shared<toppra::parametrizer::ConstAccel>(path, grid, vsq);
        if (ca->validate()) {
            traj_out = std::make_shared<ConstAccelTraj>(std::move(ca));
            return true;
        }
        // 回退到 Spline（去毛刺）
        sanitizeVsq(vsq);
        try {
            traj_out = std::make_shared<SplineTraj>(path, grid, vsq);
@ -114,204 +159,6 @@ namespace cmvr {
        }
    }
    bool ToppraBSpline::plan(const std::vector<double>& start_joints,
                             const std::vector<double>& goal_joints,
                             TrajPtr& traj_out) {
        if (start_joints.empty() || start_joints.size()!=goal_joints.size()) return false;
        std::vector<std::vector<double>> wpts{start_joints, goal_joints};
        return plan(wpts, traj_out);
    }
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildPathUnified(const std::vector<Eigen::VectorXd>& q,
                                    const std::vector<toppra::value_type>& S) {
        const size_t M = q.size();
        if (M == 2) return buildTwoPointPath(q[0], q[1]);
        switch (path_type_) {
            case PathType::Linear:       return buildLinearMulti(q, S);
            case PathType::CubicHermite: return buildCubicHermiteMulti(q, S);
            case PathType::Natural:       return buildNaturalMulti(q, S);
            case PathType::Quintic:
            default:                     return buildQuinticC2Multi(q, S);
        }
    }
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildTwoPointPath(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
        switch (path_type_) {
            case PathType::Linear:       return buildLinearTwo(q0, q1);
            case PathType::CubicHermite: return buildCubicHermiteTwo(q0, q1);
            case PathType::Natural:       return buildNaturalTwo(q0, q1);
            case PathType::Quintic:
            default:                     return buildQuinticRestToRestTwo(q0, q1);
        }
    }
    //  二点：Linear ——
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildLinearTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
        const size_t DoF = static_cast<size_t>(q0.size());
        toppra::Matrix seg(2, DoF);
        seg.row(0) = (q1 - q0).transpose();
        seg.row(1) = q0.transpose();
        return std::make_shared<toppra::PiecewisePolyPath>(
                toppra::Matrices{seg}, std::vector<double>{0.0, 1.0});
    }
    //  二点：Cubic Hermite（端点速度 0） ——
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildCubicHermiteTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
        toppra::Vectors pos{q0, q1};
        toppra::Vectors vel{Eigen::VectorXd::Zero(q0.size()),
                            Eigen::VectorXd::Zero(q1.size())};
        std::vector<toppra::value_type> s{0.0, 1.0};
        auto herm = toppra::PiecewisePolyPath::CubicHermiteSpline(pos, vel, s);
        return std::make_shared<toppra::PiecewisePolyPath>(herm);
    }
    //  二点：Quintic rest-to-rest ——
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildQuinticRestToRestTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
        const size_t DoF = static_cast<size_t>(q0.size());
        const Eigen::VectorXd dq = q1 - q0;
        toppra::Matrix seg(6, DoF); // x^5..x^0
        seg.row(0) = ( 6.0 * dq).transpose();
        seg.row(1) = (-15.0 * dq).transpose();
        seg.row(2) = (10.0 * dq).transpose();
        seg.row(3).setZero(); seg.row(4).setZero();
        seg.row(5) =  q0.transpose();
        return std::make_shared<toppra::PiecewisePolyPath>(
                toppra::Matrices{seg}, std::vector<double>{0.0, 1.0});
    }
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildNaturalTwo(const Eigen::VectorXd& q0, const Eigen::VectorXd& q1) {
        using PWP = toppra::PiecewisePolyPath;
        // positions（两点）
        toppra::Vectors pos{ q0, q1 };
        // 自变量S：两点用等距最简单
        toppra::Vector S(2);
        S[0] = 0.0; S[1] = 1.0;
        toppra::BoundaryCondFull bcA = { toppra::BoundaryCond("clamped"),
                                         toppra::BoundaryCond("clamped") };
        auto path = PWP::CubicSpline(pos, S, bcA);
        return std::make_shared<PWP>(path);
        // 手动指定二阶导为 0
        // const int DoF = static_cast<int>(q0.size());
        // toppra::BoundaryCondFull bcB = {
        //     toppra::BoundaryCond(2, Eigen::VectorXd::Zero(DoF)),
        //     toppra::BoundaryCond(2, Eigen::VectorXd::Zero(DoF))
        // };
        // auto path2 = PWP::CubicSpline(pos, S, bcB);
        // return std::make_shared<PWP>(path2);
    }
    // —— 多点：Linear ——
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildLinearMulti(const std::vector<Eigen::VectorXd>& q,
                                    const std::vector<toppra::value_type>& S) {
        const size_t M = q.size(), DoF = q[0].size();
        toppra::Matrices segs; segs.reserve(M-1);
        for (size_t k=0;k+1<M;++k) {
            double ds = std::max<double>(S[k+1]-S[k], 1e-12);
            toppra::Matrix seg(2, DoF);
            Eigen::RowVectorXd A1 = ((q[k+1]-q[k])/ds).transpose();
            Eigen::RowVectorXd A0 = (q[k] - A1.transpose()*S[k]).transpose();
            seg.row(0)=A1; seg.row(1)=A0;
            segs.emplace_back(std::move(seg));
        }
        return std::make_shared<toppra::PiecewisePolyPath>(segs, std::vector<double>(S.begin(), S.end()));
    }
    // —— 多点：Cubic Hermite ——
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildCubicHermiteMulti(const std::vector<Eigen::VectorXd>& q,
                                          const std::vector<toppra::value_type>& S) {
        auto v = estimateVelsCatmull(q, S);
        clampNodeVels(v, q, /*k=*/1.0);
        toppra::Vectors pos(q.begin(), q.end());
        toppra::Vectors vel(v.begin(), v.end());
        auto herm = toppra::PiecewisePolyPath::CubicHermiteSpline(pos, vel, S);
        return std::make_shared<toppra::PiecewisePolyPath>(herm);
    }
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildNaturalMulti(const std::vector<Eigen::VectorXd>& q,
                                     const std::vector<toppra::value_type>& S) {
        using PWP = toppra::PiecewisePolyPath;
        const size_t M = q.size();
        if (M < 2 || M != S.size()) return nullptr;
        // positions
        toppra::Vectors pos(q.begin(), q.end());
        // times（全局自变量，需严格递增）
        toppra::Vector times(static_cast<int>(M));
        for (size_t i = 0; i < M; ++i) times[static_cast<int>(i)] = S[i];
        //Natural
        toppra::BoundaryCondFull bcA = { toppra::BoundaryCond("clamped"),
                                         toppra::BoundaryCond("clamped") };
        auto path = PWP::CubicSpline(pos, times, bcA);
        return std::make_shared<PWP>(path);
        // 手动指定二阶导为 0 ——
        // const int DoF = static_cast<int>(q[0].size());
        // toppra::BoundaryCondFull bcB = {
        //     toppra::BoundaryCond(2, Eigen::VectorXd::Zero(DoF)),
        //     toppra::BoundaryCond(2, Eigen::VectorXd::Zero(DoF))
        // };
        // auto path2 = PWP::CubicSpline(pos, times, bcB);
        // return std::make_shared<PWP>(path2);
    }
    // —— 多点：Quintic C² ——
    std::shared_ptr<toppra::PiecewisePolyPath>
    ToppraBSpline::buildQuinticC2Multi(const std::vector<Eigen::VectorXd>& q,
                                       const std::vector<toppra::value_type>& S) {
        const size_t M = q.size(), DoF = q[0].size();
        auto v = estimateVelsCatmull(q, S);
        clampNodeVels(v, q, /*k=*/1.0);
        auto a = estimateAccelsSecondDiff(q, S);
        toppra::Matrices segs; segs.reserve(M-1);
        for (size_t k=0;k+1<M;++k) {
            const double ds = std::max<double>(S[k+1]-S[k], 1e-12);
            const Eigen::VectorXd& q0=q[k];   const Eigen::VectorXd& q1=q[k+1];
            const Eigen::VectorXd& v0=v[k];   const Eigen::VectorXd& v1=v[k+1];
            const Eigen::VectorXd& a0=a[k];   const Eigen::VectorXd& a1=a[k+1];
            const Eigen::VectorXd  dq = q1-q0;
            Eigen::VectorXd A0 = q0;
            Eigen::VectorXd A1 = v0 * ds;
            Eigen::VectorXd A2 = a0 * (ds*ds) / 2.0;
            Eigen::VectorXd C3 = ( 10.0*dq - (6.0*A1 + 1.5*(a0*ds*ds)) - (4.0*(v1*ds) - 0.5*(a1*ds*ds)) );
            Eigen::VectorXd C4 = (-15.0*dq + (8.0*A1 + 1.5*(a0*ds*ds)) + (7.0*(v1*ds) - 1.0*(a1*ds*ds)) );
            Eigen::VectorXd C5 = (  6.0*dq - (3.0*A1 + 0.5*(a0*ds*ds)) - (3.0*(v1*ds) - 0.5*(a1*ds*ds)) );
            toppra::Matrix seg(6, DoF);
            seg.row(0)=C5.transpose();
            seg.row(1)=C4.transpose();
            seg.row(2)=C3.transpose();
            seg.row(3)=A2.transpose();
            seg.row(4)=A1.transpose();
            seg.row(5)=A0.transpose();
            segs.emplace_back(std::move(seg));
        }
        return std::make_shared<toppra::PiecewisePolyPath>(segs, std::vector<double>(S.begin(), S.end()));
    }
    std::vector<TrajSample> ToppraBSpline::sampleTrajectory(const TrajPtr &traj, double dt) {
        std::vector<TrajSample> out;
        if (!traj) return out;