unexcepted velocity limit warning when using forward_position_controller

[migra1315]

Affected ROS2 Driver version(s)

humble

Used ROS distribution.

Humble

Which combination of platform is the ROS driver running on.

Ubuntu Linux with standard kernel

How is the UR ROS2 Driver installed.

From binary packets

Which robot platform is the driver connected to.

UR CB3 robot

Robot SW / URSim version(s)

1.0.5

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

1. send Float64MultiArray msgs through /forward_position_controller/commands into robot, sometimes the teach pendant show info and warning with a huge number like " velocity 6394858586509657767868684523424243 required to reach the received target [0.38711, 66883463842942424345646， -0.601, ....]" . sometimes the huge nubmer in target array show as -nan
2. after this, even I have stop send control msgs through /forward_position_controller/commands, the teach pendant still show above info until I stop ur cap . The warning shows "C218A: a thread used to much time: thread: servoThread "

Issue details

class JointPositionPublisher(Node):
    def __init__(self):
        super().__init__('joint_position_publisher')
        self.positions = {}

        # 创建一个 QoS 配置文件，设置为 Transient Local
        qos_profile = QoSProfile(
            depth=10,  # 消息队列深度
            durability=DurabilityPolicy.TRANSIENT_LOCAL  # 设置为 Transient Local
        )
        
        # Define the joints of interest
        self.joints_of_interest = [
            # 'right_shoulder_pan_joint', 
            # 'right_shoulder_lift_joint', 
            # 'right_elbow_joint',
            # 'right_wrist_1_joint', 
            # 'right_wrist_2_joint', 
            # 'right_wrist_3_joint',
            'left_shoulder_pan_joint', 
            'left_shoulder_lift_joint',
            'left_elbow_joint', 
            'left_wrist_1_joint', 
            'left_wrist_2_joint', 
            'left_wrist_3_joint'
        ]
        self.toListen  = True
        self.subscription = self.create_subscription(
            JointState,
            '/left/joint_states',
            self.joint_state_callback,
            10  # QoS depth
        )
        self.subscription  # Prevent unused variable warning

        time.sleep(1)
        self.get_logger().info(f'wait 1 sec ...')

        self.publisher = self.create_publisher(Float64MultiArray, '/left/forward_position_controller/commands', qos_profile)

        self.timer = self.create_timer(0.004, self.publish_joint_positions)

        self.step = 0.03
        self.total_steps = 100
        self.current_step = 0

    def publish_joint_positions(self):
        # self.get_logger().info(f'{"wrist_2_joint"}: {self.positions["left_wrist_2_joint"]}')
        # self.get_logger().info(f'{"wrist_2_joint"}: {self.positions["right_wrist_2_joint"]}')
        if self.current_step < self.total_steps:
            # Update the position of the second joint (elbow_joint)
            self.joint_positions[0] = self.positions["left_shoulder_pan_joint"]
            self.joint_positions[1] = self.positions["left_shoulder_lift_joint"]
            self.joint_positions[2] = self.positions["left_elbow_joint"]
            self.joint_positions[3] = self.positions["left_wrist_1_joint"]
            self.joint_positions[4] = (self.positions["left_wrist_2_joint"]-self.step * self.current_step ) 
            self.joint_positions[5] = self.positions["left_wrist_3_joint"]
            # Create and publish the message
            msg = Float64MultiArray()
            msg.data = [float(pos) for pos in self.joint_positions]
            # self.joint_positions

            self.publisher.publish(msg)
            # self.publisher.publish(msg)
            self.current_step += 1
            self.get_logger().info(f'Published joint positions: {type(msg.data[2]),msg.data[4]}')

            
        else:
            self.get_logger().info('Completed all steps.')
            self.timer.cancel()

    def joint_state_callback(self, msg: JointState):
            if(self.toListen):
                # Extract positions for the joints of interest
                for name, position in zip(msg.name, msg.position):
                    # self.get_logger().info(f'{name}')
                    if name in self.joints_of_interest:
                        self.positions[name] = position
                        self.get_logger().info(f'{name}: {self.positions[name]}')
                self.toListen = False

Steps to Reproduce

This code may not always cause this issue, but attempting to trigger velocity limits may result in the above phenomenon.

Expected Behavior

What did you expect and why?

Actual Behavior

What did you observe? If possible please attach relevant information.

