diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py index bff3e37182..c311683975 100644 --- a/src/Mechanical_arm_grasping/main.py +++ b/src/Mechanical_arm_grasping/main.py @@ -1,79 +1,74 @@ -# MuJoCo 3.4.0 桌面分拣机械臂（无传感器，零XML错误，批量分拣演示） +# MuJoCo 3.4.0 带自动复位的3自由度机械臂精准取放（无传感器，零XML错误） import mujoco import mujoco.viewer import time import numpy as np -def desktop_sorting_robot_demo(): - # 纯MuJoCo 3.4.0原生语法，仅保留支持的根标签 - sorting_robot_xml = """ - +def robot_arm_auto_reset_demo(): + # 纯MuJoCo 3.4.0原生语法，无任何高版本扩展标签 + robot_xml = """ + """ - # 加载模型（确保无XML错误，适配MuJoCo 3.4.0） + # 加载模型（确保零XML错误） try: - model = mujoco.MjModel.from_xml_string(sorting_robot_xml) + model = mujoco.MjModel.from_xml_string(robot_xml) data = mujoco.MjData(model) - print("✅ 桌面分拣机械臂模型加载成功，启动仿真...") + print("✅ 3自由度机械臂模型加载成功，启动仿真...") except Exception as e: print(f"❌ 模型加载失败：{e}") return - # 获取执行器索引（原生API，无兼容问题） + # 获取执行器索引 joint_idxs = { "joint1": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint1_act"), "joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act"), @@ -114,9 +109,10 @@ def desktop_sorting_robot_demo(): left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "gripper_left_act") right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "gripper_right_act") - # 核心控制函数（分段式动作，精准可控） - def precise_joint_control(joint_name, target_val, duration, viewer): - """精准控制关节移动/伸缩到目标值""" + # ---------------------- 模块化功能函数 ---------------------- + def joint_move(joint_name, target_val, duration, viewer, step_desc): + """单关节精准移动""" + print(f"\n🔧 {step_desc}") idx = joint_idxs[joint_name] start_val = data.ctrl[idx] start_time = time.time() @@ -126,19 +122,18 @@ def precise_joint_control(joint_name, target_val, duration, viewer): current_val = start_val + progress * (target_val - start_val) data.ctrl[idx] = current_val - # 实时打印动作状态 - print(f"\r{joint_name} 当前值：{current_val:.2f} | 目标值：{target_val:.2f}", end="") - + print(f"\r{joint_name} 进度：{progress * 100:.1f}% | 当前值：{current_val:.2f}", end="") mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) print() # 换行 + return True - def soft_gripper_close(viewer, target_name): - """软接触闭合夹爪（低速+定时，保护目标物体）""" - print(f"\n🔧 开始闭合夹爪，抓取{target_name}") - grip_speed = -0.3 - close_duration = 1.0 # 短时间闭合，避免过夹 + def gripper_close(viewer, desc="目标"): + """软接触闭合夹爪""" + print(f"\n🔧 闭合夹爪抓取{desc}") + grip_speed = -0.25 + close_duration = 1.0 start_time = time.time() while (time.time() - start_time) < close_duration and viewer.is_running(): @@ -147,95 +142,106 @@ def soft_gripper_close(viewer, target_name): data.ctrl[right_grip_idx] = -grip_speed print(f"\r夹爪闭合进度：{progress * 100:.1f}%", end="") - mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) - # 锁定夹爪，保持抓取状态 data.ctrl[left_grip_idx] = 0 data.ctrl[right_grip_idx] = 0 - print(f"\n✅ {target_name} 抓取完成，夹爪已锁定") + print(f"\n✅ {desc} 抓取完成，夹爪锁定") + return True - def gripper_open_full(viewer, target_name): - """完全张开夹爪，放置目标物体""" - print(f"\n🔧 开始张开夹爪，放置{target_name}") + def gripper_open(viewer, desc="目标"): + """张开夹爪放置目标""" + print(f"\n🔧 张开夹爪放置{desc}") open_duration = 0.8 start_time = time.time() while (time.time() - start_time) < open_duration and viewer.is_running(): - data.ctrl[left_grip_idx] = 0.3 - data.ctrl[right_grip_idx] = -0.3 - + data.ctrl[left_grip_idx] = 0.25 + data.ctrl[right_grip_idx] = -0.25 mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) - # 复位夹爪控制 data.ctrl[left_grip_idx] = 0 data.ctrl[right_grip_idx] = 0 - print(f"✅ {target_name} 放置完成，夹爪已复位") - - def sort_single_target(viewer, target_name, joint1_angle, joint3_extend): - """分拣单个目标物体的完整流程""" - # 步骤1：旋转基座对准目标 - print(f"\n\n===== 开始分拣 {target_name} =====") - print("\n🔧 步骤1：旋转基座对准目标") - precise_joint_control("joint1", joint1_angle, 2.5, viewer) - - # 步骤2：俯仰大臂降低高度 - print("\n🔧 步骤2：降低机械臂高度，接近目标") - precise_joint_control("joint2", -0.6, 2.0, viewer) - - # 步骤3：伸缩小臂靠近目标 - print("\n🔧 步骤3：伸缩小臂，对准目标中心") - precise_joint_control("joint3", joint3_extend, 2.0, viewer) - - # 步骤4：软接触闭合夹爪，抓取目标 - soft_gripper_close(viewer, target_name) - - # 步骤5：抬升机械臂，脱离桌面 - print("\n🔧 步骤5：抬升目标，脱离桌面") - precise_joint_control("joint2", 0.0, 1.8, viewer) - - # 步骤6：旋转基座，对准分拣区域 - print("\n🔧 步骤6：旋转机械臂，对准绿色分拣区域") - precise_joint_control("joint1", 3.14, 3.0, viewer) - - # 步骤7：降低高度，准备放置目标 - print("\n🔧 步骤7：降低目标高度，接近分拣区域") - precise_joint_control("joint2", -0.6, 1.8, viewer) - - # 步骤8：张开夹爪，放置目标 - gripper_open_full(viewer, target_name) - - # 步骤9：复位机械臂，准备下一个目标 - print("\n🔧 步骤9：复位机械臂，准备分拣下一个目标") - precise_joint_control("joint2", 0.0, 1.5, viewer) - precise_joint_control("joint3", 0.0, 1.5, viewer) - precise_joint_control("joint1", 0.0, 2.0, viewer) - - # 启动完整分拣流程 + print(f"✅ {desc} 放置完成，夹爪复位") + return True + + def robot_auto_reset(viewer): + """机械臂自动复位到初始位置""" + print("\n\n🔧 开始机械臂自动复位") + # 步骤1：抬升大臂 + joint_move("joint2", 0.0, 1.5, viewer, "复位：抬升大臂") + # 步骤2：收缩小臂 + joint_move("joint3", 0.0, 1.5, viewer, "复位：收缩小臂") + # 步骤3：旋转基座回正 + joint_move("joint1", 0.0, 2.0, viewer, "复位：基座回正") + print("✅ 机械臂已完成自动复位，准备下一次抓取") + return True + + def grab_and_place(viewer, retry_max=2): + """完整取放流程（含自动重试）""" + retry_count = 0 + success = False + + while retry_count < retry_max and not success: + print(f"\n\n===== 开始第 {retry_count + 1} 次抓取尝试 =====") + try: + # 阶段1：对准目标 + joint_move("joint1", 0.0, 2.0, viewer, "步骤1：旋转基座对准蓝色目标") + joint_move("joint2", -0.7, 2.0, viewer, "步骤2：俯仰大臂接近目标") + joint_move("joint3", 0.35, 2.0, viewer, "步骤3：伸缩小臂对准目标") + + # 阶段2：抓取目标 + gripper_close(viewer, "蓝色球体") + + # 阶段3：抬升并转移目标 + joint_move("joint2", 0.0, 1.5, viewer, "步骤4：抬升目标脱离平台") + joint_move("joint1", 3.14, 2.5, viewer, "步骤5：旋转基座对准绿色放置区域") + joint_move("joint2", -0.7, 1.5, viewer, "步骤6：降低目标接近放置区域") + + # 阶段4：放置目标 + gripper_open(viewer, "蓝色球体") + + # 抓取成功，退出重试循环 + success = True + print("\n\n🎉 第 {retry_count+1} 次抓取尝试成功！") + except Exception as e: + retry_count += 1 + print(f"\n❌ 第 {retry_count} 次抓取失败：{e}，准备重试...") + robot_auto_reset(viewer) + + if not success: + print(f"\n❌ 已达到最大重试次数（{retry_max}次），抓取失败") + return success + + # ---------------------- 启动主流程 ---------------------- with mujoco.viewer.launch_passive(model, data) as viewer: - print("\n📌 开始桌面机械臂双目标分拣流程...") + print("\n📌 开始带自动复位的机械臂精准取放流程...") print("-" * 60) - # 第一阶段：分拣红色方块 - sort_single_target(viewer, "红色方块", 0.0, 0.35) + # 执行完整取放流程 + grab_success = grab_and_place(viewer) - # 第二阶段：分拣蓝色方块（调整关节参数，对准蓝色目标） - sort_single_target(viewer, "蓝色方块", -0.5, 0.35) + # 无论成功与否，最终执行自动复位 + if grab_success: + robot_auto_reset(viewer) + else: + print("\n🔧 强制执行机械臂自动复位") + robot_auto_reset(viewer) - # 保持可视化，查看分拣结果 - print("\n\n📌 双目标分拣流程全部完成，保持可视化6秒...") + # 保持可视化查看结果 + print("\n\n📌 流程结束，保持可视化5秒...") start_hold = time.time() - while (time.time() - start_hold) < 6 and viewer.is_running(): + while (time.time() - start_hold) < 5 and viewer.is_running(): mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) - print("\n\n🎉 桌面分拣机械臂双目标分拣演示完毕！") + print("\n\n🎉 3自由度机械臂自动复位取放演示完毕！") if __name__ == "__main__": - desktop_sorting_robot_demo() \ No newline at end of file + robot_arm_auto_reset_demo() \ No newline at end of file