当前位置：首页 > news >正文

【代码讲解】SO-ARM100 双场景演示：手柄驱动 Mujoco 仿真 + 实机控制

news 2025/9/17 6:26:37

视频讲解：

今天介绍下使用使用北通手柄通过控制 Mujoco 中的 SO-ARM100 机械臂，然后将关节数据通过 zmq 通信转发控制实际机械臂。

本期中会涉及如下点，需要注意：

末端姿态设置为固定姿态，但控制量可以进行修改，目前手柄没有想好用哪些键映射比较好
机械臂会严格按照 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()