Dynamically Control Trajectory Execution

[shuobh]

Summary

Parameters such as servoj_gain and servoj_lookahead_time can be set during initialization. However, is there a way to set it dynamically per trajectory execution?

Also, is there a way to get a more detailed tracking status such as which waypoint in the input trajectory is the robot moving towards?

Versions

• ROS Driver version: ROS Noetic
• Affected Robot Software Version(s): 5.11.1
• Affected Robot Hardware Version(s): UR5e

Impact

By changing servoj_gain and servoj_lookahead_time, we could leverage between follow a path accurately or less accurate with a smoother path.

By knowing the current robot joint position with regards to the input trajectory allows in-parallel control of other part of the system such as suction\grasping.

Issue details

Feature Clarification/Request

Thanks in advance!

[fmauch]

The servoj parameters cannot be easily changed in a dynamic fashion. As explained in the README the robot program is fetched once at the beginning. At this time, those parameters are sent to the robot. Making them dynamically configurable would require a significant extension of our communication protocol with the driver or it would require using configurable ports in the RTDE interface which would be sent every 2 ms and bear the potential to clash with other software running on the robot.

If this is necessary for your application, you could of course implement such a thing yourself. You would basically change the script code to read the value from the respective RTDE field instead of the current {{...REPLACE} snippet, update the RTDE input_recipe to use input_double_register_X fields. Then, you would need services in the ROS driver to set those fields similar to

https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/c09807d79b5eb2e8b4d671995ec885ceeb48fc5d/ur_robot_driver/src/hardware_interface.cpp#L1074-L1102 but using sendInputDoubleRegister

Regarding your second question: There are multiple levels of command feedback:

First of all, you can look at the trajectory_controller's feedback topic. It contains the desired positions, the actual joint positions and the error computed from both. As the control structure here is cascaded (The trajectory controller wraps around the underlying servoj command), this represents the outermost cascade.

Then, there's the commands that are actually being sent to the robot and the commands being sent to the internal motor controllers. The latter can be read using the RTDE-Field ´target_q. Those are read by the driver, but by default not published. I used that in the past to debug timing, see [this old commit](https://github.com/fmauch/Universal_Robots_ROS_Driver/commit/84716fb9c031327a0abb38d0e3188766366f72ae) (It is rather old, so it will most probably not apply to the current master without conflicts, but you'll get the idea from that). The command_pub_` in that commit publishes the commands being actually sent to the robot.

I hope, this explanation helps. If you come up with something useful, feel free to open a PR than we can discuss whether things should get merged.

[shuobh]

The servoj parameters cannot be easily changed in a dynamic fashion. As explained in the README the robot program is fetched once at the beginning. At this time, those parameters are sent to the robot. Making them dynamically configurable would require a significant extension of our communication protocol with the driver or it would require using configurable ports in the RTDE interface which would be sent every 2 ms and bear the potential to clash with other software running on the robot.

If this is necessary for your application, you could of course implement such a thing yourself. You would basically change the script code to read the value from the respective RTDE field instead of the current {{...REPLACE} snippet, update the RTDE input_recipe to use input_double_register_X fields. Then, you would need services in the ROS driver to set those fields similar to

https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/c09807d79b5eb2e8b4d671995ec885ceeb48fc5d/ur_robot_driver/src/hardware_interface.cpp#L1074-L1102 but using sendInputDoubleRegister

Regarding your second question: There are multiple levels of command feedback:

First of all, you can look at the trajectory_controller's feedback topic. It contains the desired positions, the actual joint positions and the error computed from both. As the control structure here is cascaded (The trajectory controller wraps around the underlying servoj command), this represents the outermost cascade.

Then, there's the commands that are actually being sent to the robot and the commands being sent to the internal motor controllers. The latter can be read using the RTDE-Field ´target_q. Those are read by the driver, but by default not published. I used that in the past to debug timing, see [this old commit](https://github.com/fmauch/Universal_Robots_ROS_Driver/commit/84716fb9c031327a0abb38d0e3188766366f72ae) (It is rather old, so it will most probably not apply to the current master without conflicts, but you'll get the idea from that). The command_pub_` in that commit publishes the commands being actually sent to the robot.

