diff --git a/scripts/robot_arm.py b/scripts/robot_arm.py new file mode 100644 index 0000000000..c12ad7a153 --- /dev/null +++ b/scripts/robot_arm.py @@ -0,0 +1,68 @@ +""" +MuJoCo机械臂仿真初始版本 +功能:基础2关节机械臂可视化+关节角度控制 +项目根目录:Robot_arm_motion +代码目录:scripts +作者:邓卓 +日期:2025-12-22 +""" +import mujoco +import mujoco.viewer as viewer +import numpy as np + +def create_simple_arm_model(): + """创建简易2关节机械臂+立方体模型的XML字符串""" + xml = """ + + + """ + return mujoco.MjModel.from_xml_string(xml) + +def main(): + """主函数:加载模型+启动仿真""" + # 加载模型 + model = create_simple_arm_model() + data = mujoco.MjData(model) + + # 设置初始关节角度 + joint1_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "joint1") + joint2_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "joint2") + data.ctrl[joint1_id] = np.pi/4 # 关节1转45° + data.ctrl[joint2_id] = -np.pi/6 # 关节2转-30° + + # 启动可视化 + print("=== Robot_arm_motion 初始版本仿真启动 ===") + with viewer.launch_passive(model, data) as v: + while v.is_running(): + mujoco.mj_step(model, data) + v.sync() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py index ec35bf8ad0..213f40b563 100644 --- a/src/Robot_arm_motion/robot_arm.py +++ b/src/Robot_arm_motion/robot_arm.py @@ -5,6 +5,12 @@ import sys import os +# 【优化1】添加文件头注释,标注项目信息 +""" +Franka Panda 机械臂自动抓取仿真 v1.1 +基于MuJoCo实现的基础抓取控制器 +""" + # ========== 路径适配 ========== SCENE_PATH = os.path.join(os.path.dirname(__file__), "mujoco_menagerie-main", @@ -220,18 +226,25 @@ def run(self): print("\n🚀 仿真已启动,开始自动抓取...") print("💡 关闭Viewer窗口可退出程序") + # 【优化2】提取休眠时间为常量,便于后续调整 + SIMULATION_SLEEP = 1/200 + # 单线程主循环 - while self.viewer.is_running(): - if self.running and not self.grab_complete: - self._grab_phase_machine() - else: - # 抓取完成后归零力矩 - for i in range(7): - self.data.ctrl[self.joint_ids[i]] = 0 - - mujoco.mj_step(self.model, self.data) - self.viewer.sync() - time.sleep(1/200) + # 【优化3】添加KeyboardInterrupt捕获,支持Ctrl+C优雅退出 + try: + while self.viewer.is_running(): + if self.running and not self.grab_complete: + self._grab_phase_machine() + else: + # 抓取完成后归零力矩 + for i in range(7): + self.data.ctrl[self.joint_ids[i]] = 0 + + mujoco.mj_step(self.model, self.data) + self.viewer.sync() + time.sleep(SIMULATION_SLEEP) + except KeyboardInterrupt: + print("\n⚠️ 检测到Ctrl+C,正在退出仿真...") # 清理 self.running = False