Workaround Suggestion

If a workaround has been found, you are welcome to share it.

Relevant log output

Accept Public visibility

• [x] I agree to make this context public

[migra1315]

addtional info:

1. [UR_Client_Library:ur_]: Pipeline producer overflowed! <RTDE Data Pipeline> was printed when bringup code is running sometimes and when click ctrl+c always
2. the teach pendant logs show

[urfeex]

As you are using Humble: Humble is currently a bit behind with the ur_client_library version 1.6.0. There should be a repo sync later today that should make a newer version available for Humble where we updated the limit check, Could you please try again with that?

I had to update the script you provided a little bit to get it running (please try to provide running minimal working examples when you want people to help reproducing your issues) and with that I was not able to reproduce the error you mentioned:

#!/usr/bin/env python3

import time

import rclpy
from rclpy.node import Node

from sensor_msgs.msg import JointState
from std_msgs.msg import Float64MultiArray
from rclpy.qos import QoSProfile, DurabilityPolicy

class JointPositionPublisher(Node):
    def __init__(self):
        super().__init__('joint_position_publisher')
        self.positions = {}
        self.joint_positions = [0, 0, 0, 0, 0, 0]

        # 创建一个 QoS 配置文件，设置为 Transient Local
        qos_profile = QoSProfile(
            depth=10,  # 消息队列深度
            durability=DurabilityPolicy.TRANSIENT_LOCAL  # 设置为 Transient Local
        )
        
        # Define the joints of interest
        self.joints_of_interest = [
            'shoulder_pan_joint', 
            'shoulder_lift_joint',
            'elbow_joint', 
            'wrist_1_joint', 
            'wrist_2_joint', 
            'wrist_3_joint'
        ]
        self.toListen  = True
        self.subscription = self.create_subscription(
            JointState,
            '/joint_states',
            self.joint_state_callback,
            10  # QoS depth
        )
        self.subscription  # Prevent unused variable warning

        time.sleep(1)
        self.get_logger().info('wait 1 sec ...')

        self.publisher = self.create_publisher(Float64MultiArray, '/forward_position_controller/commands', qos_profile)

        self.timer = self.create_timer(0.004, self.publish_joint_positions)

        self.step = 0.03
        self.total_steps = 100
        self.current_step = 0

    def publish_joint_positions(self):
        # self.get_logger().info(f'{"wrist_2_joint"}: {self.positions["wrist_2_joint"]}')
        # self.get_logger().info(f'{"wrist_2_joint"}: {self.positions["right_wrist_2_joint"]}')
        if not self.toListen: # wait for the joint state to be received
            if self.current_step < self.total_steps:
                # Update the position of the second joint (elbow_joint)
                self.joint_positions[0] = self.positions["shoulder_pan_joint"]
                self.joint_positions[1] = self.positions["shoulder_lift_joint"]
                self.joint_positions[2] = self.positions["elbow_joint"]
                self.joint_positions[3] = self.positions["wrist_1_joint"]
                self.joint_positions[4] = (self.positions["wrist_2_joint"]-self.step * self.current_step ) 
                self.joint_positions[5] = self.positions["wrist_3_joint"]
                # Create and publish the message
                msg = Float64MultiArray()
                msg.data = [float(pos) for pos in self.joint_positions]
                # self.joint_positions

                self.publisher.publish(msg)
                # self.publisher.publish(msg)
                self.current_step += 1
                self.get_logger().info(f'Published joint positions: {type(msg.data[2]),msg.data[4]}')

                
            else:
                self.get_logger().info('Completed all steps.')
                self.timer.cancel()

    def joint_state_callback(self, msg: JointState):
            if(self.toListen):
                # Extract positions for the joints of interest
                for name, position in zip(msg.name, msg.position):
                    # self.get_logger().info(f'{name}')
                    if name in self.joints_of_interest:
                        self.positions[name] = position
                        self.get_logger().info(f'{name}: {self.positions[name]}')
                self.toListen = False


def main(args=None):
    rclpy.init(args=args)

    move_test = JointPositionPublisher()
    try:
        rclpy.spin(move_test)
    except RuntimeError as err:
        move_test.get_logger().error(str(err))
    except SystemExit:
        rclpy.logging.get_logger("move_test").info("Done")

    rclpy.shutdown()


if __name__ == "__main__":
    main()