sim2real！so-arm100 机械臂 Mujoco 仿真与实机控制

视频讲解：

sim2real！so-arm100 机械臂 Mujoco 仿真与实机控制

结合前面做好的so-arm100以及封装好的机械臂控制代码（封装的代码已经考虑了offset和实际的关节限位）

接下来我们用Mujoco仿真，将仿真中的关节位置实时发送给实机的机械臂进行控制。

仓库代码分成两个，mujoco部分作为控制客户端，so-arm100部分作为被控服务端。通信传输使用zmq的进程间通信。

Mujoco部分：GitHub - LitchiCheng/mujoco-learning

SO-ARM100部分：GitHub - LitchiCheng/SO-ARM100: Some Test code on SO-ARM100

需要注意：so-arm100如果参考过lerobot进行标定的话，其零位位置和mujoco中的home是不一致的，存在偏置，如下图

所以需要进行offset偏置，要不然实际控制会不一致。

首先在mujoco中的控制代码就比较简单，直接实时获取qpos，然后通过通信转发关节位置即可。

import mujoco
import time
import mujoco_viewer
import numpy as np
import math
import zmq
import json

# # ZMQ配置
# ZMQ_IP = "127.0.0.1"  # 绑定到所有可用网络接口
# ZMQ_PORT = "5555"
# context = zmq.Context()
# socket = context.socket(zmq.PUB)
# socket.bind(f"tcp://{ZMQ_IP}:{ZMQ_PORT}")

# 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.PUB)
socket.bind(ZMQ_IPC_PATH)  # 绑定到IPC路径

print(f"仿真端发布者启动，绑定到：{ZMQ_IPC_PATH}")

def send_data_to_robot(processed_q_deg):
    """将预处理后的关节角通过ZMQ发布"""
    # 数据封装为JSON
    data = {
        "joint_pos": processed_q_deg,  # 关节角（角度）
        "timestamp": time.time(),      # 时间戳（用于同步）
        "control_mode": "position"     # 控制模式（位置控制/速度控制）
    }
    # 转换为JSON字符串并发布
    json_data = json.dumps(data)
    socket.send_string(json_data)

class Test(mujoco_viewer.CustomViewer):
    def __init__(self, path):
        super().__init__(path, 1.5, azimuth=135, elevation=-30)
        self.path = path
    
    def runBefore(self):
        pass
    
    def runFunc(self):
        sim_joint_rad = self.data.qpos[:6].copy()
        sim_joint_deg = [math.degrees(q) for q in sim_joint_rad]
        send_data_to_robot(sim_joint_deg)
        time.sleep(0.01)  # 控制发送频率

try:
    test = Test("./model/trs_so_arm100/scene.xml")
    test.run_loop()
except KeyboardInterrupt:
    print("仿真程序被用户中断")
finally:
    # 关闭ZMQ连接
    socket.close()
    context.term()

在电机控制的程序中做好接收即可

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 fm

import 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(1000)  # 设置目标速度为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()

同时运行两边的程序，在mujoco中控制机械臂关节及使用拖动均可以看到实际机械臂也同样进行1:1运动。