From edff57a2ad965cb6d0b020f5d2f77fdcc382b4f5 Mon Sep 17 00:00:00 2001
From: sladro <sladro@163.com>
Date: Sun, 14 Sep 2025 13:35:24 +0800
Subject: [PATCH] =?UTF-8?q?fix:=20=E8=A7=A3=E5=86=B3Apply=E6=8C=89?=
 =?UTF-8?q?=E9=92=AE=E5=90=8E=E6=9C=BA=E6=A2=B0=E8=87=82=E6=91=87=E5=A4=B4?=
 =?UTF-8?q?=E9=97=AE=E9=A2=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- 在IK计算前保存机械臂状态，计算后立即恢复
- 移除Apply配置中的状态恢复逻辑避免冲突
- 删除未使用的inverse_kinematics_static方法
- 保持Apply按钮和路径执行功能的兼容性

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

Co-Authored-By: Claude <noreply@anthropic.com>
---
 CLAUDE.md                                 |   34 +
 simulation_recording_20250914_130048.json | 2447 +++++++++++++++++++++
 simulation_recording_20250914_132401.json |  142 ++
 src/gui/main_window.py                    |   27 +-
 src/robot/arm_controller.py               |    3 +-
 5 files changed, 2633 insertions(+), 20 deletions(-)
 create mode 100644 simulation_recording_20250914_130048.json
 create mode 100644 simulation_recording_20250914_132401.json

diff --git a/CLAUDE.md b/CLAUDE.md
index 9e9e73c..ca1c66e 100644
--- a/CLAUDE.md
+++ b/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）- 深刻教训
 
 ### 绝对禁止的行为 - 致命错误清单
