[Question]: Not able to control robot via publihing on command/joint_group

[vignesh-anand]

Question

How do i precisely control joint positions via a publisher in ros2 humble

Robot Model

wx250s

Operating System

Ubuntu 22.04

ROS Version

ROS 2 Humble

Additional Info

vignesh@vignesh-OMEN:~$ ros2 topic list
/follower/commands/joint_group
/follower/commands/joint_single
/follower/commands/joint_trajectory
/follower/joint_states

I see these topics and I am trying to command joint positions at 30 Hz via a publisher that looks like this

self.arm_pub.publish_commands("arm", leader_joints[:-1])

class WidowXPublisher:
    def __init__(self, node: Node, topic_name: str, debug: bool = False) -> None:
        self.node = node
        self.debug = debug
        
        # Create QoS profile matching the subscriber
        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,
            durability=QoSDurabilityPolicy.VOLATILE,
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10
        )
        
        # Create the publisher using the provided node
        self.publisher = self.node.create_publisher(
            JointGroupCommand,
            topic_name,
            qos
        )
        
    def publish_commands(self, group_name: str, commands: np.ndarray) -> None:
        """
        Publish joint commands to the specified group
        
        Args:
            group_name: Name of the joint group to control
            commands: numpy array of joint commands
        """
        msg = JointGroupCommand()
        msg.name = group_name
        msg.cmd = commands.tolist()  # Convert numpy array to list for ROS message
        self.publisher.publish(msg)
        if self.debug:
            self.node.get_logger().info(f"Published {group_name} commands: {commands}")
        else:
            self.node.get_logger().debug(f"Published {group_name} commands: {commands}")

Either I get the very first pose being acted on and then everything else ignored or , nothing at all.

[lukeschmitt-tr]

Please enable debug logging on the xs_sdk of the follower and provide the results.

[vignesh-anand]

[xs_sdk-2] [INFO] [1734762026.178721740] [interbotix_xs_sdk.xs_sdk]: InterbotixRobotXS is up!
[xs_sdk-2] [DEBUG] [1734762026.185234] ID: 1, writing position command 2096.
[xs_sdk-2] [DEBUG] [1734762026.185297] ID: 2, writing position command 796.
[xs_sdk-2] [DEBUG] [1734762026.185310] ID: 4, writing position command 3115.
[xs_sdk-2] [DEBUG] [1734762026.185319] ID: 6, writing position command 2058.
[xs_sdk-2] [DEBUG] [1734762026.185327] ID: 7, writing position command 1730.
[xs_sdk-2] [DEBUG] [1734762026.185334] ID: 8, writing position command 2158.
[xs_sdk-2] [DEBUG] [1734762026.185570] ID: 1, writing position command 2096.
[xs_sdk-2] [DEBUG] [1734762026.185612] ID: 2, writing position command 796.
[xs_sdk-2] [DEBUG] [1734762026.185626] ID: 4, writing position command 3115.
[xs_sdk-2] [DEBUG] [1734762026.185635] ID: 6, writing position command 2058.
[xs_sdk-2] [DEBUG] [1734762026.185642] ID: 7, writing position command 1730.
[xs_sdk-2] [DEBUG] [1734762026.185649] ID: 8, writing position command 2158.
[xs_sdk-2] [DEBUG] [1734762026.185866] ID: 1, writing position command 2096.
[xs_sdk-2] [DEBUG] [1734762026.185898] ID: 2, writing position command 796.
[xs_sdk-2] [DEBUG] [1734762026.185908] ID: 4, writing position command 3115.
[xs_sdk-2] [DEBUG] [1734762026.185915] ID: 6, writing position command 2059.
[xs_sdk-2] [DEBUG] [1734762026.185923] ID: 7, writing position command 1730.
[xs_sdk-2] [DEBUG] [1734762026.185930] ID: 8, writing position command 2158.

Only this is output and nothing else.

jetson1@jetson4-desktop:~/marcel-brain/ros2_ws (manipulation)$ ros2 topic hz /follower/commands/joint_group 
average rate: 33.326
	min: 0.029s max: 0.031s std dev: 0.00034s window: 35
average rate: 33.338
	min: 0.028s max: 0.032s std dev: 0.00040s window: 69
average rate: 33.339
	min: 0.028s max: 0.032s std dev: 0.00035s window: 103

Commands are definitely being published

Here are my configs

port: /dev/ttyDXL

groups:
  arm:
    operating_mode: position
    profile_type: time
    profile_velocity: 2000
    profile_acceleration: 1000
    torque_enable: true

singles:
  gripper:
    operating_mode: position
    torque_enable: true

port: /dev/ttyDXL

joint_order: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate, gripper]
sleep_positions: [0, -1.80, 1.55, 0, 0.8, 0, 0]

