視頻講解：

今天介紹下使用使用北通手柄通過控制 Mujoco 中的 SO-ARM100 機械臂，然后將關節數據通過 zmq 通信轉發控制實際機械臂。

本期中會涉及如下點，需要注意：

1. 末端姿態設置為固定姿態，但控制量可以進行修改，目前手柄沒有想好用哪些鍵映射比較好
2. 機械臂會嚴格按照 Mujoco 的 qpos 進行執行，所以最好做限位等關節處理做保護，這部分我在 SO-ARM100 的被控代碼中增加了，大家自己寫這部分的時候需要注意

完整代碼倉庫：

https://github.com/LitchiCheng/mujoco-learning

https://github.com/LitchiCheng/SO-ARM100

Joystick 接收 和 Mujoco 仿真轉發端

import mujoco
import numpy as np
import mujoco_viewer
import casadi_ik
import time
import pygame
import os
import math
from so100_real_control import ZMQCommunicator# 設置環境變量以確保正確訪問游戲桿設備
os.environ["SDL_JOYSTICK_DEVICE"] = "/dev/input/js0"
SCENE_XML_PATH = 'model/trs_so_arm100/scene.xml'
ARM_XML_PATH = 'model/trs_so_arm100/so_arm100.xml'class XboxController:"""Xbox手柄控制器類，負責處理所有手柄輸入"""def __init__(self):# 初始化位置參數self.x = 0.0self.y = -0.3self.z = 0.15# 位置限制self.x_min, self.x_max = -0.3, 0.3self.y_min, self.y_max = -0.4, 0.0self.z_min, self.z_max = 0.05, 0.3# 控制靈敏度self.sensitivity = 0.005# 死區閾值（過濾搖桿中心微小漂移）self.deadzone = 0.1self.dof5_target = Noneself.button1_pressed = Falseself.button2_pressed = Falseself.controller = self.init_controller()def init_controller(self):"""初始化Xbox手柄（通過pygame訪問js設備）"""pygame.init()pygame.joystick.init()if pygame.joystick.get_count() == 0:print("joystick not found")return None# 對應/dev/input/js0joystick = pygame.joystick.Joystick(0)joystick.init()print(f"joystick found: {joystick.get_name()}")print(f"button count: {joystick.get_numbuttons()}")return joystickdef is_connected(self):"""檢查手柄是否連接"""return self.controller is not Nonedef handle_input(self, arm):"""處理手柄輸入并更新控制參數"""if not self.is_connected():return# 處理pygame事件（必須調用，否則無法更新軸值和按鈕狀態）pygame.event.pump()# 讀取左搖桿X軸（0號軸）x_axis = self.controller.get_axis(1)# 應用死區過濾if abs(x_axis) < self.deadzone:x_axis = 0.0# 讀取左搖桿Y軸（1號軸）y_axis = self.controller.get_axis(0)if abs(y_axis) < self.deadzone:y_axis = 0.0# 讀取右搖桿Y軸（4號軸）z_axis = -self.controller.get_axis(4)if abs(z_axis) < self.deadzone:z_axis = 0.0# 根據軸值更新位置self.x = np.clip(self.x + x_axis * self.sensitivity, self.x_min, self.x_max)self.y = np.clip(self.y + y_axis * self.sensitivity, self.y_min, self.y_max)self.z = np.clip(self.z + z_axis * self.sensitivity, self.z_min, self.z_max)# 檢測按鈕1（假設是0號按鈕，通常是A鍵）button1 = self.controller.get_button(0)if button1 and not self.button1_pressed:print("Button1 pressed - 設置dof[5]為滿量程")self.dof5_target = arm.model.upperPositionLimit[5]self.button1_pressed = Trueelif not button1:self.button1_pressed = False# 檢測按鈕2（假設是1號按鈕，通常是B鍵）button2 = self.controller.get_button(1)if button2 and not self.button2_pressed:print("Button2 pressed - 設置dof[5]為最小值")self.dof5_target = arm.model.lowerPositionLimit[5]self.button2_pressed = Trueelif not button2:self.button2_pressed = Falsedef get_position_target(self):return self.x, self.y, self.zdef get_dof5_target(self):return self.dof5_targetdef cleanup(self):pygame.quit()class RobotController(mujoco_viewer.CustomViewer):    def __init__(self, scene_path, arm_path, controller):super().__init__(scene_path, distance=1.5, azimuth=135, elevation=-30)self.scene_path = scene_pathself.arm_path = arm_pathself.controller = controllerself.communicator = ZMQCommunicator()self.arm = casadi_ik.Kinematics("Wrist_Roll")self.arm.buildFromMJCF(arm_path)self.last_dof = Nonedef runBefore(self):passdef runFunc(self):"""主循環中執行的函數"""self.controller.handle_input(self.arm)x, y, z = self.controller.get_position_target()print(f"當前位置: x={x:.3f}, y={y:.3f}, z={z:.3f}")# 構建變換矩陣（保持末端執行器姿態不變）tf = self.build_transform_simple(x, y, z, np.pi / 4, 0, 0)self.dof, info = self.arm.ik(tf, current_arm_motor_q=self.last_dof)self.last_dof = self.dofdof5_target = self.controller.get_dof5_target()if dof5_target is not None:self.dof[5] = dof5_targetself.data.qpos[:6] = self.dof[:6]sim_joint_rad = self.data.qpos[:6].copy()sim_joint_deg = [math.degrees(q) for q in sim_joint_rad]self.communicator.send_data(sim_joint_deg)mujoco.mj_step(self.model, self.data)time.sleep(0.01)def build_transform_simple(self, x, y, z, roll, pitch, yaw):cr, sr = np.cos(roll), np.sin(roll)cp, sp = np.cos(pitch), np.sin(pitch)cy, sy = np.cos(yaw), np.sin(yaw)return np.array([[cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr, x],[sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr, y],[-sp,   cp*sr,            cp*cr,            z],[0,     0,                0,                1]])if __name__ == "__main__":controller = XboxController()if not controller.is_connected():print("joystick not connected")exit(1)try:robot = RobotController(SCENE_XML_PATH, ARM_XML_PATH, controller)robot.run_loop()finally:controller.cleanup()

