diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py index 548c07660f..b6bc64b457 100644 --- a/src/Robot_arm_motion/robot_arm.py +++ b/src/Robot_arm_motion/robot_arm.py @@ -70,10 +70,14 @@ def __init__(self): # 位置坐标参数 self.INIT_EE_POS = np.array([0.4, 0.0, 0.2]) # 末端执行器初始目标位置 - - # 【优化1】提取抬升高度增量为类内常量 self.LIFT_HEIGHT_INCREMENT = 0.05 # 抓取后额外抬升的高度增量 + # 【优化1】提取相机视角参数为类内常量 + self.CAM_AZIMUTH = 70 # 相机方位角 + self.CAM_ELEVATION = -25 # 相机仰角 + self.CAM_DISTANCE = 1.8 # 相机距离 + self.CAM_LOOKAT = np.array([0.4, 0.0, 0.1]) # 相机注视点 + # 打印模型信息 print("=" * 50) print("📌 模型Body列表:", [self.model.body(i).name for i in range(min(self.model.nbody, 10))]) @@ -105,7 +109,7 @@ def _compute_jacobian(self) -> np.ndarray: mujoco.mj_jac(self.model, self.data, self.jacp, self.jacr, self.get_ee_pos(), self.ee_body_id) return self.jacp[:, self.joint_ids] - def _move_step(self, target, speed=0.3): + def _move_step(self, target: np.ndarray, speed: float = 0.3) -> bool: """单步位置控制:基于雅克比伪逆实现末端执行器的位置跟踪 Args: @@ -214,7 +218,6 @@ def _grab_phase_machine(self): elif self.current_phase == 6: # 阶段6:抬升立方体 - # 【优化2】使用类内常量替代硬编码的抬升增量 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✅ 抬升立方体") @@ -270,10 +273,11 @@ def run(self): """单线程仿真主循环""" # 初始化Viewer self.viewer = viewer.launch_passive(self.model, self.data) - self.viewer.cam.azimuth = 70 - self.viewer.cam.elevation = -25 - self.viewer.cam.distance = 1.8 - self.viewer.cam.lookat = np.array([0.4, 0.0, 0.1]) + # 【优化2】使用类内常量替代硬编码的相机参数 + 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 print("\n🚀 仿真已启动,开始自动抓取...") print("💡 关闭Viewer窗口可退出程序")