The MPT algorithm results in curvature anomalies

[JokerWhy233]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I'm convinced that this is not my fault but a bug.

Description

when i set max_steer_angle in param of MPT algorithm , something wrong . our vehicle max_steer_angle is 0.3755, However ,Using this parameter will result in abnormal algorithm results。

in our vehicle info ：
wheel_base : 5.52
max_steer_angle: 0.3755
curvature_limit_: 14

I am trying to optimize the following trajectory，there are points in the trajectory that exceed the limit，such like index: 71ref_k: -0.143847delta_ref_k: -0.671094 in < input trajectory >

The result of MPT optimization, Uex, did not exceed the limit, but there are many points that happen to be within the limit values such like Uex[50] = -0.3755 in <The result of MPT optimization, Uex: >

However, the trajectory obtained through the function getMPTPoints has many points that exceed the limit value，such like index: 77ref_k: -0.104724delta_ref_k: -0.524142 in

data

============================================================================

input trajectory ：

index: 1ref_k: -0.0223623delta_ref_k: -0.122819
index: 2ref_k: -0.0186476delta_ref_k: -0.102574
index: 3ref_k: -0.0137021delta_ref_k: -0.075492
index: 4ref_k: -0.00866801delta_ref_k: -0.0478109
index: 5ref_k: -0.00645125delta_ref_k: -0.0355959
index: 6ref_k: -0.00742327delta_ref_k: -0.0409535
index: 7ref_k: -0.0114595delta_ref_k: -0.0631721
index: 8ref_k: -0.0187874delta_ref_k: -0.103337
index: 9ref_k: -0.0285311delta_ref_k: -0.156209
index: 10ref_k: -0.0363998delta_ref_k: -0.198286
index: 11ref_k: -0.0424133delta_ref_k: -0.229979
index: 12ref_k: -0.0467293delta_ref_k: -0.252443
index: 13ref_k: -0.0489783delta_ref_k: -0.264048
index: 14ref_k: -0.049437delta_ref_k: -0.266406
index: 15ref_k: -0.0500547delta_ref_k: -0.269576
index: 16ref_k: -0.0507307delta_ref_k: -0.27304
index: 17ref_k: -0.0513493delta_ref_k: -0.276203
index: 18ref_k: -0.0519787delta_ref_k: -0.279416
index: 19ref_k: -0.052621delta_ref_k: -0.282689
index: 20ref_k: -0.0533015delta_ref_k: -0.286149
index: 21ref_k: -0.0540389delta_ref_k: -0.289892
index: 22ref_k: -0.0548572delta_ref_k: -0.294034
index: 23ref_k: -0.0557585delta_ref_k: -0.298585
index: 24ref_k: -0.0567372delta_ref_k: -0.303513
index: 25ref_k: -0.05779delta_ref_k: -0.308796
index: 26ref_k: -0.0589127delta_ref_k: -0.314411
index: 27ref_k: -0.060098delta_ref_k: -0.320317
index: 28ref_k: -0.0613394delta_ref_k: -0.326477
index: 29ref_k: -0.0626356delta_ref_k: -0.332882
index: 30ref_k: -0.0639178delta_ref_k: -0.339191
index: 31ref_k: -0.0651108delta_ref_k: -0.345034
index: 32ref_k: -0.0661479delta_ref_k: -0.350095
index: 33ref_k: -0.0669573delta_ref_k: -0.354032
index: 34ref_k: -0.0674603delta_ref_k: -0.356472
index: 35ref_k: -0.0677133delta_ref_k: -0.357698
index: 36ref_k: -0.0677878delta_ref_k: -0.358059
index: 37ref_k: -0.067747delta_ref_k: -0.357862
index: 38ref_k: -0.067666delta_ref_k: -0.357469
index: 39ref_k: -0.0676211delta_ref_k: -0.357252
index: 40ref_k: -0.067621delta_ref_k: -0.357251
index: 41ref_k: -0.0676667delta_ref_k: -0.357473
index: 42ref_k: -0.0677608delta_ref_k: -0.357928
index: 43ref_k: -0.0679055delta_ref_k: -0.358629
index: 44ref_k: -0.068078delta_ref_k: -0.359464
index: 45ref_k: -0.0682169delta_ref_k: -0.360135
index: 46ref_k: -0.0682444delta_ref_k: -0.360268
index: 47ref_k: -0.0680911delta_ref_k: -0.359527
index: 48ref_k: -0.0676769delta_ref_k: -0.357522
index: 49ref_k: -0.0669765delta_ref_k: -0.354125
index: 50ref_k: -0.0660417delta_ref_k: -0.349578
index: 51ref_k: -0.06496delta_ref_k: -0.344297
index: 52ref_k: -0.0638094delta_ref_k: -0.338658
index: 53ref_k: -0.0626757delta_ref_k: -0.33308
index: 54ref_k: -0.0616174delta_ref_k: -0.327853
index: 55ref_k: -0.0606536delta_ref_k: -0.323077
index: 56ref_k: -0.0597868delta_ref_k: -0.318768
index: 57ref_k: -0.0590185delta_ref_k: -0.314939
index: 58ref_k: -0.0583578delta_ref_k: -0.311639
index: 59ref_k: -0.057809delta_ref_k: -0.308892
index: 60ref_k: -0.0573188delta_ref_k: -0.306434
index: 61ref_k: -0.057276delta_ref_k: -0.306219
index: 62ref_k: -0.0581956delta_ref_k: -0.310827
index: 63ref_k: -0.0605681delta_ref_k: -0.322653
index: 64ref_k: -0.064963delta_ref_k: -0.344312
index: 65ref_k: -0.0721102delta_ref_k: -0.378823
index: 66ref_k: -0.0819039delta_ref_k: -0.424607
index: 67ref_k: -0.0939499delta_ref_k: -0.47842
index: 68ref_k: -0.108123delta_ref_k: -0.538092
index: 69ref_k: -0.122787delta_ref_k: -0.595659
index: 70ref_k: -0.135352delta_ref_k: -0.64167
index: 71ref_k: -0.143847delta_ref_k: -0.671094
index: 72ref_k: -0.146547delta_ref_k: -0.68017
index: 73ref_k: -0.141379delta_ref_k: -0.662682
index: 74ref_k: -0.128938delta_ref_k: -0.618561
index: 75ref_k: -0.111416delta_ref_k: -0.551388
index: 76ref_k: -0.0907198delta_ref_k: -0.464266
index: 77ref_k: -0.0687805delta_ref_k: -0.362857
index: 78ref_k: -0.0476301delta_ref_k: -0.2571
index: 79ref_k: -0.0277995delta_ref_k: -0.152265
index: 80ref_k: -0.00837091delta_ref_k: -0.0461746
index: 81ref_k: -0.00837091delta_ref_k: -0.0461746
index: 82ref_k: -0.00837091delta_ref_k: -0.0461746

