Best approach for 10Hz closed-loop control

ROS
1

I am attempting to control a UR10e arm at 10Hz with a controller I am designing. The control can be in joint-space or cartesian, it does not matter. For now I have been sending commands via the /scaled_joint_trajectory_controller/joint_trajectory topic. The topic of course expects a trajectory, for which I provide a single point trajectory (of a nearby point, it is never far) with a duration of 0.1s. This method works great in simulation (using the URSim and Isaac Sim), but on the real robot, the robot jitters violently during the whole movement, even thoough is does complete the movement as expected. Is there a better way to control the robot at this frequency? I can’t seem to find what the best practices are, and everyone I know controls the UR robots via open-loop pre-programmed trajectories that are 5s-60s long.

Thank you so much!

Best,
Federico

1 Like
2

This is interesting :slight_smile:

If I understand things correctly, you want to do dynamic trajectory replacement. You want the robot to be at a specific point in 10 ms. Is there a reason for the relatively low frequency? Are the targets definitively achievable?

And more importantly: Which ROS version are you using? If you have some example code to share that generates these commands, this could be debugged a bit better.

3

Hi,

Thank you so much for your help. I just want to control the robot position at 10Hz, not really dynamic trajectory replacement but I guess that is what I have been doing. I am developing a classical controller (takes in state and observations, outputs next action) that operates at 10Hz. For now I am sending a joint pose command on the /scaled_joint_trajectory_controller/joint_trajectory topic that is a single point trajectory. The single point of the trajectory is the action outputted by my controller, and it is certainly achievable and always very close to the current pose of the robot. I am unsure if there is a more obvious or logical way of controlling the robot at 10Hz.

I am using ROS2 Foxy and Humble, seems to work the same in both. Below is the basic code I call to send a command to the URSim and the real robot. My controller calls the send_action command at approximately 10Hz:

from rclpy.node import Node
from rclpy.time import Duration

from std_msgs.msg import Header
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint


class JointsPublisher(Node):
    def __init__(self):
        super().__init__("joints_publisher")

        self.publisher = self.create_publisher(
            JointTrajectory,
            "/scaled_joint_trajectory_controller/joint_trajectory",
            10,
        )

        self.ordered_link_names = [
            "shoulder_pan_joint",
            "shoulder_lift_joint",
            "elbow_joint",
            "wrist_1_joint",
            "wrist_2_joint",
            "wrist_3_joint",
        ]

    def send_action(self, action):
        header = Header()
        header.stamp = self.get_clock().now().to_msg()
        header.frame_id = ""

        joint_command = JointTrajectory()
        joint_command.header = header
        joint_command.joint_names = self.ordered_link_names
        joint_command.points = [JointTrajectoryPoint()]
        joint_command.points[0].positions = action
        joint_command.points[0].time_from_start = Duration(
            seconds=0.1
        ).to_msg()

        self.publisher.publish(joint_command)
4

The question is: What do you expect the robot to do in the meantime? Should the robot come to a stand-still configuration at the end of your 0.1 sec interval? Do you expect the robot to have reached the pose exactly after the 0.1 sec interval?

Since you are currently giving positions only, you will have infinite accelerations at your waypoints. You could try specifying a velocity vector for your waypoint (could also be a 0 vector) which will then result in a smoother interpolation. See Trajectory Representation — ROS2_Control: Rolling Aug 2024 documentation for details on different trajectory interpolations.

One option could also be to write your own ros2_control controller that implements the behavior that you actually want to achieve. From what I understand you want something like the robot following a target pose that is updated with 10 Hz. FZI has implemented something like that in GitHub - fzi-forschungszentrum-informatik/cartesian_controllers: A set of Cartesian controllers for the ROS1 and ROS2-control framework.. It is Cartesian-based, though.

5

Thank you, I will take a look at those resources!! I have tried specifying a velocity at each waypoint and it hasn’t worked but I will take a deeper look. I’ll let you know what solution works or if I have further questions!

6

Hi, I have found some success by passing only velocity values using forward_velocity_controller. The acceleration is extremely fast and causes some jerkiness but I think I can fix that. Unsure if this is the standard approach for high-rate control but definitely velocity control seems to be less jerky than position control. Thanks for all the help!