diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py index e9377ebed5..974441299e 100644 --- a/src/Robot_arm_motion/robot_arm.py +++ b/src/Robot_arm_motion/robot_arm.py @@ -62,9 +62,12 @@ def __init__(self): # 关节速度参数 self.JOINT_VEL_LIMIT = 0.5 # 关节速度上限 - # 【优化1】提取位置误差容忍阈值为类内常量 + # 位置控制参数 self.POS_TOLERANCE = 0.003 # 末端执行器位置误差容忍阈值 + # 【优化1】提取夹爪等待步数为类内常量 + self.GRIPPER_WAIT_STEPS = 100 # 夹爪动作完成所需的等待步数 + # 打印模型信息 print("=" * 50) print("📌 模型Body列表:", [self.model.body(i).name for i in range(min(self.model.nbody, 10))]) @@ -102,7 +105,6 @@ def _move_step(self, target, speed=0.3): error = target - ee_pos error_norm = np.linalg.norm(error) - # 【优化2】使用类内常量替代硬编码的位置误差阈值 if error_norm < self.POS_TOLERANCE: return True # 到达目标 @@ -144,7 +146,11 @@ def _gripper_step(self, pos): self.data.ctrl[j_id] = pos def _grab_phase_machine(self): - """抓取状态机""" + """抓取状态机:按阶段执行机械臂的抓取、移动、放置等一系列动作 + + 状态机分为12个阶段,从初始位置移动→识别立方体→抓取→放置→返回, + 每个阶段完成后自动切换到下一个阶段,直到抓取任务完成。 + """ if self.current_phase == 0: # 阶段0:移动到初始位置 if self._move_step(np.array([0.4, 0.0, 0.2])): @@ -170,7 +176,8 @@ def _grab_phase_machine(self): if self.step_counter == 0: self._gripper_step(self.gripper_open_pos) print("\n✋ 打开夹爪") - if self.step_counter > 100: # 等待夹爪动作 + # 【优化2】使用类内常量替代硬编码的等待步数 + if self.step_counter > self.GRIPPER_WAIT_STEPS: self.current_phase = 4 self.step_counter = 0 self.step_counter += 1 @@ -187,7 +194,8 @@ def _grab_phase_machine(self): if self.step_counter == 0: self._gripper_step(self.gripper_close_pos) print("\n🤏 闭合夹爪抓取") - if self.step_counter > 100: + # 【优化2】使用类内常量替代硬编码的等待步数 + if self.step_counter > self.GRIPPER_WAIT_STEPS: self.current_phase = 6 self.step_counter = 0 self.step_counter += 1 @@ -218,7 +226,8 @@ def _grab_phase_machine(self): if self.step_counter == 0: self._gripper_step(self.gripper_open_pos) print("\n🫳 释放立方体") - if self.step_counter > 100: + # 【优化2】使用类内常量替代硬编码的等待步数 + if self.step_counter > self.GRIPPER_WAIT_STEPS: self.current_phase = 10 self.step_counter = 0 self.step_counter += 1