=============================================

The result of MPT optimization, Uex:

Uex[0] = 0.0154528
Uex[1] = -0.0406416
Uex[2] = -0.0498725
Uex[3] = -0.0911338
Uex[4] = -0.127853
Uex[5] = -0.157029
Uex[6] = -0.178296
Uex[7] = -0.194006
Uex[8] = -0.189243
Uex[9] = -0.138981
Uex[10] = -0.102044
Uex[11] = -0.120173
Uex[12] = -0.152137
Uex[13] = -0.18237
Uex[14] = -0.204703
Uex[15] = -0.218627
Uex[16] = -0.228667
Uex[17] = -0.237592
Uex[18] = -0.246545
Uex[19] = -0.255258
Uex[20] = -0.263492
Uex[21] = -0.27128
Uex[22] = -0.278658
Uex[23] = -0.285611
Uex[24] = -0.291946
Uex[25] = -0.297576
Uex[26] = -0.302529
Uex[27] = -0.306878
Uex[28] = -0.31071
Uex[29] = -0.314088
Uex[30] = -0.317012
Uex[31] = -0.319409
Uex[32] = -0.321294
Uex[33] = -0.322974
Uex[34] = -0.326051
Uex[35] = -0.33064
Uex[36] = -0.334929
Uex[37] = -0.338317
Uex[38] = -0.340605
Uex[39] = -0.341712
Uex[40] = -0.3418
Uex[41] = -0.341004
Uex[42] = -0.339475
Uex[43] = -0.337547
Uex[44] = -0.336203
Uex[45] = -0.337887
Uex[46] = -0.346984
Uex[47] = -0.365915
Uex[48] = -0.3755
Uex[49] = -0.3755
Uex[50] = -0.3755
Uex[51] = -0.3755
Uex[52] = -0.3755
Uex[53] = -0.3755
Uex[54] = -0.3755
Uex[55] = -0.3755
Uex[56] = -0.3755
Uex[57] = -0.3755
Uex[58] = -0.3755
Uex[59] = -0.3755
Uex[60] = -0.3755
Uex[61] = -0.3755
Uex[62] = -0.3755
Uex[63] = -0.3755
Uex[64] = -0.3755
Uex[65] = -0.3755
Uex[66] = -0.3755
Uex[67] = -0.3755
Uex[68] = -0.3755
Uex[69] = -0.3755
Uex[70] = -0.3755
Uex[71] = -0.3755
Uex[72] = -0.321819
Uex[73] = -0.17508
Uex[74] = -0.163766
Uex[75] = -0.222929
Uex[76] = -0.279167
Uex[77] = -0.311037
Uex[78] = -0.312836
Uex[79] = -0.29125
Uex[80] = -0.262125
Uex[81] = -0.239572

