diff --git a/src/gui/main_window.py b/src/gui/main_window.py index 0bc8486..cee9e93 100644 --- a/src/gui/main_window.py +++ b/src/gui/main_window.py @@ -571,9 +571,6 @@ class MainWindow: # 第一段执行完毕后打印坐标 self._print_debug_coordinates("第一段执行完毕后") - # 第一段执行完成后退出 - return - # =================== # 第二阶段：物体 → A点 # =================== @@ -644,55 +641,74 @@ class MainWindow: self.start_simulation() def _visualize_path_segment(self, path, color, label): - """可视化单个路径段 - 显示实际执行轨迹""" + """可视化单个路径段 - 使用简单的线段连接路径点""" if not hasattr(self, 'path_visualization'): self.path_visualization = [] # 导入numpy用于插值计算 import numpy as np - # 对每两个waypoint之间进行插值，显示实际轨迹 - for i in range(len(path) - 1): - current_config = path[i] - target_config = path[i + 1] + # 直接使用路径点的末端位置来绘制，避免移动机械臂 + # 使用PyBullet IK来获取每个配置的末端位置 + positions = [] - # 计算插值步数（与path_executor相同的逻辑） - distance = np.linalg.norm(np.array(target_config) - np.array(current_config)) - if distance < 0.001: # 跳过太近的点 - continue + for config in path: + # 使用PyBullet的正向运动学计算末端位置 + # 通过IK的副作用获取末端位置，而不实际移动机械臂 + # 使用calculateInverseKinematics的restPoses参数来设置关节配置 - # 使用与path_executor相同的参数 - velocity_scaling = self.config_loader.get_full_config()['path_planning']['execution']['velocity_scaling'] - timestep = self.config_loader.get_full_config()['simulation']['timestep'] + # 获取当前末端执行器的目标位置 + # 这里我们使用一个技巧：用IK求解当前配置对应的末端位置 + # 首先保存当前状态 + saved_states = [] + for j in range(len(config)): + state = p.getJointState( + self.arm_controller.robot_loader.robot_id, + j, + physicsClientId=self.physics_client + ) + saved_states.append(state[0]) - move_time = distance / velocity_scaling - num_steps = max(5, int(move_time / timestep)) # 至少5步以保证平滑 - - # 插值并绘制 - prev_pos = None - for step in range(num_steps + 1): - ratio = step / num_steps - interpolated = ( - np.array(current_config) * (1 - ratio) + - np.array(target_config) * ratio + # 临时设置关节状态来计算末端位置 + for j, angle in enumerate(config): + p.resetJointState( + self.arm_controller.robot_loader.robot_id, + j, + angle, + targetVelocity=0, + physicsClientId=self.physics_client ) - # 计算插值点的末端位置 - pos, _ = self.arm_controller.forward_kinematics(interpolated.tolist()) + # 获取末端位置 + link_state = p.getLinkState( + self.arm_controller.robot_loader.robot_id, + self.arm_controller.robot_loader.end_effector_index, + physicsClientId=self.physics_client + ) + positions.append(list(link_state[0])) - if prev_pos is not None: - # 画线段 - line_id = p.addUserDebugLine( - prev_pos, pos, - lineColorRGB=color, - lineWidth=2, - physicsClientId=self.physics_client - ) - self.path_visualization.append(line_id) + # 立即恢复原始状态 + for j, angle in enumerate(saved_states): + p.resetJointState( + self.arm_controller.robot_loader.robot_id, + j, + angle, + targetVelocity=0, + physicsClientId=self.physics_client + ) - prev_pos = pos + # 绘制路径线段 + for i in range(len(positions) - 1): + line_id = p.addUserDebugLine( + positions[i], + positions[i + 1], + lineColorRGB=color, + lineWidth=3, + physicsClientId=self.physics_client + ) + self.path_visualization.append(line_id) - self.update_status(f"Visualized {label} path: {len(path)} waypoints with interpolation") + self.update_status(f"Visualized {label} path: {len(path)} waypoints") def _clear_visualizations(self): """清除所有可视化元素""" diff --git a/src/robot/arm_controller.py b/src/robot/arm_controller.py index 153a0a9..947fa77 100644 --- a/src/robot/arm_controller.py +++ b/src/robot/arm_controller.py @@ -135,28 +135,40 @@ class ArmController: except Exception as e: raise RuntimeError(f"Forward kinematics calculation failed: {e}") - def inverse_kinematics(self, target_position: List[float], + def inverse_kinematics(self, target_position: List[float], target_orientation: List[float] = None, seed_angles: List[float] = None) -> Optional[List[float]]: """Calculate inverse kinematics from target pose to joint positions""" self._ensure_initialized() - + # Use current joint angles as seed if not provided if seed_angles is None: seed_angles = self.get_current_joint_positions() - + try: - joint_angles = self.kinematics_engine.inverse_kinematics( - target_position, target_orientation, seed_angles + # 使用PyBullet的IK求解器以确保与仿真环境的一致性 + import pybullet as p + + pb_solution = p.calculateInverseKinematics( + self.robot_loader.robot_id, + self.robot_loader.end_effector_index, + target_position, + targetOrientation=target_orientation, + maxNumIterations=100, + residualThreshold=0.001, + physicsClientId=self.physics_client ) - + + # 转换为列表并截取正确的关节数量 + joint_angles = list(pb_solution)[:self.kinematics_engine.get_num_joints()] + # Validate solution if found if joint_angles is not None: if not self.validate_joint_configuration(joint_angles): return None - + return joint_angles - + except Exception as e: raise RuntimeError(f"Inverse kinematics calculation failed: {e}")