Universal_Robots_ROS2_Driver icon indicating copy to clipboard operation
Universal_Robots_ROS2_Driver copied to clipboard
verified publisher icon UniversalRobots

scaled_joint_trajectory_controller delayed execution of trajectory

Open SuperDiodo opened this issue 2 years ago • 6 comments

Configuration:

  • Ubuntu 22.04 (RT kernel)
  • ROS2 Humble
  • ur_driver cloned from github Humble branch
  • externalcontrol-1.0.5.urcap

Hi, I'm trying to plan trajectories with Moveit and send them to a real UR10e through a C++ script. The problem is that the execution of the trajectories is randomly delayed (sometimes they are executed instantly and sometimes it can takes up to minutes). However, If I try to plan a trajectory from Rviz and click the "execute" button the trajectory is always instantly executed.

Adding some prints in scaled_joint_trajectory_controller.cpp I discovered that the internal timestamp of the SJTC (traj_time) is lower than the timestamp of the ros node when the trajectory has been sent (let's call it ros_time). The controller isn't able to start executing the trajectory until traj_time == ros_time, probably because of this part of the script:

const bool valid_point =
        (*traj_point_active_ptr_)
            ->sample(traj_time,
                     joint_trajectory_controller::interpolation_methods::InterpolationMethod::VARIABLE_DEGREE_SPLINE,
                     state_desired, start_segment_itr, end_segment_itr);

I have noticed that:

  • After a trajectory execution (called from the C++ script or Rviz) the driver is synchronized and the following trajectories will be executed instantly
  • Each time I restart the external control urcap the driver desynchronizes

Thanks in advance for any suggestion :smile:

Edit: for higher speed scaling param the driver synchronizes faster, here there are the lines where traj_time is computed:

time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * period);
time_data.uptime = time_data_.readFromRT()->uptime + time_data.period;
rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(period);

SuperDiodo avatar Jun 09 '23 11:06 SuperDiodo

@SuperDiodo to clarify the issue. Does this mean then Scaled JTC also prolongs the first time-stamp when trajectory should be started?

This might be the issue (without detailed) code-check. And this should be quite well solvable by not adjusting trajectory start time (which controller should do as I remember).

destogl avatar Jun 11 '23 14:06 destogl

The SJTC starts the execution when traj_time reaches the ros time-stamp related to the trajectory reception. The problem is that traj_time in some cases is lower than the expected timestamp and the execution is delayed.

