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