diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py index 4a95ab7e0d..3b8b00d148 100644 --- a/src/Mechanical_arm_grasping/main.py +++ b/src/Mechanical_arm_grasping/main.py @@ -1,54 +1,69 @@ -# MuJoCo 3.4.0 多自由度旋转机械臂演示(全新版本) +# MuJoCo 3.4.0 SCARA型机械臂(末端反馈+目标跟随)演示 import mujoco import mujoco.viewer import time +import numpy as np -def multi_dof_robot_arm_demo(): - # 1. 内置多自由度机械臂XML模型(含旋转+升降+伸展+夹爪) - multi_dof_xml = """ - +def scara_robot_arm_demo(): + # 1. 内置SCARA机械臂XML模型(工业常用构型) + scara_xml = """ + """ # 2. 加载模型 try: - model = mujoco.MjModel.from_xml_string(multi_dof_xml) + model = mujoco.MjModel.from_xml_string(scara_xml) data = mujoco.MjData(model) - print("✅ 多自由度机械臂模型加载成功,启动仿真...") + print("✅ SCARA机械臂模型加载成功,启动仿真...") except Exception as e: print(f"❌ 模型加载失败:{e}") return - # 3. 获取所有执行器索引 - rotate_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "rotate_actuator") - lift_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "lift_actuator") - extend_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "extend_actuator") - left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "left_gripper_actuator") - right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_gripper_actuator") - - # 4. 控制函数(新增旋转控制) - def control_rotate(val): - data.ctrl[rotate_idx] = val - - def control_lift(val): - data.ctrl[lift_idx] = val - - def control_extend(val): - data.ctrl[extend_idx] = val + # 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") + 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] + 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) + # 步进仿真 + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(0.001) - def control_gripper(val): - data.ctrl[left_grip_idx] = val - data.ctrl[right_grip_idx] = -val + def smooth_set_gripper(target, duration, viewer): + start_left = data.ctrl[left_grip_idx] + start_right = data.ctrl[right_grip_idx] + target_right = -target + 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) + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(0.001) - # 5. 多自由度动作流程(含旋转调整朝向) - action_sequence = [ - ("旋转调整朝向", control_rotate, 1.0, 2.0), # 水平旋转(朝向黄色标记点) - ("上升准备", control_lift, 0.6, 1.5), - ("伸展接近目标", control_extend, 0.7, 2.0), - ("下降到位", control_lift, 0.2, 1.5), - ("夹紧夹爪", control_gripper, -0.4, 1.0), - ("抓取上升", control_lift, 0.7, 1.5), - ("反向旋转归位", control_rotate, 0.0, 2.0), - ("收缩机械臂", control_extend, 0.0, 2.0), - ("下降放置", control_lift, 0.2, 1.5), - ("放松夹爪", control_gripper, 0.0, 1.0), - ("最终归位", control_lift, 0.5, 1.5), + 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), # 最终归位 ] - # 6. 启动仿真并执行动作(新增关节状态打印) + # 7. 启动仿真 with mujoco.viewer.launch_passive(model, data) as viewer: - # 打印状态表头 - print("\n📊 实时关节状态(旋转角度/升降高度/伸展长度)") - print("-" * 50) - - for action_name, func, target, duration in action_sequence: - print(f"\n🔧 正在执行:{action_name}") - start_time = time.time() - while (time.time() - start_time) < duration and viewer.is_running(): - func(target) - mujoco.mj_step(model, data) - - # 实时打印关键关节状态 - rotate_angle = data.joint("rotate_joint").qpos[0] - lift_height = data.joint("lift_joint").qpos[0] - extend_length = data.joint("extend_joint").qpos[0] - print( - f"\r旋转角度:{rotate_angle:.2f} rad | 升降高度:{lift_height:.2f} m | 伸展长度:{extend_length:.2f} m", - end="") - - viewer.sync() - time.sleep(0.001) - - # 最后保持4秒查看效果 - print("\n\n📌 动作流程完成,保持可视化4秒...") - start = time.time() - while (time.time() - start) < 4 and viewer.is_running(): + print("\n📌 开始SCARA机械臂目标跟随流程...") + print("-" * 60) + + for step_name, joint_or_grip, target, duration in scara_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) + + # 保持5秒查看最终效果 + print("\n\n\n📌 SCARA机械臂操作完成,保持可视化5秒...") + start_hold = time.time() + while (time.time() - start_hold) < 5 and viewer.is_running(): + print_end_effector_position(data, end_effector_id, target_id) mujoco.mj_step(model, data) viewer.sync() time.sleep(0.001) - print("\n🎉 多自由度机械臂演示完毕!") + print("\n\n🎉 SCARA机械臂末端反馈+目标跟随演示完毕!") if __name__ == "__main__": - multi_dof_robot_arm_demo() \ No newline at end of file + scara_robot_arm_demo() \ No newline at end of file