clean: 移除调试输出,保持代码整洁
移除临时调试信息: - main_window.py: 移除第一阶段坐标对比调试输出 - path_executor.py: 移除waypoint执行过程调试信息 保留核心功能修复: - RRT*精度改进 (GOAL_TOLERANCE) - 路径执行仿真步进优化 (10x simulation steps) 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
parent
627a6b5045
commit
bf8882bfd9
@ -575,6 +575,7 @@ class MainWindow:
|
||||
# Stage 1: Current position to object (independent planning & execution)
|
||||
self.update_status("Stage 1: Planning path to object...")
|
||||
current_config = self.arm_controller.get_current_joint_positions()
|
||||
|
||||
config_object = self.arm_controller.inverse_kinematics(object_position, seed_angles=current_config)
|
||||
if config_object is None:
|
||||
raise RuntimeError("Cannot find joint configuration for object position")
|
||||
|
||||
@ -114,11 +114,6 @@ class PathExecutor:
|
||||
np.array(target_config) - np.array(final_config)
|
||||
)
|
||||
if final_distance <= self.position_tolerance:
|
||||
# 输出成功到达的调试信息
|
||||
target_pos, _ = self.arm_controller.forward_kinematics(target_config)
|
||||
actual_pos, _ = self.arm_controller.get_current_pose()
|
||||
pos_error = np.linalg.norm(np.array(target_pos) - np.array(actual_pos))
|
||||
print(f"waypoint {getattr(self, '_current_waypoint', 'N')} 成功: 关节误差{final_distance:.4f}, 末端位置误差{pos_error:.4f}m")
|
||||
return True
|
||||
# 执行多次仿真步进让机械臂继续收敛
|
||||
for _ in range(10):
|
||||
@ -127,13 +122,6 @@ class PathExecutor:
|
||||
|
||||
final_config = self.arm_controller.get_current_joint_positions()
|
||||
final_distance = np.linalg.norm(np.array(target_config) - np.array(final_config))
|
||||
|
||||
# 输出调试信息
|
||||
target_pos, _ = self.arm_controller.forward_kinematics(target_config)
|
||||
actual_pos, _ = self.arm_controller.get_current_pose()
|
||||
pos_error = np.linalg.norm(np.array(target_pos) - np.array(actual_pos))
|
||||
print(f"waypoint {getattr(self, '_current_waypoint', 'X')} 失败: 关节误差{final_distance:.4f}, 末端位置误差{pos_error:.4f}m")
|
||||
|
||||
raise RuntimeError(f"Failed to reach target, error: {final_distance}")
|
||||
|
||||
def _close_gripper(self):
|
||||
|
||||
Loading…
Reference in New Issue
Block a user