I hope, this explanation helps. If you come up with something useful, feel free to open a PR than we can discuss whether things should get merged.

Thanks for the insight, and sorry for getting back so late. As I looked into the code, it seemed to me that if a set gain/lookaheadtime function is provided in the the ur_client_library, it would be as simple as to call that function and execute startRTDECommunication. Is it correct?

[fmauch]

Not exactly.

As explained in the README the robot program is fetched once at the beginning. At this time, those parameters are sent to the robot.

startRTDECommunication is actually unrelated to that. The script gets sent inside the ScriptSender. In order to get a modified script code on the robot, the program would have to be requeried. That means that the program on the robot would have to be restarted (e.g. by pressing STOP and PLAY on the Teach pendant or doing the equivalent using the dasboard client or by calling the resend_program service when being in headless mode).

[shuobh]

Not exactly.

As explained in the README the robot program is fetched once at the beginning. At this time, those parameters are sent to the robot.

startRTDECommunication is actually unrelated to that. The script gets sent inside the ScriptSender. In order to get a modified script code on the robot, the program would have to be requeried. That means that the program on the robot would have to be restarted (e.g. by pressing STOP and PLAY on the Teach pendant or doing the equivalent using the dasboard client or by calling the resend_program service when being in headless mode).

What about controlling using remote control, it still counts as a program, right? From what I understand is that if changing gain/lookaheadtime is supported in the library, in order to make it work with remote control, I need to restart the program? If this is the case, although it will have introduce a delay but it would work for us since changing gain/lookaheadtime is important but it is only for a limited amount of actions.

[shuobh]

The servoj parameters cannot be easily changed in a dynamic fashion. As explained in the README the robot program is fetched once at the beginning. At this time, those parameters are sent to the robot. Making them dynamically configurable would require a significant extension of our communication protocol with the driver or it would require using configurable ports in the RTDE interface which would be sent every 2 ms and bear the potential to clash with other software running on the robot.

If this is necessary for your application, you could of course implement such a thing yourself. You would basically change the script code to read the value from the respective RTDE field instead of the current {{...REPLACE} snippet, update the RTDE input_recipe to use input_double_register_X fields. Then, you would need services in the ROS driver to set those fields similar to

https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/c09807d79b5eb2e8b4d671995ec885ceeb48fc5d/ur_robot_driver/src/hardware_interface.cpp#L1074-L1102 but using sendInputDoubleRegister

Regarding your second question: There are multiple levels of command feedback:

First of all, you can look at the trajectory_controller's feedback topic. It contains the desired positions, the actual joint positions and the error computed from both. As the control structure here is cascaded (The trajectory controller wraps around the underlying servoj command), this represents the outermost cascade.

Then, there's the commands that are actually being sent to the robot and the commands being sent to the internal motor controllers. The latter can be read using the RTDE-Field ´target_q. Those are read by the driver, but by default not published. I used that in the past to debug timing, see [this old commit](https://github.com/fmauch/Universal_Robots_ROS_Driver/commit/84716fb9c031327a0abb38d0e3188766366f72ae) (It is rather old, so it will most probably not apply to the current master without conflicts, but you'll get the idea from that). The command_pub_` in that commit publishes the commands being actually sent to the robot.

I hope, this explanation helps. If you come up with something useful, feel free to open a PR than we can discuss whether things should get merged.

I finally worked it out following your guideline. Thanks a lot! Please let me know if you think this would in general be useful and I can open a PR for my implementation.

[fmauch]

I'm glad to hear that you got this working!

Since using the input_double_register_X fields potentially clashes with other applications we explicitly designed the driver not to use them. So, I don't think we would merge it in like that. However, if youcould leave a link to your implementation for others to find in the future, this would probably be great.

[shuobh]

Adding our implementation here. Note that when using lower sharpness trajectory tolerance needs to be relaxed accordingly. https://github.com/shuobh/Universal_Robots_ROS_Driver/pull/1/files