fix: 解决Apply按钮后机械臂摇头问题
- 在IK计算前保存机械臂状态，计算后立即恢复 - 移除Apply配置中的状态恢复逻辑避免冲突 - 删除未使用的inverse_kinematics_static方法 - 保持Apply按钮和路径执行功能的兼容性 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
parent
5efcfb0a9e
commit
edff57a2ad
34
CLAUDE.md
34
CLAUDE.md
@ -205,6 +205,40 @@ wall_position = [2.0, 0.0, 1.0] # 错误！
|
||||
- `src/planning/rrt_star.py` - 删除空文件
|
||||
- 全项目关节顺序转换验证通过
|
||||
|
||||
### Apply按钮机械臂摇头问题（2025-09-14）✅ 已解决
|
||||
|
||||
**问题**：修复Apply按钮后，机械臂在执行路径前会"摇头"几次，这是之前没有的现象
|
||||
|
||||
**根本原因**：
|
||||
- 修复Apply按钮时移除了状态保存恢复机制
|
||||
- PyBullet的`calculateInverseKinematics`函数会实际移动机械臂来计算IK
|
||||
- 路径规划阶段连续计算3次IK（物体→A→B），导致机械臂摇摆
|
||||
|
||||
**解决方案**：
|
||||
- 在IK计算前保存机械臂当前状态
|
||||
- 计算完所有IK后立即恢复机械臂状态
|
||||
- 避免创建临时机器人的复杂方案
|
||||
|
||||
**代码实现**：
|
||||
```python
|
||||
# 保存当前机器人状态
|
||||
saved_joint_positions = self.arm_controller.get_current_joint_positions()
|
||||
|
||||
# 计算各点的逆运动学
|
||||
config_object = self.arm_controller.inverse_kinematics(object_position, seed_angles=saved_joint_positions)
|
||||
config_a = self.arm_controller.inverse_kinematics(point_a, seed_angles=config_object)
|
||||
config_b = self.arm_controller.inverse_kinematics(point_b, seed_angles=config_a)
|
||||
|
||||
# 立即恢复机器人状态
|
||||
self.arm_controller.set_joint_positions(saved_joint_positions)
|
||||
```
|
||||
|
||||
**修复文件**：
|
||||
- `src/gui/main_window.py`（execute_three_stages方法，添加状态保存恢复）
|
||||
- 删除了未使用的`inverse_kinematics_static`方法
|
||||
|
||||
**注意**：保持与Apply按钮修复的兼容性，两个功能都正常工作
|
||||
|
||||
## 致命错误记录（2025-09-14）- 深刻教训
|
||||
|
||||
### 绝对禁止的行为 - 致命错误清单
|
||||
|
||||
2447
simulation_recording_20250914_130048.json
Normal file
2447
simulation_recording_20250914_130048.json
Normal file
File diff suppressed because it is too large
Load Diff
142
simulation_recording_20250914_132401.json
Normal file
142
simulation_recording_20250914_132401.json
Normal file
@ -0,0 +1,142 @@
|
||||
{
|
||||
"metadata": {
|
||||
"timestamp": "2025-09-14T13:23:57.816790",
|
||||
"start_time": 1757827437.8167903,
|
||||
"config": {
|
||||
"robot": {
|
||||
"model_path": "models/manual_robot.urdf",
|
||||
"base_position": [
|
||||
0.0,
|
||||
1.6,
|
||||
0.0
|
||||
],
|
||||
"base_orientation": [
|
||||
0.0,
|
||||
0.0,
|
||||
1.5
|
||||
]
|
||||
},
|
||||
"wall": {
|
||||
"position": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"dimensions": {
|
||||
"width": 3.0,
|
||||
"height": 2.5,
|
||||
"thickness": 0.2
|
||||
},
|
||||
"material": {
|
||||
"color": [
|
||||
0.8,
|
||||
0.8,
|
||||
0.8,
|
||||
1.0
|
||||
],
|
||||
"type": "concrete"
|
||||
}
|
||||
},
|
||||
"hole": {
|
||||
"position_relative_to_wall": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"dimensions": {
|
||||
"width": 2.0,
|
||||
"height": 2.0
|
||||
},
|
||||
"shape": "rectangle"
|
||||
},
|
||||
"task_points": {
|
||||
"point_A": {
|
||||
"position": [
|
||||
0.5,
|
||||
0.5,
|
||||
0.8
|
||||
],
|
||||
"description": "取物点，与机械臂同侧"
|
||||
},
|
||||
"point_B": {
|
||||
"position": [
|
||||
0.5,
|
||||
-0.3,
|
||||
0.8
|
||||
],
|
||||
"description": "目标点，墙体另一侧"
|
||||
}
|
||||
},
|
||||
"transport_object": {
|
||||
"initial_position": [
|
||||
0.6,
|
||||
0.5,
|
||||
0.5
|
||||
],
|
||||
"dimensions": {
|
||||
"width": 0.1,
|
||||
"height": 0.1,
|
||||
"depth": 0.1
|
||||
},
|
||||
"mass": 0.5,
|
||||
"shape": "cube",
|
||||
"material": {
|
||||
"color": [
|
||||
1.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
],
|
||||
"friction": 0.7
|
||||
}
|
||||
},
|
||||
"simulation": {
|
||||
"timestep": 0.01,
|
||||
"gravity": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"real_time": false
|
||||
},
|
||||
"path_planning": {
|
||||
"collision": {
|
||||
"check_resolution": 0.05,
|
||||
"safety_margin": 0.05
|
||||
},
|
||||
"execution": {
|
||||
"velocity_scaling": 1.0,
|
||||
"position_tolerance": 0.02,
|
||||
"motor_max_force": 150.0
|
||||
}
|
||||
}
|
||||
},
|
||||
"task_points": {
|
||||
"point_A": {
|
||||
"position": [
|
||||
0.5,
|
||||
0.5,
|
||||
0.8
|
||||
],
|
||||
"description": "取物点，与机械臂同侧"
|
||||
},
|
||||
"point_B": {
|
||||
"position": [
|
||||
0.5,
|
||||
-0.3,
|
||||
0.8
|
||||
],
|
||||
"description": "目标点，墙体另一侧"
|
||||
}
|
||||
},
|
||||
"version": "1.0",
|
||||
"end_time": 1757827441.8121047,
|
||||
"duration": 3.995314359664917
|
||||
},
|
||||
"frames": [],
|
||||
"path_data": {
|
||||
"planned_path": [],
|
||||
"executed_path": []
|
||||
},
|
||||
"events": []
|
||||
}
|
||||
@ -295,11 +295,6 @@ class MainWindow:
|
||||
self.update_status("Applying configuration...")
|
||||
|
||||
try:
|
||||
# Save current robot state if exists
|
||||
robot_state = None
|
||||
if self.arm_controller and self.arm_controller.is_initialized:
|
||||
robot_state = self.arm_controller.get_current_joint_states()
|
||||
|
||||
# Clear current environment
|
||||
if self.environment:
|
||||
self.environment.cleanup()
|
||||
@ -326,16 +321,7 @@ class MainWindow:
|
||||
|
||||
# Reinitialize path planning components with new arm controller
|
||||
self._initialize_planning_components()
|
||||
|
||||
# Restore robot state if available
|
||||
if robot_state:
|
||||
try:
|
||||
positions = [state['position'] for state in robot_state.values()]
|
||||
self.arm_controller.move_to_joint_positions(positions)
|
||||
except Exception:
|
||||
# State restoration failed, continue with defaults
|
||||
pass
|
||||
|
||||
|
||||
self.update_status("Configuration applied")
|
||||
|
||||
except Exception as e:
|
||||
@ -569,11 +555,11 @@ class MainWindow:
|
||||
|
||||
self.update_status("Planning complete path: Current → Object → A → B...")
|
||||
|
||||
# 获取当前关节配置
|
||||
current_config = self.arm_controller.get_current_joint_positions()
|
||||
# 保存当前机器人状态
|
||||
saved_joint_positions = self.arm_controller.get_current_joint_positions()
|
||||
|
||||
# 计算各点的逆运动学
|
||||
config_object = self.arm_controller.inverse_kinematics(object_position, seed_angles=current_config)
|
||||
config_object = self.arm_controller.inverse_kinematics(object_position, seed_angles=saved_joint_positions)
|
||||
if config_object is None:
|
||||
raise RuntimeError("Cannot find joint configuration for object position")
|
||||
|
||||
@ -585,11 +571,14 @@ class MainWindow:
|
||||
if config_b is None:
|
||||
raise RuntimeError("Cannot find joint configuration for point B")
|
||||
|
||||
# 立即恢复机器人状态
|
||||
self.arm_controller.set_joint_positions(saved_joint_positions)
|
||||
|
||||
# 规划完整路径（忽略物体碰撞，因为要抓取它）
|
||||
self.collision_checker.ignore_transport_object = True
|
||||
|
||||
# 分段规划然后合并
|
||||
path1 = self.path_planner.plan_auto(current_config, config_object)
|
||||
path1 = self.path_planner.plan_auto(saved_joint_positions, config_object)
|
||||
path2 = self.path_planner.plan_auto(config_object, config_a)
|
||||
path3 = self.path_planner.plan_auto(config_a, config_b)
|
||||
|
||||
|
||||
@ -143,7 +143,8 @@ class ArmController:
|
||||
|
||||
except Exception as e:
|
||||
raise RuntimeError(f"Inverse kinematics calculation failed: {e}")
|
||||
|
||||
|
||||
|
||||
def compute_jacobian(self, joint_positions: List[float]) -> np.ndarray:
|
||||
"""Compute Jacobian matrix for given joint configuration"""
|
||||
self._ensure_initialized()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user