Delay in execution

[kokkalisko]

Affected ROS2 Driver version(s)

2.1.2

Used ROS distribution.

Galactic

Which combination of platform is the ROS driver running on.

Ubuntu Linux with realtime patch

How is the UR ROS2 Driver installed.

From binary packets

Which robot platform is the driver connected to.

UR E-series robot

Robot SW / URSim version(s)

URSim 5.15.0 and real robot 5.14.5

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

I am trying to achieve precise synchronization of a UR5e robot with a moving conveyor to pick up objects. Either using URSim or the actual robot and MoveIt2 there is significant time difference (25 up to 100ms) between the plan and the actual execution.

Issue details

I have tried to minimize this delay making multiple changes:

• Change the controller to joint_trajectory_controller
• Change the kernel to PREEMPT_RT (have not changed any other related settings yet)
• Change the planner e.g. use the pilz_industrial_motion_planner
• Try any type of motions

For reference, there are a couple of things that have reduce drastically the delay:

• Change of the safety limits of the robot to "Least Restricted"
• Increase the monitor frequency of the controller up to a maximum (in my case 50Hz)

Steps to Reproduce

1. Launch URSim (you can also test it with the real robot and change the robot IP)

2. ros2 launch ur_bringup ur5e.launch.py robot_ip:=127.0.0.1 use_fake_hardware:=false launch_rviz:=false

3. ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur5e

4. Create a C++ node performing motions and time them using something along these lines:
   Positions: [0,0,0,0,0,3M_PI_22], [0,0,0,0,0,-3M_PI_22]

    moveit_msgs::msg::RobotTrajectory trajectory_msg;
    moveit::planning_interface::MoveGroupInterface::Plan plan;
   
    moveInterfacePtr->setStartStateToCurrentState();
    moveInterfacePtr->setJointValueTarget(firstPosition);
   
    moveInterfacePtr->plan(plan);
    trajectory_msg = plan.trajectory_;
   
    double executionTime = rclcpp::Duration(trajectory_msg.joint_trajectory.points.back().time_from_start).seconds();
    RCLCPP_DEBUG_STREAM(this->get_logger(), "The move should last: " << executionTime);
   
    auto start = this->get_clock()->now();
    moveInterfacePtr->execute(trajectory_msg);
    auto end = this->get_clock()->now();
    rclcpp::Duration diff = end - start;
    RCLCPP_INFO_STREAM(this->get_logger(), "Execution lasted " << diff.seconds() << " seconds");
    RCLCPP_INFO_STREAM(this->get_logger(), "Time difference is " << (diff.seconds() - executionTime)*1000 << " milliseconds");
   
    ////////////////////////////
   
    moveInterfacePtr->setStartStateToCurrentState();
    moveInterfacePtr->setJointValueTarget(secondPosition);
   
    moveInterfacePtr->plan(plan);
    trajectory_msg = plan.trajectory_;
   
    executionTime = rclcpp::Duration(trajectory_msg.joint_trajectory.points.back().time_from_start).seconds();
    RCLCPP_DEBUG_STREAM(this->get_logger(), "The move should last: " << executionTime);
   
    start = this->get_clock()->now();
    moveInterfacePtr->execute(trajectory_msg);
    end = this->get_clock()->now();
    diff = end - start;
    RCLCPP_INFO_STREAM(this->get_logger(), "Execution lasted " << diff.seconds() << " seconds");
    RCLCPP_INFO_STREAM(this->get_logger(), "Time difference is " << (diff.seconds() - executionTime)*1000 << " milliseconds");
   

Expected Behavior

The motion should last as much as predicted by the plan (up to a small time difference).

Actual Behavior

The log similar to this:

    [simple_move-2] [INFO] [1716557813.182926855] [move_group_interface]: MoveGroup action client/server ready
    [simple_move-2] [INFO] [1716557813.183367335] [move_group_interface]: Planning request accepted
    [simple_move-2] [INFO] [1716557813.191581977] [move_group_interface]: Planning request complete!
    [simple_move-2] [INFO] [1716557813.283252330] [move_group_interface]: time taken to generate plan: 7.9301e-05 seconds
    [simple_move-2] [INFO] [1716557813.283328532] [static_pipeline_scenario]: Computed a plan successfully
    [simple_move-2] [INFO] [1716557813.283809924] [move_group_interface]: Execute request accepted
    [simple_move-2] [INFO] [1716557815.251111738] [move_group_interface]: Execute request success!
    [simple_move-2] [INFO] [1716557815.284803171] [static_pipeline_scenario]: Execution lasted 2.00142 seconds
    [simple_move-2] [INFO] [1716557815.284831580] [static_pipeline_scenario]: Time difference is 51.1607 milliseconds

Relevant log output

No response

Accept Public visibility

• I agree to make this context public

[fmauch]

Thanks for sharing your thought with us.

I have a couple of remarks:

• You seem to be using a rather old version on an end-of-life ROS version.
• There's definitively a small delay in execution, since the underlying command on the robot controller (servoj) needs some cycles to correctly compute motion commands to get to a target position. It basically needs to look ahead of the next setpoint.
• If you need synchronous motions you should definitively combine both drivers into one controller_manager. I assume, you are already doing that, you just simplified things to demonstrate the problem here.
• I'm not sure about MoveIt!'s role here. You are measuring trajectory execution plus the overhead of the action call. As the latter is not real-time critical this is not deterministic.
• You might be better off using a velocity-based controller, since the expected delay is lower there. However, this currently seems not to be working correctly (see JTC using velocity as command state interface yields to vibration due to sudden deceleration #905)

[kokkalisko]

Just an update:

• I have tried to move the robot to an end position in URSim by publishing a setpoint to the controller topic, but the delay seems to persist.
• According to my timing tests, the added delay due to the MoveIt action call is negligible.

At this point, I strongly think that this is not related to the UR driver (maybe related to the controllers) and I will close the issue. In case I ever come up with a solution I will update it.