joint_trajectory_controller interpolation change between Humble and Jazzy #1298
Description
Describe the bug
I have just upgraded from Humble to Jazzy, and a trajectory that previously executed correctly is now deviating quite a bit, which my guess is from the controller interpolation.
I'm using moveit_task_constructor to create a cartesian UR5e trajectory that contains a segment with a very sharp jerk (essentially three co-linear waypoints, each in an opposite direction). When I visualize / plot the planned trajectory in rviz, it is correct:
However, when I execute the task, the shake gets wildly exaggerated (this behavior also occurs on a real UR5e):
Plotting the shoulder_pan_joint positions from /joint_trajectory_controller/follow_joint_trajectory/_action/feedback shows that the motion matches "desired" (in the planned trajectory, shoulder_pan_joint is supposed to go to 0.996, not all the way to -0.76):
I'm not super familiar with the internals of joint_trajectory_controller, but did something change since Humble? Is this a bug, or expected interpolation behavior given the jerkiness of the commanded trajectory? If the latter, what's the right way to command such motions?
To Reproduce
Steps to reproduce the behavior:
Execute the following RobotTrajectory:
# moveit_msgs/RobotTrajectory.msg
joint_trajectory:
header:
stamp:
sec: 0
nanosec: 0
frame_id: "world"
joint_names:
- "shoulder_pan_joint"
- "shoulder_lift_joint"
- "elbow_joint"
- "wrist_1_joint"
- "wrist_2_joint"
- "wrist_3_joint"
points:
-
positions:
- 1.24934
- -1.54731
- 1.91419
- -0.368635
- 1.25010
- -1.56987
velocities:
- -0.00398828
- -2.05360e-06
- 0.000135010
- -0.000151254
- -0.00398789
- -0.00800000
accelerations:
- -3.98828
- -0.00205360
- 0.135010
- -0.151254
- -3.98789
- -8.00000
effort: []
time_from_start:
sec: 0
nanosec: 0
-
positions:
- 1.22943
- -1.54725
- 1.91480
- -0.369390
- 1.23019
- -1.60987
velocities:
- -0.398909
- 0.00444572
- 0.00877938
- -0.0150741
- -0.398864
- -0.803432
accelerations:
- -3.91806
- 0.199055
- -0.0716036
- -0.146244
- -3.91744
- -8.00053
effort: []
time_from_start:
sec: 0
nanosec: 100000000
-
positions:
- 1.17053
- -1.54485
- 1.91432
- -0.371595
- 1.17131
- -1.72988
velocities:
- -0.775450
- 0.0554213
- -0.0305022
- -0.0287251
- -0.775307
- -1.60093
accelerations:
- -3.57971
- 0.892094
- -0.790107
- -0.122714
- -3.57829
- -8.00108
effort: []
time_from_start:
sec: 0
nanosec: 200000000
-
positions:
- 1.07807
- -1.53322
- 1.90637
- -0.375752
- 1.07887
- -1.92988
velocities:
- -0.944175
- 0.190869
- -0.0933872
- -0.102580
- -0.943669
- -2.40120
accelerations:
- -2.50318
- 1.61543
- -1.37209
- -0.265933
- -2.50039
- -8.00099
effort: []
time_from_start:
sec: 0
nanosec: 300000000
-
positions:
- 0.989230
- -1.50859
- 1.89505
- -0.389575
- 0.990093
- -2.18462
velocities:
- -0.778584
- 0.295315
- -0.0958476
- -0.204156
- -0.777678
- -2.71811
accelerations:
- 2.87949
- -0.128852
- -0.592775
- 0.730971
- 2.87747
- 7.99880
effort: []
time_from_start:
sec: 0
nanosec: 400000000
-
positions:
- 0.929159
- -1.47851
- 1.89227
- -0.417230
- 0.930132
- -2.45249
velocities:
- -0.439438
- 0.318366
- 0.0822037
- -0.403090
- -0.437999
- -2.84979
accelerations:
- 6.53474
- 0.0516573
- 6.29765
- -6.29685
- 6.55060
- -7.97542
effort: []
time_from_start:
sec: 0
nanosec: 500000000
-
positions:
- 0.904417
- -1.44886
- 1.91199
- -0.466707
- 0.905552
- -2.71945
velocities:
- -0.0575002
- 0.256635
- 0.299311
- -0.555549
- -0.0558123
- -2.30724
accelerations:
- 5.67228
- -1.04274
- 4.11955
- -3.03160
- 5.67785
- 7.98452
effort: []
time_from_start:
sec: 0
nanosec: 600000000
-
positions:
- 0.908768
- -1.42650
- 1.94471
- -0.521678
- 0.910063
- -2.91055
velocities:
- 0.114089
- 0.191314
- 0.327513
- -0.517259
- 0.115533
- -1.50771
accelerations:
- -0.581990
- -0.609501
- -2.05274
- 2.65385
- -0.589817
- 8.00004
effort: []
time_from_start:
sec: 0
nanosec: 700000000
-
positions:
- 0.920157
- -1.41116
- 1.97108
- -0.563242
- 0.921565
- -3.02168
velocities:
- 0.0973556
- 0.107358
- 0.187686
- -0.293953
- 0.0981459
- -0.708521
accelerations:
- -0.523105
- -1.02201
- -1.62559
- 2.63999
- -0.530386
- 8.00669
effort: []
time_from_start:
sec: 0
nanosec: 800000000
-
positions:
- 0.925146
- -1.41051
- 1.97573
- -0.568502
- 0.926566
- -3.07041
velocities:
- 0.0196388
- -0.129796
- -0.113250
- 0.242804
- 0.0189342
- -0.548244
accelerations:
- 0.412044
- -1.89220
- -1.64960
- 3.53926
- 0.401602
- -7.99987
effort: []
time_from_start:
sec: 0
nanosec: 900000000
-
positions:
- 0.930420
- -1.43279
- 1.95630
- -0.526815
- 0.931718
- -3.16476
velocities:
- 0.102657
- -0.316204
- -0.275780
- 0.591806
- 0.100881
- -1.34465
accelerations:
- 1.39074
- -1.81858
- -1.59672
- 3.42020
- 1.37975
- -7.99930
effort: []
time_from_start:
sec: 1
nanosec: 0
-
positions:
- 0.950412
- -1.47302
- 1.92101
- -0.451275
- 0.951477
- -3.33909
velocities:
- 0.329940
- -0.481812
- -0.426750
- 0.909553
- 0.327075
- -2.14474
accelerations:
- 3.30533
- -1.40519
- -1.38622
- 2.81094
- 3.29495
- -7.99726
effort: []
time_from_start:
sec: 1
nanosec: 100000000
-
positions:
- 0.987337
- -1.51318
- 1.88463
- -0.374586
- 0.988155
- -3.52657
velocities:
- 0.356730
- -0.298528
- -0.279038
- 0.579191
- 0.354834
- -1.48010
accelerations:
- -0.899123
- 1.95080
- 1.64774
- -3.60021
- -0.887800
- 8.00398
effort: []
time_from_start:
sec: 1
nanosec: 200000000
-
positions:
- 1.01618
- -1.53398
- 1.86466
- -0.333672
- 1.01687
- -3.63463
velocities:
- 0.199321
- -0.124082
- -0.123157
- 0.248218
- 0.198496
- -0.680018
accelerations:
- -2.11560
- 1.55323
- 1.48309
- -3.04640
- -2.10555
- 8.00076
effort: []
time_from_start:
sec: 1
nanosec: 300000000
-
positions:
- 1.02481
- -1.53920
- 1.85943
- -0.323188
- 1.02546
- -3.66357
velocities:
- 1.99595e-17
- -1.19706e-17
- -1.19941e-17
- 2.40633e-17
- 1.98794e-17
- -6.65882e-17
accelerations:
- 0.00000
- 0.00000
- 0.00000
- 0.00000
- 0.00000
- 0.00000
effort: []
time_from_start:
sec: 1
nanosec: 385065517
-
positions:
- 1.02481
- -1.53920
- 1.85943
- -0.323188
- 1.02546
- -3.66357
velocities:
- -0.00798522
- 0.000345756
- -0.00596621
- 0.00556572
- -0.00800000
- 4.16514e-05
accelerations:
- -7.98522
- 0.345756
- -5.96621
- 5.56572
- -8.00000
- 0.0416514
effort: []
time_from_start:
sec: 4
nanosec: 987758616
-
positions:
- 0.995919
- -1.53752
- 1.83727
- -0.302912
- 0.996514
- -3.66342
velocities:
- -0.202139
- 0.0166327
- -0.161468
- 0.143388
- -0.202515
- 0.00111855
accelerations:
- 7.98706
- -0.580598
- 6.27794
- -5.64076
- 8.00188
- -0.0435675
effort: []
time_from_start:
sec: 5
nanosec: 87758616
-
positions:
- 1.00263
- -1.53687
- 1.84557
- -0.311808
- 1.00325
- -3.65616
velocities:
- 0.0351852
- 0.0515227
- 0.169314
- -0.220379
- 0.0358554
- 0.343027
accelerations:
- -7.13672
- 1.83022
- -2.44433
- 0.567806
- -7.13605
- 7.92882
effort: []
time_from_start:
sec: 5
nanosec: 187758616
-
positions:
- 0.987093
- -1.52471
- 1.86004
- -0.338502
- 0.987802
- -3.58966
velocities:
- -0.250067
- 0.200631
- 0.217444
- -0.419238
- -0.248687
- 1.05638
accelerations:
- -1.38228
- 1.70639
- 1.74647
- -3.45810
- -1.37111
- 8.00039
effort: []
time_from_start:
sec: 5
nanosec: 287758616
-
positions:
- 0.957498
- -1.49548
- 1.89086
- -0.398687
- 0.958402
- -3.44412
velocities:
- -0.315712
- 0.392245
- 0.404150
- -0.797521
- -0.313158
- 1.85951
accelerations:
- 0.193124
- 2.11382
- 1.97534
- -4.08207
- 0.205176
- 7.99940
effort: []
time_from_start:
sec: 5
nanosec: 387758616
-
positions:
- 0.932359
- -1.45124
- 1.93545
- -0.487565
- 0.933541
- -3.24415
velocities:
- -0.143080
- 0.402216
- 0.398284
- -0.800343
- -0.140645
- 1.75042
accelerations:
- 1.99140
- -1.63584
- -1.72228
- 3.36797
- 1.97987
- -7.99912
effort: []
time_from_start:
sec: 5
nanosec: 487758616
-
positions:
- 0.925246
- -1.41969
- 1.96641
- -0.550030
- 0.926613
- -3.10907
velocities:
- -0.0225253
- 0.224802
- 0.218777
- -0.443067
- -0.0212343
- 0.950426
accelerations:
- 0.579576
- -1.86144
- -1.83551
- 3.69557
- 0.568383
- -7.99994
effort: []
time_from_start:
sec: 5
nanosec: 587758616
-
positions:
- 0.923893
- -1.40731
- 1.97791
- -0.573885
- 0.925330
- -3.04924
velocities:
- -0.0419778
- -0.0118107
- -0.0445351
- 0.0559880
- -0.0421149
- 0.433545
accelerations:
- -1.11201
- -3.59174
- -4.42430
- 7.99671
- -1.13430
- -1.31978
effort: []
time_from_start:
sec: 5
nanosec: 687758616
-
positions:
- 0.915017
- -1.41729
- 1.96043
- -0.546533
- 0.916380
- -2.97955
velocities:
- -0.120927
- -0.153003
- -0.267010
- 0.418563
- -0.122068
- 1.09197
accelerations:
- 0.415033
- -0.767740
- -0.806232
- 1.57416
- 0.410270
- 8.00714
effort: []
time_from_start:
sec: 5
nanosec: 787758616
-
positions:
- 0.904690
- -1.43619
- 1.92911
- -0.496452
- 0.905913
- -2.83090
velocities:
- -0.0589268
- -0.221585
- -0.338230
- 0.558568
- -0.0605389
- 1.89092
accelerations:
- -0.243593
- -0.497979
- -1.78140
- 2.27446
- -0.250641
- 8.00001
effort: []
time_from_start:
sec: 5
nanosec: 887758616
-
positions:
- 0.911001
- -1.46183
- 1.89996
- -0.441693
- 0.912057
- -2.60228
velocities:
- 0.241815
- -0.298993
- -0.198518
- 0.498535
- 0.240184
- 2.68715
accelerations:
- 0.826097
- -0.464205
- -1.05753
- 1.52580
- 0.821028
- 7.99987
effort: []
time_from_start:
sec: 5
nanosec: 987758616
-
positions:
- 0.953549
- -1.49307
- 1.89153
- -0.401780
- 0.954464
- -2.32390
velocities:
- 0.553137
- -0.292921
- 0.00900925
- 0.287246
- 0.552021
- 2.62208
accelerations:
- -1.37547
- 1.33074
- -0.561668
- -0.780069
- -1.37179
- -8.09139
effort: []
time_from_start:
sec: 6
nanosec: 87758616
-
positions:
- 1.02865
- -1.52151
- 1.90024
- -0.381608
- 1.02948
- -2.06039
velocities:
- 0.846854
- -0.245396
- 0.100646
- 0.149608
- 0.846155
- 2.52213
accelerations:
- -2.06789
- 1.60852
- -1.26083
- -0.365592
- -2.06485
- -8.00076
effort: []
time_from_start:
sec: 6
nanosec: 187758616
-
positions:
- 1.12495
- -1.54046
- 1.91138
- -0.373262
- 1.12573
- -1.82512
velocities:
- 0.953054
- -0.115110
- 0.0856741
- 0.0343565
- 0.952822
- 2.01620
accelerations:
- -3.22476
- 1.42753
- -1.36270
- -0.0873050
- -3.22273
- -8.00126
effort: []
time_from_start:
sec: 6
nanosec: 287758616
-
positions:
- 1.20316
- -1.54662
- 1.91503
- -0.370378
- 1.20393
- -1.66302
velocities:
- 0.597868
- -0.0221314
- 0.00255392
- 0.0224131
- 0.597783
- 1.21606
accelerations:
- -3.78208
- 0.509230
- -0.391386
- -0.137448
- -3.78110
- -8.00084
effort: []
time_from_start:
sec: 6
nanosec: 387758616
-
positions:
- 1.24383
- -1.54731
- 1.91438
- -0.368844
- 1.24459
- -1.58094
velocities:
- 0.207390
- 0.000106815
- -0.00701999
- 0.00786400
- 0.207370
- 0.416000
accelerations:
- -3.98828
- -0.00205414
- 0.135000
- -0.151231
- -3.98789
- -8.00000
effort: []
time_from_start:
sec: 6
nanosec: 487758616
-
positions:
- 1.24934
- -1.54731
- 1.91419
- -0.368635
- 1.25010
- -1.56987
velocities:
- -3.57576e-16
- -1.84167e-19
- 1.21037e-17
- -1.35589e-17
- -3.57541e-16
- -7.17254e-16
accelerations:
- 0.00000
- 0.00000
- 0.00000
- 0.00000
- 0.00000
- 0.00000
effort: []
time_from_start:
sec: 6
nanosec: 540359020
multi_dof_joint_trajectory:
header:
stamp:
sec: 0
nanosec: 0
frame_id: ""
joint_names: []
points: []Expected behavior
A short shaking motion.
Environment (please complete the following information):
- OS: Ubuntu 24.04
- ROS: Jazzy
- joint_trajectory_controller: 4.14.0
- ur_robot_driver: 2.4.10
- moveit_task_constructor: https://github.com/moveit/moveit_task_constructor/tree/cb867aeddfdca6e3791a4ede998fe23ce9958754
Additional context
The controller configuration used is https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/2.4.10/ur_robot_driver/config/ur_controllers.yaml, which doesn't seem to have changed materially since Humble.
The commanded trajectory comes from the result of the MTC Task::plan call, which is then executed with Task::execute. I'm making the assumption that MTC doesn't alter the trajectory in execute before it sends it to the controller.