本文将使用isaac sim 导入SO101 机械臂并使用ROS2进行控制和获取机械臂状态

在github上下载SO101机械臂，里面有其urdf文件

导入SO101机械臂

导入网格

file -> import

随后点yes

机械臂就load 进来了

打开physical inspector 让其动起来




拖动蓝色bar就可以让机械臂动起来了

用ROS2控制机械臂

选择机械臂



在右边的就能看到其graph了



把articulation Controller加进来

连接节点的各topic

在终端输入

ros2 topic list

能看到两个话题

ros2 topic echo /joint_states

能看到发布的消息，在isaacsim里拖动机械臂，可以看到joint states 消息在变化

同样也可以自己写一个publish节点，让isaac sim里面的机械臂动起来

用千问写了个publisher的代码：
提示词如下：

已知节点名称为/joint_command
节点接口的定义如下
std_msgs/Header header
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id

string[] name
float64[] position
float64[] velocity
float64[] effort



要求：
分别用键盘上的数字控制各个joint：
1： shoulder_pan
2: shoulder_lift
3: elbow_flex
4: wrist_flex
5: wrist_roll
6: gripper

分别用键盘上的字母控制关节位置：
a: 减小
d: 增大

按键盘上的q键退出程序

这段代码使用控制Lerobot SO101机械臂，写代码时需要考虑这一块

以此写一段ROS2 的python代码，代码需要在python 3.11 上运行（需要在代码里明确，因为ubuntu22.04默认使用3.10）

#!/usr/bin/python3.11
# -*- coding: utf-8 -*-

"""
LeRobot SO-101 Keyboard Joint Controller for ROS 2
明确要求：需要在 Python 3.11 环境下运行
"""

import sys
import termios
import tty
import select
import threading

# 强制检查 Python 版本
if sys.version_info < (3, 10):
    print(f"\033[91m[ERROR] This script requires Python 3.11+, but current version is {sys.version_info.major}.{sys.version_info.minor}\033[0m")
    print("Please run with: python3.11 so101_keyboard_control.py")
    sys.exit(1)

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState

class SO101KeyboardControl(Node):
    def __init__(self):
        super().__init__('so101_keyboard_control')
        
        # 1. 创建发布者，话题为 /joint_command
        self.publisher = self.create_publisher(JointState, '/joint_command', 10)
        
        # 2. 关节名称映射（数字键 -> 关节名）
        self.key_to_joint = {
            '1': 'shoulder_pan',
            '2': 'shoulder_lift',
            '3': 'elbow_flex',
            '4': 'wrist_flex',
            '5': 'wrist_roll',
            '6': 'gripper'
        }
        
        # 3. SO-101 关节限位保护（单位：弧度 rad）
        # 根据 SO-101 的物理结构，舵机通常在 -180° 到 180° 之间，这里设置安全范围
        self.joint_limits = {
            'shoulder_pan': (-3.14, 3.14),
            'shoulder_lift': (-2.0, 2.0),
            'elbow_flex': (-2.0, 2.0),
            'wrist_flex': (-1.57, 1.57),
            'wrist_roll': (-3.14, 3.14),
            'gripper': (0.0, 1.0)
        }
        
        # 4. 初始化当前关节位置（单位：弧度 rad）
        self.current_positions = {name: 0.0 for name in self.key_to_joint.values()}
        
        # 步长设置（每次按键增加/减小的弧度值，约 2.8度）
        self.step_size = 0.05 
        
        # 当前选中的关节
        self.active_joint = None
        self.running = True
        
        # 5. 启动键盘监听线程
        self.keyboard_thread = threading.Thread(target=self.keyboard_listener)
        self.keyboard_thread.daemon = True
        self.keyboard_thread.start()
        
        # 6. 启动定时器，以 50Hz 的频率持续发布当前关节状态
        self.timer = self.create_timer(0.02, self.publish_joint_state)
        
        self.get_logger().info("\033[92mSO-101 Keyboard Controller Started.\033[0m")
        self.get_logger().info("Press 1-6 to select joint, 'a' to decrease, 'd' to increase, 'q' to quit.")

    def keyboard_listener(self):
        """非阻塞键盘监听线程"""
        old_settings = termios.tcgetattr(sys.stdin)
        try:
            tty.setcbreak(sys.stdin.fileno())
            while self.running:
                ready, _, _ = select.select([sys.stdin], [], [], 0.1)
                if ready:
                    key = sys.stdin.read(1)
                    self.process_key(key)
        finally:
            termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)

    def process_key(self, key):
        """处理按键逻辑"""
        if key == 'q':
            self.get_logger().info("Exiting...")
            self.running = False
            rclpy.shutdown()
            sys.exit(0)
        elif key in self.key_to_joint:
            self.active_joint = self.key_to_joint[key]
            self.get_logger().info(f"\033[93mActive Joint: {self.active_joint}\033[0m")
        elif key == 'a' and self.active_joint:
            new_pos = self.current_positions[self.active_joint] - self.step_size
            min_lim, max_lim = self.joint_limits[self.active_joint]
            self.current_positions[self.active_joint] = max(min_lim, min(max_lim, new_pos))
        elif key == 'd' and self.active_joint:
            new_pos = self.current_positions[self.active_joint] + self.step_size
            min_lim, max_lim = self.joint_limits[self.active_joint]
            self.current_positions[self.active_joint] = max(min_lim, min(max_lim, new_pos))

    def publish_joint_state(self):
        """构建并发布 JointState 消息"""
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = "base_link"
        
        joint_names = list(self.key_to_joint.values())
        msg.name = joint_names
        msg.position = [self.current_positions[name] for name in joint_names]
        msg.velocity = []
        msg.effort = []
        
        self.publisher.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    node = SO101KeyboardControl()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()