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

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

news 2025/8/25 8:02:52

视频讲解：

结合前面做好的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发布"""# 数据封装为JSONdata = {"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 = pathdef runBefore(self):passdef 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 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(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运动。