interbotix_ros_manipulators
interbotix_ros_manipulators copied to clipboard
Interbotix
[Question]: Not able to control robot via publihing on command/joint_group
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.
[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()
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.
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