MuJoCo学习（二）——RML63II-6F仿真、正逆解控制器、相机渲染
概述
初步实现了 RML63II-6F 六轴机械臂在 MuJoCo 中的仿真控制平台。核心功能包括：
基于 MDH 参数的数值逆解器：采用自适应阻尼最小二乘法 + 四元数姿态误差，解决多解与奇异问题。
相机渲染管线：通过预分配缓冲区与固定帧率调度，实现 15FPS 稳定低延迟视觉反馈。
UDP 实时通信架构：GUI 显示与物理控制完全解耦，支持外部算法接入。
开发环境
WIN11 python3.10.18
系统结构
通信
┌─────────────────┐ UDP 5006 ┌──────────────────┐
│ External Algo │ ◄──────────────── │ UdpRecvThread │
│ (ROS/Python) │ │ (Daemon Thread) │
└─────────────────┘ └────────┬─────────┘
│ q_deg
┌─────────────────┐ UDP 5005 ┌────────▼─────────┐
│ MuJoCo Viewer │ ◄──────────────── │ MujocoGuiApp │
│ (Visual Only) │ │ ├─ MDHIKSolver │
└─────────────────┘ │ ├─ Renderer │
│ ├─ ArUco Det. │
│ └─ Tkinter GUI │
└──────────────────┘
输入/显示分离：关节实际值用 Label 只读显示，指令值用独立 Entry，彻底杜绝焦点抢占导致的数值跳动。
线程安全：UDP 接收线程通过 threading.Lock 保护共享数据，主循环以 15Hz 轮询消费。
退出：stop_event 信号量确保 UDP 线程、渲染器、Tk 窗口按序释放资源。
相机渲染
1 零拷贝缓冲设计
为避免每帧 np.empty() 带来的 GC 抖动，预分配 BGR 和 Gray 缓冲区：
self._bgr_buf = np.empty((480, 640, 3), dtype=np.uint8)
self._gray_buf = np.empty((480, 640), dtype=np.uint8)
2 固定 15FPS 调度
采用 root.after() 非阻塞定时机制，确保 GUI 响应性与物理步进稳定性：
elapsed_ms = (time.monotonic() - loop_start) * 1000.0
next_delay = max(1, self._frame_interval_ms - int(elapsed_ms))
self.root.after(next_delay, self._main_loop)
ik求解
睿尔曼机械臂采用 Modified DH (MDH) 参数建立正运动学模型
dh参数
self.mdh_params = [
[0.0, 0.0, 0.177, 0.0],
[-0.086, -math.pi / 2, 0.0, -math.pi / 2],
[0.380, 0.0, 0.0, math.pi / 2],
[0.069, math.pi / 2, 0.405, 0.0],
[0.0, -math.pi / 2, 0.0, math.pi],
[0.0, -math.pi / 2, 0.1436, math.pi]
]
代码
rml63ii-with435.xml
<mujoco model="RML63II-6F">
<visual>
<global offwidth="1920" offheight="1080"/>
</visual>
<option iterations="50" timestep="0.001" solver="PGS" gravity="0 0 -9.81" />
<compiler angle="radian" meshdir="meshes" eulerseq="zyx" autolimits="true" />
<default>
<joint limited="true" damping="0.01" armature="0.01" frictionloss="0.01" />
<geom condim="4" contype="1" conaffinity="15" friction="0.9 0.2 0.2" solref="0.001 2" />
<motor ctrllimited="true" />
<equality solref="0.001 2" />
<default class="visualgeom">
<geom material="visualgeom" condim="1" contype="0" conaffinity="0" />
</default>
</default>
<asset>
<mesh name="base_link" content_type="model/stl" file="base_link.STL" />
<mesh name="link_1" content_type="model/stl" file="link_1.STL" />
<mesh name="link_2" content_type="model/stl" file="link_2.STL" />
<mesh name="link_3" content_type="model/stl" file="link_3.STL" />
<mesh name="link_4" content_type="model/stl" file="link_4.STL" />
<mesh name="link_5" content_type="model/stl" file="link_5.STL" />
<mesh name="link_6" content_type="model/stl" file="link_6.STL" />
<texture name="texplane" type="2d" builtin="checker" rgb1=".0 .0 .0" rgb2=".8 .8 .8" width="100" height="100" />
<material name="matplane" reflectance="0." texture="texplane" texrepeat="1 1" texuniform="true" />
<material name="visualgeom" rgba="0.5 0.9 0.2 1" />
<!-- ArUco 标定板纹理与材质 -->
<texture name="aruco_tex" type="2d" file="aruco_board.png" vflip="true" />
<material name="aruco_mat" texture="aruco_tex" specular="0" shininess="0" />
</asset>
<worldbody>
<light directional="true" diffuse="0.4 0.4 0.4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false" />
<light directional="true" diffuse="0.6 0.6 0.6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1" />
<!-- geom name="ground" type="plane" pos="0 0 0" size="100 100 0.001" quat="1 0 0 0" material="matplane" condim="3" conaffinity="15" /-->
<geom name="floor" type="plane" size="2 2 0.1" rgba="0.2 0.3 0.8 1" contype="1" conaffinity="1" group="0"/>
<camera name="fixed" pos="0 -3.0 0.5164595954617898" xyaxes="1 0 0 0 0 1" />
<camera name="track" mode="trackcom" pos="0 -3.0 0.5164595954617898" xyaxes="1 0 0 0 0 1" />
<!-- ArUco 标定板 (放置在机械臂工作空间内) -->
<body name="calib_board" pos="0.5 0 0.4" euler="0 1.57 0">
<geom name="board_geom" type="box" size="0.06 0.075 0.001"
material="aruco_mat" contype="0" conaffinity="0" group="1" />
</body>
<body name="base_link" pos="0 0 0.0164595954617898">
<site name="imu" size="0.01" pos="0 0 0" />
<geom type="mesh" rgba="1 1 1 1" mesh="base_link" contype="0" conaffinity="0" density="0" group="1" class="visualgeom" />
<geom type="mesh" rgba="1 1 1 1" mesh="base_link" contype="1" conaffinity="2" />
<body name="link_1" pos="0 0 0.177">
<inertial pos="-0.068442 -0.023913 -0.006938" quat="-0.301712 0.808775 -0.161786 0.478204" mass="1.837" diaginertia="0.00431382 0.00419084 0.00169105" />
<joint name="joint_1" pos="0 0 0" axis="0 0 1" range="-3.106 3.106" actuatorfrcrange="-60 60" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_1"
contype="0" conaffinity="0" density="0" group="1" class="visualgeom" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_1"/>
<body name="link_2" pos="-0.086 0 0" quat="-0.499604 0.500398 0.499602 0.500396">
<inertial pos="0.1667 -2e-06 -0.09259" quat="3.22488e-05 0.705169 6.05123e-05 0.70904" mass="2.006" diaginertia="0.0309728 0.0306185 0.00146463" />
<joint name="joint_2" pos="0 0 0" axis="0 0 1" range="-3.106 3.106" actuatorfrcrange="-60 60" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_2" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_2" />
<body name="link_3" pos="0.38 0 0" quat="0.707105 0 0 0.707108">
<inertial pos="0.033399 -0.029498 -0.017697" quat="0.80431 0.471123 0.298554 0.204926" mass="1.961" diaginertia="0.00616216 0.00596452 0.00164315" />
<joint name="joint_3" pos="0 0 0" axis="0 0 1" range="-3.106 2.5307" actuatorfrcrange="-30 30" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_3" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_3" />
<body name="link_4" pos="0.069 -0.405 0" quat="0.707105 0.707108 0 0">
<inertial pos="0 -0.035177 -0.1844" quat="0.700005 0.0992675 -0.0993032 0.700199" mass="1.201" diaginertia="0.0107104 0.0106971 0.000608018" />
<joint name="joint_4" pos="0 0 0" axis="0 0 1" range="-3.106 3.106" actuatorfrcrange="-30 30" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_4" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_4" />
<body name="link_5" quat="-2.59734e-06 -2.59735e-06 0.707108 0.707105">
<inertial pos="-3.1e-05 0.030146 -0.012341" quat="0.556806 0.830198 -0.0231731 0.0141771" mass="1.026" diaginertia="0.001644 0.00160941 0.000516024" />
<joint name="joint_5" pos="0 0 0" axis="0 0 1" range="-3.106 3.106" actuatorfrcrange="-10 10" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_5" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_5" />
<body name="link_6" pos="0 0.1436 0" quat="-2.59734e-06 -2.59735e-06 0.707108 0.707105">
<inertial pos="-0.000481 0.000365 -0.010605" quat="0.61293 0.348634 -0.614807 0.353248" mass="0.248" diaginertia="6.4289e-05 4.32907e-05 3.61763e-05" />
<joint name="joint_6" pos="0 0 0" axis="0 0 1" range="-6.28 6.28" actuatorfrcrange="-10 10" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_6" contype="1" conaffinity="0" density="0" group="1" class="visualgeom" />
<geom type="mesh" rgba="1 1 1 1" mesh="link_6" />
<!-- 手眼相机 -->
<camera name="hand_camera" pos="0.04 0 0.03 " xyaxes="1 0 0 0 -1 0" fovy="58" />
<!-- 红色圆柱体标注相机位姿 -->
<geom name="cam_marker"
type="cylinder"
size="0.008 0.015"
pos="0.04 0 0.03 "
quat="1 0 0 0"
rgba="1 0 0 0.9"
contype="0"
conaffinity="0"
group="1" />
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<position name="joint_1" joint="joint_1" ctrllimited="true" ctrlrange="-3.106 3.106" kp="200" kv="20" />
<position name="joint_2" joint="joint_2" ctrllimited="true" ctrlrange="-3.106 3.106" kp="200" kv="20" />
<position name="joint_3" joint="joint_3" ctrllimited="true" ctrlrange="-3.106 2.5307" kp="150" kv="15" />
<position name="joint_4" joint="joint_4" ctrllimited="true" ctrlrange="-3.106 3.106" kp="150" kv="15" />
<position name="joint_5" joint="joint_5" ctrllimited="true" ctrlrange="-3.106 3.106" kp="100" kv="10" />
<position name="joint_6" joint="joint_6" ctrllimited="true" ctrlrange="-6.28 6.28" kp="100" kv="10" />
</actuator>
<sensor>
<actuatorpos name="joint_1_p" actuator="joint_1" />
<actuatorvel name="joint_1_v" actuator="joint_1" />
<actuatorfrc name="joint_1_f" actuator="joint_1" noise="0.001" />
<actuatorpos name="joint_2_p" actuator="joint_2" />
<actuatorvel name="joint_2_v" actuator="joint_2" />
<actuatorfrc name="joint_2_f" actuator="joint_2" noise="0.001" />
<actuatorpos name="joint_3_p" actuator="joint_3" />
<actuatorvel name="joint_3_v" actuator="joint_3" />
<actuatorfrc name="joint_3_f" actuator="joint_3" noise="0.001" />
<actuatorpos name="joint_4_p" actuator="joint_4" />
<actuatorvel name="joint_4_v" actuator="joint_4" />
<actuatorfrc name="joint_4_f" actuator="joint_4" noise="0.001" />
<actuatorpos name="joint_5_p" actuator="joint_5" />
<actuatorvel name="joint_5_v" actuator="joint_5" />
<actuatorfrc name="joint_5_f" actuator="joint_5" noise="0.001" />
<actuatorpos name="joint_6_p" actuator="joint_6" />
<actuatorvel name="joint_6_v" actuator="joint_6" />
<actuatorfrc name="joint_6_f" actuator="joint_6" noise="0.001" />
<framequat name="orientation" objtype="site" noise="0.001" objname="imu" />
<gyro name="angular-velocity" site="imu" noise="0.005" cutoff="34.9" />
</sensor>
</mujoco>
viewer.py
import mujoco
import mujoco.viewer
import numpy as np
import socket
import threading
import time
import json
# ============ UDP 通信配置 ============
GUI_RECV_PORT = 5005 # 接收来自 GUI 的控制指令
VIEWER_SEND_PORT = 5006 # 向 GUI 发送当前关节角度
MJCF_PATH = "mjcf/rml63ii-with435.xml" # ⚠️ 确保路径与主程序一致
class UdpBridge:
def __init__(self):
self.recv_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.recv_sock.bind(("127.0.0.1", GUI_RECV_PORT))
self.recv_sock.settimeout(0.01) # 非阻塞接收
self.send_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.gui_addr = ("127.0.0.1", VIEWER_SEND_PORT)
self.latest_cmd = None
self.lock = threading.Lock()
def recv_thread(self):
"""后台线程持续接收 GUI 发来的控制指令"""
while True:
try:
data, _ = self.recv_sock.recvfrom(1024)
cmd = json.loads(data.decode())
with self.lock:
self.latest_cmd = cmd
except socket.timeout:
pass
except Exception as e:
print(f"[Viewer] Recv error: {e}")
def get_cmd(self):
with self.lock:
cmd = self.latest_cmd
self.latest_cmd = None
return cmd
def send_state(self, q_deg: np.ndarray):
try:
msg = json.dumps({"q_deg": q_deg.tolist()})
self.send_sock.sendto(msg.encode(), self.gui_addr)
except Exception:
pass
def main():
print("🚀 Starting MuJoCo Viewer...")
model = mujoco.MjModel.from_xml_path(MJCF_PATH)
data = mujoco.MjData(model)
bridge = UdpBridge()
t = threading.Thread(target=bridge.recv_thread, daemon=True)
t.start()
print(f"✅ Viewer ready | Listening on UDP:{GUI_RECV_PORT}, Sending to UDP:{VIEWER_SEND_PORT}")
last_send_time = 0.0
with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running():
# 1. 应用来自 GUI 的控制指令
cmd = bridge.get_cmd()
if cmd is not None:
if cmd["type"] == "joint":
q_rad = np.deg2rad(np.array(cmd["value"]))
safe_q = np.clip(q_rad, model.actuator_ctrlrange[:, 0], model.actuator_ctrlrange[:, 1])
data.ctrl[:] = safe_q
elif cmd["type"] == "ik":
safe_q = np.clip(np.array(cmd["value"]), model.actuator_ctrlrange[:, 0],
model.actuator_ctrlrange[:, 1])
data.ctrl[:] = safe_q
# 2. 物理步进 & 渲染
mujoco.mj_step(model, data)
viewer.sync()
# 3. 以 ~30Hz 向 GUI 回传当前关节角度
now = time.monotonic()
if now - last_send_time >= 1.0 / 30.0:
last_send_time = now
q_deg = np.rad2deg(data.qpos[:model.nu].copy())
bridge.send_state(q_deg)
print("🔴 Viewer closed")
if __name__ == "__main__":
main()
gui.py
import os
import mujoco
import cv2
import numpy as np
import tkinter as tk
from tkinter import ttk, filedialog, messagebox
from PIL import Image, ImageTk
import threading
import time
import math
import socket
import json
# ============ 1. 优化版 MDH 数值逆解器 ============
class MDHIKSolver:
def __init__(self):
self.mdh_params = [
[0.0, 0.0, 0.177, 0.0],
[-0.086, -math.pi / 2, 0.0, -math.pi / 2],
[0.380, 0.0, 0.0, math.pi / 2],
[0.069, math.pi / 2, 0.405, 0.0],
[0.0, -math.pi / 2, 0.0, math.pi],
[0.0, -math.pi / 2, 0.1436, math.pi]
]
self.njoints = len(self.mdh_params)
# ⭐ 从模型中提取真实关节限位 (需在外部传入 model 或使用默认安全值)
# 这里使用 RML63 典型限位作为兜底，建议在初始化时通过 mujoco model 覆盖
self.q_min = np.array([-170, -120, -170, -120, -170, -120]) * np.pi / 180.0
self.q_max = np.array([170, 120, 170, 120, 170, 120]) * np.pi / 180.0
# ⭐ 自适应阻尼参数
self.lambda_min = 0.01 # 最小阻尼 (高精度区)
self.lambda_max = 0.3 # 最大阻尼 (奇异区/大误差区)
self.error_threshold = 0.05 # 误差阈值 (m/rad)，低于此值进入精细模式
def _mdh_matrix(self, a, alpha, d, theta):
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)
return np.array([
[ct, -st, 0, a],
[st * ca, ct * ca, -sa, -sa * d],
[st * sa, ct * sa, ca, ca * d],
[0, 0, 0, 1]
])
def forward(self, q: np.ndarray) -> np.ndarray:
T = np.eye(4)
for i, p in enumerate(self.mdh_params):
T = T @ self._mdh_matrix(p[0], p[1], p[2], q[i] + p[3])
return T
def get_all_joint_positions(self, q: np.ndarray) -> np.ndarray:
positions = [np.zeros(3)]
T = np.eye(4)
for i, p in enumerate(self.mdh_params):
T = T @ self._mdh_matrix(p[0], p[1], p[2], q[i] + p[3])
positions.append(T[:3, 3].copy())
return np.array(positions)
# ⭐ 四元数转换与姿态误差计算 (替代原有的轴角法)
@staticmethod
def _rot_to_quat(R):
"""旋转矩阵转单位四元数 (w, x, y, z)"""
tr = np.trace(R)
if tr > 0:
s = 0.5 / np.sqrt(tr + 1.0)
w = 0.25 / s
x = (R[2, 1] - R[1, 2]) * s
y = (R[0, 2] - R[2, 0]) * s
z = (R[1, 0] - R[0, 1]) * s
elif R[0, 0] > R[1, 1] and R[0, 0] > R[2, 2]:
s = 2.0 * np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2])
w = (R[2, 1] - R[1, 2]) / s
x = 0.25 * s
y = (R[0, 1] + R[1, 0]) / s
z = (R[0, 2] + R[2, 0]) / s
elif R[1, 1] > R[2, 2]:
s = 2.0 * np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2])
w = (R[0, 2] - R[2, 0]) / s
x = (R[0, 1] + R[1, 0]) / s
y = 0.25 * s
z = (R[1, 2] + R[2, 1]) / s
else:
s = 2.0 * np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1])
w = (R[1, 0] - R[0, 1]) / s
x = (R[0, 2] + R[2, 0]) / s
y = (R[1, 2] + R[2, 1]) / s
z = 0.25 * s
q = np.array([w, x, y, z])
return q / np.linalg.norm(q)
@staticmethod
def _quat_error(q_current, q_target):
"""四元数姿态误差：返回等效轴角向量 (3D)"""
# 确保走最短路径 (点积为正)
dot = np.dot(q_current, q_target)
if dot < 0:
q_target = -q_target
dot = -dot
dot = np.clip(dot, -1.0, 1.0)
# 误差四元数: q_err = q_target * q_current^{-1}
# 对于单位四元数, 虚部即为 sin(theta/2)*axis
w_c, xc, yc, zc = q_current
w_t, xt, yt, zt = q_target
# q_err = q_t ⊗ conj(q_c)
ew = w_t * w_c + xt * xc + yt * yc + zt * zc
ex = w_t * xc - xt * w_c + yt * zc - zt * yc
ey = w_t * yc - xt * zc - yt * w_c + zt * xc
ez = w_t * zc + xt * yc - yt * xc - zt * w_c
# 提取等效轴角
sin_half = np.sqrt(ex * ex + ey * ey + ez * ez)
if sin_half < 1e-10:
return np.zeros(3)
angle = 2.0 * np.arctan2(sin_half, ew)
axis = np.array([ex, ey, ez]) / sin_half
return axis * angle
@staticmethod
def _rpy_xyz_to_matrix(xyz, rpy):
cx, sx = np.cos(rpy[0]), np.sin(rpy[0])
cy, sy = np.cos(rpy[1]), np.sin(rpy[1])
cz, sz = np.cos(rpy[2]), np.sin(rpy[2])
R = np.array([[cy * cz, sx * sy * cz - cx * sz, cx * sy * cz + sx * sz],
[cy * sz, sx * sy * sz + cx * cz, cx * sy * sz - sx * cz],
[-sy, sx * cy, cx * cy]])
T = np.eye(4);
T[:3, :3] = R;
T[:3, 3] = xyz;
return T
def _numerical_jacobian(self, q, eps=1e-6):
J = np.zeros((6, self.njoints))
T0 = self.forward(q)
q0_quat = self._rot_to_quat(T0[:3, :3])
for i in range(self.njoints):
qp = q.copy();
qp[i] += eps
Tp = self.forward(qp)
dp = (Tp[:3, 3] - T0[:3, 3]) / eps
qp_quat = self._rot_to_quat(Tp[:3, :3])
domega = self._quat_error(q0_quat, qp_quat) / eps
J[:, i] = np.concatenate([dp, domega])
return J
def _clamp_joints(self, q):
"""将关节角严格限制在物理限位内"""
return np.clip(q, self.q_min, self.q_max)
def _solve_single(self, target_T, q_init, max_iter=300, tol=1e-4):
"""单次 IK 求解 (自适应阻尼 + 四元数误差 + 限位投影)"""
q = self._clamp_joints(q_init.copy())
target_quat = self._rot_to_quat(target_T[:3, :3])
for iteration in range(max_iter):
current_T = self.forward(q)
current_quat = self._rot_to_quat(current_T[:3, :3])
pos_err = target_T[:3, 3] - current_T[:3, 3]
rot_err = self._quat_error(current_quat, target_quat)
error = np.concatenate([pos_err, rot_err])
err_norm = np.linalg.norm(error)
if err_norm < tol:
return q
J = self._numerical_jacobian(q)
# ⭐ 自适应阻尼：基于误差范数 + 雅可比最小奇异值
sv = np.linalg.svd(J, compute_uv=False)
sigma_min = sv[-1] if len(sv) > 0 else 0.0
# 当接近奇异 (sigma_min → 0) 或误差较大时，增大阻尼
lambda_sq = self.lambda_min ** 2
if sigma_min < 0.01:
lambda_sq = self.lambda_max ** 2 * (1.0 - sigma_min / 0.01)
if err_norm > self.error_threshold:
lambda_sq = max(lambda_sq, (err_norm / self.error_threshold) * 0.01)
# 阻尼最小二乘: dq = J^T (J J^T + λ²I)^{-1} e
JJt = J @ J.T
damped_inv = np.linalg.solve(JJt + lambda_sq * np.eye(6), error)
dq = J.T @ damped_inv
# ⭐ 步长限制：防止单次更新过大导致飞出工作空间
dq_norm = np.linalg.norm(dq)
max_step = 0.3 # rad
if dq_norm > max_step:
dq = dq * (max_step / dq_norm)
q = self._clamp_joints(q + dq)
return None # 未收敛
def inverse(self, target_xyz, target_rpy_deg, q_init_rad, max_iter=300, tol=1e-4):
target_T = self._rpy_xyz_to_matrix(target_xyz, np.deg2rad(target_rpy_deg))
# ⭐ 策略1: 以用户提供的当前关节角为初值求解
result = self._solve_single(target_T, q_init_rad, max_iter, tol)
if result is not None:
return result
print("⚠️ 主初值未收敛，启动多初值重试...")
# ⭐ 策略2: 在零位附近生成多组随机扰动初值重试
rng = np.random.default_rng(42)
retry_configs = [
np.zeros(self.njoints), # 零位
rng.uniform(-0.3, 0.3, self.njoints), # 小随机
rng.uniform(-0.8, 0.8, self.njoints), # 中随机
np.array([0, -np.pi / 4, np.pi / 2, 0, np.pi / 4, 0]), # 典型肘上构型
np.array([0, np.pi / 4, -np.pi / 2, 0, -np.pi / 4, 0]), # 典型肘下构型
]
for i, q_try in enumerate(retry_configs):
result = self._solve_single(target_T, q_try, max_iter, tol)
if result is not None:
print(f"✅ 重试配置 {i} 收敛成功")
return result
print("❌ 所有初值均未收敛，目标可能超出可达工作空间")
return None
# ============ 2. MDH 姿态预检 ============
def preview_mdh_pose(solver: MDHIKSolver):
import matplotlib.pyplot as plt
q_zero = np.zeros(solver.njoints)
joint_positions = solver.get_all_joint_positions(q_zero)
T_end = solver.forward(q_zero)
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
xs, ys, zs = joint_positions[:, 0], joint_positions[:, 1], joint_positions[:, 2]
ax.plot(xs, ys, zs, 'o-', color='#2196F3', linewidth=3, markersize=8, label='Links & Joints')
for i, pos in enumerate(joint_positions):
label = "Base" if i == 0 else f"J{i}"
ax.text(pos[0] + 0.02, pos[1] + 0.02, pos[2] + 0.02, label, fontsize=9, color='black')
end_pos = T_end[:3, 3]
arrow_len = 0.1
ax.quiver(*end_pos, *T_end[:3, 0], color='r', length=arrow_len, linewidth=2, label='End-X')
ax.quiver(*end_pos, *T_end[:3, 1], color='g', length=arrow_len, linewidth=2, label='End-Y')
ax.quiver(*end_pos, *T_end[:3, 2], color='b', length=arrow_len, linewidth=2, label='End-Z')
max_range = np.array([xs.max() - xs.min(), ys.max() - ys.min(), zs.max() - zs.min()]).max() / 2.0
mid_x, mid_y, mid_z = (xs.max() + xs.min()) * 0.5, (ys.max() + ys.min()) * 0.5, (zs.max() + zs.min()) * 0.5
ax.set_xlim(mid_x - max_range, mid_x + max_range)
ax.set_ylim(mid_y - max_range, mid_y + max_range)
ax.set_zlim(mid_z - max_range, mid_z + max_range)
ax.set_xlabel('X (m)');
ax.set_ylabel('Y (m)');
ax.set_zlabel('Z (m)')
ax.set_title("MDH Kinematics Preview (Zero Position)\nClose window to launch GUI", fontsize=14)
ax.legend(fontsize=11);
ax.view_init(elev=25, azim=-60)
print("📊 MDH预检窗口已打开，请检查连杆与关节是否正确...")
plt.show()
# ============ 3. 纯 UDP 关节角接收线程 ============
class UdpRecvThread(threading.Thread):
def __init__(self, stop_event):
super().__init__(daemon=True)
self.stop_event = stop_event
self.latest_q_deg = None
self.lock = threading.Lock()
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.sock.bind(("127.0.0.1", 5006))
self.sock.settimeout(0.05)
def run(self):
while not self.stop_event.is_set():
try:
raw, _ = self.sock.recvfrom(1024)
state = json.loads(raw.decode())
with self.lock:
self.latest_q_deg = np.array(state["q_deg"])
except socket.timeout:
pass
except Exception:
pass
self.sock.close()
def get_q_deg(self):
with self.lock:
q = self.latest_q_deg
self.latest_q_deg = None
return q
# ============ 4. 主 GUI (⭐ 输入与显示完全分离) ============
class MujocoGuiApp:
def __init__(self, mjcf_path):
self.solver = MDHIKSolver()
self.stop_event = threading.Event()
self.mjcf_path = mjcf_path
preview_mdh_pose(self.solver)
self.model = mujoco.MjModel.from_xml_path(mjcf_path)
self.data = mujoco.MjData(self.model)
self.cam_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_CAMERA, "hand_camera")
self.cam_width, self.cam_height = 640, 480
self.renderer = mujoco.Renderer(self.model, self.cam_height, self.cam_width)
# ⭐ 预分配缓冲区 (零拷贝优化在软件渲染下同样有效)
self._bgr_buf = np.empty((self.cam_height, self.cam_width, 3), dtype=np.uint8)
self._gray_buf = np.empty((self.cam_height, self.cam_width), dtype=np.uint8)
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
self.detector = cv2.aruco.ArucoDetector(aruco_dict, cv2.aruco.DetectorParameters())
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
self.detector = cv2.aruco.ArucoDetector(aruco_dict, cv2.aruco.DetectorParameters())
self.udp_send_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.viewer_addr = ("127.0.0.1", 5005)
self.udp_thread = UdpRecvThread(self.stop_event)
self.udp_thread.start()
self.root = tk.Tk()
self.root.title("RML63 MDH IK Controller")
self.root.protocol("WM_DELETE_WINDOW", self.on_close)
self.current_bgr_image = None
self.last_q_deg = np.zeros(6)
# ⭐ 固定 15 FPS 参数
self._target_fps = 15.0
self._frame_interval_ms = int(1000.0 / self._target_fps)
self._build_gui()
self._main_loop()
def _build_gui(self):
frame = ttk.Frame(self.root, padding=10)
frame.pack(fill=tk.BOTH, expand=True)
left_panel = ttk.Frame(frame)
left_panel.pack(side=tk.LEFT, fill=tk.Y, padx=(0, 10))
# --- 位姿输入 ---
pose_frame = ttk.LabelFrame(left_panel, text="Target Pose (m / deg)", padding=10)
pose_frame.pack(fill=tk.X, pady=(0, 10))
self.pose_entries = []
for lbl, val in zip(["X(m):", "Y(m):", "Z(m):", "Rx(°):", "Ry(°):", "Rz(°):"],
["0.3", "0.0", "0.5", "0.0", "60.0", "0.0"]):
row = ttk.Frame(pose_frame);
row.pack(fill=tk.X, pady=2)
ttk.Label(row, text=lbl, width=6).pack(side=tk.LEFT)
e = ttk.Entry(row, width=10);
e.insert(0, val);
e.pack(side=tk.LEFT)
self.pose_entries.append(e)
ttk.Button(pose_frame, text="🚀 Send IK", command=self.on_send_ik).pack(pady=10, fill=tk.X)
# --- ⭐ 关节控制：显示与输入彻底分离 ---
joint_frame = ttk.LabelFrame(left_panel, text="Joint Control (deg)", padding=10)
joint_frame.pack(fill=tk.X, pady=(0, 10))
# 表头
header = ttk.Frame(joint_frame)
header.pack(fill=tk.X, pady=(0, 5))
ttk.Label(header, text="Joint", width=5, font=("Arial", 9, "bold")).pack(side=tk.LEFT)
ttk.Label(header, text="Actual (Read-only)", width=14, font=("Arial", 9, "bold")).pack(side=tk.LEFT)
ttk.Label(header, text="Command (Input)", width=14, font=("Arial", 9, "bold")).pack(side=tk.LEFT, padx=(5, 0))
self.actual_labels = [] # ⭐ 只读显示标签
self.cmd_entries = [] # ⭐ 独立输入框
for i in range(6):
row = ttk.Frame(joint_frame);
row.pack(fill=tk.X, pady=2)
# 关节编号
ttk.Label(row, text=f"J{i + 1}:", width=5).pack(side=tk.LEFT)
# ⭐ 实际值：使用 Label 而非 Entry，彻底杜绝焦点抢占和文字跳动
actual_lbl = ttk.Label(row, text="0.0", width=12, anchor=tk.CENTER,
relief=tk.SUNKEN, background="#f0f0f0")
actual_lbl.pack(side=tk.LEFT, padx=(0, 5))
self.actual_labels.append(actual_lbl)
# ⭐ 指令值：独立输入框，仅在回车时读取
cmd_entry = ttk.Entry(row, width=12, font=("Consolas", 10), justify=tk.CENTER)
cmd_entry.insert(0, "0.0")
cmd_entry.pack(side=tk.LEFT)
cmd_entry.bind("<Return>", lambda e, idx=i: self._on_cmd_commit(idx))
self.cmd_entries.append(cmd_entry)
btn_row = ttk.Frame(joint_frame)
btn_row.pack(fill=tk.X, pady=(8, 0))
ttk.Button(btn_row, text="✅ Apply All Cmds", command=self._apply_all_cmds).pack(side=tk.LEFT, fill=tk.X,
expand=True, padx=(0, 3))
ttk.Button(btn_row, text="🔄 Reset Cmds", command=self._reset_cmds).pack(side=tk.RIGHT, fill=tk.X, expand=True,
padx=(3, 0))
# --- 右侧相机 + 保存按钮 ---
right_panel = ttk.Frame(frame)
right_panel.pack(side=tk.RIGHT, fill=tk.BOTH, expand=True)
cam_frame = ttk.LabelFrame(right_panel, text="Hand Camera", padding=5)
cam_frame.pack(fill=tk.BOTH, expand=True)
self.cam_label = ttk.Label(cam_frame)
self.cam_label.pack(fill=tk.BOTH, expand=True)
btn_frame = ttk.Frame(right_panel)
btn_frame.pack(fill=tk.X, pady=(5, 0))
ttk.Button(btn_frame, text="💾 Save Current Frame", command=self.save_image).pack(side=tk.RIGHT)
self.status_var = tk.StringVar(value="✅ Ready | Left=Actual | Right=Command")
ttk.Label(self.root, textvariable=self.status_var, relief=tk.SUNKEN).pack(fill=tk.X, side=tk.BOTTOM)
# ⭐ 单个指令回车提交
def _on_cmd_commit(self, idx):
try:
deg = float(self.cmd_entries[idx].get())
deg = max(-180.0, min(180.0, deg))
self.cmd_entries[idx].delete(0, tk.END)
self.cmd_entries[idx].insert(0, f"{deg:.1f}")
self._send_current_cmds()
self.status_var.set(f"✅ J{idx + 1} cmd sent: {deg:.1f}°")
except ValueError:
self.status_var.set(f"❌ J{idx + 1} cmd invalid")
# ⭐ 一键应用所有指令
def _apply_all_cmds(self):
valid = True
for i, entry in enumerate(self.cmd_entries):
try:
deg = float(entry.get())
deg = max(-180.0, min(180.0, deg))
entry.delete(0, tk.END)
entry.insert(0, f"{deg:.1f}")
except ValueError:
self.status_var.set(f"❌ J{i + 1} cmd invalid")
valid = False
break
if valid:
self._send_current_cmds()
self.status_var.set("✅ All cmds applied")
def _send_current_cmds(self):
q_deg = [float(e.get()) for e in self.cmd_entries]
self._send_udp({"type": "joint", "value": q_deg})
def _reset_cmds(self):
for e in self.cmd_entries:
e.delete(0, tk.END);
e.insert(0, "0.0")
self._send_udp({"type": "joint", "value": [0.0] * 6})
self.status_var.set("🔄 Cmds reset to zero")
def on_send_ik(self):
try:
vals = [float(e.get()) for e in self.pose_entries]
target_q_rad = self.solver.inverse(vals[:3], vals[3:], np.deg2rad(self.last_q_deg))
if target_q_rad is not None:
self._send_udp({"type": "ik", "value": target_q_rad.tolist()})
self.status_var.set(f"✅ IK Sent | q={np.round(np.rad2deg(target_q_rad), 1)}°")
else:
self.status_var.set("❌ IK Failed")
except ValueError:
self.status_var.set("❌ Invalid Pose Input")
def save_image(self):
if self.current_bgr_image is None:
messagebox.showwarning("Warning", "No camera frame available yet.")
return
default_name = f"cam_capture_{int(time.time())}.png"
path = filedialog.asksaveasfilename(
title="Save Camera Image", defaultextension=".png",
initialfile=default_name,
filetypes=[("PNG files", "*.png"), ("JPEG files", "*.jpg"), ("All files", "*.*")]
)
if path:
try:
cv2.imwrite(path, self.current_bgr_image)
self.status_var.set(f"✅ Saved: {os.path.basename(path)}")
except Exception as e:
messagebox.showerror("Save Error", str(e))
def _send_udp(self, cmd):
try:
msg = json.dumps(cmd).encode()
self.udp_send_sock.sendto(msg, self.viewer_addr)
except Exception as e:
self.status_var.set(f"❌ UDP Error: {e}")
# ⭐ 核心主循环
def _main_loop(self):
if not self.stop_event.is_set():
loop_start = time.monotonic()
# --- UDP 关节角同步 ---
q_deg = self.udp_thread.get_q_deg()
if q_deg is not None:
q_rad = np.deg2rad(q_deg)
safe_q = np.clip(q_rad, self.model.actuator_ctrlrange[:, 0], self.model.actuator_ctrlrange[:, 1])
self.data.ctrl[:] = safe_q
self.last_q_deg = q_deg.copy()
for i in range(6):
self.actual_labels[i].config(text=f"{q_deg[i]:.1f}")
# --- 物理步进 ---
mujoco.mj_step(self.model, self.data)
# --- 渲染 + 图像处理 ---
self.renderer.update_scene(self.data, camera=self.cam_id)
rgb_frame = self.renderer.render() # 返回新分配的 (H,W,3) uint8 数组
# --- OpenCV 处理仍使用预分配缓冲区 (避免额外内存分配) ---
cv2.cvtColor(rgb_frame, cv2.COLOR_RGB2BGR, dst=self._bgr_buf)
cv2.flip(self._bgr_buf, 0, dst=self._bgr_buf)
cv2.cvtColor(rgb_frame, cv2.COLOR_RGB2GRAY, dst=self._gray_buf)
cv2.equalizeHist(self._gray_buf, self._gray_buf)
corners, ids, _ = self.detector.detectMarkers(self._gray_buf)
if ids is not None:
cv2.aruco.drawDetectedMarkers(self._bgr_buf, corners, ids)
self.current_bgr_image = self._bgr_buf
# --- Tkinter 更新 ---
pil_img = Image.fromarray(self._bgr_buf)
tk_img = ImageTk.PhotoImage(image=pil_img)
self.cam_label.configure(image=tk_img)
self.cam_label.image = tk_img
# --- 固定 15 FPS 调度 ---
elapsed_ms = (time.monotonic() - loop_start) * 1000.0
next_delay = max(1, self._frame_interval_ms - int(elapsed_ms))
actual_fps = 1000.0 / max(elapsed_ms, 1e-6)
self.status_var.set(f"Target: 15 FPS | Actual: {actual_fps:.1f} | Next: {next_delay}ms")
self.root.after(next_delay, self._main_loop)
def on_close(self):
self.stop_event.set()
self.udp_send_sock.close()
self.renderer.close()
self.root.destroy()
print("🔴 GUI Stopped")
def run(self):
self.root.mainloop()
if __name__ == "__main__":
MJCF_PATH = "mjcf/rml63ii-with435.xml"
app = MujocoGuiApp(MJCF_PATH)
app.run()
效果演示
viewer画面
gui画面
TODO
当前，对标定板识别精确程度仍然有待提高，并且控制指令存在一定角度偏差情况
- 接入 AprilTag (tag36h11) 替代 ArUco，提升小尺寸标签检测鲁棒性
- 增加标定板 freejoint 支持，实现交互式/自动化多位姿采集
- 集成手眼标定模块，自动输出 Eye-in-Hand 变换矩阵
- 添加关节力矩可视化与碰撞预警
浙公网安备 33010602011771号