diff --git a/simulation_recording_20250914_130048.json b/simulation_recording_20250914_130048.json
new file mode 100644
index 0000000..8232e03
--- /dev/null
+++ b/simulation_recording_20250914_130048.json
@@ -0,0 +1,2447 @@
+{
+  "metadata": {
+    "timestamp": "2025-09-14T13:00:29.800464",
+    "start_time": 1757826029.8004642,
+    "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": 1757826048.5513368,
+    "duration": 18.75087261199951
+  },
+  "frames": [
+    {
+      "time": 6.2113635540008545,
+      "joint_positions": [
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0,
+        0.0
+      ],
+      "end_effector_position": [
+        1.885265524681663,
+        1.466306688848041,
+        0.6200000000000001
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.2749342918396,
+      "joint_positions": [
+        0.01535560441403731,
+        -0.019014502236859687,
+        0.019708961856605416,
+        -0.01874913528079449,
+        -0.030162469119821674,
+        -0.03237507308054976,
+        0.0012887557209885763,
+        0.009889942560317623,
+        -3.101810550525917e-05
+      ],
+      "end_effector_position": [
+        1.8736926358703125,
+        1.3973470450464154,
+        0.6148065562994576
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.337804794311523,
+      "joint_positions": [
+        0.03187356128035159,
+        -0.03695994426033407,
+        0.03957097764266352,
+        -0.03685872933414427,
+        -0.06004558723402192,
+        -0.06601544694405798,
+        0.0019493182413613168,
+        0.019719349185677384,
+        -0.0012266939929601289
+      ],
+      "end_effector_position": [
+        1.8576363417508768,
+        1.330925400902133,
+        0.6072883169971324
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.382634878158569,
+      "joint_positions": [
+        0.04117573484604341,
+        -0.044118033196525346,
+        0.04914831144333226,
+        -0.045513379721110546,
+        -0.0751458777555792,
+        -0.08348462619832515,
+        0.0014832315918830324,
+        0.024290616577583087,
+        -0.003236633469837189
+      ],
+      "end_effector_position": [
+        1.8484081263148844,
+        1.299614742194952,
+        0.600957010036915
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.4284350872039795,
+      "joint_positions": [
+        0.050505550109207174,
+        -0.051230936090504504,
+        0.05871515178720852,
+        -0.05418468617254157,
+        -0.09028966149491413,
+        -0.10096998243311478,
+        0.0009973159309473669,
+        0.028853357623906815,
+        -0.005283932324040408
+      ],
+      "end_effector_position": [
+        1.8380138247535964,
+        1.2684986274590242,
+        0.5946030575928513
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.474335432052612,
+      "joint_positions": [
+        0.05983650250502212,
+        -0.058341906982183445,
+        0.06828143975041301,
+        -0.06285595464788094,
+        -0.10543415615395846,
+        -0.11845604568292059,
+        0.0005105596952485706,
+        0.03341574998503714,
+        -0.007332548296967388
+      ],
+      "end_effector_position": [
+        1.8264638568768234,
+        1.2376534029799218,
+        0.5883060405176564
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.55173134803772,
+      "joint_positions": [
+        0.07849851783896857,
+        -0.07256362157617656,
+        0.08741386687224692,
+        -0.08019816837547183,
+        -0.13572272325396464,
+        -0.1534282962278657,
+        -0.00046302555254618117,
+        0.042540543932744125,
+        -0.011429161842264427
+      ],
+      "end_effector_position": [
+        1.799942049217721,
+        1.1769103689153704,
+        0.5759453789487855
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.597735166549683,
+      "joint_positions": [
+        0.08782953145755101,
+        -0.07967444078615707,
+        0.09698002245827896,
+        -0.08886906582578355,
+        -0.15086670065285898,
+        -0.1709144527382866,
+        -0.0009498189957370673,
+        0.047102960452254206,
+        -0.013477013365816082
+      ],
+      "end_effector_position": [
+        1.7849962315263377,
+        1.1470792476624314,
+        0.5699085025694018
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.660913944244385,
+      "joint_positions": [
+        0.10649156673584462,
+        -0.09389599444175993,
+        0.11611220044587692,
+        -0.10621036432850453,
+        -0.18115394671795584,
+        -0.20588682152762774,
+        -0.0019234058876627523,
+        0.05622783202280132,
+        -0.017571620007121275
+      ],
+      "end_effector_position": [
+        1.7518086206343981,
+        1.0886614237464414,
+        0.5581859079937317
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.723473072052002,
+      "joint_positions": [
+        0.125153608897269,
+        -0.10811742143360188,
+        0.13524418485765138,
+        -0.12355089798452631,
+        -0.2114401191957829,
+        -0.24085925635330058,
+        -0.0028969927797134662,
+        0.06535275293358936,
+        -0.021664552116031816
+      ],
+      "end_effector_position": [
+        1.7143381699298847,
+        1.0321156875979889,
+        0.547012763443964
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.78616738319397,
+      "joint_positions": [
+        0.1438156510246419,
+        -0.12233870909437963,
+        0.15437596772185438,
+        -0.14089054670227344,
+        -0.24172506372403382,
+        -0.27583174613736156,
+        -0.003870579671764405,
+        0.07447772046634911,
+        -0.02575559643329884
+      ],
+      "end_effector_position": [
+        1.672734485048635,
+        0.9776902913668923,
+        0.5364803868999367
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.848512649536133,
+      "joint_positions": [
+        0.1624776834554073,
+        -0.13655984931507262,
+        0.17350755401736923,
+        -0.1582291982003057,
+        -0.2720086300322521,
+        -0.31080427888990886,
+        -0.004844166563815344,
+        0.08360273181344798,
+        -0.02984459029660202
+      ],
+      "end_effector_position": [
+        1.6271641406804072,
+        0.9256243692450317,
+        0.5266721204239099
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.925729036331177,
+      "joint_positions": [
+        0.17789918373472727,
+        -0.1561826607862145,
+        0.19360260795904286,
+        -0.1767507938768057,
+        -0.3017538546911043,
+        -0.343637042864331,
+        -0.0034095310303358336,
+        0.09371697557913442,
+        -0.029675066520408137
+      ],
+      "end_effector_position": [
+        1.5756047934901072,
+        0.871805477265117,
+        0.523792927620169
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 6.9717161655426025,
+      "joint_positions": [
+        0.18487443731042671,
+        -0.16721248126756824,
+        0.20387153429143406,
+        -0.1862113695217187,
+        -0.31640551723955784,
+        -0.35957041243705806,
+        -0.002148988118110364,
+        0.09899708326350866,
+        -0.02860564202412324
+      ],
+      "end_effector_position": [
+        1.5480289669699552,
+        0.8451994472436661,
+        0.5237699729713623
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.017879009246826,
+      "joint_positions": [
+        0.191832730883914,
+        -0.1782347458802243,
+        0.21416855017674907,
+        -0.19567512752102215,
+        -0.3310987295742011,
+        -0.37544035064121944,
+        -0.0009038130894290843,
+        0.10429485902069932,
+        -0.02755516598514106
+      ],
+      "end_effector_position": [
+        1.519529715081128,
+        0.8193694853846999,
+        0.523655217914221
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.0804290771484375,
+      "joint_positions": [
+        0.2042212103133201,
+        -0.19905594808475752,
+        0.23763554019217892,
+        -0.21500223692683615,
+        -0.36513624661996597,
+        -0.40063402248769503,
+        -0.00023069510669800435,
+        0.11668162672305635,
+        -0.02792792328590076
+      ],
+      "end_effector_position": [
+        1.4592951730252932,
+        0.7682845622080545,
+        0.5170088605286409
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.14297342300415,
+      "joint_positions": [
+        0.21657376738972026,
+        -0.21984950583045326,
+        0.26116561239934905,
+        -0.23431350093523812,
+        -0.39923978269289706,
+        -0.4256797822975298,
+        0.0004014394563827881,
+        0.12910872465986845,
+        -0.02834505330615778
+      ],
+      "end_effector_position": [
+        1.3957101318448089,
+        0.720798861024976,
+        0.5106832925563928
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.188791751861572,
+      "joint_positions": [
+        0.22274997217942888,
+        -0.23024622448432913,
+        0.27293071419214127,
+        -0.243968482473571,
+        -0.41629076710207324,
+        -0.4382025195818522,
+        0.0007174699750096505,
+        0.13532233022446516,
+        -0.02855223400160147
+      ],
+      "end_effector_position": [
+        1.3627291355763358,
+        0.6984675675117658,
+        0.5077073313610903
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.235321044921875,
+      "joint_positions": [
+        0.2289261463438339,
+        -0.2406429218380229,
+        0.284695834656497,
+        -0.2536230342109883,
+        -0.4333411842868994,
+        -0.4507252459223953,
+        0.0010335003718363444,
+        0.14153595124729737,
+        -0.02875850221384448
+      ],
+      "end_effector_position": [
+        1.3290012387099652,
+        0.6771103354546096,
+        0.5048577007691982
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.297956943511963,
+      "joint_positions": [
+        0.24127839720666489,
+        -0.2614362610787499,
+        0.3082261574604344,
+        -0.27293086245212483,
+        -0.46744033262835155,
+        -0.4757706565428392,
+        0.0016655611549443556,
+        0.15396324480836243,
+        -0.029168449797607025
+      ],
+      "end_effector_position": [
+        1.2594457115545155,
+        0.6374102466033658,
+        0.4995401986999852
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.343616247177124,
+      "joint_positions": [
+        0.24745447126869255,
+        -0.2718329073263485,
+        0.3199913728540838,
+        -0.28258415070762927,
+        -0.4844890766986322,
+        -0.4882933361844386,
+        0.001981591546379644,
+        0.16017692021470528,
+        -0.029372209237302098
+      ],
+      "end_effector_position": [
+        1.2236903641757289,
+        0.6191105346463972,
+        0.49707273367320914
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.405869483947754,
+      "joint_positions": [
+        0.2598065111601679,
+        -0.29262616451199913,
+        0.3435219432689927,
+        -0.30188951837763306,
+        -0.5185849512698817,
+        -0.5133386343162306,
+        0.00261365232924942,
+        0.17260433578342504,
+        -0.02977750991700419
+      ],
+      "end_effector_position": [
+        1.1504101445170098,
+        0.5857105391184255,
+        0.49251844105723624
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.468708038330078,
+      "joint_positions": [
+        0.27215840039254147,
+        -0.31341939004798813,
+        0.36705274720545067,
+        -0.3211933420448576,
+        -0.5526787665561718,
+        -0.5383838411552042,
+        0.003245713112119177,
+        0.18503184802795739,
+        -0.03018013698959044
+      ],
+      "end_effector_position": [
+        1.075024921704863,
+        0.5566935826246069,
+        0.4884644755683195
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.532323122024536,
+      "joint_positions": [
+        0.2855089927574981,
+        -0.33487044552799916,
+        0.3880872194310971,
+        -0.33991451711235676,
+        -0.5827425902530614,
+        -0.5680834355718172,
+        0.005253733163694244,
+        0.1959304146885182,
+        -0.02867797828624582
+      ],
+      "end_effector_position": [
+        0.9995965951877674,
+        0.5337765220334065,
+        0.4879802976048466
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.578083038330078,
+      "joint_positions": [
+        0.2931868872594857,
+        -0.3462538386852568,
+        0.39610353566268136,
+        -0.3487311121692525,
+        -0.5938021260554236,
+        -0.5875938643348041,
+        0.007635258302551727,
+        0.19984946025171238,
+        -0.0260402056639029
+      ],
+      "end_effector_position": [
+        0.9629731654111615,
+        0.5254841128523051,
+        0.49066244089229544
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.641550302505493,
+      "joint_positions": [
+        0.3085885106791883,
+        -0.3690483919559102,
+        0.4120263274717473,
+        -0.3663753775690848,
+        -0.6158043313594693,
+        -0.6268175389668241,
+        0.01245800809785346,
+        0.20762182248612612,
+        -0.020699199588748527
+      ],
+      "end_effector_position": [
+        0.8888042891171415,
+        0.5120402306911005,
+        0.4951886646379518
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.71916389465332,
+      "joint_positions": [
+        0.3239910187732067,
+        -0.3918435477127624,
+        0.42794687399112197,
+        -0.38401887171473237,
+        -0.6378020986164978,
+        -0.6660457252258347,
+        0.01728210427616059,
+        0.2153927318941409,
+        -0.015355820503356142
+      ],
+      "end_effector_position": [
+        0.8136901628242946,
+        0.5028008523329166,
+        0.49837001264688896
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.796494007110596,
+      "joint_positions": [
+        0.33939339622149145,
+        -0.41463868986590857,
+        0.44386763254888895,
+        -0.401661628411663,
+        -0.6597984009531069,
+        -0.7052738576799833,
+        0.022106202873931866,
+        0.2231636767144438,
+        -0.010011785261092112
+      ],
+      "end_effector_position": [
+        0.7379276036404793,
+        0.4978138085470859,
+        0.5001912240503228
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 7.859541416168213,
+      "joint_positions": [
+        0.3547956521695719,
+        -0.43743382854907353,
+        0.45978862191650627,
+        -0.4193038376659889,
+        -0.6817934754084014,
+        -0.7445019313547049,
+        0.026930301476050935,
+        0.230934662494526,
+        -0.004667258425979072
+      ],
+      "end_effector_position": [
+        0.6618153598741331,
+        0.4971106820932062,
+        0.5006555172701327
+      ],
+      "object_position": [
+        0.6,
+        0.5,
+        0.5
+      ]
+    },
+    {
+      "time": 8.93776798248291,
+      "joint_positions": [
+        0.3677388083951569,
+        -0.4627499028202571,
+        0.46617075688455234,
+        -0.43063128193981276,
+        -0.6998453965091774,
+        -0.778876476035649,
+        0.030719002695810963,
+        0.2334749183306867,
+        -0.00022838093516318363
+      ],
+      "end_effector_position": [
+        0.6011058723170439,
+        0.5004452850613883,
+        0.505449532718854
+      ],
+      "object_position": [
+        0.6011058723170439,
+        0.5004452850613883,
+        0.45544953271885397
+      ]
+    },
+    {
+      "time": 9.000827550888062,
+      "joint_positions": [
+        0.3700570141632064,
+        -0.49898431698775136,
+        0.4311761898869516,
+        -0.4151662345906138,
+        -0.7017842046971579,
+        -0.792203989943781,
+        0.0300138731752276,
+        0.21333901381099737,
+        -2.7877603026130696e-05
+      ],
+      "end_effector_position": [
+        0.6041732940606249,
+        0.5038325301416392,
+        0.53477904268218
+      ],
+      "object_position": [
+        0.6041732940606249,
+        0.5038325301416392,
+        0.48477904268218003
+      ]
+    },
+    {
+      "time": 9.064923286437988,
+      "joint_positions": [
+        0.37221155037590664,
+        -0.5353970334847928,
+        0.39529026012536433,
+        -0.3995108394387615,
+        -0.7039182083745074,
+        -0.8051756372888852,
+        0.029231016106590003,
+        0.19282156293466754,
+        -2.8432754314404075e-05
+      ],
+      "end_effector_position": [
+        0.6047187678973001,
+        0.5064514236898139,
+        0.5651732720939121
+      ],
+      "object_position": [
+        0.6047187678973001,
+        0.5064514236898139,
+        0.5151732720939121
+      ]
+    },
+    {
+      "time": 9.111425161361694,
+      "joint_positions": [
+        0.37328873996699274,
+        -0.5536036623111306,
+        0.3773457012880591,
+        -0.39168349474501035,
+        -0.704985412204417,
+        -0.8116609991479847,
+        0.028839459572136524,
+        0.18256247216147428,
+        -3.0337432913152213e-05
+      ],
+      "end_effector_position": [
+        0.6039067830261152,
+        0.5075010025660921,
+        0.5804787282022997
+      ],
+      "object_position": [
+        0.6039067830261152,
+        0.5075010025660921,
+        0.5304787282022997
+      ]
+    },
+    {
+      "time": 9.15800666809082,
+      "joint_positions": [
+        0.3743659023424472,
+        -0.5718103048025918,
+        0.3594011638779412,
+        -0.3838557763482216,
+        -0.7060524669451594,
+        -0.818146350927705,
+        0.02844789633350386,
+        0.17230337000131804,
+        -3.226195577571523e-05
+      ],
+      "end_effector_position": [
+        0.6023665122103719,
+        0.5083723593373568,
+        0.5958113108798806
+      ],
+      "object_position": [
+        0.6023665122103719,
+        0.5083723593373568,
+        0.5458113108798806
+      ]
+    },
+    {
+      "time": 9.22116756439209,
+      "joint_positions": [
+        0.3765199876874713,
+        -0.6082235988733362,
+        0.32351354679371863,
+        -0.368197367196869,
+        -0.7081851901234841,
+        -0.831117164868702,
+        0.027664769275795226,
+        0.15178511522894755,
+        -3.334724567294034e-05
+      ],
+      "end_effector_position": [
+        0.5970949556172461,
+        0.509568113727288,
+        0.6264216979484597
+      ],
+      "object_position": [
+        0.5970949556172461,
+        0.509568113727288,
+        0.5764216979484597
+      ]
+    },
+    {
+      "time": 9.29905891418457,
+      "joint_positions": [
+        0.3786741571970167,
+        -0.6446369284755572,
+        0.2876244986080312,
+        -0.352539411381854,
+        -0.7103172877637645,
+        -0.8440878090747115,
+        0.026881642204996045,
+        0.1312668293702015,
+        -3.7179273006726415e-05
+      ],
+      "end_effector_position": [
+        0.5888978974588315,
+        0.5100165231239749,
+        0.6567321369941478
+      ],
+      "object_position": [
+        0.5888978974588315,
+        0.5100165231239749,
+        0.6067321369941477
+      ]
+    },
+    {
+      "time": 9.376458644866943,
+      "joint_positions": [
+        0.3808282183561756,
+        -0.6810502682843248,
+        0.2517358382155292,
+        -0.3368799736248231,
+        -0.7124483508413174,
+        -0.8570584849109041,
+        0.026098515134173342,
+        0.11074850158009343,
+        -4.033168232825524e-05
+      ],
+      "end_effector_position": [
+        0.5777975732340946,
+        0.5097039146773081,
+        0.6864687624526467
+      ],
+      "object_position": [
+        0.5777975732340946,
+        0.5097039146773081,
+        0.6364687624526466
+      ]
+    },
+    {
+      "time": 9.421980142593384,
+      "joint_positions": [
+        0.3819052100673845,
+        -0.6992569382246874,
+        0.23379174035379932,
+        -0.3290497299301084,
+        -0.7135135109037583,
+        -0.8635438475257458,
+        0.025706951598761973,
+        0.10048932358722144,
+        -4.1364002838757704e-05
+      ],
+      "end_effector_position": [
+        0.5711713505287256,
+        0.5092598641959822,
+        0.7010367251212293
+      ],
+      "object_position": [
+        0.5711713505287256,
+        0.5092598641959822,
+        0.6510367251212292
+      ]
+    },
+    {
+      "time": 9.499027490615845,
+      "joint_positions": [
+        0.38405918897432006,
+        -0.735670291272935,
+        0.19790312548491593,
+        -0.31338915295450764,
+        -0.7156430059506189,
+        -0.8765145720166589,
+        0.024923824527939228,
+        0.07997094304945153,
+        -4.40432628392935e-05
+      ],
+      "end_effector_position": [
+        0.5558043313324563,
+        0.5077956559644178,
+        0.729403188176073
+      ],
+      "object_position": [
+        0.5558043313324563,
+        0.5077956559644178,
+        0.6794031881760729
+      ]
+    },
+    {
+      "time": 9.544721841812134,
+      "joint_positions": [
+        0.38513616549524793,
+        -0.7538769697515195,
+        0.17995880837682085,
+        -0.3055587296590826,
+        -0.7167073895482473,
+        -0.8829999563936177,
+        0.024532260992527852,
+        0.06971174286682538,
+        -4.5246649235436205e-05
+      ],
+      "end_effector_position": [
+        0.5470830199014856,
+        0.5067767891070308,
+        0.7431350211479801
+      ],
+      "object_position": [
+        0.5470830199014856,
+        0.5067767891070308,
+        0.6931350211479801
+      ]
+    },
+    {
+      "time": 9.590599536895752,
+      "joint_positions": [
+        0.38621314324650796,
+        -0.7720836485728316,
+        0.1620143446464968,
+        -0.2977283621954651,
+        -0.7177716732366193,
+        -0.8894853539611433,
+        0.02414069745711648,
+        0.059452539265002234,
+        -4.66245723312031e-05
+      ],
+      "end_effector_position": [
+        0.5376848611594808,
+        0.505568344964455,
+        0.7565221255055881
+      ],
+      "object_position": [
+        0.5376848611594808,
+        0.505568344964455,
+        0.706522125505588
+      ]
+    },
+    {
+      "time": 10.653842210769653,
+      "joint_positions": [
+        0.3883670573038569,
+        -0.8084970072638635,
+        0.12612586454682642,
+        -0.2820672381874627,
+        -0.7198991206415246,
+        -0.902456235048161,
+        0.023357570386293734,
+        0.03893411523354048,
+        -4.746433038847798e-05
+      ],
+      "end_effector_position": [
+        0.5169135909966716,
+        0.5025910438173771,
+        0.7821339932455902
+      ],
+      "object_position": [
+        0.5169135909966716,
+        0.5025910438173771,
+        0.7321339932455901
+      ]
+    },
+    {
+      "time": 10.716419219970703,
+      "joint_positions": [
+        0.38242023854896284,
+        -0.8269216528036611,
+        0.09879431599903517,
+        -0.27137401427576485,
+        -0.7145292298117399,
+        -0.9020382247345698,
+        0.023949594730910812,
+        0.025656349145190632,
+        0.00019503379040687765
+      ],
+      "end_effector_position": [
+        0.5087435439403442,
+        0.4964204281564928,
+        0.8098898592460378
+      ],
+      "object_position": [
+        0.5087435439403442,
+        0.4964204281564928,
+        0.7598898592460378
+      ]
+    },
+    {
+      "time": 10.76184868812561,
+      "joint_positions": [
+        0.36915106186083846,
+        -0.8132592529634622,
+        0.09630774214425351,
+        -0.272374499051306,
+        -0.7025214731444008,
+        -0.8848059239566288,
+        0.0259940064043558,
+        0.028224892309901918,
+        0.0005752523504765626
+      ],
+      "end_effector_position": [
+        0.5235859703556123,
+        0.4903819368939832,
+        0.8277907292261848
+      ],
+      "object_position": [
+        0.5235859703556123,
+        0.4903819368939832,
+        0.7777907292261848
+      ]
+    },
+    {
+      "time": 10.807395696640015,
+      "joint_positions": [
+        0.35562022731869763,
+        -0.7990058794460175,
+        0.09410200272088705,
+        -0.27356510837319836,
+        -0.6904465329057715,
+        -0.8671356971735392,
+        0.028083387627129624,
+        0.03103155906424264,
+        0.0009331821149711796
+      ],
+      "end_effector_position": [
+        0.5390752381684394,
+        0.48462585983108153,
+        0.8451523697944433
+      ],
+      "object_position": [
+        0.5390752381684394,
+        0.48462585983108153,
+        0.7951523697944433
+      ]
+    },
+    {
+      "time": 10.869148969650269,
+      "joint_positions": [
+        0.32853608729125905,
+        -0.7704478262965767,
+        0.08971387063839319,
+        -0.2759641351939434,
+        -0.6663023984159304,
+        -0.8317573051430687,
+        0.03226604350598693,
+        0.036665604913488185,
+        0.0016441834805942738
+      ],
+      "end_effector_position": [
+        0.5709208743960255,
+        0.4741916807316788,
+        0.878250131510074
+      ],
+      "object_position": [
+        0.5709208743960255,
+        0.4741916807316788,
+        0.828250131510074
+      ]
+    },
+    {
+      "time": 10.931247234344482,
+      "joint_positions": [
+        0.3014514105879505,
+        -0.7418886424293112,
+        0.08532634476089528,
+        -0.2783628840749351,
+        -0.642159430148963,
+        -0.796378045703106,
+        0.036448787192127304,
+        0.042300131766200254,
+        0.0023549408081056603
+      ],
+      "end_effector_position": [
+        0.6038869627780156,
+        0.46510626131548055,
+        0.9090118290129158
+      ],
+      "object_position": [
+        0.6038869627780156,
+        0.46510626131548055,
+        0.8590118290129157
+      ]
+    },
+    {
+      "time": 10.977320671081543,
+      "joint_positions": [
+        0.2879090621227883,
+        -0.7276090619700323,
+        0.08313261793020772,
+        -0.2795620236454345,
+        -0.6300883307279526,
+        -0.7786884110760943,
+        0.038540159113962316,
+        0.045117399763881655,
+        0.0027102669090644732
+      ],
+      "end_effector_position": [
+        0.6207812887125623,
+        0.4610305262016442,
+        0.9234487151988544
+      ],
+      "object_position": [
+        0.6207812887125623,
+        0.4610305262016442,
+        0.8734487151988544
+      ]
+    },
+    {
+      "time": 11.055311918258667,
+      "joint_positions": [
+        0.2608243519674471,
+        -0.6990499301804959,
+        0.0787452319064553,
+        -0.2819599160989033,
+        -0.6059469339751942,
+        -0.7433091340192746,
+        0.042722902958165304,
+        0.050751941911993045,
+        0.0034207697173635736
+      ],
+      "end_effector_position": [
+        0.6553632262448061,
+        0.4537339855612606,
+        0.9503073025004496
+      ],
+      "object_position": [
+        0.6553632262448061,
+        0.4537339855612606,
+        0.9003073025004495
+      ]
+    },
+    {
+      "time": 11.118383646011353,
+      "joint_positions": [
+        0.23373963417849225,
+        -0.6704908421967893,
+        0.07435792865460951,
+        -0.28435743660370766,
+        -0.581806658971408,
+        -0.7079298473471565,
+        0.0469056468023803,
+        0.05638648934129726,
+        0.004131012121247845
+      ],
+      "end_effector_position": [
+        0.6909484983673667,
+        0.4474719589286541,
+        0.9743221663739463
+      ],
+      "object_position": [
+        0.6909484983673667,
+        0.4474719589286541,
+        0.9243221663739463
+      ]
+    },
+    {
+      "time": 11.181039333343506,
+      "joint_positions": [
+        0.20665492283522302,
+        -0.6419318032122299,
+        0.06997069654996162,
+        -0.28675479600006193,
+        -0.5576675799702728,
+        -0.6725505521792736,
+        0.05108839064659532,
+        0.0620210388952077,
+        0.004840917936844101
+      ],
+      "end_effector_position": [
+        0.7274541058468558,
+        0.44211979576022253,
+        0.9953215398444191
+      ],
+      "object_position": [
+        0.7274541058468558,
+        0.44211979576022253,
+        0.945321539844419
+      ]
+    },
+    {
+      "time": 11.227707624435425,
+      "joint_positions": [
+        0.1931125745409183,
+        -0.6276523034471505,
+        0.06777710280823926,
+        -0.2879534925246748,
+        -0.5455985155966232,
+        -0.6548609018195362,
+        0.05317976256870285,
+        0.06483831370750108,
+        0.005195719833252568
+      ],
+      "end_effector_position": [
+        0.7460207914995299,
+        0.4397470235477044,
+        1.004642111488414
+      ],
+      "object_position": [
+        0.7460207914995299,
+        0.4397470235477044,
+        0.954642111488414
+      ]
+    },
+    {
+      "time": 11.306054830551147,
+      "joint_positions": [
+        0.16952274742173754,
+        -0.5980277001460236,
+        0.06403145318189309,
+        -0.2865539034809799,
+        -0.5202797752618811,
+        -0.6198817843152168,
+        0.05596095225727541,
+        0.06929646606678988,
+        0.0059529599390869955
+      ],
+      "end_effector_position": [
+        0.7931712410422446,
+        0.44071271766113557,
+        1.0229253129222804
+      ],
+      "object_position": [
+        0.7931712410422446,
+        0.44071271766113557,
+        0.9729253129222803
+      ]
+    },
+    {
+      "time": 11.368936538696289,
+      "joint_positions": [
+        0.1597683334629153,
+        -0.5641871598300354,
+        0.06280972006280044,
+        -0.270425105031838,
+        -0.4910920672787769,
+        -0.5864857715621924,
+        0.053203316148457205,
+        0.06911489721995404,
+        0.006735952490445372
+      ],
+      "end_effector_position": [
+        0.8775638669057725,
+        0.46512730237489547,
+        1.0436035652747035
+      ],
+      "object_position": [
+        0.8775638669057725,
+        0.46512730237489547,
+        0.9936035652747035
+      ]
+    },
+    {
+      "time": 11.430999279022217,
+      "joint_positions": [
+        0.13619718710089154,
+        -0.5356772357834837,
+        0.060146385069545616,
+        -0.2575117992658409,
+        -0.46850277543364666,
+        -0.5541619377679943,
+        0.048416079341005454,
+        0.06806107468439852,
+        0.005904412606864688
+      ],
+      "end_effector_position": [
+        0.9321937599464342,
+        0.47521391219801895,
+        1.0583547251542633
+      ],
+      "object_position": [
+        0.9321937599464342,
+        0.47521391219801895,
+        1.0083547251542633
+      ]
+    },
+    {
+      "time": 11.47652816772461,
+      "joint_positions": [
+        0.11698507845179125,
+        -0.5243011194034299,
+        0.05808675992802556,
+        -0.25245917047289496,
+        -0.45971212536442074,
+        -0.5385479728197184,
+        0.04500360076777338,
+        0.06710708271930584,
+        0.004835458117780014
+      ],
+      "end_effector_position": [
+        0.9446668393264764,
+        0.47120040545351594,
+        1.0652537263087285
+      ],
+      "object_position": [
+        0.9446668393264764,
+        0.47120040545351594,
+        1.0152537263087285
+      ]
+    },
+    {
+      "time": 11.539270877838135,
+      "joint_positions": [
+        0.07850812983468315,
+        -0.5015703578734456,
+        0.053967038286063675,
+        -0.2423333379140113,
+        -0.4420773428129228,
+        -0.507323621725301,
+        0.03817174930076916,
+        0.06519553165923139,
+        0.0027080097850159554
+      ],
+      "end_effector_position": [
+        0.9697866517864864,
+        0.4631724385204807,
+        1.0768997564028973
+      ],
+      "object_position": [
+        0.9697866517864864,
+        0.4631724385204807,
+        1.0268997564028972
+      ]
+    },
+    {
+      "time": 11.585522890090942,
+      "joint_positions": [
+        0.059269104833872835,
+        -0.49020523043399533,
+        0.05190719697096421,
+        -0.2372708335599472,
+        -0.43325994729512496,
+        -0.49171148599482595,
+        0.034755745953201286,
+        0.06423969659871444,
+        0.001644326799918361
+      ],
+      "end_effector_position": [
+        0.9824060357412633,
+        0.45913989992244386,
+        1.081602867970581
+      ],
+      "object_position": [
+        0.9824060357412633,
+        0.45913989992244386,
+        1.0316028679705809
+      ]
+    },
+    {
+      "time": 11.64819073677063,
+      "joint_positions": [
+        0.020791132209334134,
+        -0.4674749999921006,
+        0.047787558662355296,
+        -0.22714711600940596,
+        -0.4156261863637171,
+        -0.46048721234335266,
+        0.0279237387328745,
+        0.06232798567858974,
+        -0.00048318042223275727
+      ],
+      "end_effector_position": [
+        1.0076892426229866,
+        0.4509868030576544,
+        1.0887477903756928
+      ],
+      "object_position": [
+        1.0076892426229866,
+        0.4509868030576544,
+        1.0387477903756928
+      ]
+    },
+    {
+      "time": 11.725193500518799,
+      "joint_positions": [
+        -0.017686738094403333,
+        -0.4447447973694519,
+        0.04366799238244833,
+        -0.21702511600750402,
+        -0.3979937145314177,
+        -0.4292629346931094,
+        0.021091731500703283,
+        0.06041622094427873,
+        -0.0026108547328580088
+      ],
+      "end_effector_position": [
+        1.032924489385728,
+        0.4426585488632353,
+        1.0928647082975493
+      ],
+      "object_position": [
+        1.032924489385728,
+        0.4426585488632353,
+        1.0428647082975493
+      ]
+    },
+    {
+      "time": 11.77113127708435,
+      "joint_positions": [
+        -0.03692563773052165,
+        -0.43337970532648923,
+        0.04160824068590087,
+        -0.211964746223306,
+        -0.3891779223754607,
+        -0.41365079411060013,
+        0.017675727884607022,
+        0.05946031845923064,
+        -0.003674744789457115
+      ],
+      "end_effector_position": [
+        1.0454819197659575,
+        0.438410658490953,
+        1.0937872952337422
+      ],
+      "object_position": [
+        1.0454819197659575,
+        0.438410658490953,
+        1.0437872952337421
+      ]
+    },
+    {
+      "time": 11.817113637924194,
+      "joint_positions": [
+        -0.05616460115132819,
+        -0.422014622857637,
+        0.039548335528242454,
+        -0.20690447437632437,
+        -0.3803626344537341,
+        -0.39803865632113417,
+        0.014259724268510719,
+        0.05850440313372857,
+        -0.004738567740905671
+      ],
+      "end_effector_position": [
+        1.0579757344614449,
+        0.43409929825998245,
+        1.0939551075238871
+      ],
+      "object_position": [
+        1.0579757344614449,
+        0.43409929825998245,
+        1.043955107523887
+      ]
+    },
+    {
+      "time": 11.863100528717041,
+      "joint_positions": [
+        -0.07540353399873553,
+        -0.4106495342754376,
+        0.03748751008727208,
+        -0.20184443435929805,
+        -0.3715477436758612,
+        -0.38242652735024,
+        0.010843720652414414,
+        0.05754847379216534,
+        -0.005802004795163202
+      ],
+      "end_effector_position": [
+        1.0703876280439726,
+        0.42972011903820806,
+        1.093372347906101
+      ],
+      "object_position": [
+        1.0703876280439726,
+        0.42972011903820806,
+        1.043372347906101
+      ]
+    },
+    {
+      "time": 11.909152746200562,
+      "joint_positions": [
+        -0.09464238780520277,
+        -0.3992844538172728,
+        0.03542669368608984,
+        -0.19678509245663417,
+        -0.3627327923528564,
+        -0.36681439026790097,
+        0.007427717036318164,
+        0.056592530459352704,
+        -0.006865650625731983
+      ],
+      "end_effector_position": [
+        1.0826999199125118,
+        0.4252696087646047,
+        1.0920420036775536
+      ],
+      "object_position": [
+        1.0826999199125118,
+        0.4252696087646047,
+        1.0420420036775535
+      ]
+    },
+    {
+      "time": 11.972133159637451,
+      "joint_positions": [
+        -0.1331202024609006,
+        -0.3765543231209475,
+        0.031307478315555214,
+        -0.1866670275382623,
+        -0.34510197410185095,
+        -0.3355901424776146,
+        0.0005957098041255861,
+        0.054680602160403456,
+        -0.00899122239563573
+      ],
+      "end_effector_position": [
+        1.106953327535193,
+        0.41614655226656533,
+        1.0871601724135618
+      ],
+      "object_position": [
+        1.106953327535193,
+        0.41614655226656533,
+        1.0371601724135617
+      ]
+    },
+    {
+      "time": 12.03633451461792,
+      "joint_positions": [
+        -0.178982631273654,
+        -0.3638040300635418,
+        0.021918084625206516,
+        -0.18628588151363745,
+        -0.3326231212631457,
+        -0.31326853934274135,
+        -0.006155364619688725,
+        0.053106859103654855,
+        -0.014052880170783948
+      ],
+      "end_effector_position": [
+        1.0890526181131486,
+        0.38483032044247345,
+        1.0938220826949574
+      ],
+      "object_position": [
+        1.0890526181131486,
+        0.38483032044247345,
+        1.0438220826949574
+      ]
+    },
+    {
+      "time": 12.114541292190552,
+      "joint_positions": [
+        -0.23181797956744046,
+        -0.3602178800715933,
+        0.007786745861365209,
+        -0.19344653819278151,
+        -0.3225907751279284,
+        -0.2991174239077529,
+        -0.012832181325634346,
+        0.051845564229063466,
+        -0.021419351755102806
+      ],
+      "end_effector_position": [
+        1.0344306570881923,
+        0.33770689890456845,
+        1.11430013939232
+      ],
+      "object_position": [
+        1.0344306570881923,
+        0.33770689890456845,
+        1.06430013939232
+      ]
+    },
+    {
+      "time": 12.17717432975769,
+      "joint_positions": [
+        -0.28466789452277674,
+        -0.3566481227448943,
+        -0.00635014717854091,
+        -0.2006012171116433,
+        -0.3125317227375267,
+        -0.2849809614406519,
+        -0.019508848854555987,
+        0.05058486658095534,
+        -0.028783422702883497
+      ],
+      "end_effector_position": [
+        0.9774564535359893,
+        0.293323764423364,
+        1.1349419106630232
+      ],
+      "object_position": [
+        0.9774564535359893,
+        0.293323764423364,
+        1.0849419106630231
+      ]
+    },
+    {
+      "time": 12.239500045776367,
+      "joint_positions": [
+        -0.3375178756378774,
+        -0.3530783639377037,
+        -0.020487172519998976,
+        -0.20775560281531158,
+        -0.3024719805538091,
+        -0.2708445256634422,
+        -0.026185516115404865,
+        0.049324162644535666,
+        -0.036147270273892376
+      ],
+      "end_effector_position": [
+        0.9182921767460724,
+        0.2518116734916065,
+        1.1556958330617504
+      ],
+      "object_position": [
+        0.9182921767460724,
+        0.2518116734916065,
+        1.1056958330617503
+      ]
+    },
+    {
+      "time": 12.302814245223999,
+      "joint_positions": [
+        -0.39036792128049097,
+        -0.3495085756923258,
+        -0.0346243168250804,
+        -0.21490970185148467,
+        -0.29241154804560104,
+        -0.25670809044304793,
+        -0.032862183375772104,
+        0.04806345397664347,
+        -0.043510893199323684
+      ],
+      "end_effector_position": [
+        0.8570981054146609,
+        0.21329529530686536,
+        1.1765500560317845
+      ],
+      "object_position": [
+        0.8570981054146609,
+        0.21329529530686536,
+        1.1265500560317845
+      ]
+    },
+    {
+      "time": 12.381500720977783,
+      "joint_positions": [
+        -0.44321800059980304,
+        -0.3459387590539576,
+        -0.048761577325306,
+        -0.22206351156336626,
+        -0.28235047987282413,
+        -0.24257165594092475,
+        -0.0395388506361384,
+        0.04680274287707581,
+        -0.05087430582271417
+      ],
+      "end_effector_position": [
+        0.7940424997550384,
+        0.1778895462334687,
+        1.1974926552778444
+      ],
+      "object_position": [
+        0.7940424997550384,
+        0.1778895462334687,
+        1.1474926552778444
+      ]
+    },
+    {
+      "time": 12.426857233047485,
+      "joint_positions": [
+        -0.46964304931463313,
+        -0.3441538405358687,
+        -0.05583024936212424,
+        -0.22564030602316354,
+        -0.27731971300966435,
+        -0.23550343900052104,
+        -0.042877184266321595,
+        0.04617238715029777,
+        -0.05455593627980228
+      ],
+      "end_effector_position": [
+        0.7618712610163128,
+        0.16138626412373297,
+        1.2079933461078987
+      ],
+      "object_position": [
+        0.7618712610163128,
+        0.16138626412373297,
+        1.1579933461078986
+      ]
+    },
+    {
+      "time": 12.4899742603302,
+      "joint_positions": [
+        -0.5224932054889214,
+        -0.3405839847505042,
+        -0.06996766644001849,
+        -0.2327936602219721,
+        -0.2672576305327292,
+        -0.22136700570989343,
+        -0.04955385152668788,
+        0.0449116774646501,
+        -0.06191903839975758
+      ],
+      "end_effector_position": [
+        0.69635503837186,
+        0.1308392317282877,
+        1.2290459917537273
+      ],
+      "object_position": [
+        0.69635503837186,
+        0.1308392317282877,
+        1.1790459917537273
+      ]
+    },
+    {
+      "time": 12.551997184753418,
+      "joint_positions": [
+        -0.5617855426037075,
+        -0.31329362557168067,
+        -0.08917530044253477,
+        -0.23045908455665526,
+        -0.2532491992586223,
+        -0.20765699186469355,
+        -0.049784667577114076,
+        0.04095112465306217,
+        -0.06177670544755249
+      ],
+      "end_effector_position": [
+        0.6787739632892154,
+        0.10854098434628273,
+        1.2292923203610586
+      ],
+      "object_position": [
+        0.6787739632892154,
+        0.10854098434628273,
+        1.1792923203610586
+      ]
+    },
+    {
+      "time": 12.598050594329834,
+      "joint_positions": [
+        -0.5783700865768571,
+        -0.2943969952411602,
+        -0.09995718163272306,
+        -0.22765356340468806,
+        -0.24610230036852693,
+        -0.20089682563188474,
+        -0.048475744773598145,
+        0.03836815481524543,
+        -0.06015554805429816
+      ],
+      "end_effector_position": [
+        0.6792508956456501,
+        0.0981083903936284,
+        1.2239690585710195
+      ],
+      "object_position": [
+        0.6792508956456501,
+        0.0981083903936284,
+        1.1739690585710194
+      ]
+    },
+    {
+      "time": 12.660706758499146,
+      "joint_positions": [
+        -0.6115170351123232,
+        -0.25656776900419836,
+        -0.12153370764653317,
+        -0.22204895746738687,
+        -0.23183340106696723,
+        -0.18737712628326292,
+        -0.04584826114794279,
+        0.03319793506119274,
+        -0.05690773366975998
+      ],
+      "end_effector_position": [
+        0.6795648716726052,
+        0.07682506194317473,
+        1.2121267303180747
+      ],
+      "object_position": [
+        0.6795648716726052,
+        0.07682506194317473,
+        1.1621267303180747
+      ]
+    },
+    {
+      "time": 12.722919940948486,
+      "joint_positions": [
+        -0.6446634755676532,
+        -0.21873765330880293,
+        -0.14311052642893243,
+        -0.21644490785546902,
+        -0.2175652919541853,
+        -0.17385743722472533,
+        -0.04322056015931693,
+        0.02802764081560618,
+        -0.05365919065489128
+      ],
+      "end_effector_position": [
+        0.6789501986156463,
+        0.05505369532691684,
+        1.1987750263837083
+      ],
+      "object_position": [
+        0.6789501986156463,
+        0.05505369532691684,
+        1.1487750263837082
+      ]
+    },
+    {
+      "time": 12.785966634750366,
+      "joint_positions": [
+        -0.6778099228222559,
+        -0.1809074511744687,
+        -0.16468729432112397,
+        -0.21084104465434336,
+        -0.20329722672854927,
+        -0.16033774715340018,
+        -0.04059285878008761,
+        0.022857370267036218,
+        -0.05040997222931098
+      ],
+      "end_effector_position": [
+        0.677339419560824,
+        0.032864038556119135,
+        1.1839484052105913
+      ],
+      "object_position": [
+        0.677339419560824,
+        0.032864038556119135,
+        1.1339484052105913
+      ]
+    },
+    {
+      "time": 12.863342761993408,
+      "joint_positions": [
+        -0.7109563848961378,
+        -0.14307716622095923,
+        -0.18626400396780582,
+        -0.20523723373896566,
+        -0.18902912234826835,
+        -0.1468180589064888,
+        -0.03796515740015633,
+        0.01768712390699001,
+        -0.04716005873545943
+      ],
+      "end_effector_position": [
+        0.6746713883026272,
+        0.010331166675220715,
+        1.1676836304660225
+      ],
+      "object_position": [
+        0.6746713883026272,
+        0.010331166675220715,
+        1.1176836304660225
+      ]
+    },
+    {
+      "time": 12.940091609954834,
+      "joint_positions": [
+        -0.7441028679988457,
+        -0.10524680064263517,
+        -0.20784065015511066,
+        -0.19963335075570948,
+        -0.17476089724100838,
+        -0.13329837528916005,
+        -0.0353374560202238,
+        0.012516901750143403,
+        -0.04390944862564198
+      ],
+      "end_effector_position": [
+        0.6708916181108816,
+        -0.012464807902059283,
+        1.1500206521449436
+      ],
+      "object_position": [
+        0.6708916181108816,
+        -0.012464807902059283,
+        1.1000206521449436
+      ]
+    },
+    {
+      "time": 13.003120183944702,
+      "joint_positions": [
+        -0.7772493775236298,
+        -0.06741635674265072,
+        -0.22941722894157976,
+        -0.19402927887010502,
+        -0.1604924721459504,
+        -0.11977869914418873,
+        -0.03270975464029128,
+        0.007346703530299803,
+        -0.04065814457727694
+      ],
+      "end_effector_position": [
+        0.6659526683285102,
+        -0.03543935818328431,
+        1.131002512946761
+      ],
+      "object_position": [
+        0.6659526683285102,
+        -0.03543935818328431,
+        1.0810025129467609
+      ]
+    },
+    {
+      "time": 13.04921579360962,
+      "joint_positions": [
+        -0.7938226436142497,
+        -0.04850110611561478,
+        -0.24020549218270512,
+        -0.19122713901040206,
+        -0.15335816024378146,
+        -0.11301886472584106,
+        -0.031395903950325,
+        0.004761613249893305,
+        -0.039032239242546056
+      ],
+      "end_effector_position": [
+        0.6630356820005932,
+        -0.04696613805501115,
+        1.12099952901393
+      ],
+      "object_position": [
+        0.6630356820005932,
+        -0.04696613805501115,
+        1.07099952901393
+      ]
+    },
+    {
+      "time": 13.113152742385864,
+      "joint_positions": [
+        -0.826969200897329,
+        -0.010670548992756232,
+        -0.2617819651351279,
+        -0.1856225894084064,
+        -0.1390892902212039,
+        -0.09949920488053729,
+        -0.02876820257039246,
+        -0.0004085500613389955,
+        -0.035779933520809035
+      ],
+      "end_effector_position": [
+        0.6562852025653988,
+        -0.07004202041917812,
+        1.1000358619319568
+      ],
+      "object_position": [
+        0.6562852025653988,
+        -0.07004202041917812,
+        1.0500358619319567
+      ]
+    },
+    {
+      "time": 13.17675232887268,
+      "joint_positions": [
+        -0.8601157942762656,
+        0.02716008067575022,
+        -0.2833583657817065,
+        -0.18001760625746824,
+        -0.1248200301671832,
+        -0.08597955912126828,
+        -0.02614050119045995,
+        -0.005578691042005666,
+        -0.0325269954224721
+      ],
+      "end_effector_position": [
+        0.6482897457217106,
+        -0.09307018478875073,
+        1.0778374749773927
+      ],
+      "object_position": [
+        0.6482897457217106,
+        -0.09307018478875073,
+        1.0278374749773926
+      ]
+    },
+    {
+      "time": 13.239844560623169,
+      "joint_positions": [
+        -0.8932624262574499,
+        0.06499078052139022,
+        -0.30493469362714143,
+        -0.1744121104378769,
+        -0.11055030780640412,
+        -0.07245992978991193,
+        -0.02351279981052745,
+        -0.010748810616473205,
+        -0.029273466418784255
+      ],
+      "end_effector_position": [
+        0.6390286945103194,
+        -0.11595592445288072,
+        1.054457375382329
+      ],
+      "object_position": [
+        0.6390286945103194,
+        -0.11595592445288072,
+        1.004457375382329
+      ]
+    },
+    {
+      "time": 13.31650972366333,
+      "joint_positions": [
+        -0.9264090986647839,
+        0.10282154813454036,
+        -0.32651094871552644,
+        -0.16880603341347294,
+        -0.09628005317704226,
+        -0.05894031899420323,
+        -0.020885098430594932,
+        -0.015918909820716226,
+        -0.026019397579746025
+      ],
+      "end_effector_position": [
+        0.6284894649650817,
+        -0.1386031932485888,
+        1.029951143653099
+      ],
+      "object_position": [
+        0.6284894649650817,
+        -0.1386031932485888,
+        0.979951143653099
+      ]
+    },
+    {
+      "time": 13.36194396018982,
+      "joint_positions": [
+        -0.9429824504429338,
+        0.12173695657146427,
+        -0.33729904907944147,
+        -0.1660027580545896,
+        -0.08914470490074529,
+        -0.052180521128482926,
+        -0.019571247740628704,
+        -0.018503952131529302,
+        -0.02439217815225386
+      ],
+      "end_effector_position": [
+        0.6227388118154633,
+        -0.14980719475357882,
+        1.0172937680801213
+      ],
+      "object_position": [
+        0.6227388118154633,
+        -0.14980719475357882,
+        0.9672937680801212
+      ]
+    },
+    {
+      "time": 13.424386262893677,
+      "joint_positions": [
+        -0.9761291856837525,
+        0.1595678210999368,
+        -0.35887519576395294,
+        -0.1603957009805497,
+        -0.07487352513369017,
+        -0.03866094145064131,
+        -0.01694354636069617,
+        -0.023674022902708657,
+        -0.02113740792662048
+      ],
+      "end_effector_position": [
+        0.6102764050571825,
+        -0.17191548458098055,
+        0.9912078506496995
+      ],
+      "object_position": [
+        0.6102764050571825,
+        -0.17191548458098055,
+        0.9412078506496995
+      ]
+    },
+    {
+      "time": 13.48796010017395,
+      "joint_positions": [
+        -1.0092759635510267,
+        0.19739874695301526,
+        -0.38045127085395164,
+        -0.15478793152535544,
+        -0.06060164763070373,
+        -0.025141384254804236,
+        -0.014315844980763668,
+        -0.02884407621831681,
+        -0.017882252466652104
+      ],
+      "end_effector_position": [
+        0.596539687373335,
+        -0.19354397445132643,
+        0.9641451191765175
+      ],
+      "object_position": [
+        0.596539687373335,
+        -0.19354397445132643,
+        0.9141451191765174
+      ]
+    },
+    {
+      "time": 13.534050941467285,
+      "joint_positions": [
+        -1.0258493685200472,
+        0.21631423201733985,
+        -0.39123928171345707,
+        -0.15198376753364332,
+        -0.05346542773512492,
+        -0.01838161440877214,
+        -0.013001994290797403,
+        -0.03142909671844529,
+        -0.016254552664052072
+      ],
+      "end_effector_position": [
+        0.5891980069155364,
+        -0.20414848898917773,
+        0.9502672136068135
+      ],
+      "object_position": [
+        0.5891980069155364,
+        -0.20414848898917773,
+        0.9002672136068135
+      ]
+    },
+    {
+      "time": 13.579784870147705,
+      "joint_positions": [
+        -1.0424229746639944,
+        0.2352297186394533,
+        -0.40202752835322075,
+        -0.1491785706099076,
+        -0.04632967410411605,
+        -0.011621851559763632,
+        -0.01168814360083115,
+        -0.03401411407801343,
+        -0.014626768915210036
+      ],
+      "end_effector_position": [
+        0.5815441456439917,
+        -0.2145974097499061,
+        0.936169627688301
+      ],
+      "object_position": [
+        0.5815441456439917,
+        -0.2145974097499061,
+        0.886169627688301
+      ]
+    },
+    {
+      "time": 13.642351865768433,
+      "joint_positions": [
+        -1.0755699829874308,
+        0.2730607481658263,
+        -0.4236041651051109,
+        -0.14356836786375943,
+        -0.03205699475566105,
+        0.001897657153158545,
+        -0.009060442220898616,
+        -0.039184136485992976,
+        -0.011370974619394542
+      ],
+      "end_effector_position": [
+        0.5653130879890557,
+        -0.23498235752442584,
+        0.9073471636600887
+      ],
+      "object_position": [
+        0.5653130879890557,
+        -0.23498235752442584,
+        0.8573471636600887
+      ]
+    },
+    {
+      "time": 13.70492696762085,
+      "joint_positions": [
+        -1.1087168795449487,
+        0.31089183950630456,
+        -0.4451806621588975,
+        -0.13795805971825914,
+        -0.017782868465547157,
+        0.015417142197715273,
+        -0.006432740840966107,
+        -0.04435414439445134,
+        -0.008115044017657705
+      ],
+      "end_effector_position": [
+        0.5478774357008868,
+        -0.25460807393740004,
+        0.8777438402301354
+      ],
+      "object_position": [
+        0.5478774357008868,
+        -0.25460807393740004,
+        0.8277438402301354
+      ]
+    },
+    {
+      "time": 13.767952680587769,
+      "joint_positions": [
+        -1.141859345165666,
+        0.3487234178243164,
+        -0.466757498439615,
+        -0.13236017277444545,
+        -0.003500171803004099,
+        0.02893663724612519,
+        -0.003805039461033558,
+        -0.04952414184714678,
+        -0.004858842665587994
+      ],
+      "end_effector_position": [
+        0.529274409486874,
+        -0.27338671043123985,
+        0.8474283917736201
+      ],
+      "object_position": [
+        0.529274409486874,
+        -0.27338671043123985,
+        0.79742839177362
+      ]
+    },
+    {
+      "time": 13.814250946044922,
+      "joint_positions": [
+        -1.1584312731990642,
+        0.3676390272482317,
+        -0.4775456282982235,
+        -0.1295563198867623,
+        0.0036372148502572953,
+        0.03569635829917075,
+        -0.0024911887710673142,
+        -0.05210913667991503,
+        -0.0032309081680221606
+      ],
+      "end_effector_position": [
+        0.5195491962653618,
+        -0.2824319959084897,
+        0.8320255351576124
+      ],
+      "object_position": [
+        0.5195491962653618,
+        -0.2824319959084897,
+        0.7820255351576123
+      ]
+    },
+    {
+      "time": 13.861272811889648,
+      "joint_positions": [
+        -1.1750034639303137,
+        0.38655463609254587,
+        -0.48833371148663995,
+        -0.12675111018741178,
+        0.010773942651065493,
+        0.04245606872627775,
+        -0.001177338081101049,
+        -0.05469412800510998,
+        -0.0016029784338131407
+      ],
+      "end_effector_position": [
+        0.5095498420976806,
+        -0.2912343053789764,
+        0.816471129985401
+      ],
+      "object_position": [
+        0.5095498420976806,
+        -0.2912343053789764,
+        0.7664711299854009
+      ]
+    }
+  ],
+  "path_data": {
+    "planned_path": [],
+    "executed_path": []
+  },
+  "events": []
+}
\ No newline at end of file
diff --git a/simulation_recording_20250914_132401.json b/simulation_recording_20250914_132401.json
new file mode 100644
index 0000000..c9fa7ff
--- /dev/null
+++ b/simulation_recording_20250914_132401.json
@@ -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": []
+}
\ No newline at end of file
diff --git a/src/gui/main_window.py b/src/gui/main_window.py
index 3ce5f48..764e62c 100644
--- a/src/gui/main_window.py
+++ b/src/gui/main_window.py
@@ -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)
 
diff --git a/src/robot/arm_controller.py b/src/robot/arm_controller.py
index 2db862d..8863fd7 100644
--- a/src/robot/arm_controller.py
+++ b/src/robot/arm_controller.py
@@ -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()