joint_state_publisher:
  update_rate: 100
  publish_states: true
  topic_name: joint_states

groups:
  arm: [waist, shoulder, elbow, forearm_roll, wrist_angle, wrist_rotate]

grippers:
  gripper:
    horn_radius: 0.014
    arm_length: 0.024
    left_finger: left_finger
    right_finger: right_finger

shadows:
  shoulder:
    shadow_list: [shoulder_shadow]
    calibrate: true
  elbow:
    shadow_list: [elbow_shadow]
    calibrate: true

sisters:

motors:
  waist:
    ID: 1
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 0
    Velocity_Limit: 131
    Min_Position_Limit: 0
    Max_Position_Limit: 4095
    Secondary_ID: 255

  shoulder:
    ID: 2
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 0
    Velocity_Limit: 131
    Min_Position_Limit: 819
    Max_Position_Limit: 3345
    Secondary_ID: 255

  shoulder_shadow:
    ID: 3
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 1
    Velocity_Limit: 131
    Min_Position_Limit: 819
    Max_Position_Limit: 3345
    Secondary_ID: 2

  elbow:
    ID: 4
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 0
    Velocity_Limit: 131
    Min_Position_Limit: 648
    Max_Position_Limit: 3094
    Secondary_ID: 255

  elbow_shadow:
    ID: 5
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 1
    Velocity_Limit: 131
    Min_Position_Limit: 648
    Max_Position_Limit: 3094
    Secondary_ID: 4

  forearm_roll:
    ID: 6
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 0
    Velocity_Limit: 131
    Min_Position_Limit: 0
    Max_Position_Limit: 4095
    Secondary_ID: 255

  wrist_angle:
    ID: 7
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 1
    Velocity_Limit: 131
    Min_Position_Limit: 910
    Max_Position_Limit: 3447
    Secondary_ID: 255

  wrist_rotate:
    ID: 8
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 0
    Velocity_Limit: 131
    Min_Position_Limit: 0
    Max_Position_Limit: 4095
    Secondary_ID: 255

  gripper:
    ID: 9
    Baud_Rate: 3
    Return_Delay_Time: 0
    Drive_Mode: 0
    Velocity_Limit: 131

Echoing the /follower/commands/joint group shows that the right commands I want are definitely being published
using self.arm_pub.publish_commands("arm", leader_joints[:-1]) in my script

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from interbotix_xs_msgs.msg import JointGroupCommand
from manipulation.utils import WidowXSubscriber, WidowXPublisher
import numpy as np
from math import pi

class FollowerNode(Node):
    def __init__(self):
        super().__init__('follower_node')
        self.get_logger().debug('Initialized follower node')
        
        # Create subscribers for both leader and follower
        self.leader_sub = WidowXSubscriber(self, '/leader/joint_states', False)
        self.get_logger().debug('Created leader subscriber')
        
        self.follower_sub = WidowXSubscriber(self, '/follower/joint_states', False)
        self.get_logger().debug('Created follower subscriber')
        
        # Create publishers using WidowXPublisher classd
        self.arm_pub = WidowXPublisher(self, '/follower/commands/joint_group', False)
        self.get_logger().debug('Created arm publisher')
        
        self.gripper_pub = WidowXPublisher(self, '/follower/commands/joint_single', False)
        self.get_logger().debug('Created gripper publisher')
        
        # Create timer for control loop (10Hz)
        self.timer = self.create_timer(0.2, self.control_loop)
        self.get_logger().debug('Created control loop timer')
        
    def control_loop(self):
        # Get current joint positions
        leader_joints = self.leader_sub.get_joint_positions()
        follower_joints = self.follower_sub.get_joint_positions()
        
        # Check if either subscriber returned None
        if leader_joints is None or follower_joints is None:
            self.get_logger().warn('Joint positions not available yet')
            return
            
        # Directly command the follower to match leader positions
        # Handle arm joints (all except last)
        self.arm_pub.publish_commands("arm", leader_joints[:-1])
        
        # Handle gripper (last joint)
        self.gripper_pub.publish_commands("gripper", np.array([leader_joints[-1]]))

def main(args=None):
    rclpy.init(args=args)
    node = FollowerNode()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    print("STARTING FOLLOWER NODE")
    main()

[lukeschmitt-tr]

Please provide the RMW implementation you're using by running printenv RMW_IMPLEMENTATION. We've seen more reliable communication when using rmw_cyclonedds_cpp over the default rmw_fastrtps_cpp.

[vignesh-anand]

Hi Lukeschmitt,
So the arm works perfectly on my laptop, and my DDS works perfectly as well, I get the joint states and everything perfectly and all the echo/hz tests I have done work perfectly. It is when I run the driver on the Jetson, that none of the services or subscribers work in the SDK. The callbacks never get triggered. The joint state publisher works great