clipped/overshoot in velocity profiles when sending trajectories with Scaled_joint_trajectory_controller

[Yazan-Soliman]

Affected ROS2 Driver version(s)

ros-humble-ur/jammy,now 2.2.16-5jammy.20241029.212959 amd64

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 E-series robot

Robot SW / URSim version(s)

5.16.0

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

UR10e experiences clipping motion when controlled with scaled_joint_trajectory_controller.

Issue details

I am conducting research for my master's thesis with a UR10e robot, where I need to send trajectories over the scaled_JTC.
on the Teach Pendant the safety limits are set to least restrictive settings.
Trajectories are computed with Ruckig trajectory planner from point A to point B and within the robot hardware limitations i.e velocities and accelerations.

our initial thoughts that the tracking error is experiencing some lag so we plotted the error from the controller state as below.

controller Feedback

as seen the controller state error does not indicate any unexpected deviations. To address the issue, we reduced acceleration limits to 8. rad/s² for all joints which should be within the capabilities of a robot like UR10e. However the behavior remained unchanged.

another example is shown below.

ps: what was also unclear why does controller_state/error/positions shows a velocity error as seen from the first plot ?

below are relevant controller parameters.

scaled_joint_trajectory_controller:
  ros__parameters:
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    allow_nonzero_velocity_at_trajectory_end: false
    constraints:
      stopped_velocity_tolerance: 0.2
      goal_time: 0.0
      shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
      shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
      elbow_joint: { trajectory: 0.2, goal: 0.1 }
      wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
      wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
      wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
    speed_scaling_interface_name: speed_scaling/speed_scaling_factor

Workaround Suggestion

The issue could be mitigated when sending trajectories with a longer duration.

Relevant log output

No response

Accept Public visibility

• I agree to make this context public

[urfeex]

Hi thank you for your report. I am not sure whether I understand things correctly. What exactly is wrong in the behavior that you see from your perspective?

Also, in order to reproduce things it would be helpful if you could provide the full trajectory as it is being sent to the ROS controller's action.

[Yazan-Soliman]

@urfeex Thank you for following up.

This is a plot of joint velocities.
The wrong behavior is within the blue rectangle, where the scaled joint trajectory controller was not able to Follow the commanded trajectory, sorry for my bad mouse drawing.

[urfeex]

Ok, but that alone doesn't necessarily indicate that the controller is doing something wrong. Especially as the state error remains more or less constant over the whole trajectory. To me it looks as if the velocity "dip" is part of the trajectory being sent to the controller. Hence, providing this would be very helpful.

[Yazan-Soliman]

Sorry from the late follow up, executing the code below might trigger the behavior, as a similar structure is implemented that produced the above behavior. But in my software it is integrated within a more complex processes.

#!/usr/bin/env python3

import numpy as np
import roboticstoolbox as rtb
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from builtin_interfaces.msg import Duration
from trajectory_msgs.msg import JointTrajectoryPoint
from control_msgs.action import FollowJointTrajectory
from sensor_msgs.msg import JointState


class ToReproduce(Node):
    def __init__(self):
        super().__init__('to_reproduce')

        # Declare and initialize parameters with descriptions
        self.declare_parameter(
            "controller_name", "scaled_joint_trajectory_controller",
            descriptor=rclpy.parameter.ParameterDescriptor(
                description="Name of the joint trajectory controller."
            )
        )
        self.declare_parameter(
            "joints", ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
                       "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"],
            descriptor=rclpy.parameter.ParameterDescriptor(
                description="List of joint names in the robot."
            )
        )

        controller_name = self.get_parameter("controller_name").get_parameter_value().string_value
        self.joints = self.get_parameter("joints").get_parameter_value().string_array_value
        controller_name += "/follow_joint_trajectory"

        # Initialize action client for trajectory control
        self.action_client = ActionClient(self, FollowJointTrajectory, controller_name)
        self.get_logger().info(f"Waiting for action server on {controller_name}")
        self.action_client.wait_for_server()

        # Subscribe to joint states
        self.joint_states_subscriber = self.create_subscription(
            JointState,
            "/joint_states",
            self.joint_states_callback,
            10
        )

        self.current_position = []
        self.joint_states = None

    def joint_states_callback(self, msg):
        """Callback to receive and store the current joint states."""
        self.joint_states = msg

    def wait_for_joint_states(self):
        """Wait until fresh joint states are available."""
        self.joint_states = None  # Reset joint states to ensure new data is received
        while rclpy.ok() and self.joint_states is None:
            rclpy.spin_once(self, timeout_sec=0.1)

    def initialize_current_position(self):
        """Initialize the current joint position if it hasn't been initialized yet."""
        if self.joint_states is not None:
            self.current_position = list(self.joint_states.position)
            self.current_position = [self.current_position[-1]] + self.current_position[:-1]  # Correct order
        else:
            self.get_logger().error("Joint states are not available to initialize current position.")

    def send_trajectory(self, target_q, duration):
        """Send a trajectory to the robot."""
        trajectory_time = np.arange(0, duration, 0.002)  # 500 Hz
        traj = rtb.jtraj(self.current_position, target_q, trajectory_time)

        goal_msg = FollowJointTrajectory.Goal()
        goal_msg.trajectory.header.stamp = self.get_clock().now().to_msg()
        goal_msg.trajectory.joint_names = self.joints
        goal_msg.goal_time_tolerance = Duration(sec=0, nanosec=50000000)

        for i in range(len(traj.q)):
            point = JointTrajectoryPoint()
            point.positions = traj.q[i].tolist()
            point.velocities = traj.qd[i].tolist()
            point.accelerations = traj.qdd[i].tolist()
            sec = int(traj.t[i])
            nanosec = int((traj.t[i] % 1) * 1e9)
            point.time_from_start = Duration(sec=sec, nanosec=nanosec)
            goal_msg.trajectory.points.append(point)

        self.get_logger().info(f"Sending trajectory with {len(goal_msg.trajectory.points)} points.")

        self.action_client.wait_for_server()
        goal_future = self.action_client.send_goal_async(goal_msg)
        rclpy.spin_until_future_complete(self, goal_future)

        goal_handle = goal_future.result()
        if not goal_handle.accepted:
            self.get_logger().error("Trajectory goal was rejected.")
            return

        result_future = goal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, result_future)

        result = result_future.result()
        if result.error_code == FollowJointTrajectory.Result.SUCCESSFUL:
            self.get_logger().info("Trajectory execution succeeded.")
        else:
            self.get_logger().error(f"Trajectory execution failed with error: {result.error_code}")


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

    try:
        node.wait_for_joint_states()
        node.initialize_current_position()

        if not node.current_position:
            node.get_logger().error("Current joint position could not be initialized. Exiting.")
            return

        # Define a target joint configuration
        target_q = [-1.5, 1.57, -1.8, 1.0, 0.6, 0.2]  # Replace with your target configuration

        # Send trajectory to the target configuration
        node.send_trajectory(target_q, duration=3)

    except KeyboardInterrupt:
        node.get_logger().info("Node interrupted by keyboard.")
    except Exception as e:
        node.get_logger().error(f"Unexpected error: {e}")
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

[urfeex]

Thank you for the example. However, I cannot really match your code to the plots above. Your example contains only a target joint configuration, which is hardly a trajectory. At least one start point would be good to have reproducible results that we can compare things on.

For instance, starting from an arbitrary point I get the following motion where I cannot really see a big issue with: