feat:add joint waypoints plan
This commit is contained in:
parent
5880a20fce
commit
058ea0f505
@ -20,9 +20,9 @@
|
||||
</Camera>
|
||||
|
||||
<DexHand>
|
||||
<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" />
|
||||
</RH56DFTP>
|
||||
<!-- <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" />-->
|
||||
<!-- </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="true" toolFrame="L_FINGER_TIP">
|
||||
<LeftArmCan id = " " devId = " " channelId ="0" enable="false" 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="false" toolFrame="R_FINGER_TIP">
|
||||
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
|
||||
<RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP">
|
||||
<!-- <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="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="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="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"/>
|
||||
|
||||
@ -1,51 +1,51 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<robot name="dual_arm">
|
||||
<mujoco>
|
||||
<compiler
|
||||
meshdir="meshes"
|
||||
balanceinertia="true"
|
||||
discardvisual="false" />
|
||||
</mujoco>
|
||||
<!-- <mujoco>-->
|
||||
<!-- <compiler-->
|
||||
<!-- meshdir="meshes"-->
|
||||
<!-- balanceinertia="true"-->
|
||||
<!-- discardvisual="false" />-->
|
||||
<!-- </mujoco>-->
|
||||
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.6" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="0.05" length="1.2"/>
|
||||
</geometry>
|
||||
<material name="gray">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
</visual>
|
||||
<!-- <link name="base_link">-->
|
||||
<!-- <visual>-->
|
||||
<!-- <origin xyz="0 0 0.6" rpy="0 0 0"/>-->
|
||||
<!-- <geometry>-->
|
||||
<!-- <cylinder radius="0.05" length="1.2"/>-->
|
||||
<!-- </geometry>-->
|
||||
<!-- <material name="gray">-->
|
||||
<!-- <color rgba="0.5 0.5 0.5 1.0"/>-->
|
||||
<!-- </material>-->
|
||||
<!-- </visual>-->
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0.6" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="0.05" length="1.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<!-- <collision>-->
|
||||
<!-- <origin xyz="0 0 0.6" rpy="0 0 0"/>-->
|
||||
<!-- <geometry>-->
|
||||
<!-- <cylinder radius="0.05" length="1.2"/>-->
|
||||
<!-- </geometry>-->
|
||||
<!-- </collision>-->
|
||||
|
||||
<inertial>
|
||||
<!-- <inertial>-->
|
||||
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<mass value="25.4469"/>
|
||||
<!-- <origin xyz="0 0 0" rpy="0 0 0"/>-->
|
||||
<!-- <mass value="25.4469"/>-->
|
||||
|
||||
<inertia
|
||||
ixx="3.06953"
|
||||
ixy="0.0"
|
||||
ixz="0.0"
|
||||
iyy="3.06953"
|
||||
iyz="0.0"
|
||||
izz="0.03181"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- <inertia-->
|
||||
<!-- ixx="3.06953"-->
|
||||
<!-- ixy="0.0"-->
|
||||
<!-- ixz="0.0"-->
|
||||
<!-- iyy="3.06953"-->
|
||||
<!-- iyz="0.0"-->
|
||||
<!-- izz="0.03181"/>-->
|
||||
<!-- </inertial>-->
|
||||
<!-- </link>-->
|
||||
|
||||
<joint name="base_fixed" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 1.2"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="PELVIS_S"/>
|
||||
</joint>
|
||||
<!-- <joint name="base_fixed" type="fixed">-->
|
||||
<!-- <origin rpy="0 0 0" xyz="0 0 1.2"/>-->
|
||||
<!-- <parent link="base_link"/>-->
|
||||
<!-- <child link="PELVIS_S"/>-->
|
||||
<!-- </joint>-->
|
||||
|
||||
<link name="PELVIS_S">
|
||||
<inertial>
|
||||
|
||||
@ -1,128 +1,177 @@
|
||||
psi,q1,q2,q3,q4,q5,q6,q7
|
||||
0.000000000,0.002038980,1.340620000,0.000000000,0.522261000,0.000000000,-0.000210733,-0.094236400
|
||||
0.000000000,0.578202939,0.486316887,0.740005579,1.570796327,0.625727922,0.531578664,0.175673351
|
||||
0.000000000,0.440761487,0.565335165,0.964336466,1.570796327,0.491292823,0.442355133,0.112628007
|
||||
0.000000000,0.371238784,0.622365950,1.090264075,1.575500039,0.409159634,0.381149325,0.079289637
|
||||
0.000000000,0.330463834,0.665306397,1.171413265,1.580187790,0.353118626,0.336100996,0.059064268
|
||||
0.000000000,0.303629491,0.698061413,1.228156884,1.584859682,0.312714061,0.302027645,0.045779819
|
||||
0.000000000,0.284432039,0.723308206,1.269901344,1.589515814,0.282624592,0.275821335,0.036565409
|
||||
0.000000000,0.269759165,0.742935173,1.301736480,1.594156283,0.259722475,0.255419601,0.029896996
|
||||
0.000000000,0.257917625,0.758312994,1.326716496,1.598781185,0.241997632,0.239372235,0.024895220
|
||||
0.000000000,0.247921267,0.770456663,1.346806717,1.603390613,0.228082340,0.226624470,0.021022719
|
||||
0.000000000,0.239166095,0.780127414,1.363333864,1.607984658,0.217010934,0.216393752,0.017936958
|
||||
0.000000000,0.231267578,0.787900642,1.377223781,1.612563410,0.208084987,0.208092599,0.015412337
|
||||
0.000000000,0.223973270,0.794213370,1.389137788,1.617126957,0.200791220,0.201276831,0.013296323
|
||||
0.000000000,0.217113255,0.799398540,1.399556180,1.621675385,0.194748299,0.195609113,0.011483536
|
||||
0.000000000,0.210570785,0.803710343,1.408832069,1.626208778,0.189670728,0.190832474,0.009899847
|
||||
0.000000000,0.204264237,0.807343170,1.417227480,1.630727217,0.185343528,0.186750724,0.008492307
|
||||
0.000000000,0.198135690,0.810445914,1.424938288,1.635230785,0.181604070,0.183213775,0.007222605
|
||||
0.000000000,0.192143504,0.813132804,1.432111826,1.639719559,0.178328830,0.180106596,0.006062701
|
||||
0.000000000,0.186257388,0.815491634,1.438859529,1.644193617,0.175423635,0.177340836,0.004991872
|
||||
0.000000000,0.180455066,0.817590024,1.445266121,1.648653034,0.172816415,0.174848488,0.003994663
|
||||
0.000000000,0.174719980,0.819480182,1.451396347,1.653097884,0.170451797,0.172577060,0.003059449
|
||||
0.000000000,0.169039695,0.821202537,1.457299956,1.657528240,0.168287051,0.170485916,0.002177423
|
||||
0.000000000,0.163404770,0.822788501,1.463015392,1.661944171,0.166289047,0.168543487,0.001341855
|
||||
0.000000000,0.157807954,0.824262581,1.468572550,1.666345748,0.164431955,0.166725144,0.000547564
|
||||
0.000000000,0.152243613,0.825643979,1.473994836,1.670733038,0.162695519,0.165011583,-0.000209470
|
||||
0.000000000,0.146707313,0.826947825,1.479300719,1.675106107,0.161063745,0.163387594,-0.000932394
|
||||
0.000000000,0.141195517,0.828186109,1.484504889,1.679465019,0.159523905,0.161841116,-0.001623691
|
||||
0.000000000,0.135705364,0.829368399,1.489619141,1.683809837,0.158065790,0.160362521,-0.002285340
|
||||
0.000000000,0.130234505,0.830502387,1.494653037,1.688140624,0.156681132,0.158944065,-0.002918932
|
||||
0.000000000,0.124780985,0.831594308,1.499614410,1.692457439,0.155363175,0.157579468,-0.003525759
|
||||
0.000000000,0.119343153,0.832649258,1.504509746,1.696760341,0.154106335,0.156263589,-0.004106883
|
||||
0.000000000,0.113919592,0.833671449,1.509344478,1.701049387,0.152905953,0.154992182,-0.004663182
|
||||
0.000000000,0.108509076,0.834664389,1.514123204,1.705324634,0.151758094,0.153761706,-0.005195391
|
||||
0.000000000,0.103110526,0.835631035,1.518849866,1.709586135,0.150659401,0.152569174,-0.005704133
|
||||
0.000000000,0.097722982,0.836573903,1.523527872,1.713833945,0.149606979,0.151412044,-0.006189938
|
||||
0.000000000,0.092345584,0.837495157,1.528160207,1.718068116,0.148598304,0.150288131,-0.006653263
|
||||
0.000000000,0.086977554,0.838396676,1.532749505,1.722288697,0.147631153,0.149195535,-0.007094505
|
||||
0.000000000,0.081618181,0.839280108,1.537298113,1.726495738,0.146703554,0.148132591,-0.007514010
|
||||
0.000000000,0.076266813,0.840146913,1.541808142,1.730689288,0.145813738,0.147097827,-0.007912084
|
||||
0.000000000,0.070922846,0.840998392,1.546281501,1.734869392,0.144960109,0.146089928,-0.008288996
|
||||
0.000000000,0.065585720,0.841835716,1.550719930,1.739036098,0.144141215,0.145107715,-0.008644986
|
||||
0.000000000,0.060254914,0.842659947,1.555125023,1.743189448,0.143355729,0.144150120,-0.008980270
|
||||
0.000000000,0.054929937,0.843472051,1.559498249,1.747329486,0.142602429,0.143216170,-0.009295039
|
||||
0.000000000,0.049610331,0.844272918,1.563840968,1.751456254,0.141880187,0.142304975,-0.009589466
|
||||
0.000000000,0.044295663,0.845063363,1.568154441,1.755569792,0.141187955,0.141415716,-0.009863707
|
||||
0.000000000,0.038985523,0.845844147,1.572439847,1.759670141,0.140524756,0.140547637,-0.010117902
|
||||
0.000000000,0.033679525,0.846615971,1.576698286,1.763757337,0.139889677,0.139700037,-0.010352177
|
||||
0.000000000,0.028377300,0.847379495,1.580930790,1.767831419,0.139281864,0.138872263,-0.010566647
|
||||
0.000000000,0.023078501,0.848135333,1.585138328,1.771892423,0.138700512,0.138063706,-0.010761414
|
||||
0.000000000,0.017782795,0.848884063,1.589321815,1.775940383,0.138144864,0.137273797,-0.010936570
|
||||
0.000000000,0.012489864,0.849626229,1.593482110,1.779975333,0.137614205,0.136502000,-0.011092198
|
||||
0.000000000,0.007199406,0.850362343,1.597620030,1.783997306,0.137107858,0.135747816,-0.011228373
|
||||
0.000000000,0.001911132,0.851092890,1.601736342,1.788006333,0.136625184,0.135010771,-0.011345161
|
||||
0.000000000,-0.003375237,0.851818328,1.605831779,1.792002444,0.136165576,0.134290419,-0.011442619
|
||||
0.000000000,-0.008659966,0.852539094,1.609907031,1.795985670,0.135728457,0.133586342,-0.011520800
|
||||
0.000000000,-0.013943309,0.853255600,1.613962757,1.799956038,0.135313278,0.132898140,-0.011579750
|
||||
0.000000000,-0.019225512,0.853968239,1.617999581,1.803913576,0.134919519,0.132225440,-0.011619505
|
||||
0.000000000,-0.024506812,0.854677386,1.622018097,1.807858309,0.134546681,0.131567884,-0.011640101
|
||||
0.000000000,-0.029787434,0.855383399,1.626018873,1.811790264,0.134194290,0.130925134,-0.011641565
|
||||
0.000000000,-0.035067598,0.856086617,1.630002447,1.815709463,0.133861895,0.130296872,-0.011623919
|
||||
0.000000000,-0.040347515,0.856787368,1.633969333,1.819615930,0.133549062,0.129682792,-0.011587182
|
||||
0.000000000,-0.045627388,0.857485962,1.637920022,1.823509687,0.133255378,0.129082605,-0.011531367
|
||||
0.000000000,-0.050907414,0.858182698,1.641854983,1.827390756,0.132980448,0.128496037,-0.011456482
|
||||
0.000000000,-0.056187785,0.858877862,1.645774662,1.831259155,0.132723892,0.127922826,-0.011362533
|
||||
0.000000000,-0.061468683,0.859571728,1.649679488,1.835114905,0.132485349,0.127362723,-0.011249521
|
||||
0.000000000,-0.066750288,0.860264561,1.653569869,1.838958023,0.132264471,0.126815490,-0.011117442
|
||||
0.000000000,-0.072032772,0.860956612,1.657446195,1.842788526,0.132060923,0.126280903,-0.010966291
|
||||
0.000000000,-0.077316303,0.861648126,1.661308841,1.846606431,0.131874386,0.125758746,-0.010796058
|
||||
0.000000000,-0.082601044,0.862339336,1.665158164,1.850411752,0.131704553,0.125248813,-0.010606730
|
||||
0.000000000,-0.087887153,0.863030468,1.668994507,1.854204504,0.131551128,0.124750909,-0.010398291
|
||||
0.000000000,-0.093174784,0.863721739,1.672818198,1.857984700,0.131413827,0.124264846,-0.010170721
|
||||
0.000000000,-0.098464085,0.864413358,1.676629550,1.861752352,0.131292378,0.123790447,-0.009923998
|
||||
0.000000000,-0.103755203,0.865105528,1.680428865,1.865507472,0.131186518,0.123327542,-0.009658099
|
||||
0.000000000,-0.109048278,0.865798443,1.684216430,1.869250069,0.131095995,0.122875967,-0.009372995
|
||||
0.000000000,-0.114343449,0.866492291,1.687992522,1.872980154,0.131020565,0.122435568,-0.009068657
|
||||
0.000000000,-0.119640850,0.867187256,1.691757406,1.876697735,0.130959994,0.122006196,-0.008745051
|
||||
0.000000000,-0.124940612,0.867883512,1.695511334,1.880402820,0.130914057,0.121587711,-0.008402144
|
||||
0.000000000,-0.130242862,0.868581230,1.699254550,1.884095415,0.130882535,0.121179977,-0.008039898
|
||||
0.000000000,-0.135547726,0.869280574,1.702987286,1.887775526,0.130865219,0.120782865,-0.007658274
|
||||
0.000000000,-0.140855324,0.869981705,1.706709765,1.891443158,0.130861907,0.120396252,-0.007257229
|
||||
0.000000000,-0.146165776,0.870684776,1.710422202,1.895098315,0.130872404,0.120020021,-0.006836721
|
||||
0.000000000,-0.151479199,0.871389938,1.714124799,1.898741001,0.130896520,0.119654058,-0.006396704
|
||||
0.000000000,-0.156795705,0.872097336,1.717817754,1.902371216,0.130934076,0.119298259,-0.005937130
|
||||
0.000000000,-0.162115406,0.872807111,1.721501253,1.905988964,0.130984894,0.118952519,-0.005457950
|
||||
0.000000000,-0.167438410,0.873519398,1.725175477,1.909594244,0.131048805,0.118616741,-0.004959112
|
||||
0.000000000,-0.172764825,0.874234332,1.728840596,1.913187056,0.131125646,0.118290833,-0.004440562
|
||||
0.000000000,-0.178094754,0.874952041,1.732496776,1.916767399,0.131215259,0.117974706,-0.003902247
|
||||
0.000000000,-0.183428299,0.875672650,1.736144172,1.920335270,0.131317489,0.117668276,-0.003344109
|
||||
0.000000000,-0.188765561,0.876396281,1.739782936,1.923890667,0.131432190,0.117371461,-0.002766090
|
||||
0.000000000,-0.194106637,0.877123052,1.743413209,1.927433586,0.131559218,0.117084186,-0.002168129
|
||||
0.000000000,-0.199451624,0.877853078,1.747035130,1.930964022,0.131698435,0.116806378,-0.001550166
|
||||
0.000000000,-0.204800616,0.878586471,1.750648827,1.934481969,0.131849707,0.116537967,-0.000912137
|
||||
0.000000000,-0.210153705,0.879323339,1.754254425,1.937987422,0.132012905,0.116278888,-0.000253978
|
||||
0.000000000,-0.215510983,0.880063789,1.757852042,1.941480372,0.132187903,0.116029078,0.000424378
|
||||
0.000000000,-0.220872538,0.880807923,1.761441791,1.944960813,0.132374581,0.115788479,0.001122999
|
||||
0.000000000,-0.226238458,0.881555841,1.765023777,1.948428734,0.132572820,0.115557034,0.001841952
|
||||
0.000000000,-0.231608829,0.882307642,1.768598103,1.951884127,0.132782508,0.115334690,0.002581309
|
||||
0.000000000,-0.236983736,0.883063421,1.772164864,1.955326981,0.133003534,0.115121397,0.003341141
|
||||
0.000000000,-0.242363261,0.883823269,1.775724150,1.958757284,0.133235792,0.114917109,0.004121521
|
||||
0.000000000,-0.247747485,0.884587277,1.779276047,1.962175024,0.133479179,0.114721780,0.004922524
|
||||
0.000000000,-0.253136490,0.885355532,1.782820636,1.965580188,0.133733595,0.114535369,0.005744225
|
||||
0.000000000,-0.258530353,0.886128121,1.786357992,1.968972763,0.133998943,0.114357837,0.006586701
|
||||
0.000000000,-0.263929153,0.886905126,1.789888186,1.972352734,0.134275129,0.114189147,0.007450029
|
||||
0.000000000,-0.269332966,0.887686629,1.793411286,1.975720085,0.134562062,0.114029264,0.008334290
|
||||
0.000000000,-0.274741865,0.888472709,1.796927352,1.979074800,0.134859656,0.113878158,0.009239563
|
||||
0.000000000,-0.280155926,0.889263443,1.800436443,1.982416862,0.135167823,0.113735798,0.010165930
|
||||
0.000000000,-0.285575221,0.890058905,1.803938611,1.985746253,0.135486482,0.113602158,0.011113473
|
||||
0.000000000,-0.290999820,0.890859169,1.807433906,1.989062955,0.135815553,0.113477212,0.012082277
|
||||
0.000000000,-0.296429795,0.891664305,1.810922373,1.992366949,0.136154957,0.113360936,0.013072425
|
||||
0.000000000,-0.301865215,0.892474384,1.814404052,1.995658213,0.136504620,0.113253312,0.014084004
|
||||
0.000000000,-0.307306147,0.893289473,1.817878980,1.998936727,0.136864469,0.113154318,0.015117101
|
||||
0.000000000,-0.312752658,0.894109637,1.821347191,2.002202469,0.137234433,0.113063939,0.016171803
|
||||
0.000000000,-0.318204815,0.894934939,1.824808713,2.005455417,0.137614443,0.112982160,0.017248199
|
||||
0.000000000,-0.323662683,0.895765444,1.828263571,2.008695547,0.138004434,0.112908967,0.018346379
|
||||
0.000000000,-0.329126324,0.896601210,1.831711788,2.011922835,0.138404340,0.112844349,0.019466434
|
||||
0.000000000,-0.334595804,0.897442297,1.835153381,2.015137257,0.138814099,0.112788297,0.020608455
|
||||
0.000000000,-0.340071182,0.898288761,1.838588365,2.018338785,0.139233651,0.112740803,0.021772535
|
||||
0.000000000,-0.345552521,0.899140659,1.842016750,2.021527395,0.139662937,0.112701861,0.022958766
|
||||
0.000000000,-0.351039881,0.899998044,1.845438544,2.024703059,0.140101899,0.112671467,0.024167244
|
||||
0.000000000,-0.356533320,0.900860968,1.848853751,2.027865749,0.140550484,0.112649617,0.025398064
|
||||
0.000000000,-0.362032897,0.901729483,1.852262372,2.031015436,0.141008637,0.112636311,0.026651320
|
||||
0.000000000,-0.367538669,0.902603637,1.855664402,2.034152091,0.141476307,0.112631550,0.027927111
|
||||
0.000000000,-0.373050693,0.903483479,1.859059837,2.037275683,0.141953442,0.112635335,0.029225533
|
||||
0.000000000,-0.378569025,0.904369054,1.862448667,2.040386181,0.142439995,0.112647669,0.030546685
|
||||
0.000000000,-0.384093719,0.905260407,1.865830879,2.043483554,0.142935918,0.112668558,0.031890665
|
||||
0.000000000,-0.389624828,0.906157581,1.869206457,2.046567770,0.143441165,0.112698008,0.033257574
|
||||
0.000000000,-0.395162407,0.907060618,1.872575383,2.049638794,0.143955692,0.112736027,0.034647511
|
||||
0.000000000,0.293251835,0.758274585,1.296200413,1.570796327,-1.328603785,0.029738090,-0.243184799
|
||||
0.000000000,0.250086584,0.987999944,1.558700421,1.570796327,-1.558796269,0.000072292,-0.012048253
|
||||
0.000000000,0.254718830,1.039656166,1.614136195,1.577578275,-1.610593775,-0.009025684,0.046125314
|
||||
0.000000000,0.261036192,1.072261700,1.649251612,1.584215943,-1.643600103,-0.016981472,0.085189632
|
||||
0.000000000,0.267611406,1.094382460,1.673588191,1.590709595,-1.666295983,-0.024886645,0.113748291
|
||||
0.000000000,0.274059516,1.109714288,1.691108731,1.597059459,-1.682328199,-0.033042438,0.135510724
|
||||
0.000000000,0.280282638,1.120309738,1.703952262,1.603265727,-1.693717627,-0.041520863,0.152536084
|
||||
0.000000000,0.286283897,1.127473061,1.713447991,1.609328561,-1.701747164,-0.050310820,0.166132315
|
||||
0.000000000,0.292103210,1.132099435,1.720491885,1.615248090,-1.707297496,-0.059370845,0.177196824
|
||||
0.000000000,0.297791027,1.134830956,1.725716897,1.621024417,-1.711001029,-0.068651392,0.186373132
|
||||
0.000000000,0.303396968,1.136139730,1.729582201,1.626657617,-1.713323590,-0.078104630,0.194134291
|
||||
0.000000000,0.308965157,1.136377620,1.732425797,1.632147741,-1.714613160,-0.087688428,0.200832821
|
||||
0.000000000,0.314532769,1.135808781,1.734498413,1.637494815,-1.715131682,-0.097367501,0.206733349
|
||||
0.000000000,0.320130097,1.134632261,1.735986728,1.642698847,-1.715077120,-0.107113203,0.212035277
|
||||
0.000000000,0.325781276,1.132998327,1.737029929,1.647759821,-1.714599391,-0.116902735,0.216889133
|
||||
0.000000000,0.331505226,1.131020502,1.737731782,1.652677706,-1.713812131,-0.126718186,0.221408639
|
||||
0.000000000,0.337316601,1.128784578,1.738169564,1.657452454,-1.712801504,-0.136545589,0.225679720
|
||||
0.000000000,0.343226634,1.126355382,1.738400710,1.662083998,-1.711632845,-0.146374089,0.229767287
|
||||
0.000000000,0.349243858,1.123781896,1.738467780,1.666572263,-1.710355678,-0.156195244,0.233720347
|
||||
0.000000000,0.355374700,1.121101117,1.738402183,1.670917156,-1.709007533,-0.166002460,0.237575874
|
||||
0.000000000,0.361623948,1.118340983,1.738226968,1.675118577,-1.707616827,-0.175790547,0.241361730
|
||||
0.000000000,0.367995123,1.115522584,1.737958914,1.679176414,-1.706205056,-0.185555370,0.245098877
|
||||
0.000000000,0.374490772,1.112661836,1.737610104,1.683090548,-1.704788463,-0.195293580,0.248803052
|
||||
0.000000000,0.381112689,1.109770746,1.737189098,1.686860851,-1.703379291,-0.205002413,0.252486032
|
||||
0.000000000,0.387862085,1.106858365,1.736701822,1.690487191,-1.701986748,-0.214679533,0.256156589
|
||||
0.000000000,0.394739723,1.103931514,1.736152226,1.693969430,-1.700617722,-0.224322917,0.259821217
|
||||
0.000000000,0.401746015,1.100995333,1.735542782,1.697307426,-1.699277342,-0.233930769,0.263484676
|
||||
0.000000000,0.408881100,1.098053688,1.734874862,1.700501036,-1.697969382,-0.243501452,0.267150407
|
||||
0.000000000,0.416144903,1.095109485,1.734149011,1.703550113,-1.696696586,-0.253033443,0.270820844
|
||||
0.000000000,0.423537174,1.092164908,1.733365164,1.706454512,-1.695460901,-0.262525293,0.274497650
|
||||
0.000000000,0.431057527,1.089221592,1.732522797,1.709214087,-1.694263660,-0.271975604,0.278181894
|
||||
0.000000000,0.438705462,1.086280764,1.731621052,1.711828693,-1.693105713,-0.281383007,0.281874188
|
||||
0.000000000,0.446480386,1.083343336,1.730658821,1.714298189,-1.691987535,-0.290746150,0.285574784
|
||||
0.000000000,0.454381625,1.080409987,1.729634815,1.716622436,-1.690909301,-0.300063689,0.289283655
|
||||
0.000000000,0.462408439,1.077481219,1.728547612,1.718801298,-1.689870942,-0.309334276,0.293000547
|
||||
0.000000000,0.470560028,1.074557400,1.727395700,1.720834646,-1.688872188,-0.318556559,0.296725028
|
||||
0.000000000,0.478835538,1.071638796,1.726177497,1.722722354,-1.687912602,-0.327729179,0.300456517
|
||||
0.000000000,0.487234070,1.068725597,1.724891381,1.724464304,-1.686991600,-0.336850765,0.304194305
|
||||
0.000000000,0.495754680,1.065817932,1.723535697,1.726060384,-1.686108472,-0.345919933,0.307937579
|
||||
0.000000000,0.504396387,1.062915888,1.722108778,1.727510489,-1.685262390,-0.354935290,0.311685431
|
||||
0.000000000,0.513158172,1.060019512,1.720608945,1.728814522,-1.684452417,-0.363895433,0.315436869
|
||||
0.000000000,0.522038981,1.057128827,1.719034519,1.729972396,-1.683677515,-0.372798945,0.319190822
|
||||
0.000000000,0.531037733,1.054243830,1.717383824,1.730984029,-1.682936542,-0.381644403,0.322946147
|
||||
0.000000000,0.540153313,1.051364501,1.715655189,1.731849352,-1.682228263,-0.390430375,0.326701630
|
||||
0.000000000,0.549384581,1.048490801,1.713846952,1.732568304,-1.681551341,-0.399155422,0.330455992
|
||||
0.000000000,0.558730372,1.045622678,1.711957460,1.733140834,-1.680904341,-0.407818104,0.334207883
|
||||
0.000000000,0.568189497,1.042760064,1.709985069,1.733566901,-1.680285729,-0.416416973,0.337955889
|
||||
0.000000000,0.577760749,1.039902877,1.707928148,1.733846474,-1.679693866,-0.424950583,0.341698527
|
||||
0.000000000,0.587442898,1.037051021,1.705785070,1.733979534,-1.679127008,-0.433417490,0.345434249
|
||||
0.000000000,0.597234700,1.034204385,1.703554221,1.733966070,-1.678583306,-0.441816248,0.349161436
|
||||
0.000000000,0.607134897,1.031362843,1.701233991,1.733806084,-1.678060795,-0.450145421,0.352878400
|
||||
0.000000000,0.617142217,1.028526250,1.698822775,1.733499587,-1.677557397,-0.458403575,0.356583379
|
||||
0.000000000,0.627255381,1.025694446,1.696318972,1.733046601,-1.677070913,-0.466589287,0.360274540
|
||||
0.000000000,0.637473100,1.022867248,1.693720982,1.732447159,-1.676599022,-0.474701143,0.363949972
|
||||
0.000000000,0.647794085,1.020044453,1.691027199,1.731701303,-1.676139274,-0.482737740,0.367607687
|
||||
0.000000000,0.658217042,1.017225835,1.688236014,1.730809087,-1.675689085,-0.490697690,0.371245616
|
||||
0.000000000,0.668740680,1.014411142,1.685345808,1.729770573,-1.675245734,-0.498579618,0.374861606
|
||||
0.000000000,0.679363713,1.011600094,1.682354949,1.728585833,-1.674806356,-0.506382168,0.378453418
|
||||
0.000000000,0.690084861,1.008792381,1.679261785,1.727254948,-1.674367937,-0.514104000,0.382018723
|
||||
0.000000000,0.700902857,1.005987660,1.676064643,1.725778009,-1.673927308,-0.521743795,0.385555100
|
||||
0.000000000,0.711816448,1.003185553,1.672761822,1.724155114,-1.673481136,-0.529300255,0.389060032
|
||||
0.000000000,0.722824400,1.000385642,1.669351585,1.722386368,-1.673025921,-0.536772102,0.392530898
|
||||
0.000000000,0.733925499,0.997587468,1.665832156,1.720471886,-1.672557987,-0.544158082,0.395964977
|
||||
0.000000000,0.745118558,0.994790524,1.662201712,1.718411787,-1.672073470,-0.551456965,0.399359435
|
||||
0.000000000,0.756402423,0.991994257,1.658458371,1.716206196,-1.671568317,-0.558667547,0.402711326
|
||||
0.000000000,0.767775970,0.989198057,1.654600193,1.713855246,-1.671038269,-0.565788646,0.406017582
|
||||
0.000000000,0.779238118,0.986401256,1.650625158,1.711359072,-1.670478855,-0.572819109,0.409275011
|
||||
0.000000000,0.790787829,0.983603125,1.646531169,1.708717813,-1.669885378,-0.579757807,0.412480287
|
||||
0.000000000,0.802424115,0.980802862,1.642316031,1.705931612,-1.669252905,-0.586603636,0.415629945
|
||||
0.000000000,0.814146043,0.977999594,1.637977442,1.703000613,-1.668576251,-0.593355521,0.418720372
|
||||
0.000000000,0.825952742,0.975192365,1.633512981,1.699924964,-1.667849966,-0.600012409,0.421747798
|
||||
0.000000000,0.837843406,0.972380129,1.628920093,1.696704808,-1.667068313,-0.606573273,0.424708286
|
||||
0.000000000,0.849817307,0.969561745,1.624196069,1.693340293,-1.666225257,-0.613037111,0.427597723
|
||||
0.000000000,0.861873796,0.966735967,1.619338032,1.689831562,-1.665314438,-0.619402942,0.430411804
|
||||
0.000000000,0.874012317,0.963901431,1.614342913,1.686178754,-1.664329149,-0.625669805,0.433146019
|
||||
0.000000000,0.886232411,0.961056649,1.609207434,1.682382009,-1.663262309,-0.631836760,0.435795641
|
||||
0.000000000,0.898533727,0.958199994,1.603928076,1.678441457,-1.662106435,-0.637902883,0.438355702
|
||||
0.000000000,0.910916037,0.955329684,1.598501056,1.674357224,-1.660853605,-0.643867262,0.440820977
|
||||
0.000000000,0.923379242,0.952443770,1.592922291,1.670129428,-1.659495422,-0.649728996,0.443185960
|
||||
0.000000000,0.935923388,0.949540115,1.587187367,1.665758179,-1.658022969,-0.655487192,0.445444838
|
||||
0.000000000,0.948548681,0.946616377,1.581291495,1.661243577,-1.656426756,-0.661140956,0.447591458
|
||||
0.000000000,0.961255505,0.943669979,1.575229462,1.656585710,-1.654696668,-0.666689390,0.449619294
|
||||
0.000000000,0.974044439,0.940698091,1.568995586,1.651784653,-1.652821891,-0.672131588,0.451521408
|
||||
0.000000000,0.986916283,0.937697588,1.562583646,1.646840468,-1.650790840,-0.677466623,0.453290397
|
||||
0.000000000,0.999872076,0.934665021,1.555986816,1.641753201,-1.648591064,-0.682693542,0.454918344
|
||||
0.000000000,1.012913137,0.931596572,1.549197580,1.636522881,-1.646209141,-0.687811353,0.456396747
|
||||
0.000000000,1.026041088,0.928488000,1.542207639,1.631149519,-1.643630554,-0.692819017,0.457716449
|
||||
0.000000000,1.039257901,0.925334586,1.535007792,1.625633104,-1.640839540,-0.697715426,0.458867536
|
||||
0.000000000,1.052565948,0.922131054,1.527587804,1.619973606,-1.637818914,-0.702499392,0.459839238
|
||||
0.000000000,1.065968052,0.918871490,1.519936244,1.614170968,-1.634549856,-0.707169622,0.460619788
|
||||
0.000000000,1.079467564,0.915549233,1.512040289,1.608225111,-1.631011650,-0.711724691,0.461196265
|
||||
0.000000000,1.093068444,0.912156749,1.503885486,1.602135929,-1.627181371,-0.716163015,0.461554398
|
||||
0.000000000,1.106775364,0.908685463,1.495455457,1.595903284,-1.623033493,-0.720482806,0.461678316
|
||||
0.000000000,1.120593846,0.905125569,1.486731541,1.589527011,-1.618539405,-0.724682026,0.461550249
|
||||
0.000000000,1.134530416,0.901465767,1.477692331,1.583006910,-1.613666797,-0.728758326,0.461150136
|
||||
0.000000000,1.148592822,0.897692941,1.468313092,1.576342747,-1.608378875,-0.732708960,0.460455133
|
||||
0.000000000,1.162790299,0.893791736,1.458565003,1.569534253,-1.602633350,-0.736530688,0.459438966
|
||||
0.000000000,1.177133934,0.889743992,1.448414163,1.562581117,-1.596381091,-0.740219633,0.458071082
|
||||
0.000000000,1.191637142,0.885527982,1.437820252,1.555482988,-1.589564322,-0.743771097,0.456315492
|
||||
0.000000000,1.206316331,0.881117365,1.426734686,1.548239473,-1.582114142,-0.747179303,0.454129186
|
||||
0.000000000,1.221191834,0.876479684,1.415098026,1.540850132,-1.573947010,-0.750437029,0.451459882
|
||||
0.000000000,1.236289270,0.871574171,1.402836188,1.533314474,-1.564959620,-0.753535076,0.448242723
|
||||
0.000000000,1.251641599,0.866348404,1.389854711,1.525631961,-1.555021132,-0.756461457,0.444395270
|
||||
0.000000000,1.267292374,0.860732939,1.376029659,1.517801999,-1.543960797,-0.759200110,0.439809491
|
||||
0.000000000,1.283301206,0.854632183,1.361192256,1.509823937,-1.531547018,-0.761728735,0.434338172
|
||||
0.000000000,1.299753711,0.847907518,1.345100768,1.501697063,-1.517448891,-0.764014853,0.427769876
|
||||
0.000000000,1.316781719,0.840342278,1.327382852,1.493420604,-1.501156940,-0.766007816,0.419777156
|
||||
0.000000000,1.334612022,0.831555008,1.307394739,1.484993719,-1.481788251,-0.767619644,0.409788756
|
||||
0.000000000,1.353726657,0.820703382,1.283749263,1.476415499,-1.457426845,-0.768662883,0.396555090
|
||||
0.000000000,1.376358759,0.803517157,1.249732350,1.467684957,-1.418592456,-0.768291811,0.373816610
|
||||
0.000000000,1.494881519,0.662922654,0.987221696,1.458801032,-1.102236486,-0.721806346,0.172010222
|
||||
0.000000000,1.622376425,0.587008625,0.771885834,1.449762579,-0.914161382,-0.669279429,0.063293625
|
||||
0.000000000,1.741132088,0.546022020,0.594563695,1.440568367,-0.792024842,-0.624694251,0.000642426
|
||||
0.000000000,2.155554266,0.490079021,-0.001316571,1.431217073,-0.478295544,-0.460208109,-0.137582524
|
||||
0.000000000,2.391871198,0.515238742,-0.333359890,1.421707279,-0.328461722,-0.360786644,-0.179375072
|
||||
0.000000000,2.528829630,0.552905712,-0.533538771,1.412037462,-0.232793556,-0.293087092,-0.194688060
|
||||
0.000000000,2.612745002,0.588985362,-0.662201311,1.402205995,-0.165213887,-0.245115316,-0.198850488
|
||||
0.000000000,2.667045396,0.620413529,-0.749106592,1.392211136,-0.114999025,-0.210595521,-0.197528729
|
||||
0.000000000,2.703747443,0.647017619,-0.809843425,1.382051020,-0.076558182,-0.185693838,-0.193294787
|
||||
0.000000000,2.729389595,0.669394033,-0.853235127,1.371723658,-0.046538833,-0.167858916,-0.187447747
|
||||
0.000000000,2.747750395,0.688279875,-0.884634202,1.361226923,-0.022745180,-0.155305438,-0.180683929
|
||||
0.000000000,2.761129762,0.704359440,-0.907473113,1.350558545,-0.003656540,-0.146746205,-0.173389055
|
||||
0.000000000,2.770988545,0.718211663,-0.924048133,1.339716101,0.011817232,-0.141236559,-0.165780362
|
||||
0.000000000,2.778286951,0.730305848,-0.935943933,1.328697007,0.024473831,-0.138076114,-0.157980813
|
||||
0.000000000,2.783673311,0.741013373,-0.944278258,1.317498505,0.034903359,-0.136742868,-0.150059680
|
||||
0.000000000,2.787594299,0.750623415,-0.949850584,1.306117653,0.043543912,-0.136847223,-0.142055389
|
||||
0.000000000,2.790361811,0.759358134,-0.953236370,1.294551311,0.050719591,-0.138099142,-0.133988634
|
||||
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
|
||||
|
||||
|
File diff suppressed because it is too large
Load Diff
@ -13,6 +13,9 @@ 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;
|
||||
|
||||
@ -65,6 +65,7 @@ target_link_libraries(srs_ik_test
|
||||
OsqpEigen::OsqpEigen
|
||||
cmvr_es::utils
|
||||
cmvr_es::ik_solver
|
||||
cmvr_es::planner
|
||||
|
||||
)
|
||||
|
||||
|
||||
@ -21,7 +21,8 @@ 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);
|
||||
|
||||
|
||||
@ -95,11 +95,9 @@ 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) {
|
||||
|
||||
std::vector<double> BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &pose, double psi) {
|
||||
try {
|
||||
// Eigen::VectorXd joints(7);
|
||||
std::vector<double> joints(7, 0);
|
||||
joints.resize(7,0);
|
||||
|
||||
// 目标位置
|
||||
Eigen::Vector3d P_target = pose.block<3, 1>(0, 3);
|
||||
@ -126,7 +124,8 @@ std::vector<double> BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &p
|
||||
// - diff_max > EPS → 太远
|
||||
// - diff_min > EPS → 太近
|
||||
if (diff_max > EPS || diff_min > EPS) {
|
||||
throw std::runtime_error("Pose outside reachable workspace, IK solve failed");
|
||||
std::cerr << "Pose outside reachable workspace, IK solve failed";
|
||||
return false;
|
||||
}
|
||||
|
||||
// 计算肘部角度 关节3
|
||||
@ -181,27 +180,6 @@ std::vector<double> BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &p
|
||||
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 提取=====
|
||||
@ -234,12 +212,11 @@ std::vector<double> BiasSRSIkSolver::inverse_kinematics(const Eigen::MatrixXd &p
|
||||
joints[5] = SupportFunctions::normalize_angle(theta_y - M_PI / 2);
|
||||
joints[6] = SupportFunctions::normalize_angle(psi_z);
|
||||
|
||||
return joints;
|
||||
} catch (const std::exception &e) {
|
||||
throw std::runtime_error(e.what());
|
||||
}
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
Eigen::Matrix3d BiasSRSIkSolver::calc_rotation_matrix(const Eigen::Vector3d &rotation_axis, double rotation_angle) {
|
||||
// 归一化旋转轴
|
||||
Eigen::Vector3d normalized_axis = rotation_axis.normalized();
|
||||
@ -258,6 +235,10 @@ 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);
|
||||
@ -303,7 +284,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);
|
||||
|
||||
@ -332,7 +313,8 @@ bool BiasSRSIkSolver::cal_coefficient_matrix(const Eigen::MatrixXd &pose, Eigen:
|
||||
// - diff_max > EPS → 太远
|
||||
// - diff_min > EPS → 太近
|
||||
if (diff_max > EPS || diff_min > EPS) {
|
||||
throw std::runtime_error("Pose outside reachable workspace, IK solve failed");
|
||||
std::cerr << "Pose outside reachable workspace, IK solve failed" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// 计算肘部角度 关节3
|
||||
@ -375,8 +357,4 @@ 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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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,
|
||||
1, 0, 0, 0.00684256,
|
||||
0, 0, 0, 1;
|
||||
0, 0, 1, 0.00801525,
|
||||
1, 0, 0, 0.00684256,
|
||||
0, 0, 0, 1;
|
||||
|
||||
T_arm_robot << 0, 1, 0, 0,
|
||||
0, 0, -1, 0,
|
||||
-1, 0, 0, 0.042,
|
||||
0, 0, 0, 1;
|
||||
0, 0, -1, 0,
|
||||
-1, 0, 0, 0.042,
|
||||
0, 0, 0, 1;
|
||||
|
||||
T_flange_urdf_mdh_ << 0, 1, 0, 0,
|
||||
0, 0, 1, 0,
|
||||
1, 0, 0, 0,
|
||||
0, 0, 0, 1;
|
||||
0, 0, 1, 0,
|
||||
1, 0, 0, 0,
|
||||
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_
|
||||
1e-3 // cur_branch_cost_threshold_
|
||||
5.5, // lambda_q_
|
||||
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,10 +88,9 @@ bool OptPsiLimitBiasSolver::estimate_state_from_current_joints() {
|
||||
BiasSRSIkSolver::ConfigDirection::INWARD
|
||||
};
|
||||
|
||||
for (BiasSRSIkSolver::ConfigDirection s_dir : dirs) {
|
||||
for (BiasSRSIkSolver::ConfigDirection e_dir : dirs) {
|
||||
for (BiasSRSIkSolver::ConfigDirection w_dir : dirs) {
|
||||
|
||||
for (BiasSRSIkSolver::ConfigDirection s_dir: dirs) {
|
||||
for (BiasSRSIkSolver::ConfigDirection e_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);
|
||||
@ -109,20 +108,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.psi = res.psi;
|
||||
slot.valid = true;
|
||||
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.psi = res.psi;
|
||||
best.s_conf = s;
|
||||
best.e_conf = e;
|
||||
best.w_conf = w;
|
||||
best_score = res.score;
|
||||
best.valid = true;
|
||||
best.psi = res.psi;
|
||||
best.s_conf = s;
|
||||
best.e_conf = e;
|
||||
best.w_conf = w;
|
||||
best_score = res.score;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -134,8 +133,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_wrist_config (dir_from_sign(best.w_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));
|
||||
|
||||
branch_state_ = best;
|
||||
return true;
|
||||
@ -147,8 +146,7 @@ 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) 这一分支下的 ψ 可行区间
|
||||
@ -171,17 +169,16 @@ 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_wrist_config (dir_from_sign(branch.w_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));
|
||||
|
||||
// 4) 解析 IK
|
||||
std::vector<double> q =
|
||||
bias_srs_ik_solver_->inverse_kinematics(target_cal_pose, best_psi);
|
||||
|
||||
if (q.size() != joints_limits.size()) {
|
||||
std::vector<double> q;
|
||||
if (!bias_srs_ik_solver_->inverse_kinematics(target_cal_pose, q, best_psi)) {
|
||||
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) {
|
||||
@ -199,18 +196,17 @@ 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_);
|
||||
@ -221,18 +217,17 @@ 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_wrist_config (dir_from_sign(branch_state_.w_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));
|
||||
|
||||
auto cur_joints_angle =
|
||||
bias_srs_ik_solver_->inverse_kinematics(cur_pose,
|
||||
branch_state_.psi);
|
||||
std::vector<double> cur_joints_angle;
|
||||
|
||||
if (cur_joints_angle.size() != cur_joints_angle_.size()) {
|
||||
if (!bias_srs_ik_solver_->inverse_kinematics(cur_pose, cur_joints_angle,
|
||||
branch_state_.psi)) {
|
||||
branch_state_.valid = false;
|
||||
} else {
|
||||
for (int i = 0; i < static_cast<int>(cur_joints_angle_.size()); ++i) {
|
||||
@ -247,7 +242,7 @@ bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
|
||||
// --- 2) 如果还没有“上一次 ψ + 分支”，用当前关节估一次 ---
|
||||
if (!branch_state_.valid) {
|
||||
if (!estimate_state_from_current_joints()) {
|
||||
return false; // 当前姿态都反推不了分支，直接失败
|
||||
return false; // 当前姿态都反推不了分支，直接失败
|
||||
}
|
||||
}
|
||||
|
||||
@ -257,16 +252,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,
|
||||
branch_state_, q_cur, psi_cur, cost_cur);
|
||||
bool cur_ok = solve_on_branch(target_cal_pose, s_mat, w_mat,
|
||||
branch_state_, q_cur, psi_cur, cost_cur);
|
||||
|
||||
// 若当前分支有解且 cost 足够小，直接用当前分支，不再搜索其它分支
|
||||
if (cur_ok && cost_cur < cur_branch_cost_threshold_) {
|
||||
joints_angle = q_cur;
|
||||
cur_joints_angle_ = joints_angle;
|
||||
branch_state_.psi = psi_cur;
|
||||
joints_angle = q_cur;
|
||||
cur_joints_angle_ = joints_angle;
|
||||
branch_state_.psi = psi_cur;
|
||||
branch_state_.valid = true;
|
||||
// s_conf/e_conf/w_conf 不变
|
||||
return true;
|
||||
@ -274,18 +269,18 @@ bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
|
||||
|
||||
// --- 5) 当前分支不合适：在 8 个分支中全局搜索最小 cost ---
|
||||
BranchPsiState best_state{};
|
||||
bool have_candidate = false;
|
||||
double best_cost = std::numeric_limits<double>::infinity();
|
||||
bool have_candidate = false;
|
||||
double best_cost = std::numeric_limits<double>::infinity();
|
||||
std::vector<double> best_q;
|
||||
|
||||
// 先把“当前分支的候选”也纳入考虑，避免再算一遍
|
||||
if (cur_ok) {
|
||||
have_candidate = true;
|
||||
best_cost = cost_cur;
|
||||
best_q = q_cur;
|
||||
best_cost = cost_cur;
|
||||
best_q = q_cur;
|
||||
|
||||
best_state = branch_state_;
|
||||
best_state.psi = psi_cur;
|
||||
best_state = branch_state_;
|
||||
best_state.psi = psi_cur;
|
||||
best_state.valid = true;
|
||||
}
|
||||
|
||||
@ -294,10 +289,9 @@ bool OptPsiLimitBiasSolver::ik(const Eigen::Matrix4d &target_pose,
|
||||
BiasSRSIkSolver::ConfigDirection::INWARD
|
||||
};
|
||||
|
||||
for (BiasSRSIkSolver::ConfigDirection s_dir : dirs) {
|
||||
for (BiasSRSIkSolver::ConfigDirection e_dir : dirs) {
|
||||
for (BiasSRSIkSolver::ConfigDirection w_dir : dirs) {
|
||||
|
||||
for (BiasSRSIkSolver::ConfigDirection s_dir: dirs) {
|
||||
for (BiasSRSIkSolver::ConfigDirection e_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);
|
||||
@ -315,37 +309,36 @@ 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)) {
|
||||
cand.psi = init_st->psi; // 用该分支自己的 ψ 估计
|
||||
if (const BranchPsiState *init_st = get_branch_init_state(s, e, w)) {
|
||||
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 &&
|
||||
e == branch_state_.e_conf &&
|
||||
w == branch_state_.w_conf);
|
||||
(s == branch_state_.s_conf &&
|
||||
e == branch_state_.e_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_q = std::move(q_cand);
|
||||
best_cost = cost_cand;
|
||||
best_q = std::move(q_cand);
|
||||
|
||||
best_state = cand;
|
||||
best_state.psi = psi_cand;
|
||||
best_state = cand;
|
||||
best_state.psi = psi_cand;
|
||||
best_state.valid = true;
|
||||
}
|
||||
}
|
||||
@ -361,11 +354,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_wrist_config (dir_from_sign(branch_state_.w_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));
|
||||
|
||||
joints_angle = best_q;
|
||||
cur_joints_angle_ = joints_angle; // 作为下一次的“当前姿态”
|
||||
joints_angle = best_q;
|
||||
cur_joints_angle_ = joints_angle; // 作为下一次的“当前姿态”
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -14,6 +14,9 @@
|
||||
#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;
|
||||
|
||||
|
||||
@ -518,125 +521,292 @@ 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);
|
||||
|
||||
|
||||
// int viewer = 0;
|
||||
// 把所有 IK 运算 + moveJ 循环放到控制线程里
|
||||
// ★ 把 IK + planner + moveJ 放在控制线程
|
||||
std::thread ctrl_thread([&viewer]() {
|
||||
std::vector<IkSample> samples;
|
||||
std::this_thread::sleep_for(std::chrono::seconds(3));
|
||||
samples.reserve(4096);
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
OptPsiLimitBiasSolver solver;
|
||||
// 等 MuJoCo / OpenGL 初始化好
|
||||
std::this_thread::sleep_for(3s);
|
||||
|
||||
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]
|
||||
// =============== 1) 设置初始关节状态 ===============
|
||||
std::vector<double> joint_angles(7, 0.0);
|
||||
joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364};
|
||||
|
||||
solver.update_joints_state(joint_angles);
|
||||
|
||||
// =============== 2) 目标位姿 (直线起点) ===============
|
||||
joint_angles = {0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0};
|
||||
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);
|
||||
|
||||
viewer.moveJ(q); // 更新目标角
|
||||
std::this_thread::sleep_for(std::chrono::duration<double>(0.1));
|
||||
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]}});
|
||||
|
||||
// 前向校验
|
||||
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);
|
||||
// 辅助函数：计算旋转误差
|
||||
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);
|
||||
};
|
||||
|
||||
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 << "=== IK path along straight line ===\n";
|
||||
cout << "idx, s(0..1), q1..q7, pos_err(m), rot_err(rad), rot_err(deg)\n";
|
||||
|
||||
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";
|
||||
// =============== 4) 直线采样 & 求 IK（只收集，不 moveJ） ===============
|
||||
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;
|
||||
|
||||
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);
|
||||
Eigen::Matrix4d T_goal = Eigen::Matrix4d::Identity();
|
||||
T_goal.block<3, 3>(0, 0) = R_fixed;
|
||||
T_goal.block<3, 1>(0, 3) = p;
|
||||
|
||||
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 了
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
//
|
||||
// TEST(SRS_IK_TEST, TR_TEST) {
|
||||
|
||||
@ -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 PRIVATE
|
||||
target_link_libraries(planner PUBLIC
|
||||
Eigen3::Eigen
|
||||
OsqpEigen::OsqpEigen
|
||||
${TINYXML2_LIBRARIES}
|
||||
|
||||
@ -5,7 +5,6 @@
|
||||
#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>
|
||||
@ -29,7 +28,11 @@ namespace cmvr {
|
||||
};
|
||||
|
||||
// 三种几何路径
|
||||
enum class PathType { Linear, CubicHermite, Quintic };
|
||||
enum class PathType { Linear,
|
||||
CubicHermite,
|
||||
Quintic,
|
||||
// 三次B样条
|
||||
Natural };
|
||||
class JointSpacePlanner {
|
||||
public:
|
||||
|
||||
@ -40,18 +43,21 @@ namespace cmvr {
|
||||
virtual bool plan(const std::vector<double>& start_joints,
|
||||
const std::vector<double>& goal_joints,
|
||||
TrajPtr& traj) {
|
||||
UNUSED_VARIABLE(start_joints, goal_joints,traj);
|
||||
return false;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
@ -71,4 +77,4 @@ namespace cmvr {
|
||||
std::vector<double> v_max_, a_max_;
|
||||
|
||||
};
|
||||
}
|
||||
}
|
||||
@ -29,4 +29,4 @@ namespace cmvr
|
||||
}
|
||||
static std::shared_ptr<JointSpacePlanner> create(const JointSpacePlannerType type);
|
||||
};
|
||||
} // namespace parking
|
||||
} // namespace parking
|
||||
@ -1,4 +1,5 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <Eigen/Dense>
|
||||
@ -9,8 +10,6 @@
|
||||
#include <toppra/parametrizer/spline.hpp>
|
||||
|
||||
namespace cmvr {
|
||||
|
||||
|
||||
// 适配器：ConstAccel
|
||||
class ConstAccelTraj : public ITrajectory {
|
||||
public:
|
||||
@ -52,30 +51,176 @@ 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;
|
||||
bool writeTrajectoryCsv(const std::string& filename,const std::vector<TrajSample>& samples) override;
|
||||
std::vector<TrajSample> sampleTrajectory(const TrajPtr &traj, double dt) override;
|
||||
|
||||
bool writeTrajectoryCsv(const std::string &filename, const std::vector<TrajSample> &samples) override;
|
||||
|
||||
private:
|
||||
// 几何路径构造
|
||||
static std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
buildLinear(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
|
||||
// —— 几何路径统一分发 ——
|
||||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
buildPathUnified(const std::vector<Eigen::VectorXd> &q,
|
||||
const std::vector<toppra::value_type> &S);
|
||||
|
||||
static std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
buildCubicHermiteRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
|
||||
// 二点专用
|
||||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
buildTwoPointPath(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
|
||||
|
||||
static std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
buildQuinticRestToRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
|
||||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
buildLinearTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
|
||||
|
||||
static std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
buildPath(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1, PathType t);
|
||||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
buildCubicHermiteTwo(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1);
|
||||
|
||||
static void sanitizeVsq(toppra::Vector &vsq);
|
||||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
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
|
||||
|
||||
@ -29,12 +29,12 @@ namespace cmvr
|
||||
std::shared_ptr<JointSpacePlanner> result;
|
||||
switch (type)
|
||||
{
|
||||
case JOINT_SPACE_PLANNER_UNKNOWN:
|
||||
return nullptr;
|
||||
case TOPPRA_BSPLINE:
|
||||
return std::make_shared<ToppraBSpline>();
|
||||
default:
|
||||
return nullptr;
|
||||
case JOINT_SPACE_PLANNER_UNKNOWN:
|
||||
return nullptr;
|
||||
case TOPPRA_BSPLINE:
|
||||
return std::make_shared<ToppraBSpline>();
|
||||
default:
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -5,58 +5,21 @@
|
||||
#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::CubicHermite);
|
||||
planner->setPathType(PathType::Quintic);
|
||||
|
||||
planner->setSymmetricLimits(std::vector<double>(1, 1.5),
|
||||
std::vector<double>(1, 6.0));
|
||||
planner->setSymmetricLimits(std::vector<double>(7, 1.5),
|
||||
std::vector<double>(7, 3.0));
|
||||
TrajPtr traj;
|
||||
std::vector<double> q0{0.0};
|
||||
std::vector<double> q1{-1.2};
|
||||
std::vector<double> q0{0.0,-0.5,0.8,0.0,0.2,-0.3,0.1};
|
||||
std::vector<double> q1{1.2,0.2,-0.6,0.7,-0.4,0.5,-0.2};
|
||||
if (!planner->plan(q0, q1, traj)) { std::cerr << "plan failed\n"; }
|
||||
|
||||
// 2) 采样 0.01 s
|
||||
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);
|
||||
}
|
||||
}
|
||||
auto samples = planner->sampleTrajectory(traj, 0.01);
|
||||
|
||||
// 3) 写 CSV
|
||||
if (!planner->writeTrajectoryCsv("/home/lgv/cmvr/cmvr-es/data/planner/traj.csv", samples)) {
|
||||
@ -65,4 +28,59 @@ 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";
|
||||
}
|
||||
}
|
||||
@ -50,72 +50,34 @@ namespace cmvr {
|
||||
v[v.size() - 1] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
ToppraBSpline::buildLinear(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1) {
|
||||
// q(s) = b + a*s, s∈[0,1]。系数矩阵行0=一次项，行1=常数项
|
||||
toppra::Matrix seg(2, q0.size());
|
||||
seg.row(0) = (q1 - q0).transpose();
|
||||
seg.row(1) = q0.transpose();
|
||||
return std::make_shared<toppra::PiecewisePolyPath>(
|
||||
toppra::Matrices{seg}, std::vector<double>{0.0, 1.0});
|
||||
}
|
||||
|
||||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
ToppraBSpline::buildCubicHermiteRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1) {
|
||||
// 起止速度 0 的三次 Hermite
|
||||
toppra::Vectors pos{q0, q1};
|
||||
toppra::Vectors vel{Eigen::VectorXd::Zero(q0.size()), Eigen::VectorXd::Zero(q1.size())};
|
||||
std::vector<toppra::value_type> s{0.0, 1.0};
|
||||
auto herm = toppra::PiecewisePolyPath::CubicHermiteSpline(pos, vel, s);
|
||||
return std::make_shared<toppra::PiecewisePolyPath>(herm);
|
||||
}
|
||||
|
||||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
ToppraBSpline::buildQuinticRestToRest(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1) {
|
||||
// q(0)=q0, q'(0)=q''(0)=0; q(1)=q1, q'(1)=q''(1)=0 的闭式解
|
||||
Eigen::VectorXd dq = q1 - q0;
|
||||
toppra::Matrix seg(6, q0.size()); // x^5..x^0
|
||||
seg.row(0) = (6.0 * dq).transpose(); // a5
|
||||
seg.row(1) = (-15.0 * dq).transpose(); // a4
|
||||
seg.row(2) = (10.0 * dq).transpose(); // a3
|
||||
seg.row(3).setZero(); // a2
|
||||
seg.row(4).setZero(); // a1
|
||||
seg.row(5) = q0.transpose(); // a0
|
||||
return std::make_shared<toppra::PiecewisePolyPath>(
|
||||
toppra::Matrices{seg}, std::vector<double>{0.0, 1.0});
|
||||
}
|
||||
|
||||
std::shared_ptr<toppra::PiecewisePolyPath>
|
||||
ToppraBSpline::buildPath(const Eigen::VectorXd &q0, const Eigen::VectorXd &q1, PathType t) {
|
||||
switch (t) {
|
||||
case PathType::Linear: return buildLinear(q0, q1);
|
||||
case PathType::CubicHermite: return buildCubicHermiteRest(q0, q1);
|
||||
case PathType::Quintic: return buildQuinticRestToRest(q0, q1);
|
||||
default: return buildQuinticRestToRest(q0, q1);
|
||||
}
|
||||
}
|
||||
|
||||
bool ToppraBSpline::plan(const std::vector<double> &start_joints,
|
||||
const std::vector<double> &goal_joints,
|
||||
TrajPtr &traj_out) {
|
||||
if (start_joints.empty() || start_joints.size() != goal_joints.size())
|
||||
return false;
|
||||
|
||||
const std::size_t DoF = start_joints.size();
|
||||
// ===== 统一入口：两点/多点 =====
|
||||
bool ToppraBSpline::plan(const std::vector<std::vector<double>>& waypoints,
|
||||
TrajPtr& traj_out) {
|
||||
traj_out.reset();
|
||||
const size_t M = waypoints.size();
|
||||
if (M < 2) return false;
|
||||
const size_t DoF = waypoints.front().size();
|
||||
for (const auto& w : waypoints) if (w.size()!=DoF) return false;
|
||||
if (!ensureLimitsSized(DoF)) return false;
|
||||
|
||||
// 构造几何路径
|
||||
Eigen::VectorXd q0 = Eigen::Map<const Eigen::VectorXd>(start_joints.data(), DoF);
|
||||
Eigen::VectorXd q1 = Eigen::Map<const Eigen::VectorXd>(goal_joints.data(), DoF);
|
||||
auto path = buildPath(q0, q1, path_type_);
|
||||
// 组装
|
||||
std::vector<Eigen::VectorXd> q; q.reserve(M);
|
||||
for (const auto& w : waypoints)
|
||||
q.emplace_back(Eigen::Map<const Eigen::VectorXd>(w.data(), DoF));
|
||||
|
||||
// 生成 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 (std::size_t i = 0; i < DoF; ++i) {
|
||||
vmax[i] = v_max_[i];
|
||||
amax[i] = a_max_[i];
|
||||
}
|
||||
for (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);
|
||||
@ -124,31 +86,24 @@ namespace cmvr {
|
||||
|
||||
// TOPPRA
|
||||
toppra::algorithm::TOPPRA algo{constraints, path};
|
||||
algo.setN(N_grid_);
|
||||
algo.solver(std::make_shared<toppra::solver::Seidel>());
|
||||
|
||||
auto solve_once = [&](int N)-> bool {
|
||||
auto solve_once = [&](int N)->bool{
|
||||
algo.setN(N);
|
||||
auto rc = algo.computePathParametrization(0.0, 0.0);
|
||||
return rc == toppra::ReturnCode::OK;
|
||||
algo.solver(std::make_shared<toppra::solver::Seidel>());
|
||||
return algo.computePathParametrization(0.0, 0.0) == 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);
|
||||
@ -159,6 +114,204 @@ 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;
|
||||
@ -231,4 +384,4 @@ namespace cmvr {
|
||||
}
|
||||
return true;
|
||||
}
|
||||
} // namespace cmvr
|
||||
} // namespace cmvr
|
||||
Loading…
Reference in New Issue
Block a user