To give a practical example, I report below the results of a small test (I'm sorry for the amount of code lines): I planned a Cartesian Path and sent it to the robot 3 times with different speed scaling (98%, 99%, 100%). Before sending a trajectory to the robot I restarted the urcap program on the teach pendant + I added a line to print traj_time until the trajectory execution starts (ideally, the execution should starts instantly and no prints should appear).

Speed scaling 98%:

[ur_ros2_control_node-2] [INFO] [1686578418.807146267] [scaled_joint_trajectory_controller]: Received new action goal
[ur_ros2_control_node-2] [INFO] [1686578418.807181674] [scaled_joint_trajectory_controller]: Accepted new action goal
[ur_ros2_control_node-2] [INFO] [1686578418.810122798] [scaled_joint_trajectory_controller]: traj_time = 1686578418646041936
[ur_ros2_control_node-2] [INFO] [1686578418.812121022] [scaled_joint_trajectory_controller]: traj_time = 1686578418648005583
[ur_ros2_control_node-2] [INFO] [1686578418.814171895] [scaled_joint_trajectory_controller]: traj_time = 1686578418650023235
[ur_ros2_control_node-2] [INFO] [1686578418.816141985] [scaled_joint_trajectory_controller]: traj_time = 1686578418651962600
[ur_ros2_control_node-2] [INFO] [1686578418.818163623] [scaled_joint_trajectory_controller]: traj_time = 1686578418653933556
[ur_ros2_control_node-2] [INFO] [1686578418.820239032] [scaled_joint_trajectory_controller]: traj_time = 1686578418655976889
[ur_ros2_control_node-2] [INFO] [1686578418.822138890] [scaled_joint_trajectory_controller]: traj_time = 1686578418657825247
[ur_ros2_control_node-2] [INFO] [1686578418.824110683] [scaled_joint_trajectory_controller]: traj_time = 1686578418659766984
[ur_ros2_control_node-2] [INFO] [1686578418.826134004] [scaled_joint_trajectory_controller]: traj_time = 1686578418661731798
[ur_ros2_control_node-2] [INFO] [1686578418.828107170] [scaled_joint_trajectory_controller]: traj_time = 1686578418663685534
[ur_ros2_control_node-2] [INFO] [1686578418.830102348] [scaled_joint_trajectory_controller]: traj_time = 1686578418665641476
[ur_ros2_control_node-2] [INFO] [1686578418.832176966] [scaled_joint_trajectory_controller]: traj_time = 1686578418667663964
[ur_ros2_control_node-2] [INFO] [1686578418.834083226] [scaled_joint_trajectory_controller]: traj_time = 1686578418669543733
[ur_ros2_control_node-2] [INFO] [1686578418.836136864] [scaled_joint_trajectory_controller]: traj_time = 1686578418671555363
[ur_ros2_control_node-2] [INFO] [1686578418.838124537] [scaled_joint_trajectory_controller]: traj_time = 1686578418673496705
[ur_ros2_control_node-2] [INFO] [1686578418.840112021] [scaled_joint_trajectory_controller]: traj_time = 1686578418675436075
[ur_ros2_control_node-2] [INFO] [1686578418.842089154] [scaled_joint_trajectory_controller]: traj_time = 1686578418677389367
[ur_ros2_control_node-2] [INFO] [1686578418.844096565] [scaled_joint_trajectory_controller]: traj_time = 1686578418679352101
[ur_ros2_control_node-2] [INFO] [1686578418.846083267] [scaled_joint_trajectory_controller]: traj_time = 1686578418681302868
[ur_ros2_control_node-2] [INFO] [1686578418.848079206] [scaled_joint_trajectory_controller]: traj_time = 1686578418683258840
[ur_ros2_control_node-2] [INFO] [1686578418.850075957] [scaled_joint_trajectory_controller]: traj_time = 1686578418685214202
[ur_ros2_control_node-2] [INFO] [1686578418.852122471] [scaled_joint_trajectory_controller]: traj_time = 1686578418687221552
[ur_ros2_control_node-2] [INFO] [1686578418.854118871] [scaled_joint_trajectory_controller]: traj_time = 1686578418689177647
[ur_ros2_control_node-2] [INFO] [1686578418.856119439] [scaled_joint_trajectory_controller]: traj_time = 1686578418691140208
[ur_ros2_control_node-2] [INFO] [1686578418.858130818] [scaled_joint_trajectory_controller]: traj_time = 1686578418693096637
[ur_ros2_control_node-2] [INFO] [1686578418.860124152] [scaled_joint_trajectory_controller]: traj_time = 1686578418695056844
[ur_ros2_control_node-2] [INFO] [1686578418.862105834] [scaled_joint_trajectory_controller]: traj_time = 1686578418697002992
[ur_ros2_control_node-2] [INFO] [1686578418.864102034] [scaled_joint_trajectory_controller]: traj_time = 1686578418698960450
[ur_ros2_control_node-2] [INFO] [1686578418.866106259] [scaled_joint_trajectory_controller]: traj_time = 1686578418700923478
[ur_ros2_control_node-2] [INFO] [1686578418.868104051] [scaled_joint_trajectory_controller]: traj_time = 1686578418702881852
[ur_ros2_control_node-2] [INFO] [1686578418.870091805] [scaled_joint_trajectory_controller]: traj_time = 1686578418704832722
[ur_ros2_control_node-2] [INFO] [1686578418.872102923] [scaled_joint_trajectory_controller]: traj_time = 1686578418706795587
[ur_ros2_control_node-2] [INFO] [1686578418.874084906] [scaled_joint_trajectory_controller]: traj_time = 1686578418708746503
[ur_ros2_control_node-2] [INFO] [1686578418.876091284] [scaled_joint_trajectory_controller]: traj_time = 1686578418710708203
[ur_ros2_control_node-2] [INFO] [1686578418.878084829] [scaled_joint_trajectory_controller]: traj_time = 1686578418712662688
[ur_ros2_control_node-2] [INFO] [1686578418.880081299] [scaled_joint_trajectory_controller]: traj_time = 1686578418714619157
[ur_ros2_control_node-2] [INFO] [1686578418.882122314] [scaled_joint_trajectory_controller]: traj_time = 1686578418716622007
[ur_ros2_control_node-2] [INFO] [1686578418.884118303] [scaled_joint_trajectory_controller]: traj_time = 1686578418718578703
[ur_ros2_control_node-2] [INFO] [1686578418.886124491] [scaled_joint_trajectory_controller]: traj_time = 1686578418720545731
[ur_ros2_control_node-2] [INFO] [1686578418.888110812] [scaled_joint_trajectory_controller]: traj_time = 1686578418722492143
[ur_ros2_control_node-2] [INFO] [1686578418.890138782] [scaled_joint_trajectory_controller]: traj_time = 1686578418724476273
[ur_ros2_control_node-2] [INFO] [1686578418.892281288] [scaled_joint_trajectory_controller]: traj_time = 1686578418726566598
[ur_ros2_control_node-2] [INFO] [1686578418.894199681] [scaled_joint_trajectory_controller]: traj_time = 1686578418728455078
[ur_ros2_control_node-2] [INFO] [1686578418.896125848] [scaled_joint_trajectory_controller]: traj_time = 1686578418730343826
[ur_ros2_control_node-2] [INFO] [1686578418.898136124] [scaled_joint_trajectory_controller]: traj_time = 1686578418732306527
[ur_ros2_control_node-2] [INFO] [1686578418.900130050] [scaled_joint_trajectory_controller]: traj_time = 1686578418734261790
[ur_ros2_control_node-2] [INFO] [1686578418.902083679] [scaled_joint_trajectory_controller]: traj_time = 1686578418736181605
[ur_ros2_control_node-2] [INFO] [1686578418.904107801] [scaled_joint_trajectory_controller]: traj_time = 1686578418738168025
[ur_ros2_control_node-2] [INFO] [1686578418.906109952] [scaled_joint_trajectory_controller]: traj_time = 1686578418740126006
[ur_ros2_control_node-2] [INFO] [1686578418.908127392] [scaled_joint_trajectory_controller]: traj_time = 1686578418742099618
[ur_ros2_control_node-2] [INFO] [1686578418.910107612] [scaled_joint_trajectory_controller]: traj_time = 1686578418744042071
[ur_ros2_control_node-2] [INFO] [1686578418.912100595] [scaled_joint_trajectory_controller]: traj_time = 1686578418745997804
[ur_ros2_control_node-2] [INFO] [1686578418.914100251] [scaled_joint_trajectory_controller]: traj_time = 1686578418747954416
[ur_ros2_control_node-2] [INFO] [1686578418.916090049] [scaled_joint_trajectory_controller]: traj_time = 1686578418749911386
[ur_ros2_control_node-2] [INFO] [1686578418.918089124] [scaled_joint_trajectory_controller]: traj_time = 1686578418751866444
[ur_ros2_control_node-2] [INFO] [1686578418.920101725] [scaled_joint_trajectory_controller]: traj_time = 1686578418753840207
[ur_ros2_control_node-2] [INFO] [1686578418.922082666] [scaled_joint_trajectory_controller]: traj_time = 1686578418755784050
[ur_ros2_control_node-2] [INFO] [1686578418.924141163] [scaled_joint_trajectory_controller]: traj_time = 1686578418757796272
[ur_ros2_control_node-2] [INFO] [1686578418.926124037] [scaled_joint_trajectory_controller]: traj_time = 1686578418759742015
[ur_ros2_control_node-2] [INFO] [1686578418.928140956] [scaled_joint_trajectory_controller]: traj_time = 1686578418761713597
[ur_ros2_control_node-2] [INFO] [1686578418.930148988] [scaled_joint_trajectory_controller]: traj_time = 1686578418763679849
[ur_ros2_control_node-2] [INFO] [1686578418.932140519] [scaled_joint_trajectory_controller]: traj_time = 1686578418765634456
[ur_ros2_control_node-2] [INFO] [1686578418.934122993] [scaled_joint_trajectory_controller]: traj_time = 1686578418767577385
[ur_ros2_control_node-2] [INFO] [1686578418.936161883] [scaled_joint_trajectory_controller]: traj_time = 1686578418769582059
[ur_ros2_control_node-2] [INFO] [1686578418.938122966] [scaled_joint_trajectory_controller]: traj_time = 1686578418771503147
[ur_ros2_control_node-2] [INFO] [1686578418.940125508] [scaled_joint_trajectory_controller]: traj_time = 1686578418773462130
[ur_ros2_control_node-2] [INFO] [1686578418.942121918] [scaled_joint_trajectory_controller]: traj_time = 1686578418775419317
[ur_ros2_control_node-2] [INFO] [1686578418.944120222] [scaled_joint_trajectory_controller]: traj_time = 1686578418777376524
[ur_ros2_control_node-2] [INFO] [1686578418.946108366] [scaled_joint_trajectory_controller]: traj_time = 1686578418779329664
[ur_ros2_control_node-2] [INFO] [1686578418.948114044] [scaled_joint_trajectory_controller]: traj_time = 1686578418781289859
[ur_ros2_control_node-2] [INFO] [1686578418.950107057] [scaled_joint_trajectory_controller]: traj_time = 1686578418783246457
[ur_ros2_control_node-2] [INFO] [1686578418.952154243] [scaled_joint_trajectory_controller]: traj_time = 1686578418785251768
[ur_ros2_control_node-2] [INFO] [1686578418.954093325] [scaled_joint_trajectory_controller]: traj_time = 1686578418787155755
[ur_ros2_control_node-2] [INFO] [1686578418.956084776] [scaled_joint_trajectory_controller]: traj_time = 1686578418789101235
[ur_ros2_control_node-2] [INFO] [1686578418.958145808] [scaled_joint_trajectory_controller]: traj_time = 1686578418791125324
[ur_ros2_control_node-2] [INFO] [1686578418.960171413] [scaled_joint_trajectory_controller]: traj_time = 1686578418793100606
[ur_ros2_control_node-2] [INFO] [1686578418.962103542] [scaled_joint_trajectory_controller]: traj_time = 1686578418794996260
[ur_ros2_control_node-2] [INFO] [1686578418.964145848] [scaled_joint_trajectory_controller]: traj_time = 1686578418797001810
[ur_ros2_control_node-2] [INFO] [1686578418.966132330] [scaled_joint_trajectory_controller]: traj_time = 1686578418798949870
[ur_ros2_control_node-2] [INFO] [1686578418.968138097] [scaled_joint_trajectory_controller]: traj_time = 1686578418800911952
[ur_ros2_control_node-2] [INFO] [1686578418.970277437] [scaled_joint_trajectory_controller]: traj_time = 1686578418803019720
[ur_ros2_control_node-2] [INFO] [1686578418.972152108] [scaled_joint_trajectory_controller]: traj_time = 1686578418804841133
[ur_ros2_control_node-2] [INFO] [1686578418.974102751] [scaled_joint_trajectory_controller]: traj_time = 1686578418806760981

Speed scaling 99%:

[ur_ros2_control_node-2] [INFO] [1686578833.876966044] [scaled_joint_trajectory_controller]: Received new action goal
[ur_ros2_control_node-2] [INFO] [1686578833.877015928] [scaled_joint_trajectory_controller]: Accepted new action goal
[ur_ros2_control_node-2] [INFO] [1686578833.879666215] [scaled_joint_trajectory_controller]: traj_time = 1686578833794054789
[ur_ros2_control_node-2] [INFO] [1686578833.881678156] [scaled_joint_trajectory_controller]: traj_time = 1686578833796046971
[ur_ros2_control_node-2] [INFO] [1686578833.883657155] [scaled_joint_trajectory_controller]: traj_time = 1686578833798012033
[ur_ros2_control_node-2] [INFO] [1686578833.885658696] [scaled_joint_trajectory_controller]: traj_time = 1686578833799996818
[ur_ros2_control_node-2] [INFO] [1686578833.887661821] [scaled_joint_trajectory_controller]: traj_time = 1686578833801979545
[ur_ros2_control_node-2] [INFO] [1686578833.889665546] [scaled_joint_trajectory_controller]: traj_time = 1686578833803950338
[ur_ros2_control_node-2] [INFO] [1686578833.891728864] [scaled_joint_trajectory_controller]: traj_time = 1686578833805992154
[ur_ros2_control_node-2] [INFO] [1686578833.893677105] [scaled_joint_trajectory_controller]: traj_time = 1686578833807936398
[ur_ros2_control_node-2] [INFO] [1686578833.895686241] [scaled_joint_trajectory_controller]: traj_time = 1686578833809924822
[ur_ros2_control_node-2] [INFO] [1686578833.897748567] [scaled_joint_trajectory_controller]: traj_time = 1686578833811961477
[ur_ros2_control_node-2] [INFO] [1686578833.899674024] [scaled_joint_trajectory_controller]: traj_time = 1686578833813867298
[ur_ros2_control_node-2] [INFO] [1686578833.901729057] [scaled_joint_trajectory_controller]: traj_time = 1686578833815903487
[ur_ros2_control_node-2] [INFO] [1686578833.903673680] [scaled_joint_trajectory_controller]: traj_time = 1686578833817823398
[ur_ros2_control_node-2] [INFO] [1686578833.905686834] [scaled_joint_trajectory_controller]: traj_time = 1686578833819810524
[ur_ros2_control_node-2] [INFO] [1686578833.907738159] [scaled_joint_trajectory_controller]: traj_time = 1686578833821850079
[ur_ros2_control_node-2] [INFO] [1686578833.909656794] [scaled_joint_trajectory_controller]: traj_time = 1686578833823745101
[ur_ros2_control_node-2] [INFO] [1686578833.911654788] [scaled_joint_trajectory_controller]: traj_time = 1686578833825725071
[ur_ros2_control_node-2] [INFO] [1686578833.913638406] [scaled_joint_trajectory_controller]: traj_time = 1686578833827694878
[ur_ros2_control_node-2] [INFO] [1686578833.915862708] [scaled_joint_trajectory_controller]: traj_time = 1686578833829865378
[ur_ros2_control_node-2] [INFO] [1686578833.917660234] [scaled_joint_trajectory_controller]: traj_time = 1686578833831671337
[ur_ros2_control_node-2] [INFO] [1686578833.919720636] [scaled_joint_trajectory_controller]: traj_time = 1686578833833713311
[ur_ros2_control_node-2] [INFO] [1686578833.921687001] [scaled_joint_trajectory_controller]: traj_time = 1686578833835659704
[ur_ros2_control_node-2] [INFO] [1686578833.923701016] [scaled_joint_trajectory_controller]: traj_time = 1686578833837643519
[ur_ros2_control_node-2] [INFO] [1686578833.925663914] [scaled_joint_trajectory_controller]: traj_time = 1686578833839599467
[ur_ros2_control_node-2] [INFO] [1686578833.927661268] [scaled_joint_trajectory_controller]: traj_time = 1686578833841575547
[ur_ros2_control_node-2] [INFO] [1686578833.929686293] [scaled_joint_trajectory_controller]: traj_time = 1686578833843576536
[ur_ros2_control_node-2] [INFO] [1686578833.931670251] [scaled_joint_trajectory_controller]: traj_time = 1686578833845536918
[ur_ros2_control_node-2] [INFO] [1686578833.933655492] [scaled_joint_trajectory_controller]: traj_time = 1686578833847508314
[ur_ros2_control_node-2] [INFO] [1686578833.935651202] [scaled_joint_trajectory_controller]: traj_time = 1686578833849484211
[ur_ros2_control_node-2] [INFO] [1686578833.937647253] [scaled_joint_trajectory_controller]: traj_time = 1686578833851454091
[ur_ros2_control_node-2] [INFO] [1686578833.939734927] [scaled_joint_trajectory_controller]: traj_time = 1686578833853522548
[ur_ros2_control_node-2] [INFO] [1686578833.941674602] [scaled_joint_trajectory_controller]: traj_time = 1686578833855449604
[ur_ros2_control_node-2] [INFO] [1686578833.943898463] [scaled_joint_trajectory_controller]: traj_time = 1686578833857649797
[ur_ros2_control_node-2] [INFO] [1686578833.945863996] [scaled_joint_trajectory_controller]: traj_time = 1686578833859577283
[ur_ros2_control_node-2] [INFO] [1686578833.947658897] [scaled_joint_trajectory_controller]: traj_time = 1686578833861368758
[ur_ros2_control_node-2] [INFO] [1686578833.949659988] [scaled_joint_trajectory_controller]: traj_time = 1686578833863352670
[ur_ros2_control_node-2] [INFO] [1686578833.951753242] [scaled_joint_trajectory_controller]: traj_time = 1686578833865425863
[ur_ros2_control_node-2] [INFO] [1686578833.953745746] [scaled_joint_trajectory_controller]: traj_time = 1686578833867398426
[ur_ros2_control_node-2] [INFO] [1686578833.955668870] [scaled_joint_trajectory_controller]: traj_time = 1686578833869304530
[ur_ros2_control_node-2] [INFO] [1686578833.957766693] [scaled_joint_trajectory_controller]: traj_time = 1686578833871380387
[ur_ros2_control_node-2] [INFO] [1686578833.959647155] [scaled_joint_trajectory_controller]: traj_time = 1686578833873241491
[ur_ros2_control_node-2] [INFO] [1686578833.961734288] [scaled_joint_trajectory_controller]: traj_time = 1686578833875306607
[ur_ros2_control_node-2] [INFO] [1686578833.963661670] [scaled_joint_trajectory_controller]: traj_time = 1686578833877219740

Speed scaling 100%:

[ur_ros2_control_node-2] [INFO] [1686578317.781871010] [scaled_joint_trajectory_controller]: Received new action goal
[ur_ros2_control_node-2] [INFO] [1686578317.781909442] [scaled_joint_trajectory_controller]: Accepted new action goal

As it is possible to notice, with a speed scaling < 100%, traj_time is lower than the ros timestamp when the goal is accepted and the execution is delayed. Conversely, when the speed scaling is 100%, no prints occurs.

For now I "solved" this problem forcing scaling_factor_ to be equal to 100% until the trajectory execution is started.

SuperDiodo avatar Jun 12 '23 14:06 SuperDiodo

I am also facing the same problem, trajectories can take minutes to start when speed scaling is small. @SuperDiodo can you please share in more detail your temporary fix?

g-argyropoulos avatar Jun 28 '23 09:06 g-argyropoulos

Hi, I've cloned the driver package from the repo and modified the scaled_joint_trajectory_controller cpp/hpp code (attached txt files).

scaled_joint_trajectory_controller.hpp.txt

scaled_joint_trajectory_controller.cpp.txt

I don't know if it's the best solution, but it's working for me. Basically: I've added a boolean variable executing_trajectory_ that tracks the execution of a trajectory. Until the trajectory has not started the variable is false and the speed scaling factor is by default 1. If the variable is true the speed scaling becomes equal to the value set in the teach pendant.

Let me know If it works for you.

SuperDiodo avatar Jun 28 '23 09:06 SuperDiodo

@SuperDiodo tank you for putting that work into this. I'll try looking into the scaled JTC the next couple of days. Please keep this open!

fmauch avatar Jun 28 '23 21:06 fmauch

@SuperDiodo it worked like charm !! thx

JaisonJose241 avatar Aug 23 '23 05:08 JaisonJose241

Tried reproducing the issue but it seems to no longer exist. I know that we have been touching that part of the code in the meantime. Closing.

urfeex avatar Feb 07 '25 15:02 urfeex