diff --git a/src/Robot_arm_grasping_task/main.py b/src/Robot_arm_grasping_task/main.py index 926491c149..b7a39efd37 100644 --- a/src/Robot_arm_grasping_task/main.py +++ b/src/Robot_arm_grasping_task/main.py @@ -6,153 +6,91 @@ import os import warnings import time -import glfw # 直接用glfw检测按键,兼容所有版本 +import glfw from contextlib import suppress -# ===================== 基础配置(消除警告) ===================== +# ===================== 基础配置 ===================== warnings.filterwarnings('ignore') mpl.use('TkAgg') mpl.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans'] mpl.rcParams['axes.unicode_minus'] = False -# 路径配置(适配你的原有robot.xml路径) +# 路径配置 CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) MODEL_PATH = os.path.join(CURRENT_DIR, "robot.xml") -# ===================== 核心控制参数(微调适配原有模型) ===================== -# 手动控制参数(适配原有模型的关节范围,低速易控) -MANUAL_SPEED = 0.03 # 比之前略小,适配原有模型的关节灵敏度 -GRASP_FORCE = 3.5 # 微调力度,适配原有夹爪尺寸 -# 自动控制参数(适配原有模型的物体位置) -AUTO_LIFT_HEIGHT = 0.12 # 适配原有模型的抬升范围 -AUTO_TRANSPORT_X = -0.15 # 适配原有模型的搬运范围 - -# ===================== 全局控制变量 ===================== +# ===================== 核心优化参数(流畅性重点) ===================== +# 手动控制:更低速、更平滑 +MANUAL_SPEED = 0.025 # 进一步降低速度,减少抖动 +GRASP_FORCE = 3.8 # 微调抓取力度,保证抓稳且不卡顿 +# 自动抓取:平滑轨迹参数 +AUTO_LIFT_HEIGHT = 0.12 +AUTO_TRANSPORT_X = -0.15 +SMOOTH_GAIN = 3.0 # 降低控制增益,减少超调 +SMOOTH_CLIP = 1.0 # 更严格的输出限制,避免猛冲 +ACCEL_FACTOR = 0.05 # 加速度因子,让动作渐进加速/减速 + +# ===================== 全局变量 ===================== control_cmd = { - 'forward': 0, # 前(W) - 'backward': 0, # 后(S) - 'left': 0, # 左(A) - 'right': 0, # 右(D) - 'up': 0, # 上(Q) - 'down': 0, # 下(E) - 'grasp': 0, # 抓取(空格) - 'release': 0, # 释放(R) - 'auto': False, # 一键自动抓取(Z) - 'reset': False # 重置(C) + 'forward': 0, 'backward': 0, 'left': 0, 'right': 0, + 'up': 0, 'down': 0, 'grasp': 0, 'release': 0, + 'auto': False, 'reset': False } +# 平滑控制缓存(记录上一步的控制输出,避免突变) +last_ctrl = np.zeros(10) # 适配最多10个关节 -# ===================== 兼容版按键检测函数(核心修复) ===================== +# ===================== 兼容版按键检测 ===================== def check_keyboard_input(viewer): - """ - 兼容所有版本mujoco-viewer的按键检测 - 替代原有get_key()方法,解决属性不存在问题 - """ - # 重置所有指令(避免按键粘连) for key in control_cmd.keys(): if key != 'auto' and key != 'reset': control_cmd[key] = 0 - # 方式1:适配新版mujoco-viewer(有window属性) if hasattr(viewer, 'window') and viewer.window is not None: window = viewer.window - # W键 - 前 - if glfw.get_key(window, glfw.KEY_W) == glfw.PRESS: - control_cmd['forward'] = 1 - # S键 - 后 - if glfw.get_key(window, glfw.KEY_S) == glfw.PRESS: - control_cmd['backward'] = 1 - # A键 - 左 - if glfw.get_key(window, glfw.KEY_A) == glfw.PRESS: - control_cmd['left'] = 1 - # D键 - 右 - if glfw.get_key(window, glfw.KEY_D) == glfw.PRESS: - control_cmd['right'] = 1 - # Q键 - 上 - if glfw.get_key(window, glfw.KEY_Q) == glfw.PRESS: - control_cmd['up'] = 1 - # E键 - 下 - if glfw.get_key(window, glfw.KEY_E) == glfw.PRESS: - control_cmd['down'] = 1 - # 空格键 - 抓取 - if glfw.get_key(window, glfw.KEY_SPACE) == glfw.PRESS: - control_cmd['grasp'] = 1 - # R键 - 释放 - if glfw.get_key(window, glfw.KEY_R) == glfw.PRESS: - control_cmd['release'] = 1 - # Z键 - 一键自动抓取 - if glfw.get_key(window, glfw.KEY_Z) == glfw.PRESS: - control_cmd['auto'] = True - # C键 - 重置 - if glfw.get_key(window, glfw.KEY_C) == glfw.PRESS: - control_cmd['reset'] = True - # ESC键 - 关闭窗口 + # 基础移动按键 + control_cmd['forward'] = 1 if glfw.get_key(window, glfw.KEY_W) == glfw.PRESS else 0 + control_cmd['backward'] = 1 if glfw.get_key(window, glfw.KEY_S) == glfw.PRESS else 0 + control_cmd['left'] = 1 if glfw.get_key(window, glfw.KEY_A) == glfw.PRESS else 0 + control_cmd['right'] = 1 if glfw.get_key(window, glfw.KEY_D) == glfw.PRESS else 0 + control_cmd['up'] = 1 if glfw.get_key(window, glfw.KEY_Q) == glfw.PRESS else 0 + control_cmd['down'] = 1 if glfw.get_key(window, glfw.KEY_E) == glfw.PRESS else 0 + # 抓取/释放/自动/重置 + control_cmd['grasp'] = 1 if glfw.get_key(window, glfw.KEY_SPACE) == glfw.PRESS else 0 + control_cmd['release'] = 1 if glfw.get_key(window, glfw.KEY_R) == glfw.PRESS else 0 + control_cmd['auto'] = True if glfw.get_key(window, glfw.KEY_Z) == glfw.PRESS else False + control_cmd['reset'] = True if glfw.get_key(window, glfw.KEY_C) == glfw.PRESS else False + # ESC退出 if glfw.get_key(window, glfw.KEY_ESCAPE) == glfw.PRESS: glfw.set_window_should_close(window, True) - - # 方式2:适配旧版mujoco-viewer(无window属性,备用方案) else: - # 旧版无法实时检测按键,提供替代操作方式 - print("\n⚠️ 检测到旧版mujoco-viewer,按键控制受限!") - print(" 替代操作:按Z键(一键自动抓取)或C键(重置)继续") - # 仅保留核心功能(自动抓取/重置) - # 按任意键触发自动抓取(简化适配) + print("\n⚠️ 旧版mujoco-viewer,按Z触发自动抓取,C重置") control_cmd['auto'] = True -# ===================== 核心控制函数(仅微调适配原有模型) ===================== -def init_model_and_viewer(): - """初始化模型(完全适配原有robot.xml,不修改模型)""" - if not os.path.exists(MODEL_PATH): - raise FileNotFoundError(f"未找到原有robot.xml文件: {MODEL_PATH}") - model = mujoco.MjModel.from_xml_path(MODEL_PATH) - data = mujoco.MjData(model) - mujoco.mj_resetData(model, data) - mujoco.mj_forward(model, data) - - # 初始化Viewer(微调视角,适配原有模型的显示) - viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) - viewer.cam.distance = 1.8 # 微调视角距离,看清原有模型 - viewer.cam.elevation = 12 # 微调仰角,适配原有模型的高度 - viewer.cam.azimuth = 50 # 微调方位角,看清物体位置 - viewer.cam.lookat = [0.15, 0.0, 0.12] # 适配原有模型的物体位置 - - # 兼容原有模型的ID命名(不修改模型,仅适配识别) - ee_id = -1 - obj_id = -1 - # 尝试所有可能的末端命名(适配原有模型) - for name in ["ee_site", "ee", "end_effector"]: - ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, name) - if ee_id >= 0: - break - if ee_id < 0: - for name in ["ee", "end_effector"]: - ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) - if ee_id >= 0: - break - # 尝试所有可能的物体命名(适配原有模型) - for name in ["target_object", "object", "ball"]: - obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) - if obj_id >= 0: - break - if obj_id < 0: - for name in ["object_geom", "ball_geom"]: - obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) - if obj_id >= 0: - break - - print("✅ 适配原有robot.xml完成!") - print("🎮 操作指南(适配原有模型):") - print(" W/S:前后移动 A/D:左右移动 Q/E:上下移动(低速易控)") - print(" 空格:抓取 R:释放 Z:一键自动抓取(适配原有模型)") - print(" C:重置 ESC:退出") - - return model, data, viewer, ee_id, obj_id +# ===================== 平滑控制函数(核心优化) ===================== +def smooth_control(target_ctrl, last_ctrl, joint_idx): + """ + 平滑控制输出,避免关节突变(解决抖动/卡顿) + :param target_ctrl: 目标控制值 + :param last_ctrl: 上一步控制值 + :param joint_idx: 关节索引 + :return: 平滑后的控制值 + """ + # 渐进逼近目标值,避免猛冲 + delta = target_ctrl - last_ctrl[joint_idx] + smoothed = last_ctrl[joint_idx] + delta * ACCEL_FACTOR + # 限制最大变化量,彻底避免抖动 + smoothed = np.clip(smoothed, -SMOOTH_CLIP, SMOOTH_CLIP) + # 更新缓存 + last_ctrl[joint_idx] = smoothed + return smoothed def manual_control(model, data, ee_id): - """手动控制(仅微调参数,适配原有模型的关节响应)""" - # 安全获取末端位置(适配原有模型) + """手动控制(增加平滑逻辑,动作更丝滑)""" + global last_ctrl + # 安全获取末端位置 ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: @@ -160,178 +98,251 @@ def manual_control(model, data, ee_id): except: ee_pos = data.xpos[ee_id].copy() - # 计算目标位置(微调速度,适配原有模型) + # 计算目标位置(低速,易控) target_pos = ee_pos.copy() - target_pos[0] += control_cmd['forward'] * MANUAL_SPEED - target_pos[0] -= control_cmd['backward'] * MANUAL_SPEED - target_pos[1] += control_cmd['left'] * MANUAL_SPEED - target_pos[1] -= control_cmd['right'] * MANUAL_SPEED - target_pos[2] += control_cmd['up'] * MANUAL_SPEED - target_pos[2] -= control_cmd['down'] * MANUAL_SPEED - - # 微调控制增益(适配原有模型的关节传动比,避免转圈) + target_pos[0] += (control_cmd['forward'] - control_cmd['backward']) * MANUAL_SPEED + target_pos[1] += (control_cmd['left'] - control_cmd['right']) * MANUAL_SPEED + target_pos[2] += (control_cmd['up'] - control_cmd['down']) * MANUAL_SPEED + + # 计算误差并平滑控制(核心优化) error = target_pos - ee_pos - gain = 4.0 # 微调增益,适配原有模型的关节灵敏度 for i in range(min(3, model.njnt)): - # 更严格的输出限制,彻底避免转圈 - data.ctrl[i] = np.clip(error[i] * gain, -1.8, 1.8) + target_ctrl = error[i] * SMOOTH_GAIN + # 平滑输出,避免关节突变 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) - # 抓取控制(微调力度,适配原有夹爪) + # 抓取控制(渐进加力,避免夹爪猛夹) if control_cmd['grasp']: - # 适配原有模型的夹爪控制维度 + # 渐进增加抓取力,避免物体被弹飞 if model.nu >= 4: - data.ctrl[3] = GRASP_FORCE + data.ctrl[3] = min(data.ctrl[3] + 0.1, GRASP_FORCE) if model.nu >= 5: - data.ctrl[4] = -GRASP_FORCE + data.ctrl[4] = max(data.ctrl[4] - 0.1, -GRASP_FORCE) elif control_cmd['release']: + # 渐进释放,避免物体掉落 if model.nu >= 4: - data.ctrl[3] = 0.0 + data.ctrl[3] = max(data.ctrl[3] - 0.1, 0.0) if model.nu >= 5: - data.ctrl[4] = 0.0 + data.ctrl[4] = min(data.ctrl[4] + 0.1, 0.0) def auto_grasp(model, data, ee_id, obj_id): - """一键自动抓取(仅微调轨迹,适配原有模型的物体位置)""" - print("🔄 开始适配原有模型的一键自动抓取...") - # 安全获取物体位置(适配原有模型) - obj_pos = np.array([0.2, 0.0, 0.05]) # 适配原有模型的默认物体位置 + """一键自动抓取(全流程平滑优化,无卡顿)""" + global last_ctrl + print("🔄 开始平滑自动抓取...") + # 重置平滑缓存 + last_ctrl = np.zeros(10) + + # 安全获取物体位置 + obj_pos = np.array([0.2, 0.0, 0.05]) if obj_id >= 0: try: obj_pos = data.xpos[obj_id].copy() except: pass - # 阶段1:移动到物体上方(微调距离,适配原有模型) + # 阶段1:缓慢移动到物体上方(平滑逼近,无猛冲) step = 0 - while step < 600 and viewer.is_alive: # 增加窗口存活检测 + while step < 800 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() - target = obj_pos + [0, 0, 0.07] # 微调高度,适配原有模型 + + # 目标位置:物体上方(高度微调,避免碰撞) + target = obj_pos + [0, 0, 0.08] error = target - ee_pos + + # 平滑控制关节,无抖动 for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 3.5, -1.2, 1.2) + target_ctrl = error[i] * SMOOTH_GAIN * 0.8 # 更慢速度 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + mujoco.mj_step(model, data) - viewer.render() # 自动抓取时也渲染,避免窗口卡死 + viewer.render() step += 1 - # 阶段2:下降抓取(微调力度,适配原有夹爪) + # 阶段2:缓慢下降(渐进接近,避免压碎物体) step = 0 - while step < 400 and viewer.is_alive: + while step < 600 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() - target = obj_pos + + # 动态调整目标:根据物体位置实时微调,避免偏差 + target = obj_pos + [0, 0, 0.01] # 仅贴近物体,不压下去 error = target - ee_pos + for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 2.8, -1.0, 1.0) - # 适配原有模型的夹爪控制 + target_ctrl = error[i] * SMOOTH_GAIN * 0.5 # 极慢速度 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + + # 渐进闭合夹爪(核心优化:避免猛夹导致物体掉落) if model.nu >= 4: - data.ctrl[3] = GRASP_FORCE + data.ctrl[3] = min(data.ctrl[3] + 0.05, GRASP_FORCE) if model.nu >= 5: - data.ctrl[4] = -GRASP_FORCE + data.ctrl[4] = max(data.ctrl[4] - 0.05, -GRASP_FORCE) + mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段3:抬升(微调高度,适配原有模型) + # 阶段3:缓慢抬升(确认抓稳后再抬升) step = 0 - while step < 450 and viewer.is_alive: + grasp_confirmed = False + while step < 500 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() - target = obj_pos + [0, 0, AUTO_LIFT_HEIGHT] + + # 确认抓稳后再抬升(避免刚夹就抬导致掉落) + if step > 100: + grasp_confirmed = True + + if grasp_confirmed: + target = obj_pos + [0, 0, AUTO_LIFT_HEIGHT] + else: + target = obj_pos + [0, 0, 0.01] # 先保持位置 + error = target - ee_pos for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 3.2, -1.1, 1.1) + target_ctrl = error[i] * SMOOTH_GAIN * 0.7 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段4:搬运(微调距离,适配原有模型) + # 阶段4:平稳搬运(匀速移动,无晃动) step = 0 - while step < 700 and viewer.is_alive: + while step < 800 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() + target = obj_pos + [AUTO_TRANSPORT_X, 0, AUTO_LIFT_HEIGHT] error = target - ee_pos for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 3.5, -1.2, 1.2) + target_ctrl = error[i] * SMOOTH_GAIN * 0.6 # 更平稳的速度 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段5:下放释放(适配原有模型) + # 阶段5:缓慢下放(精准定位,无掉落) step = 0 - while step < 450 and viewer.is_alive: + while step < 600 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() - target = obj_pos + [AUTO_TRANSPORT_X, 0, 0.04] # 微调下放高度 + + target = obj_pos + [AUTO_TRANSPORT_X, 0, 0.03] # 更贴近地面 error = target - ee_pos for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 2.8, -1.0, 1.0) - # 延迟释放,适配原有模型 - if step > 250: + target_ctrl = error[i] * SMOOTH_GAIN * 0.5 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + + # 延迟且渐进释放(核心优化:避免提前释放) + if step > 300: if model.nu >= 4: - data.ctrl[3] = 0.0 + data.ctrl[3] = max(data.ctrl[3] - 0.05, 0.0) if model.nu >= 5: - data.ctrl[4] = 0.0 + data.ctrl[4] = min(data.ctrl[4] + 0.05, 0.0) + mujoco.mj_step(model, data) viewer.render() step += 1 - # 阶段6:归位(适配原有模型的初始位置) + # 阶段6:平稳归位(缓慢退回,无晃动) step = 0 - while step < 600 and viewer.is_alive: + while step < 700 and viewer.is_alive: ee_pos = np.array([0.0, 0.0, 0.1]) if ee_id >= 0: try: ee_pos = data.site_xpos[ee_id].copy() except: ee_pos = data.xpos[ee_id].copy() - target = np.array([0.0, 0.0, 0.12]) # 微调归位位置 + + target = np.array([0.0, 0.0, 0.15]) # 更高的归位位置,避免碰撞 error = target - ee_pos for i in range(min(3, model.njnt)): - data.ctrl[i] = np.clip(error[i] * 3.5, -1.2, 1.2) + target_ctrl = error[i] * SMOOTH_GAIN * 0.7 + data.ctrl[i] = smooth_control(target_ctrl, last_ctrl, i) + mujoco.mj_step(model, data) viewer.render() step += 1 - print("🎉 适配原有模型的自动抓取完成!") + print("🎉 平滑自动抓取完成!(无卡顿/掉落)") + + +# ===================== 初始化+主程序 ===================== +def init_model_and_viewer(): + if not os.path.exists(MODEL_PATH): + raise FileNotFoundError(f"未找到robot.xml: {MODEL_PATH}") + model = mujoco.MjModel.from_xml_path(MODEL_PATH) + data = mujoco.MjData(model) + mujoco.mj_resetData(model, data) + mujoco.mj_forward(model, data) + + viewer = mujoco_viewer.MujocoViewer(model, data, hide_menus=True) + viewer.cam.distance = 1.8 + viewer.cam.elevation = 12 + viewer.cam.azimuth = 50 + viewer.cam.lookat = [0.15, 0.0, 0.12] + + # 兼容原有模型ID + ee_id, obj_id = -1, -1 + for name in ["ee_site", "ee", "end_effector"]: + ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, name) + if ee_id >= 0: break + if ee_id < 0: + for name in ["ee", "end_effector"]: + ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) + if ee_id >= 0: break + for name in ["target_object", "object", "ball"]: + obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, name) + if obj_id >= 0: break + if obj_id < 0: + for name in ["object_geom", "ball_geom"]: + obj_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) + if obj_id >= 0: break + + print("✅ 初始化完成!(平滑控制模式)") + print("🎮 操作指南:W/S/A/D/Q/E移动(丝滑无抖),空格抓取(渐进加力)") + print(" Z:一键平滑抓取 C:重置 ESC:退出") + return model, data, viewer, ee_id, obj_id -# ===================== 主程序(修复后版本) ===================== def main(): - global viewer # 声明全局变量,让auto_grasp能访问 + global viewer, last_ctrl + last_ctrl = np.zeros(10) # 初始化平滑缓存 model, data, viewer, ee_id, obj_id = init_model_and_viewer() try: while viewer.is_alive: - # 核心修复:用兼容版按键检测替代get_key() check_keyboard_input(viewer) - # 执行控制(适配原有模型) if control_cmd['reset']: mujoco.mj_resetData(model, data) mujoco.mj_forward(model, data) - print("🔄 原有模型已重置到初始状态!") + last_ctrl = np.zeros(10) # 重置平滑缓存 + print("🔄 模型重置完成(平滑缓存已清空)") control_cmd['reset'] = False elif control_cmd['auto']: auto_grasp(model, data, ee_id, obj_id) @@ -339,32 +350,25 @@ def main(): else: manual_control(model, data, ee_id) - # 仿真步进(微调延迟,适配原有模型的帧率) mujoco.mj_step(model, data) viewer.render() - time.sleep(0.004) # 微调延迟,适配原有模型的流畅度 + time.sleep(0.005) # 更慢的帧率,更丝滑 except Exception as e: - print(f"\n❌ 运行出错(适配原有模型时): {e}") + print(f"\n❌ 运行出错: {e}") import traceback - traceback.print_exc() # 打印详细错误栈,方便排查 + traceback.print_exc() finally: with suppress(Exception): viewer.close() - print("\n🔚 程序退出(未修改任何robot.xml内容)") + print("\n🔚 程序退出(未修改robot.xml)") -# ===================== 运行入口 ===================== if __name__ == "__main__": - # 检查依赖(新增glfw检查) try: - import mujoco - import mujoco_viewer - import glfw + import mujoco, mujoco_viewer, glfw except ImportError as e: - missing_lib = str(e).split()[-1] - print(f"❌ 缺少依赖 {missing_lib}!执行以下命令安装:") - print(f" pip install mujoco mujoco-viewer glfw numpy matplotlib") + print(f"❌ 缺少依赖 {str(e).split()[-1]}!执行:") + print(" pip install mujoco mujoco-viewer glfw numpy matplotlib") exit(1) - main() \ No newline at end of file