diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py index 882ce9dc20..a5177aa7aa 100644 --- a/src/Robot_arm_motion/robot_arm.py +++ b/src/Robot_arm_motion/robot_arm.py @@ -78,7 +78,7 @@ def __init__(self): self.CAM_DISTANCE = 1.8 # 相机距离 self.CAM_LOOKAT = np.array([0.4, 0.0, 0.1]) # 相机注视点 - # 【优化1】将仿真休眠时间提升为类内常量 + # 仿真控制参数 self.SIMULATION_SLEEP = 1 / 200 # 仿真循环的休眠时间 # 打印模型信息 @@ -129,28 +129,18 @@ def _move_step(self, target: np.ndarray, speed: float = 0.3) -> bool: if error_norm < self.POS_TOLERANCE: return True # 到达目标 - # 计算雅克比矩阵 - jacobian = self._compute_jacobian() # 3×7 - - # ========== 修正：正确的阻尼伪逆计算 ========== - # 方法1：使用正则化参数的伪逆（推荐） + jacobian = self._compute_jacobian() jacobian_pinv = jacobian.T @ np.linalg.inv(jacobian @ jacobian.T + self.JACOBIAN_DAMPING * np.eye(3)) - # 方法2：若方法1仍报错，可改用numpy伪逆（自动处理维度） - # jacobian_pinv = np.linalg.pinv(jacobian, rcond=1e-3) - - # 关节速度指令 joint_vel_cmd = speed * jacobian_pinv @ error joint_vel_cmd = np.clip(joint_vel_cmd, -self.JOINT_VEL_LIMIT, self.JOINT_VEL_LIMIT) - # PD力矩计算 torque = np.zeros(7) for i in range(7): angle_error = joint_vel_cmd[i] * 0.1 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): self.data.ctrl[self.joint_ids[i]] = torque[i] @@ -166,34 +156,30 @@ def _gripper_step(self, pos: float) -> None: j_id = self.model.joint(j_name).id self.data.ctrl[j_id] = pos - def _grab_phase_machine(self): + def _grab_phase_machine(self) -> None: """抓取状态机：按阶段执行机械臂的抓取、移动、放置等一系列动作 状态机分为12个阶段，从初始位置移动→识别立方体→抓取→放置→返回， 每个阶段完成后自动切换到下一个阶段，直到抓取任务完成。 """ if self.current_phase == 0: - # 阶段0：移动到初始位置 if self._move_step(self.INIT_EE_POS): print("\n✅ 到达初始位置") self.current_phase = 1 self.step_counter = 0 elif self.current_phase == 1: - # 阶段1：获取立方体位置 self.cube_pos = self.get_cube_pos() print(f"\n🎯 识别到立方体位置：{np.round(self.cube_pos, 3)}") self.current_phase = 2 elif self.current_phase == 2: - # 阶段2：移动到立方体上方 if self._move_step(self.cube_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4): print("\n✅ 到达立方体上方") self.current_phase = 3 self.step_counter = 0 elif self.current_phase == 3: - # 阶段3：打开夹爪 if self.step_counter == 0: self._gripper_step(self.gripper_open_pos) print("\n✋ 打开夹爪") @@ -203,14 +189,12 @@ def _grab_phase_machine(self): self.step_counter += 1 elif self.current_phase == 4: - # 阶段4：下降抓取 if self._move_step(self.cube_pos + np.array([0, 0, self.grab_height]), speed=0.2): print("\n✅ 下降到抓取高度") self.current_phase = 5 self.step_counter = 0 elif self.current_phase == 5: - # 阶段5：闭合夹爪 if self.step_counter == 0: self._gripper_step(self.gripper_close_pos) print("\n🤏 闭合夹爪抓取") @@ -220,7 +204,6 @@ def _grab_phase_machine(self): self.step_counter += 1 elif self.current_phase == 6: - # 阶段6：抬升立方体 lift_target = self.cube_pos + np.array([0, 0, self.safe_lift_height + self.LIFT_HEIGHT_INCREMENT]) if self._move_step(lift_target, speed=0.3): print("\n✅ 抬升立方体") @@ -228,21 +211,18 @@ def _grab_phase_machine(self): self.step_counter = 0 elif self.current_phase == 7: - # 阶段7：移动到放置点上方 if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4): print("\n✅ 到达放置点上方") self.current_phase = 8 self.step_counter = 0 elif self.current_phase == 8: - # 阶段8：下降放置 if self._move_step(self.target_place_pos + np.array([0, 0, self.grab_height]), speed=0.2): print("\n✅ 下降到放置高度") self.current_phase = 9 self.step_counter = 0 elif self.current_phase == 9: - # 阶段9：释放立方体 if self.step_counter == 0: self._gripper_step(self.gripper_open_pos) print("\n🫳 释放立方体") @@ -252,57 +232,56 @@ def _grab_phase_machine(self): self.step_counter += 1 elif self.current_phase == 10: - # 阶段10：撤离机械臂 if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.3): print("\n✅ 撤离机械臂") self.current_phase = 11 self.step_counter = 0 elif self.current_phase == 11: - # 阶段11：返回初始位置 if self._move_step(self.INIT_EE_POS, speed=0.4): print("\n✅ 返回初始位置") self.current_phase = 12 elif self.current_phase == 12: - # 阶段12：抓取完成 if not self.grab_complete: print("\n" + "=" * 50) print("✅ 智能抓取任务完成！") print("=" * 50) self.grab_complete = True - def run(self): - """单线程仿真主循环""" - # 初始化Viewer - self.viewer = viewer.launch_passive(self.model, self.data) + # 【优化1】新增方法：集中初始化相机参数 + def _init_camera(self) -> None: + """初始化Viewer的相机视角""" self.viewer.cam.azimuth = self.CAM_AZIMUTH self.viewer.cam.elevation = self.CAM_ELEVATION self.viewer.cam.distance = self.CAM_DISTANCE self.viewer.cam.lookat = self.CAM_LOOKAT + def run(self): + """单线程仿真主循环""" + # 初始化Viewer + self.viewer = viewer.launch_passive(self.model, self.data) + + # 【优化2】调用新方法初始化相机 + self._init_camera() + print("\n🚀 仿真已启动，开始自动抓取...") print("💡 关闭Viewer窗口可退出程序") - # 单线程主循环 - # 添加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() - # 【优化2】使用类内常量替代局部变量 time.sleep(self.SIMULATION_SLEEP) except KeyboardInterrupt: print("\n⚠️ 检测到Ctrl+C，正在退出仿真...") - # 清理 self.running = False self.viewer.close() print("\n👋 仿真结束")