=========================================================

MPT output

index: 0ref_k: 0.00488671delta_ref_k: 0.0269681
index: 1ref_k: 0.00488671delta_ref_k: 0.0269681
index: 2ref_k: 0.00488671delta_ref_k: 0.0269681
index: 3ref_k: 0.00488671delta_ref_k: 0.0269681
index: 4ref_k: 0.00488671delta_ref_k: 0.0269681
index: 5ref_k: 0.00488671delta_ref_k: 0.0269681
index: 6ref_k: 0.00488671delta_ref_k: 0.0269681
index: 7ref_k: 0.00230673delta_ref_k: 0.0127324
index: 8ref_k: -0.00056496delta_ref_k: -0.00311857
index: 9ref_k: -0.00348273delta_ref_k: -0.0192223
index: 10ref_k: -0.00637905delta_ref_k: -0.0351978
index: 11ref_k: -0.009351delta_ref_k: -0.0515717
index: 12ref_k: -0.012296delta_ref_k: -0.0677699
index: 13ref_k: -0.0151905delta_ref_k: -0.0836557
index: 14ref_k: -0.0193335delta_ref_k: -0.106318
index: 15ref_k: -0.0236268delta_ref_k: -0.129688
index: 16ref_k: -0.0274843delta_ref_k: -0.150565
index: 17ref_k: -0.031348delta_ref_k: -0.171344
index: 18ref_k: -0.0351356delta_ref_k: -0.19157
index: 19ref_k: -0.0363561delta_ref_k: -0.198055
index: 20ref_k: -0.0369565delta_ref_k: -0.201238
index: 21ref_k: -0.0377212delta_ref_k: -0.205288
index: 22ref_k: -0.0384043delta_ref_k: -0.208899
index: 23ref_k: -0.0390804delta_ref_k: -0.212468
index: 24ref_k: -0.0409813delta_ref_k: -0.222472
index: 25ref_k: -0.0431361delta_ref_k: -0.233758
index: 26ref_k: -0.0451796delta_ref_k: -0.244406
index: 27ref_k: -0.0471151delta_ref_k: -0.254439
index: 28ref_k: -0.0489275delta_ref_k: -0.263786
index: 29ref_k: -0.0506515delta_ref_k: -0.272634
index: 30ref_k: -0.0523244delta_ref_k: -0.281179
index: 31ref_k: -0.0539516delta_ref_k: -0.289449
index: 32ref_k: -0.0555066delta_ref_k: -0.297315
index: 33ref_k: -0.056964delta_ref_k: -0.304652
index: 34ref_k: -0.0583097delta_ref_k: -0.311398
index: 35ref_k: -0.0595355delta_ref_k: -0.317517
index: 36ref_k: -0.0606382delta_ref_k: -0.323001
index: 37ref_k: -0.0616266delta_ref_k: -0.327899
index: 38ref_k: -0.0625258delta_ref_k: -0.332341
index: 39ref_k: -0.0633118delta_ref_k: -0.336212
index: 40ref_k: -0.0639868delta_ref_k: -0.339529
index: 41ref_k: -0.0645735delta_ref_k: -0.342406
index: 42ref_k: -0.0650806delta_ref_k: -0.344887
index: 43ref_k: -0.0654912delta_ref_k: -0.346893
index: 44ref_k: -0.0658842delta_ref_k: -0.34881
index: 45ref_k: -0.0662765delta_ref_k: -0.350721
index: 46ref_k: -0.0666407delta_ref_k: -0.352493
index: 47ref_k: -0.0669517delta_ref_k: -0.354005
index: 48ref_k: -0.0671926delta_ref_k: -0.355174
index: 49ref_k: -0.0673103delta_ref_k: -0.355745
index: 50ref_k: -0.0673208delta_ref_k: -0.355796
index: 51ref_k: -0.0673413delta_ref_k: -0.355895
index: 52ref_k: -0.0675935delta_ref_k: -0.357118
index: 53ref_k: -0.0682376delta_ref_k: -0.360235
index: 54ref_k: -0.0692905delta_ref_k: -0.365315
index: 55ref_k: -0.0706731delta_ref_k: -0.371956
index: 56ref_k: -0.0721902delta_ref_k: -0.379204
index: 57ref_k: -0.0734626delta_ref_k: -0.385251
index: 58ref_k: -0.0742538delta_ref_k: -0.388995
index: 59ref_k: -0.0745882delta_ref_k: -0.390575
index: 60ref_k: -0.0745829delta_ref_k: -0.39055
index: 61ref_k: -0.0743679delta_ref_k: -0.389534
index: 62ref_k: -0.0741518delta_ref_k: -0.388513
index: 63ref_k: -0.0740577delta_ref_k: -0.388068
index: 64ref_k: -0.0740721delta_ref_k: -0.388136
index: 65ref_k: -0.0741485delta_ref_k: -0.388498
index: 66ref_k: -0.0742662delta_ref_k: -0.389054
index: 67ref_k: -0.0744125delta_ref_k: -0.389745
index: 68ref_k: -0.0745213delta_ref_k: -0.390259
index: 69ref_k: -0.074464delta_ref_k: -0.389988
index: 70ref_k: -0.0745644delta_ref_k: -0.390462
index: 71ref_k: -0.0751003delta_ref_k: -0.392989
index: 72ref_k: -0.0763588delta_ref_k: -0.398903
index: 73ref_k: -0.0788delta_ref_k: -0.41029
index: 74ref_k: -0.0830739delta_ref_k: -0.429956
index: 75ref_k: -0.0889543delta_ref_k: -0.456444
index: 76ref_k: -0.0963406delta_ref_k: -0.488763
index: 77ref_k: -0.104724delta_ref_k: -0.524142
index: 78ref_k: -0.110437delta_ref_k: -0.547458
index: 79ref_k: -0.110859delta_ref_k: -0.549153
index: 80ref_k: -0.105673delta_ref_k: -0.528059
index: 81ref_k: -0.0949187delta_ref_k: -0.482624
index: 82ref_k: -0.079578delta_ref_k: -0.413895
index: 83ref_k: -0.0654628delta_ref_k: -0.346754
index: 84ref_k: -0.055117delta_ref_k: -0.295348

Expected behavior

The curvature of the algorithm output trajectory should not exceed the limit

Actual behavior

The curvature of the algorithm output trajectory exceed the limit

Steps to reproduce

ego pose :
pose:
pose:
position:
x: 62047.140625
y: 92609.9453125
z: 110.44488308807159
orientation:
x: 0.0
y: 0.0
z: -0.04759445011734239
w: 0.9988667420221917

goal pose :
pose:
position:
x: 62057.52734375
y: 92581.8359375
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.9493319601030331
w: 0.3142750857559874

map.zip

Versions

No response

Possible causes

No response

Additional context

No response

[shulanbushangshu]

@JokerWhy233
As the data show ,the control out steer angle of the MPT is within the limit.
What do you mean that curvature of the algorithm output trajectory should not exceed the limit?

[stale]

This pull request has been automatically marked as stale because it has not had recent activity.