SO-ARM100 實機控制端

import zmq
import json
import sys
import os
project_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.insert(0, project_root)
from hardware import FeetechMotor as fmimport time# 初始化機械臂
motor = fm.FeetechMotor(1, "/dev/ttyACM0")
motor.connect()# # ZMQ配置
# ZMQ_IP = "127.0.0.1"
# ZMQ_PORT = "5555"
# context = zmq.Context()
# socket = context.socket(zmq.SUB)
# socket.connect(f"tcp://{ZMQ_IP}:{ZMQ_PORT}")
# # 訂閱所有消息（空字符串表示訂閱所有主題）
# socket.setsockopt_string(zmq.SUBSCRIBE, "")# print(f"等待仿真端發布數據... 連接到：tcp://{ZMQ_IP}:{ZMQ_PORT}")# ZMQ IPC配置 - 使用進程間通信協議
ZMQ_IPC_PATH = "ipc:///tmp/robot_arm_comm.ipc"  # 與發布者相同的IPC路徑
context = zmq.Context()
socket = context.socket(zmq.SUB)
socket.connect(ZMQ_IPC_PATH)  # 連接到IPC路徑
socket.setsockopt_string(zmq.SUBSCRIBE, "")  # 訂閱所有消息print(f"控制端訂閱者啟動，連接到：{ZMQ_IPC_PATH}")# 接收并解析數據
try:while True:# 接收數據data = socket.recv_string().strip()if not data:continue# 解析JSON數據json_data = json.loads(data)joint_pos_deg = json_data["joint_pos"]control_mode = json_data["control_mode"]# 發送控制指令給實際機械臂if control_mode == "position":for i in range(1, 7):motor.setMotorId(i)motor.setSpeed(2000)  # 設置目標速度為1000步/秒motor.printFlag(False)  # 打開打印if i == 1 or i == 2:motor.setPID(16, 16, 0)else:   motor.setPID(32, 32, 0)  # 設置PID參數motor.setPosition(joint_pos_deg[i - 1])# print(f"Motor ID {i} set to position {joint_pos_deg[i - 1]}°")# 短暫延遲，避免CPU占用過高# time.sleep(0.01)except KeyboardInterrupt:print("程序被用戶中斷")
except Exception as e:print(f"接收/控制錯誤：{e}")
finally:# 關閉連接與機械臂socket.close()context.term()motor.disconnect()