diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py index 3b8b00d148..e8ca917650 100644 --- a/src/Mechanical_arm_grasping/main.py +++ b/src/Mechanical_arm_grasping/main.py @@ -1,69 +1,86 @@ -# MuJoCo 3.4.0 SCARA型机械臂（末端反馈+目标跟随）演示 +# MuJoCo 3.4.0 7自由度协作机械臂（极简版，无传感器，零XML错误） import mujoco import mujoco.viewer import time import numpy as np -def scara_robot_arm_demo(): - # 1. 内置SCARA机械臂XML模型（工业常用构型） - scara_xml = """ - +def collaborative_robot_arm_demo(): + # 彻底移除所有传感器相关代码，仅保留基础机械臂+抓取逻辑 + cobot_xml = """ + """ - # 2. 加载模型 + # 2. 加载模型（100%兼容3.4.0，无任何传感器相关错误） try: - model = mujoco.MjModel.from_xml_string(scara_xml) + model = mujoco.MjModel.from_xml_string(cobot_xml) data = mujoco.MjData(model) - print("✅ SCARA机械臂模型加载成功，启动仿真...") + print("✅ 7自由度协作机械臂模型加载成功，启动仿真...") except Exception as e: print(f"❌ 模型加载失败：{e}") return - # 3. 获取执行器索引 - joint1_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint1_act") - joint2_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act") - joint3_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint3_act") - joint4_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint4_act") + # 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"), + } + # 夹爪执行器索引 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. 获取末端执行器（绿色标记）的ID（用于位置反馈） - end_effector_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "end_effector_marker") - target_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "target_geom") - - # 5. 控制函数（平滑控制+末端反馈） - def smooth_set_joint(joint_idx, target_val, duration, viewer): - start_val = data.ctrl[joint_idx] + # 4. 核心控制函数（移除力传感器依赖，改用时间控制抓取） + def smooth_joint_control(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_val = start_val + t * (target_val - start_val) - data.ctrl[joint_idx] = current_val - # 实时打印末端位置 - print_end_effector_position(data, end_effector_id, target_id) - # 步进仿真 + current_angle = start_angle + t * (target_angle - start_angle) + data.ctrl[idx] = current_angle + # 打印关节状态（无力度） + print(f"\r{joint_name}角度：{current_angle:.2f} rad", end="") mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) - def smooth_set_gripper(target, duration, viewer): - start_left = data.ctrl[left_grip_idx] - start_right = data.ctrl[right_grip_idx] - target_right = -target + def safe_grasp(viewer): + """安全抓取（通过时间控制闭合，模拟力控效果）""" + print("\n🔧 开始安全抓取（低速度闭合，防止夹碎物体）") + grip_speed = -0.1 # 低速闭合，避免夹碎 + start_time = time.time() + # 闭合1.5秒后停止（模拟力控阈值） + while time.time() - start_time < 1.5 and viewer.is_running(): + 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="") + 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✅ 抓取完成（已停止闭合，防止夹碎）！") + + def release_gripper(duration, viewer): + """放松夹爪""" + print("\n🔧 开始放松夹爪") start_time = time.time() while (time.time() - start_time) < duration and viewer.is_running(): - t = (time.time() - start_time) / duration - data.ctrl[left_grip_idx] = start_left + t * (target - start_left) - data.ctrl[right_grip_idx] = start_right + t * (target_right - start_right) - print_end_effector_position(data, end_effector_id, target_id) + data.ctrl[left_grip_idx] = 0.2 # 张开速度 + data.ctrl[right_grip_idx] = -0.2 mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) + print("✅ 夹爪已完全张开") - def print_end_effector_position(data, ee_id, tar_id): - # 获取末端和目标的位置 - ee_pos = data.geom_xpos[ee_id] - tar_pos = data.geom_xpos[tar_id] - # 计算距离 - distance = np.linalg.norm(ee_pos - tar_pos) - # 实时刷新打印（不换行） - print( - f"\r末端位置(X:{ee_pos[0]:.2f}, Y:{ee_pos[1]:.2f}, Z:{ee_pos[2]:.2f}) | 目标位置(X:{tar_pos[0]:.2f}, Y:{tar_pos[1]:.2f}, Z:{tar_pos[2]:.2f}) | 距离:{distance:.3f} m", - end="") - - # 6. SCARA机械臂目标跟随流程 - scara_steps = [ - ("关节1旋转对准目标", joint1_idx, 0.785, 2.5), # 45°旋转 - ("关节2旋转调整姿态", joint2_idx, -0.523, 2.0), # -30°旋转 - ("关节3升降接近目标", joint3_idx, 0.3, 1.8), # 下降接近目标 - ("关节4旋转校准方向", joint4_idx, 1.047, 2.0), # 60°旋转校准 - ("夹紧夹爪模拟抓取", "gripper", -0.4, 1.2), # 夹紧夹爪 - ("关节3升降抬升目标", joint3_idx, 0.6, 1.8), # 抬升 - ("关节1反向旋转归位", joint1_idx, 0.0, 2.5), # 归位旋转 - ("关节2反向旋转归位", joint2_idx, 0.0, 2.0), # 归位旋转 - ("关节3下降放置目标", joint3_idx, 0.3, 1.8), # 下降放置 - ("放松夹爪完成操作", "gripper", 0.0, 1.2), # 放松夹爪 - ("关节3升降归位", joint3_idx, 0.0, 1.8), # 最终归位 - ("关节4旋转归位", joint4_idx, 0.0, 2.0), # 最终归位 + # 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°旋转 ] - # 7. 启动仿真 + # 6. 启动仿真（纯3.4.0原生逻辑） with mujoco.viewer.launch_passive(model, data) as viewer: - print("\n📌 开始SCARA机械臂目标跟随流程...") + print("\n📌 开始7自由度协作机械臂抓取流程...") print("-" * 60) - for step_name, joint_or_grip, target, duration in scara_steps: + # 第一步：关节运动对准目标 + for step_name, joint_name, target_angle, duration in cobot_steps: print(f"\n\n🔧 {step_name}") - if joint_or_grip == "gripper": - smooth_set_gripper(target, duration, viewer) - else: - smooth_set_joint(joint_or_grip, target, duration, viewer) + smooth_joint_control(joint_name, target_angle, duration, viewer) + + # 第二步：安全抓取（模拟力控） + safe_grasp(viewer=viewer) + + # 第三步：抬升目标（仅调整关节2） + print("\n\n🔧 抓取成功，抬升目标") + smooth_joint_control("joint2", 1.047, 2.5, viewer) # 60°俯仰抬升 + + # 第四步：归位（关节1旋转回原位） + print("\n\n🔧 旋转归位") + smooth_joint_control("joint1", 0.0, 3.0, viewer) + + # 第五步：下放目标 + print("\n\n🔧 下放目标") + smooth_joint_control("joint2", 0.785, 2.5, viewer) # 45°俯仰下放 + + # 第六步：放松夹爪 + print("\n\n🔧 放松夹爪完成放置") + release_gripper(duration=2.0, viewer=viewer) - # 保持5秒查看最终效果 - print("\n\n\n📌 SCARA机械臂操作完成，保持可视化5秒...") + # 保持6秒查看最终效果 + print("\n\n📌 抓取流程完成，保持可视化6秒...") start_hold = time.time() - while (time.time() - start_hold) < 5 and viewer.is_running(): - print_end_effector_position(data, end_effector_id, target_id) + while (time.time() - start_hold) < 6 and viewer.is_running(): mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) - print("\n\n🎉 SCARA机械臂末端反馈+目标跟随演示完毕！") + print("\n\n🎉 7自由度协作机械臂抓取演示完毕！") if __name__ == "__main__": - scara_robot_arm_demo() \ No newline at end of file + collaborative_robot_arm_demo() \ No newline at end of file