Doni/RoboticArmTest
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: 修复execute_path方法使用已规划路径而非重新规划

Browse Source 
- 修改execute_path()方法使其直接使用self.planned_path中的已优化路径
- 通过self.object_index和self.point_a_index正确分段执行三个阶段
- 消除重复规划导致的性能浪费和逻辑不一致问题
- 确保执行的路径与用户看到的可视化路径完全一致
- 保留原有的夹爪控制和碰撞检测逻辑

🤖 Generated with [Claude Code](https://claude.ai/code)

Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
sladro 2025-09-13 16:49:30 +08:00
parent bf8882bfd9
commit ed6593a4f3
1 changed files with 26 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
src/gui/main_window.py
View File

@ -561,82 +561,61 @@ class MainWindow:
self.start_simulation()
def execute_path(self):
"""Execute three independent path segments"""
self.update_status("Starting three-stage independent execution...")
"""Execute the planned path using pre-planned segments"""
self.update_status("Starting three-stage execution using planned path...")
try:
# Get task points and object position
task_points = self.config_loader.get_task_points()
point_a = task_points['point_A']['position']
point_b = task_points['point_B']['position']
transport_object_config = self.config_loader.get_full_config()['transport_object']
object_position = transport_object_config['initial_position']
# 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")
# Check if planned path exists
if self.planned_path is None:
raise RuntimeError("No planned path available. Please run path planning first.")
# Stage 1: Execute path to object using pre-planned segment
self.update_status("Stage 1: Moving to object...")
path_to_object = self.planned_path[:self.object_index + 1]
self.collision_checker.ignore_transport_object = True
try:
path_to_object = self.path_planner.plan_auto(current_config, config_object)
success = self.path_executor.execute_path(path_to_object, gripper_action=None)
finally:
self.collision_checker.ignore_transport_object = False
self.update_status("Stage 1: Moving to object...")
success = self.path_executor.execute_path(path_to_object, gripper_action=None)
if not success:
raise RuntimeError("Stage 1 failed: Could not reach object")
# Close gripper
self.update_status("Closing gripper at object...")
if hasattr(self.arm_controller, 'close_gripper'):
self.arm_controller.close_gripper()
import time
time.sleep(0.5)
# After gripping, ignore transport object collision
self.collision_checker.ignore_transport_object = True
# Stage 2: Object to A (independent planning & execution)
self.update_status("Stage 2: Planning path to point A...")
current_config = self.arm_controller.get_current_joint_positions()
config_a = self.arm_controller.inverse_kinematics(point_a, seed_angles=current_config)
if config_a is None:
raise RuntimeError("Cannot find joint configuration for point A")
path_to_a = self.path_planner.plan_auto(current_config, config_a)
# Stage 2: Execute path from object to A using pre-planned segment
self.update_status("Stage 2: Moving to point A...")
path_to_a = self.planned_path[self.object_index:self.point_a_index + 1]
success = self.path_executor.execute_path(path_to_a, gripper_action=None)
if not success:
raise RuntimeError("Stage 2 failed: Could not reach point A")
# Stage 3: A to B (independent planning & execution)
self.update_status("Stage 3: Planning path to point B...")
current_config = self.arm_controller.get_current_joint_positions()
config_b = self.arm_controller.inverse_kinematics(point_b, seed_angles=current_config)
if config_b is None:
raise RuntimeError("Cannot find joint configuration for point B")
path_to_b = self.path_planner.plan_auto(current_config, config_b)
# Stage 3: Execute path from A to B using pre-planned segment
self.update_status("Stage 3: Moving to point B...")
path_to_b = self.planned_path[self.point_a_index:]
success = self.path_executor.execute_path(path_to_b, gripper_action=None)
if not success:
raise RuntimeError("Stage 3 failed: Could not reach point B")
# Open gripper at B
self.update_status("Opening gripper at point B...")
if hasattr(self.arm_controller, 'open_gripper'):
self.arm_controller.open_gripper()
time.sleep(0.5)
self.update_status("All three stages completed successfully!")
except Exception as e:
import traceback
self.update_status(f"Path execution error: {str(e)}")