diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py index 213f40b563..31cbafb06c 100644 --- a/src/Robot_arm_motion/robot_arm.py +++ b/src/Robot_arm_motion/robot_arm.py @@ -5,7 +5,6 @@ import sys import os -# 【优化1】添加文件头注释,标注项目信息 """ Franka Panda 机械臂自动抓取仿真 v1.1 基于MuJoCo实现的基础抓取控制器 @@ -24,6 +23,7 @@ # ========== 智能抓取控制器 ========== class PandaAutoGrab: def __init__(self): + """初始化Franka Panda机械臂抓取控制器,加载模型和初始化参数""" self.model = mujoco.MjModel.from_xml_path(SCENE_PATH) self.data = mujoco.MjData(self.model) self.viewer = None @@ -50,6 +50,11 @@ def __init__(self): self.safe_lift_height = 0.15 self.grab_height = 0.05 + # 【优化1】提取PD控制参数为类内常量 + self.PD_KP = 250 # 比例增益 + self.PD_KD = 100 # 微分增益 + self.TORQUE_LIMIT = 20 # 力矩限制 + # 打印模型信息 print("="*50) print("📌 模型Body列表:", [self.model.body(i).name for i in range(min(self.model.nbody, 10))]) @@ -97,8 +102,9 @@ def _move_step(self, target, tol=0.003, speed=0.3): torque = np.zeros(7) for i in range(7): angle_error = joint_vel_cmd[i] * 0.1 - torque[i] = 250 * angle_error - 100 * self.data.qvel[self.joint_ids[i]] - torque[i] = np.clip(torque[i], -20, 20) + # 【优化2】使用类内常量替代硬编码的PD参数 + torque[i] = self.PD_KP * angle_error - self.PD_KD * self.data.qvel[self.joint_ids[i]] + torque[i] = np.clip(torque[i], -self.TORQUE_LIMIT, self.TORQUE_LIMIT) # 设置关节力矩 for i in range(7): @@ -226,11 +232,11 @@ def run(self): print("\n🚀 仿真已启动,开始自动抓取...") print("💡 关闭Viewer窗口可退出程序") - # 【优化2】提取休眠时间为常量,便于后续调整 + # 提取休眠时间为常量,便于后续调整 SIMULATION_SLEEP = 1/200 # 单线程主循环 - # 【优化3】添加KeyboardInterrupt捕获,支持Ctrl+C优雅退出 + # 添加KeyboardInterrupt捕获,支持Ctrl+C优雅退出 try: while self.viewer.is_running(): if self.running and not self.grab_complete: