diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py index 7db2149def..bff3e37182 100644 --- a/src/Mechanical_arm_grasping/main.py +++ b/src/Mechanical_arm_grasping/main.py @@ -1,66 +1,80 @@ -# MuJoCo 3.4.0 轻量版2D平面机械臂抓取（无传感器，零XML错误） +# MuJoCo 3.4.0 桌面分拣机械臂（无传感器，零XML错误，批量分拣演示） import mujoco import mujoco.viewer import time import numpy as np -def simple_2d_robot_arm_demo(): - # 纯2D平面模型，仅保留MuJoCo 3.4.0原生支持标签 - robot_2d_xml = """ - +def desktop_sorting_robot_demo(): + # 纯MuJoCo 3.4.0原生语法，仅保留支持的根标签 + sorting_robot_xml = """ + """ - # 加载模型（确保100%兼容MuJoCo 3.4.0） + # 加载模型（确保无XML错误，适配MuJoCo 3.4.0） try: - model = mujoco.MjModel.from_xml_string(robot_2d_xml) + model = mujoco.MjModel.from_xml_string(sorting_robot_xml) data = mujoco.MjData(model) - print("✅ 2D平面机械臂模型加载成功，启动仿真...") + print("✅ 桌面分拣机械臂模型加载成功，启动仿真...") 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") + "joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act"), + "joint3": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint3_act") } - left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "left_grip_act") - right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_grip_act") + 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 smooth_joint_move(joint_name, target_angle, duration, viewer): - """平滑移动关节到目标角度""" + # 核心控制函数（分段式动作，精准可控） + def precise_joint_control(joint_name, target_val, duration, viewer): + """精准控制关节移动/伸缩到目标值""" idx = joint_idxs[joint_name] - start_angle = data.ctrl[idx] + start_val = data.ctrl[idx] start_time = time.time() while (time.time() - start_time) < duration and viewer.is_running(): progress = (time.time() - start_time) / duration - current_angle = start_angle + progress * (target_angle - start_angle) - data.ctrl[idx] = current_angle + current_val = start_val + progress * (target_val - start_val) + data.ctrl[idx] = current_val - # 打印实时状态 - print(f"\r{joint_name} 当前角度：{current_angle:.2f} rad | 目标角度：{target_angle:.2f} rad", end="") + # 实时打印动作状态 + print(f"\r{joint_name} 当前值：{current_val:.2f} | 目标值：{target_val:.2f}", end="") mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) print() # 换行 - def safe_gripper_close(viewer): - """安全闭合夹爪（低速+定时，模拟力控）""" - print("\n🔧 开始闭合夹爪（安全低速）") + def soft_gripper_close(viewer, target_name): + """软接触闭合夹爪（低速+定时，保护目标物体）""" + print(f"\n🔧 开始闭合夹爪，抓取{target_name}") grip_speed = -0.3 + close_duration = 1.0 # 短时间闭合，避免过夹 start_time = time.time() - close_duration = 1.2 # 闭合1.2秒后停止，防止夹碎 while (time.time() - start_time) < close_duration and viewer.is_running(): progress = (time.time() - start_time) / close_duration @@ -136,17 +152,18 @@ def safe_gripper_close(viewer): viewer.sync() time.sleep(0.001) - # 停止夹爪运动 + # 锁定夹爪，保持抓取状态 data.ctrl[left_grip_idx] = 0 data.ctrl[right_grip_idx] = 0 - print("\n✅ 夹爪闭合完成，已锁定目标") + print(f"\n✅ {target_name} 抓取完成，夹爪已锁定") - def gripper_open(duration, viewer): - """张开夹爪""" - print("\n🔧 开始张开夹爪") + def gripper_open_full(viewer, target_name): + """完全张开夹爪，放置目标物体""" + print(f"\n🔧 开始张开夹爪，放置{target_name}") + open_duration = 0.8 start_time = time.time() - while (time.time() - start_time) < duration and viewer.is_running(): + 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 @@ -154,51 +171,71 @@ def gripper_open(duration, viewer): viewer.sync() time.sleep(0.001) + # 复位夹爪控制 data.ctrl[left_grip_idx] = 0 data.ctrl[right_grip_idx] = 0 - print("✅ 夹爪已完全张开，目标放置完成") + print(f"✅ {target_name} 放置完成，夹爪已复位") - # 2D机械臂抓取流程 - with mujoco.viewer.launch_passive(model, data) as viewer: - print("\n📌 开始2D平面机械臂抓取流程...") - print("-" * 60) + 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) - # 步骤1：关节1旋转对准目标 - print("\n\n🔧 步骤1：基座旋转对准目标") - smooth_joint_move("joint1", 0.0, 2.5, viewer) + # 步骤4：软接触闭合夹爪，抓取目标 + soft_gripper_close(viewer, target_name) - # 步骤2：关节2俯仰接近目标 - print("\n\n🔧 步骤2：大臂俯仰接近目标") - smooth_joint_move("joint2", -0.785, 2.5, viewer) # -45°俯仰 + # 步骤5：抬升机械臂，脱离桌面 + print("\n🔧 步骤5：抬升目标，脱离桌面") + precise_joint_control("joint2", 0.0, 1.8, viewer) - # 步骤3：安全闭合夹爪抓取目标 - safe_gripper_close(viewer) + # 步骤6：旋转基座，对准分拣区域 + print("\n🔧 步骤6：旋转机械臂，对准绿色分拣区域") + precise_joint_control("joint1", 3.14, 3.0, viewer) - # 步骤4：抬升目标（关节2回正） - print("\n\n🔧 步骤4：抬升抓取目标") - smooth_joint_move("joint2", 0.0, 2.0, viewer) + # 步骤7：降低高度，准备放置目标 + print("\n🔧 步骤7：降低目标高度，接近分拣区域") + precise_joint_control("joint2", -0.6, 1.8, viewer) - # 步骤5：基座旋转归位 - print("\n\n🔧 步骤5：机械臂旋转归位") - smooth_joint_move("joint1", 1.57, 3.0, viewer) # 90°旋转归位 + # 步骤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) + + # 启动完整分拣流程 + with mujoco.viewer.launch_passive(model, data) as viewer: + print("\n📌 开始桌面机械臂双目标分拣流程...") + print("-" * 60) - # 步骤6：下放目标（关节2再次俯仰） - print("\n\n🔧 步骤6：下放抓取目标") - smooth_joint_move("joint2", -0.785, 2.0, viewer) + # 第一阶段：分拣红色方块 + sort_single_target(viewer, "红色方块", 0.0, 0.35) - # 步骤7：张开夹爪完成放置 - gripper_open(1.5, viewer) + # 第二阶段：分拣蓝色方块（调整关节参数，对准蓝色目标） + sort_single_target(viewer, "蓝色方块", -0.5, 0.35) - # 保持可视化5秒 - print("\n\n📌 抓取流程全部完成，保持可视化5秒...") + # 保持可视化，查看分拣结果 + print("\n\n📌 双目标分拣流程全部完成，保持可视化6秒...") start_hold = time.time() - while (time.time() - start_hold) < 5 and viewer.is_running(): + while (time.time() - start_hold) < 6 and viewer.is_running(): mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) - print("\n\n🎉 2D平面机械臂抓取演示完毕！") + print("\n\n🎉 桌面分拣机械臂双目标分拣演示完毕！") if __name__ == "__main__": - simple_2d_robot_arm_demo() \ No newline at end of file + desktop_sorting_robot_demo() \ No newline at end of file