diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py index e8ca917650..7db2149def 100644 --- a/src/Mechanical_arm_grasping/main.py +++ b/src/Mechanical_arm_grasping/main.py @@ -1,87 +1,66 @@ -# MuJoCo 3.4.0 7自由度协作机械臂（极简版，无传感器，零XML错误） +# MuJoCo 3.4.0 轻量版2D平面机械臂抓取（无传感器，零XML错误） import mujoco import mujoco.viewer import time import numpy as np -def collaborative_robot_arm_demo(): - # 彻底移除所有传感器相关代码，仅保留基础机械臂+抓取逻辑 - cobot_xml = """ - +def simple_2d_robot_arm_demo(): + # 纯2D平面模型，仅保留MuJoCo 3.4.0原生支持标签 + robot_2d_xml = """ + """ - # 2. 加载模型（100%兼容3.4.0，无任何传感器相关错误） + # 加载模型（确保100%兼容MuJoCo 3.4.0） try: - model = mujoco.MjModel.from_xml_string(cobot_xml) + model = mujoco.MjModel.from_xml_string(robot_2d_xml) data = mujoco.MjData(model) - print("✅ 7自由度协作机械臂模型加载成功，启动仿真...") + print("✅ 2D平面机械臂模型加载成功，启动仿真...") except Exception as e: print(f"❌ 模型加载失败：{e}") return - # 3. 获取执行器索引（仅保留基础执行器，无传感器） - # 关节执行器索引 + # 获取执行器索引 joint_idxs = { "joint1": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint1_act"), - "joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act"), - "joint3": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint3_act"), - "joint4": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint4_act"), - "joint5": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint5_act"), - "joint6": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint6_act"), - "joint7": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint7_act"), + "joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_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") - # 4. 核心控制函数（移除力传感器依赖，改用时间控制抓取） - def smooth_joint_control(joint_name, target_angle, duration, viewer): - """平滑关节角度控制""" + # 核心控制函数 + def smooth_joint_move(joint_name, target_angle, duration, viewer): + """平滑移动关节到目标角度""" idx = joint_idxs[joint_name] start_angle = data.ctrl[idx] start_time = time.time() + while (time.time() - start_time) < duration and viewer.is_running(): - t = (time.time() - start_time) / duration - current_angle = start_angle + t * (target_angle - start_angle) + progress = (time.time() - start_time) / duration + current_angle = start_angle + progress * (target_angle - start_angle) data.ctrl[idx] = current_angle - # 打印关节状态（无力度） - print(f"\r{joint_name}角度：{current_angle:.2f} rad", end="") + + # 打印实时状态 + print(f"\r{joint_name} 当前角度：{current_angle:.2f} rad | 目标角度：{target_angle:.2f} rad", end="") + mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) + print() # 换行 - def safe_grasp(viewer): - """安全抓取（通过时间控制闭合，模拟力控效果）""" - print("\n🔧 开始安全抓取（低速度闭合，防止夹碎物体）") - grip_speed = -0.1 # 低速闭合，避免夹碎 + def safe_gripper_close(viewer): + """安全闭合夹爪（低速+定时，模拟力控）""" + print("\n🔧 开始闭合夹爪（安全低速）") + grip_speed = -0.3 start_time = time.time() - # 闭合1.5秒后停止（模拟力控阈值） - while time.time() - start_time < 1.5 and viewer.is_running(): + close_duration = 1.2 # 闭合1.2秒后停止，防止夹碎 + + while (time.time() - start_time) < close_duration and viewer.is_running(): + progress = (time.time() - start_time) / close_duration data.ctrl[left_grip_idx] = grip_speed data.ctrl[right_grip_idx] = -grip_speed - print(f"\r抓取进度：{((time.time() - start_time) / 1.5) * 100:.1f}%", end="") + + 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("\n✅ 抓取完成（已停止闭合，防止夹碎）！") + print("\n✅ 夹爪闭合完成，已锁定目标") - def release_gripper(duration, viewer): - """放松夹爪""" - print("\n🔧 开始放松夹爪") + def gripper_open(duration, viewer): + """张开夹爪""" + print("\n🔧 开始张开夹爪") start_time = time.time() + while (time.time() - start_time) < duration and viewer.is_running(): - data.ctrl[left_grip_idx] = 0.2 # 张开速度 - data.ctrl[right_grip_idx] = -0.2 + data.ctrl[left_grip_idx] = 0.3 + data.ctrl[right_grip_idx] = -0.3 + mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) - print("✅ 夹爪已完全张开") - - # 5. 7自由度机械臂抓取流程 - cobot_steps = [ - ("关节1旋转对准目标", "joint1", 0.87, 3.0), # 50°旋转 - ("关节2俯仰调整高度", "joint2", 0.785, 2.5), # 45°俯仰 - ("关节3俯仰接近目标", "joint3", -0.61, 2.5), # -35°俯仰 - ("关节4腕部旋转校准", "joint4", 1.047, 2.0), # 60°旋转 - ("关节5腕部俯仰调整", "joint5", 0.523, 2.0), # 30°俯仰 - ("关节6腕部偏摆校准", "joint6", 0.349, 2.0), # 20°偏摆 - ("关节7末端旋转对准", "joint7", 0.174, 2.0), # 10°旋转 - ] - - # 6. 启动仿真（纯3.4.0原生逻辑） + + data.ctrl[left_grip_idx] = 0 + data.ctrl[right_grip_idx] = 0 + print("✅ 夹爪已完全张开，目标放置完成") + + # 2D机械臂抓取流程 with mujoco.viewer.launch_passive(model, data) as viewer: - print("\n📌 开始7自由度协作机械臂抓取流程...") + print("\n📌 开始2D平面机械臂抓取流程...") print("-" * 60) - # 第一步：关节运动对准目标 - for step_name, joint_name, target_angle, duration in cobot_steps: - print(f"\n\n🔧 {step_name}") - smooth_joint_control(joint_name, target_angle, duration, viewer) + # 步骤1：关节1旋转对准目标 + print("\n\n🔧 步骤1：基座旋转对准目标") + smooth_joint_move("joint1", 0.0, 2.5, viewer) + + # 步骤2：关节2俯仰接近目标 + print("\n\n🔧 步骤2：大臂俯仰接近目标") + smooth_joint_move("joint2", -0.785, 2.5, viewer) # -45°俯仰 - # 第二步：安全抓取（模拟力控） - safe_grasp(viewer=viewer) + # 步骤3：安全闭合夹爪抓取目标 + safe_gripper_close(viewer) - # 第三步：抬升目标（仅调整关节2） - print("\n\n🔧 抓取成功，抬升目标") - smooth_joint_control("joint2", 1.047, 2.5, viewer) # 60°俯仰抬升 + # 步骤4：抬升目标（关节2回正） + print("\n\n🔧 步骤4：抬升抓取目标") + smooth_joint_move("joint2", 0.0, 2.0, viewer) - # 第四步：归位（关节1旋转回原位） - print("\n\n🔧 旋转归位") - smooth_joint_control("joint1", 0.0, 3.0, viewer) + # 步骤5：基座旋转归位 + print("\n\n🔧 步骤5：机械臂旋转归位") + smooth_joint_move("joint1", 1.57, 3.0, viewer) # 90°旋转归位 - # 第五步：下放目标 - print("\n\n🔧 下放目标") - smooth_joint_control("joint2", 0.785, 2.5, viewer) # 45°俯仰下放 + # 步骤6：下放目标（关节2再次俯仰） + print("\n\n🔧 步骤6：下放抓取目标") + smooth_joint_move("joint2", -0.785, 2.0, viewer) - # 第六步：放松夹爪 - print("\n\n🔧 放松夹爪完成放置") - release_gripper(duration=2.0, viewer=viewer) + # 步骤7：张开夹爪完成放置 + gripper_open(1.5, viewer) - # 保持6秒查看最终效果 - print("\n\n📌 抓取流程完成，保持可视化6秒...") + # 保持可视化5秒 + 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🎉 7自由度协作机械臂抓取演示完毕！") + print("\n\n🎉 2D平面机械臂抓取演示完毕！") if __name__ == "__main__": - collaborative_robot_arm_demo() \ No newline at end of file + simple_2d_robot_arm_demo() \ No newline at end of file