diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py index d6c12a5dac..27a685095c 100644 --- a/src/Robotic_arm/main.py +++ b/src/Robotic_arm/main.py @@ -1,12 +1,15 @@ #!/usr/bin/env python3 # -*- coding: utf-8 -*- """ -机械臂关节控制器 - 最终兼容版 +机械臂关节负载性能优化版（终极全XML错误修复） 核心优化： -1. 修复Mujoco mj_name2id API类型错误（兼容所有版本） -2. 移除所有不兼容属性和依赖 -3. 纯原生实现，无Numba/特殊依赖 -4. Windows深度适配+优雅退出 +1. 彻底修复所有XML Schema错误（body/mass/forcelimit 均移除违规配置） +2. 精准关节负载建模（末端负载/关节负载可配置） +3. 负载自适应PD控制（抗干扰、无超调、响应快） +4. 物理仿真优化（减少负载下的计算冗余，提升实时性） +5. 负载状态监控（实时显示负载大小、关节受力、控制误差） +6. 软件层面过载保护（替代forcelimit，兼容所有Mujoco版本） +7. 全Mujoco版本兼容（支持新旧版本，无任何语法隐患） """ import sys @@ -18,149 +21,222 @@ import numpy as np import mujoco -# ====================== 全局配置 ====================== -# 系统适配（Windows优先） +# ====================== 全局配置（负载优化专用） ====================== +# 系统适配（Windows优先，极致CPU优化） if os.name == 'nt': try: kernel32 = ctypes.windll.kernel32 kernel32.SetConsoleMode(kernel32.GetStdHandle(-11), 7) os.system('chcp 65001 >nul 2>&1') kernel32.SetThreadPriority(kernel32.GetCurrentThread(), 1) - except: - pass + except Exception as e: + print(f"⚠️ Windows系统优化失败（不影响核心功能）: {e}") + # 强制单线程，避免负载下多线程竞争导致卡顿 os.environ['OMP_NUM_THREADS'] = '1' os.environ['MKL_NUM_THREADS'] = '1' + os.environ['NUMEXPR_NUM_THREADS'] = '1' # Mujoco Viewer兼容 +MUJOCO_NEW_VIEWER = False try: from mujoco import viewer MUJOCO_NEW_VIEWER = True except ImportError: - import mujoco.viewer as viewer - - MUJOCO_NEW_VIEWER = False + try: + import mujoco.viewer as viewer + except ImportError as e: + print(f"⚠️ Mujoco Viewer导入失败（无法可视化）: {e}") # 核心参数配置 +# 关节基础配置 JOINT_COUNT = 5 JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5"] -JOINT_LIMITS = np.array([ +JOINT_LIMITS_RAD = np.array([ [-np.pi, np.pi], # joint1 (Z轴) [-np.pi / 2, np.pi / 2], # joint2 (Y轴) [-np.pi / 2, np.pi / 2], # joint3 (Y轴) [-np.pi / 2, np.pi / 2], # joint4 (Y轴) [-np.pi / 2, np.pi / 2], # joint5 (Y轴) ], dtype=np.float64) -JOINT_MAX_VELOCITY = np.array([1.0, 0.8, 0.8, 0.6, 0.6], dtype=np.float64) +JOINT_MAX_VELOCITY_RAD = np.array([1.0, 0.8, 0.8, 0.6, 0.6], dtype=np.float64) +JOINT_MAX_TORQUE = np.array([15.0, 12.0, 10.0, 8.0, 5.0], dtype=np.float64) # 关节最大扭矩（软件过载保护） -# 仿真参数 -SIMULATION_TIMESTEP = 0.005 -CONTROL_FREQUENCY = 200 +# 仿真配置（负载下实时性优化） +SIMULATION_TIMESTEP = 0.002 # 更小步长，提升负载下控制精度 +CONTROL_FREQUENCY = 500 # 更高控制频率，应对负载下响应滞后 CONTROL_TIMESTEP = 1.0 / CONTROL_FREQUENCY FPS = 60 SLEEP_TIME = 1.0 / FPS EPS = 1e-8 RUNNING = True +SIMULATION_START_TIME = None + +# PD控制参数（负载自适应配置） +PD_PARAMS = { + 'kp_base': 80.0, + 'kd_base': 5.0, + 'kp_load_gain': 1.5, # 负载下比例增益放大系数 + 'kd_load_gain': 1.2, # 负载下微分增益放大系数 + 'max_vel': JOINT_MAX_VELOCITY_RAD.copy() +} -# PD控制参数 -KP = 80.0 -KD = 5.0 +# 负载配置（可动态调整） +LOAD_PARAMS = { + 'end_effector_mass': 0.5, # 末端负载质量（kg），默认0.5kg + 'joint_loads': np.zeros(JOINT_COUNT), # 各关节附加负载（N·m） + 'max_allowed_load': 2.0, # 最大允许末端负载（过载保护） + 'load_smoothing_factor': 0.1 # 负载检测平滑系数，避免抖动 +} -# ====================== 信号处理（优雅退出） ====================== +# ====================== 信号处理（负载下优雅退出） ====================== def signal_handler(sig, frame): global RUNNING - print("\n⚠️ 收到退出信号，正在优雅退出...") + if not RUNNING: + sys.exit(0) + print("\n⚠️ 收到退出信号，正在优雅退出（清理负载相关资源）...") RUNNING = False signal.signal(signal.SIGINT, signal_handler) - -# ====================== 预分配内存 ====================== -WORK_ARRAYS = { - 'current_angles': np.zeros(JOINT_COUNT, dtype=np.float64), - 'target_angles': np.zeros(JOINT_COUNT, dtype=np.float64), - 'joint_velocities': np.zeros(JOINT_COUNT, dtype=np.float64), - 'control_signals': np.zeros(JOINT_COUNT, dtype=np.float64), - 'ee_position': np.zeros(3, dtype=np.float64), - 'angle_error': np.zeros(JOINT_COUNT, dtype=np.float64), - 'desired_vel': np.zeros(JOINT_COUNT, dtype=np.float64) -} +signal.signal(signal.SIGTERM, signal_handler) -# ====================== 兼容型Mujoco ID查询函数 ====================== +# ====================== 工具函数（负载优化专用） ====================== def get_mujoco_id(model, obj_type, name): + """兼容所有Mujoco版本的ID查询函数（容错增强）""" + if model is None: + return -1 + type_map = { + 'joint': mujoco.mjtObj.mjOBJ_JOINT, + 'actuator': mujoco.mjtObj.mjOBJ_ACTUATOR, + 'site': mujoco.mjtObj.mjOBJ_SITE, + 'body': mujoco.mjtObj.mjOBJ_BODY, + 'geom': mujoco.mjtObj.mjOBJ_GEOM + } + obj_type_int = type_map.get(obj_type, mujoco.mjtObj.mjOBJ_JOINT) + try: + obj_id = mujoco.mj_name2id(model, int(obj_type_int), str(name)) + return obj_id if obj_id >= 0 else -1 + except Exception as e: + print(f"⚠️ 查询{obj_type} {name} ID失败: {e}") + return -1 + + +def deg2rad(degrees): + """角度值（度）转弧度（容错增强）""" + try: + degrees = np.array(degrees, dtype=np.float64) + return np.deg2rad(degrees) + except Exception as e: + print(f"⚠️ 角度转换失败: {e}") + return 0.0 if np.isscalar(degrees) else np.zeros(JOINT_COUNT, dtype=np.float64) + + +def rad2deg(radians): + """弧度转角度值（度）（容错增强）""" + try: + radians = np.array(radians, dtype=np.float64) + return np.rad2deg(radians) + except Exception as e: + print(f"⚠️ 弧度转换失败: {e}") + return 0.0 if np.isscalar(radians) else np.zeros(JOINT_COUNT, dtype=np.float64) + + +def calculate_load_adaptive_gains(current_load): """ - 兼容所有Mujoco版本的ID查询函数 - :param model: MjModel对象 - :param obj_type: 对象类型（字符串或枚举） - :param name: 对象名称 - :return: 对象ID + 负载自适应增益计算（核心优化） + 根据当前末端负载，动态调整PD增益，抵消负载干扰 + :param current_load: 当前末端负载（kg） + :return: 自适应kp, kd """ - # 处理类型转换（关键修复） - if isinstance(obj_type, str): - # 字符串类型映射 - type_map = { - 'joint': mujoco.mjtObj.mjOBJ_JOINT, - 'actuator': mujoco.mjtObj.mjOBJ_ACTUATOR, - 'site': mujoco.mjtObj.mjOBJ_SITE - } - obj_type_int = type_map.get(obj_type, mujoco.mjtObj.mjOBJ_JOINT) - else: - # 枚举类型转为整数（核心修复） - obj_type_int = int(obj_type) + # 负载归一化（0 ~ 1） + normalized_load = min(current_load / LOAD_PARAMS['max_allowed_load'], 1.0) + # 动态调整增益（负载越大，增益越高，保证响应性） + adaptive_kp = PD_PARAMS['kp_base'] * (1 + normalized_load * (PD_PARAMS['kp_load_gain'] - 1)) + adaptive_kd = PD_PARAMS['kd_base'] * (1 + normalized_load * (PD_PARAMS['kd_load_gain'] - 1)) + return adaptive_kp, adaptive_kd - # 兼容不同版本的mj_name2id调用方式 - try: - # 新版本调用方式 - return mujoco.mj_name2id(model, obj_type_int, name) - except: - # 旧版本兼容 - return mujoco.mj_name2id(model, obj_type, name) +# ====================== 机械臂模型生成（终极全XML错误修复） ====================== +def create_arm_model_with_load(): + """ + 生成带负载建模的机械臂XML模型（终极全修复+全版本兼容） + 1. 彻底修复所有XML Schema错误： + - 移除body的mass属性（改用geom定义质量） + - 移除motor的forcelimit属性（改用软件层面过载保护） + 2. 末端负载通过geom的mass属性配置，保留可动态调整功能 + 3. 关节添加阻尼和惯量，模拟真实负载特性 + 4. 简化非核心几何，提升负载下仿真速度 + 5. 依赖compiler的inertiafromgeom="true"，自动由geom质量推导body惯性属性 + 6. 无任何违规配置，兼容所有Mujoco版本 + """ + end_effector_mass = LOAD_PARAMS['end_effector_mass'] + # 连杆geom质量（对应原body质量，通过geom定义，兼容所有Mujoco版本） + link1_geom_mass = 0.8 + link2_geom_mass = 0.6 + link3_geom_mass = 0.6 + link4_geom_mass = 0.4 + link5_geom_mass = 0.2 -# ====================== 机械臂模型生成 ====================== -def create_arm_model(): - """生成极简兼容版XML模型""" xml = f""" - - -