From 17c299df876d071fa734917b7534c34b013caf55 Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Fri, 19 Dec 2025 23:11:27 +0800
Subject: [PATCH 01/19] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E9=80=89=E9=A2=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/README.md | 1 +
 1 file changed, 1 insertion(+)
 create mode 100644 src/Robot_arm_motion/README.md

diff --git a/src/Robot_arm_motion/README.md b/src/Robot_arm_motion/README.md
new file mode 100644
index 0000000000..c23571d758
--- /dev/null
+++ b/src/Robot_arm_motion/README.md
@@ -0,0 +1 @@
+机械臂抓取
\ No newline at end of file

From 5e6eb70745871dbb5d2c5531002103d41b15088c Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Sun, 21 Dec 2025 21:09:20 +0800
Subject: [PATCH 02/19] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=88=9D=E5=A7=8B?=
 =?UTF-8?q?=E4=BB=A3=E7=A0=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/requirements.txt        |   0
 src/Robot_arm_motion/scripts/simple_grasp.py | 103 +++++++++++++++++++
 src/Robot_arm_motion/test_pybullet_final.py  |   0
 3 files changed, 103 insertions(+)
 create mode 100644 src/Robot_arm_motion/requirements.txt
 create mode 100644 src/Robot_arm_motion/scripts/simple_grasp.py
 create mode 100644 src/Robot_arm_motion/test_pybullet_final.py

diff --git a/src/Robot_arm_motion/requirements.txt b/src/Robot_arm_motion/requirements.txt
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/Robot_arm_motion/scripts/simple_grasp.py b/src/Robot_arm_motion/scripts/simple_grasp.py
new file mode 100644
index 0000000000..e1f6e82e2b
--- /dev/null
+++ b/src/Robot_arm_motion/scripts/simple_grasp.py
@@ -0,0 +1,103 @@
+import pybullet as p
+import time
+import math
+
+# 初始化pybullet，使用GUI模式并重置仿真
+physicsClient = p.connect(p.GUI)
+p.resetSimulation()
+p.setGravity(0, 0, -9.81)
+p.setRealTimeSimulation(0)  # 非实时仿真，便于精确控制
+print("✅ pybullet仿真环境初始化完成")
+
+# ---------------------- 纯代码创建环境和物体 ----------------------
+# 1. 创建地面
+ground_shape = p.createCollisionShape(p.GEOM_PLANE)
+ground_id = p.createMultiBody(0, ground_shape, basePosition=[0, 0, 0])
+p.changeDynamics(ground_id, -1, lateralFriction=0.8)
+print("✅ 已创建地面")
+
+# 2. 创建机械臂底座（固定）
+base_shape = p.createCollisionShape(p.GEOM_CYLINDER, radius=0.1, height=0.2)
+base_id = p.createMultiBody(0, base_shape, basePosition=[0, 0, 0.1])
+print(f"✅ 已创建机械臂底座，ID：{base_id}")
+
+# 3. 创建机械臂大臂（可动连杆1）
+arm1_shape = p.createCollisionShape(p.GEOM_CYLINDER, radius=0.06, height=0.4)
+arm1_id = p.createMultiBody(1.0, arm1_shape, basePosition=[0, 0, 0.3])
+p.changeDynamics(arm1_id, -1, lateralFriction=0.5, restitution=0.1)
+print(f"✅ 已创建机械臂大臂，ID：{arm1_id}")
+
+# 4. 创建抓取目标立方体
+cube_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.05, 0.05, 0.05])
+cube_id = p.createMultiBody(0.2, cube_shape, basePosition=[0.4, 0, 0.2])
+p.changeDynamics(cube_id, -1, lateralFriction=0.5)
+print(f"✅ 已创建抓取目标立方体，ID：{cube_id}")
+
+# ---------------------- 机械臂运动与抓取逻辑（无关节约束版） ----------------------
+def calculate_arm_position(angle):
+    """根据旋转角度计算大臂的位置和姿态"""
+    # 大臂的旋转中心在底座顶部（0,0,0.2）
+    center_x, center_y, center_z = 0, 0, 0.2
+    # 大臂长度（半高+底座高度）
+    arm_length = 0.2  # 大臂半高0.2m
+    # 计算大臂中心的新位置（绕Y轴旋转）
+    new_x = center_x + arm_length * math.sin(angle)
+    new_z = center_z + arm_length * math.cos(angle)
+    # 计算大臂的姿态（四元数）
+    orientation = p.getQuaternionFromEuler([0, angle, 0])  # 绕Y轴旋转angle弧度
+    return (new_x, 0, new_z), orientation
+
+def check_grasp(arm_pos, cube_pos, threshold=0.1):
+    """检测是否可以抓取（距离判断）"""
+    distance = math.sqrt(
+        (arm_pos[0]-cube_pos[0])**2 +
+        (arm_pos[1]-cube_pos[1])**2 +
+        (arm_pos[2]-cube_pos[2])**2
+    )
+    return distance < threshold
+
+# 初始化变量
+grasped = False  # 是否已抓取立方体
+angle = 0.0      # 机械臂旋转角度
+angle_speed = 0.02  # 旋转速度（弧度/步）
+max_angle = math.pi / 2  # 最大旋转角度（90°）
+
+print("\n🚀 仿真开始，机械臂将开始运动，靠近立方体后自动抓取...")
+print("💡 按下Ctrl+C可终止仿真")
+
+# ---------------------- 主仿真循环 ----------------------
+try:
+    while True:
+        # 1. 更新机械臂旋转角度（来回摆动）
+        angle += angle_speed
+        if abs(angle) > max_angle:
+            angle_speed = -angle_speed  # 反向旋转
+
+        # 2. 计算并设置大臂的位置和姿态
+        arm_pos, arm_ori = calculate_arm_position(angle)
+        p.resetBasePositionAndOrientation(arm1_id, arm_pos, arm_ori)
+
+        # 3. 获取立方体位置，判断是否抓取
+        cube_pos, cube_ori = p.getBasePositionAndOrientation(cube_id)
+        if not grasped:
+            if check_grasp(arm_pos, cube_pos):
+                grasped = True
+                print(f"\n✅ 已抓取立方体！当前机械臂角度：{math.degrees(angle):.1f}°")
+        else:
+            # 已抓取：将立方体位置绑定到机械臂末端
+            # 机械臂末端位置（大臂顶部）
+            end_effector_pos = (
+                arm_pos[0] + 0.2 * math.sin(angle),
+                0,
+                arm_pos[2] + 0.2 * math.cos(angle)
+            )
+            p.resetBasePositionAndOrientation(cube_id, end_effector_pos, cube_ori)
+
+        # 4. 执行物理仿真步长
+        p.stepSimulation()
+        time.sleep(1/240)  # 240Hz仿真频率
+
+except KeyboardInterrupt:
+    # 断开仿真连接
+    p.disconnect()
+    print("\n\n🔚 仿真已手动终止，感谢使用！")
\ No newline at end of file
diff --git a/src/Robot_arm_motion/test_pybullet_final.py b/src/Robot_arm_motion/test_pybullet_final.py
new file mode 100644
index 0000000000..e69de29bb2

From 265bcd4867ed03ce489bb12325ebfb6b6f0fe545 Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Mon, 22 Dec 2025 10:46:30 +0800
Subject: [PATCH 03/19] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=88=9D=E5=A7=8B?=
 =?UTF-8?q?=E4=BB=A3=E7=A0=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/scripts/robot_arm.py     |  68 ++++++++++++
 src/Robot_arm_motion/scripts/simple_grasp.py  | 103 ------------------
 .../{test_pybullet_final.py => test.py}       |   0
 3 files changed, 68 insertions(+), 103 deletions(-)
 create mode 100644 src/Robot_arm_motion/scripts/robot_arm.py
 delete mode 100644 src/Robot_arm_motion/scripts/simple_grasp.py
 rename src/Robot_arm_motion/{test_pybullet_final.py => test.py} (100%)

diff --git a/src/Robot_arm_motion/scripts/robot_arm.py b/src/Robot_arm_motion/scripts/robot_arm.py
new file mode 100644
index 0000000000..c12ad7a153
--- /dev/null
+++ b/src/Robot_arm_motion/scripts/robot_arm.py
@@ -0,0 +1,68 @@
+"""
+MuJoCo机械臂仿真初始版本
+功能：基础2关节机械臂可视化+关节角度控制
+项目根目录：Robot_arm_motion
+代码目录：scripts
+作者：邓卓
+日期：2025-12-22
+"""
+import mujoco
+import mujoco.viewer as viewer
+import numpy as np
+
+def create_simple_arm_model():
+    """创建简易2关节机械臂+立方体模型的XML字符串"""
+    xml = """
+    <mujoco>
+      <option gravity="0 0 -9.81" timestep="0.001"/>
+      <worldbody>
+        <!-- 地面 -->
+        <geom name="ground" type="plane" size="5 5 0.1" pos="0 0 0" rgba="0.8 0.8 0.8 1"/>
+        <!-- 机械臂基座 -->
+        <body name="base" pos="0 0 0">
+          <geom type="cylinder" size="0.1 0.1" rgba="0.2 0.2 0.8 1"/>
+          <joint name="joint1" type="hinge" axis="0 1 0" pos="0 0 0.1"/>
+          <body name="link1" pos="0 0 0.1">
+            <geom type="capsule" size="0.05" fromto="0 0 0 0 0 0.5" rgba="0.2 0.8 0.2 1"/>
+            <joint name="joint2" type="hinge" axis="1 0 0" pos="0 0 0.5"/>
+            <body name="link2" pos="0 0 0.5">
+              <geom type="capsule" size="0.05" fromto="0 0 0 0 0 0.4" rgba="0.2 0.8 0.2 1"/>
+              <geom name="gripper" type="box" size="0.08 0.08 0.02" pos="0 0 0.4" rgba="0.8 0.2 0.2 1"/>
+            </body>
+          </body>
+        </body>
+        <!-- 抓取目标：立方体 -->
+        <body name="cube" pos="0.3 0 0.5">
+          <joint type="free"/>
+          <geom type="box" size="0.05 0.05 0.05" rgba="0.8 0.8 0.2 1"/>
+        </body>
+      </worldbody>
+      <actuator>
+        <motor joint="joint1" ctrlrange="-3.14 3.14" gear="100"/>
+        <motor joint="joint2" ctrlrange="-3.14 3.14" gear="100"/>
+      </actuator>
+    </mujoco>
+    """
+    return mujoco.MjModel.from_xml_string(xml)
+
+def main():
+    """主函数：加载模型+启动仿真"""
+    # 加载模型
+    model = create_simple_arm_model()
+    data = mujoco.MjData(model)
+
+    # 设置初始关节角度
+    joint1_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "joint1")
+    joint2_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "joint2")
+    data.ctrl[joint1_id] = np.pi/4  # 关节1转45°
+    data.ctrl[joint2_id] = -np.pi/6 # 关节2转-30°
+
+    # 启动可视化
+    print("=== Robot_arm_motion 初始版本仿真启动 ===")
+    with viewer.launch_passive(model, data) as v:
+        while v.is_running():
+            mujoco.mj_step(model, data)
+            v.sync()
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file
diff --git a/src/Robot_arm_motion/scripts/simple_grasp.py b/src/Robot_arm_motion/scripts/simple_grasp.py
deleted file mode 100644
index e1f6e82e2b..0000000000
--- a/src/Robot_arm_motion/scripts/simple_grasp.py
+++ /dev/null
@@ -1,103 +0,0 @@
-import pybullet as p
-import time
-import math
-
-# 初始化pybullet，使用GUI模式并重置仿真
-physicsClient = p.connect(p.GUI)
-p.resetSimulation()
-p.setGravity(0, 0, -9.81)
-p.setRealTimeSimulation(0)  # 非实时仿真，便于精确控制
-print("✅ pybullet仿真环境初始化完成")
-
-# ---------------------- 纯代码创建环境和物体 ----------------------
-# 1. 创建地面
-ground_shape = p.createCollisionShape(p.GEOM_PLANE)
-ground_id = p.createMultiBody(0, ground_shape, basePosition=[0, 0, 0])
-p.changeDynamics(ground_id, -1, lateralFriction=0.8)
-print("✅ 已创建地面")
-
-# 2. 创建机械臂底座（固定）
-base_shape = p.createCollisionShape(p.GEOM_CYLINDER, radius=0.1, height=0.2)
-base_id = p.createMultiBody(0, base_shape, basePosition=[0, 0, 0.1])
-print(f"✅ 已创建机械臂底座，ID：{base_id}")
-
-# 3. 创建机械臂大臂（可动连杆1）
-arm1_shape = p.createCollisionShape(p.GEOM_CYLINDER, radius=0.06, height=0.4)
-arm1_id = p.createMultiBody(1.0, arm1_shape, basePosition=[0, 0, 0.3])
-p.changeDynamics(arm1_id, -1, lateralFriction=0.5, restitution=0.1)
-print(f"✅ 已创建机械臂大臂，ID：{arm1_id}")
-
-# 4. 创建抓取目标立方体
-cube_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.05, 0.05, 0.05])
-cube_id = p.createMultiBody(0.2, cube_shape, basePosition=[0.4, 0, 0.2])
-p.changeDynamics(cube_id, -1, lateralFriction=0.5)
-print(f"✅ 已创建抓取目标立方体，ID：{cube_id}")
-
-# ---------------------- 机械臂运动与抓取逻辑（无关节约束版） ----------------------
-def calculate_arm_position(angle):
-    """根据旋转角度计算大臂的位置和姿态"""
-    # 大臂的旋转中心在底座顶部（0,0,0.2）
-    center_x, center_y, center_z = 0, 0, 0.2
-    # 大臂长度（半高+底座高度）
-    arm_length = 0.2  # 大臂半高0.2m
-    # 计算大臂中心的新位置（绕Y轴旋转）
-    new_x = center_x + arm_length * math.sin(angle)
-    new_z = center_z + arm_length * math.cos(angle)
-    # 计算大臂的姿态（四元数）
-    orientation = p.getQuaternionFromEuler([0, angle, 0])  # 绕Y轴旋转angle弧度
-    return (new_x, 0, new_z), orientation
-
-def check_grasp(arm_pos, cube_pos, threshold=0.1):
-    """检测是否可以抓取（距离判断）"""
-    distance = math.sqrt(
-        (arm_pos[0]-cube_pos[0])**2 +
-        (arm_pos[1]-cube_pos[1])**2 +
-        (arm_pos[2]-cube_pos[2])**2
-    )
-    return distance < threshold
-
-# 初始化变量
-grasped = False  # 是否已抓取立方体
-angle = 0.0      # 机械臂旋转角度
-angle_speed = 0.02  # 旋转速度（弧度/步）
-max_angle = math.pi / 2  # 最大旋转角度（90°）
-
-print("\n🚀 仿真开始，机械臂将开始运动，靠近立方体后自动抓取...")
-print("💡 按下Ctrl+C可终止仿真")
-
-# ---------------------- 主仿真循环 ----------------------
-try:
-    while True:
-        # 1. 更新机械臂旋转角度（来回摆动）
-        angle += angle_speed
-        if abs(angle) > max_angle:
-            angle_speed = -angle_speed  # 反向旋转
-
-        # 2. 计算并设置大臂的位置和姿态
-        arm_pos, arm_ori = calculate_arm_position(angle)
-        p.resetBasePositionAndOrientation(arm1_id, arm_pos, arm_ori)
-
-        # 3. 获取立方体位置，判断是否抓取
-        cube_pos, cube_ori = p.getBasePositionAndOrientation(cube_id)
-        if not grasped:
-            if check_grasp(arm_pos, cube_pos):
-                grasped = True
-                print(f"\n✅ 已抓取立方体！当前机械臂角度：{math.degrees(angle):.1f}°")
-        else:
-            # 已抓取：将立方体位置绑定到机械臂末端
-            # 机械臂末端位置（大臂顶部）
-            end_effector_pos = (
-                arm_pos[0] + 0.2 * math.sin(angle),
-                0,
-                arm_pos[2] + 0.2 * math.cos(angle)
-            )
-            p.resetBasePositionAndOrientation(cube_id, end_effector_pos, cube_ori)
-
-        # 4. 执行物理仿真步长
-        p.stepSimulation()
-        time.sleep(1/240)  # 240Hz仿真频率
-
-except KeyboardInterrupt:
-    # 断开仿真连接
-    p.disconnect()
-    print("\n\n🔚 仿真已手动终止，感谢使用！")
\ No newline at end of file
diff --git a/src/Robot_arm_motion/test_pybullet_final.py b/src/Robot_arm_motion/test.py
similarity index 100%
rename from src/Robot_arm_motion/test_pybullet_final.py
rename to src/Robot_arm_motion/test.py

From 8611cefbf8082c4709652733af6de138cedd34c7 Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Mon, 22 Dec 2025 11:22:00 +0800
Subject: [PATCH 04/19] =?UTF-8?q?feat:=20v0.2-=E6=B7=BB=E5=8A=A0=E5=A4=B9?=
 =?UTF-8?q?=E7=88=AA=E5=BC=80=E5=90=88=E6=8E=A7=E5=88=B6=E5=8A=9F=E8=83=BD?=
 =?UTF-8?q?=EF=BC=88=E4=BF=AE=E5=A4=8DXML=20ctrlrange=E5=B1=9E=E6=80=A7?=
 =?UTF-8?q?=E9=94=99=E8=AF=AF=EF=BC=89?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/scripts/robot_arm.py | 37 +++++++++++++++++++----
 1 file changed, 31 insertions(+), 6 deletions(-)

diff --git a/src/Robot_arm_motion/scripts/robot_arm.py b/src/Robot_arm_motion/scripts/robot_arm.py
index c12ad7a153..a810cb96f1 100644
--- a/src/Robot_arm_motion/scripts/robot_arm.py
+++ b/src/Robot_arm_motion/scripts/robot_arm.py
@@ -1,17 +1,18 @@
 """
-MuJoCo机械臂仿真初始版本
-功能：基础2关节机械臂可视化+关节角度控制
+MuJoCo机械臂仿真v0.2
+功能：基础2关节机械臂可视化+关节角度控制+夹爪开合控制
 项目根目录：Robot_arm_motion
 代码目录：scripts
 作者：邓卓
 日期：2025-12-22
+版本：v0.2（添加夹爪开合控制，修复XML Schema错误）
 """
 import mujoco
 import mujoco.viewer as viewer
 import numpy as np
 
 def create_simple_arm_model():
-    """创建简易2关节机械臂+立方体模型的XML字符串"""
+    """创建简易2关节机械臂+立方体模型的XML字符串（修复ctrlrange属性错误）"""
     xml = """
     <mujoco>
       <option gravity="0 0 -9.81" timestep="0.001"/>
@@ -27,7 +28,20 @@ def create_simple_arm_model():
             <joint name="joint2" type="hinge" axis="1 0 0" pos="0 0 0.5"/>
             <body name="link2" pos="0 0 0.5">
               <geom type="capsule" size="0.05" fromto="0 0 0 0 0 0.4" rgba="0.2 0.8 0.2 1"/>
-              <geom name="gripper" type="box" size="0.08 0.08 0.02" pos="0 0 0.4" rgba="0.8 0.2 0.2 1"/>
+              <!-- 夹爪基座 -->
+              <body name="gripper_base" pos="0 0 0.4">
+                <geom type="box" size="0.02 0.08 0.02" pos="0 0 0" rgba="0.5 0.5 0.5 1"/>
+                <!-- 左夹爪（移除joint的ctrlrange属性） -->
+                <body name="left_gripper" pos="-0.04 0 0">
+                  <joint name="left_gripper_joint" type="slide" axis="1 0 0" pos="0 0 0"/>
+                  <geom name="left_gripper_geom" type="box" size="0.04 0.02 0.02" pos="0 0 0" rgba="0.8 0.2 0.2 1"/>
+                </body>
+                <!-- 右夹爪（移除joint的ctrlrange属性） -->
+                <body name="right_gripper" pos="0.04 0 0">
+                  <joint name="right_gripper_joint" type="slide" axis="1 0 0" pos="0 0 0"/>
+                  <geom name="right_gripper_geom" type="box" size="0.04 0.02 0.02" pos="0 0 0" rgba="0.8 0.2 0.2 1"/>
+                </body>
+              </body>
             </body>
           </body>
         </body>
@@ -38,15 +52,19 @@ def create_simple_arm_model():
         </body>
       </worldbody>
       <actuator>
+        <!-- 机械臂关节电机 -->
         <motor joint="joint1" ctrlrange="-3.14 3.14" gear="100"/>
         <motor joint="joint2" ctrlrange="-3.14 3.14" gear="100"/>
+        <!-- 夹爪关节电机（将ctrlrange移到此处） -->
+        <motor joint="left_gripper_joint" ctrlrange="0 0.06" gear="50"/>
+        <motor joint="right_gripper_joint" ctrlrange="-0.06 0" gear="50"/>
       </actuator>
     </mujoco>
     """
     return mujoco.MjModel.from_xml_string(xml)
 
 def main():
-    """主函数：加载模型+启动仿真"""
+    """主函数：加载模型+启动仿真（新增夹爪控制）"""
     # 加载模型
     model = create_simple_arm_model()
     data = mujoco.MjData(model)
@@ -57,8 +75,15 @@ def main():
     data.ctrl[joint1_id] = np.pi/4  # 关节1转45°
     data.ctrl[joint2_id] = -np.pi/6 # 关节2转-30°
 
+    # 新增：设置夹爪初始开合角度（半开状态）
+    left_gripper_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "left_gripper_joint")
+    right_gripper_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "right_gripper_joint")
+    data.ctrl[left_gripper_id] = 0.03    # 左夹爪张开3mm
+    data.ctrl[right_gripper_id] = -0.03  # 右夹爪张开3mm
+
     # 启动可视化
-    print("=== Robot_arm_motion 初始版本仿真启动 ===")
+    print("=== Robot_arm_motion v0.2 仿真启动（新增夹爪开合控制） ===")
+    print("当前夹爪状态：半开（3mm）")
     with viewer.launch_passive(model, data) as v:
         while v.is_running():
             mujoco.mj_step(model, data)

From 81cc517ea8c82d4ff9cd9216977bb5f5f61e4aae Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Mon, 22 Dec 2025 11:37:25 +0800
Subject: [PATCH 05/19] =?UTF-8?q?feat:=20v0.3-=E5=AE=9E=E7=8E=B0=E5=A4=B9?=
 =?UTF-8?q?=E7=88=AA=E4=B8=8E=E7=AB=8B=E6=96=B9=E4=BD=93=E7=9A=84=E6=8E=A5?=
 =?UTF-8?q?=E8=A7=A6=E6=A3=80=E6=B5=8B=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/scripts/robot_arm.py | 76 +++++++++++++++++++----
 1 file changed, 64 insertions(+), 12 deletions(-)

diff --git a/src/Robot_arm_motion/scripts/robot_arm.py b/src/Robot_arm_motion/scripts/robot_arm.py
index a810cb96f1..3a17f8b51a 100644
--- a/src/Robot_arm_motion/scripts/robot_arm.py
+++ b/src/Robot_arm_motion/scripts/robot_arm.py
@@ -1,18 +1,18 @@
 """
-MuJoCo机械臂仿真v0.2
-功能：基础2关节机械臂可视化+关节角度控制+夹爪开合控制
+MuJoCo机械臂仿真v0.3
+功能：基础2关节机械臂可视化+关节角度控制+夹爪开合控制+接触检测
 项目根目录：Robot_arm_motion
 代码目录：scripts
 作者：邓卓
 日期：2025-12-22
-版本：v0.2（添加夹爪开合控制，修复XML Schema错误）
+版本：v0.3（实现夹爪与立方体接触检测）
 """
 import mujoco
 import mujoco.viewer as viewer
 import numpy as np
 
 def create_simple_arm_model():
-    """创建简易2关节机械臂+立方体模型的XML字符串（修复ctrlrange属性错误）"""
+    """创建简易2关节机械臂+立方体模型的XML字符串"""
     xml = """
     <mujoco>
       <option gravity="0 0 -9.81" timestep="0.001"/>
@@ -31,12 +31,12 @@ def create_simple_arm_model():
               <!-- 夹爪基座 -->
               <body name="gripper_base" pos="0 0 0.4">
                 <geom type="box" size="0.02 0.08 0.02" pos="0 0 0" rgba="0.5 0.5 0.5 1"/>
-                <!-- 左夹爪（移除joint的ctrlrange属性） -->
+                <!-- 左夹爪 -->
                 <body name="left_gripper" pos="-0.04 0 0">
                   <joint name="left_gripper_joint" type="slide" axis="1 0 0" pos="0 0 0"/>
                   <geom name="left_gripper_geom" type="box" size="0.04 0.02 0.02" pos="0 0 0" rgba="0.8 0.2 0.2 1"/>
                 </body>
-                <!-- 右夹爪（移除joint的ctrlrange属性） -->
+                <!-- 右夹爪 -->
                 <body name="right_gripper" pos="0.04 0 0">
                   <joint name="right_gripper_joint" type="slide" axis="1 0 0" pos="0 0 0"/>
                   <geom name="right_gripper_geom" type="box" size="0.04 0.02 0.02" pos="0 0 0" rgba="0.8 0.2 0.2 1"/>
@@ -45,17 +45,17 @@ def create_simple_arm_model():
             </body>
           </body>
         </body>
-        <!-- 抓取目标：立方体 -->
+        <!-- 抓取目标：立方体（添加唯一geom名称） -->
         <body name="cube" pos="0.3 0 0.5">
           <joint type="free"/>
-          <geom type="box" size="0.05 0.05 0.05" rgba="0.8 0.8 0.2 1"/>
+          <geom name="cube_geom" type="box" size="0.05 0.05 0.05" rgba="0.8 0.8 0.2 1"/>
         </body>
       </worldbody>
       <actuator>
         <!-- 机械臂关节电机 -->
         <motor joint="joint1" ctrlrange="-3.14 3.14" gear="100"/>
         <motor joint="joint2" ctrlrange="-3.14 3.14" gear="100"/>
-        <!-- 夹爪关节电机（将ctrlrange移到此处） -->
+        <!-- 夹爪关节电机 -->
         <motor joint="left_gripper_joint" ctrlrange="0 0.06" gear="50"/>
         <motor joint="right_gripper_joint" ctrlrange="-0.06 0" gear="50"/>
       </actuator>
@@ -63,8 +63,43 @@ def create_simple_arm_model():
     """
     return mujoco.MjModel.from_xml_string(xml)
 
+# 新增：接触检测函数
+def check_gripper_cube_contact(model, data, gripper_geom_names, cube_geom_name):
+    """
+    检测夹爪与立方体是否接触
+    参数：
+        model: MuJoCo模型
+        data: MuJoCo数据
+        gripper_geom_names: 夹爪几何名称列表
+        cube_geom_name: 立方体几何名称
+    返回：
+        bool: True=接触，False=未接触
+    """
+    # 获取立方体几何的ID
+    cube_geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, cube_geom_name)
+    if cube_geom_id == -1:
+        print("错误：未找到立方体几何名称！")
+        return False
+
+    # 遍历所有接触对
+    for i in range(data.ncon):
+        contact = data.contact[i]
+        # 获取接触的两个几何ID
+        geom1_id = contact.geom1
+        geom2_id = contact.geom2
+
+        # 检查是否是夹爪与立方体的接触
+        for gripper_geom in gripper_geom_names:
+            gripper_geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, gripper_geom)
+            if gripper_geom_id == -1:
+                continue
+            if (geom1_id == gripper_geom_id and geom2_id == cube_geom_id) or \
+               (geom1_id == cube_geom_id and geom2_id == gripper_geom_id):
+                return True
+    return False
+
 def main():
-    """主函数：加载模型+启动仿真（新增夹爪控制）"""
+    """主函数：加载模型+启动仿真（新增接触检测）"""
     # 加载模型
     model = create_simple_arm_model()
     data = mujoco.MjData(model)
@@ -75,17 +110,34 @@ def main():
     data.ctrl[joint1_id] = np.pi/4  # 关节1转45°
     data.ctrl[joint2_id] = -np.pi/6 # 关节2转-30°
 
-    # 新增：设置夹爪初始开合角度（半开状态）
+    # 设置夹爪初始开合角度（半开状态）
     left_gripper_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "left_gripper_joint")
     right_gripper_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "right_gripper_joint")
     data.ctrl[left_gripper_id] = 0.03    # 左夹爪张开3mm
     data.ctrl[right_gripper_id] = -0.03  # 右夹爪张开3mm
 
+    # 定义夹爪和立方体的几何名称（用于接触检测）
+    gripper_geom_names = ["left_gripper_geom", "right_gripper_geom"]
+    cube_geom_name = "cube_geom"
+
     # 启动可视化
-    print("=== Robot_arm_motion v0.2 仿真启动（新增夹爪开合控制） ===")
+    print("=== Robot_arm_motion v0.3 仿真启动（新增接触检测） ===")
     print("当前夹爪状态：半开（3mm）")
+    print("开始实时检测夹爪与立方体的接触状态...\n")
     with viewer.launch_passive(model, data) as v:
+        last_contact_state = False  # 记录上一帧的接触状态
         while v.is_running():
+            # 检测夹爪与立方体的接触
+            is_contact = check_gripper_cube_contact(model, data, gripper_geom_names, cube_geom_name)
+
+            # 仅当状态变化时打印（避免控制台刷屏）
+            if is_contact != last_contact_state:
+                if is_contact:
+                    print("✅ 检测到夹爪与立方体接触！")
+                else:
+                    print("❌ 夹爪与立方体分离！")
+                last_contact_state = is_contact
+
             mujoco.mj_step(model, data)
             v.sync()
 

From e8e7b3540a097500beb45f6a0419a9ead0680f8e Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Mon, 22 Dec 2025 11:48:23 +0800
Subject: [PATCH 06/19] =?UTF-8?q?feat:=20v0.1-=E5=88=9D=E5=A7=8B=E7=89=88?=
 =?UTF-8?q?=E6=9C=AC-=E5=9F=BA=E7=A1=80=E6=9C=BA=E6=A2=B0=E8=87=82?=
 =?UTF-8?q?=E5=8F=AF=E8=A7=86=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 scripts/robot_arm.py | 68 ++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 68 insertions(+)
 create mode 100644 scripts/robot_arm.py

diff --git a/scripts/robot_arm.py b/scripts/robot_arm.py
new file mode 100644
index 0000000000..c12ad7a153
--- /dev/null
+++ b/scripts/robot_arm.py
@@ -0,0 +1,68 @@
+"""
+MuJoCo机械臂仿真初始版本
+功能：基础2关节机械臂可视化+关节角度控制
+项目根目录：Robot_arm_motion
+代码目录：scripts
+作者：邓卓
+日期：2025-12-22
+"""
+import mujoco
+import mujoco.viewer as viewer
+import numpy as np
+
+def create_simple_arm_model():
+    """创建简易2关节机械臂+立方体模型的XML字符串"""
+    xml = """
+    <mujoco>
+      <option gravity="0 0 -9.81" timestep="0.001"/>
+      <worldbody>
+        <!-- 地面 -->
+        <geom name="ground" type="plane" size="5 5 0.1" pos="0 0 0" rgba="0.8 0.8 0.8 1"/>
+        <!-- 机械臂基座 -->
+        <body name="base" pos="0 0 0">
+          <geom type="cylinder" size="0.1 0.1" rgba="0.2 0.2 0.8 1"/>
+          <joint name="joint1" type="hinge" axis="0 1 0" pos="0 0 0.1"/>
+          <body name="link1" pos="0 0 0.1">
+            <geom type="capsule" size="0.05" fromto="0 0 0 0 0 0.5" rgba="0.2 0.8 0.2 1"/>
+            <joint name="joint2" type="hinge" axis="1 0 0" pos="0 0 0.5"/>
+            <body name="link2" pos="0 0 0.5">
+              <geom type="capsule" size="0.05" fromto="0 0 0 0 0 0.4" rgba="0.2 0.8 0.2 1"/>
+              <geom name="gripper" type="box" size="0.08 0.08 0.02" pos="0 0 0.4" rgba="0.8 0.2 0.2 1"/>
+            </body>
+          </body>
+        </body>
+        <!-- 抓取目标：立方体 -->
+        <body name="cube" pos="0.3 0 0.5">
+          <joint type="free"/>
+          <geom type="box" size="0.05 0.05 0.05" rgba="0.8 0.8 0.2 1"/>
+        </body>
+      </worldbody>
+      <actuator>
+        <motor joint="joint1" ctrlrange="-3.14 3.14" gear="100"/>
+        <motor joint="joint2" ctrlrange="-3.14 3.14" gear="100"/>
+      </actuator>
+    </mujoco>
+    """
+    return mujoco.MjModel.from_xml_string(xml)
+
+def main():
+    """主函数：加载模型+启动仿真"""
+    # 加载模型
+    model = create_simple_arm_model()
+    data = mujoco.MjData(model)
+
+    # 设置初始关节角度
+    joint1_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "joint1")
+    joint2_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "joint2")
+    data.ctrl[joint1_id] = np.pi/4  # 关节1转45°
+    data.ctrl[joint2_id] = -np.pi/6 # 关节2转-30°
+
+    # 启动可视化
+    print("=== Robot_arm_motion 初始版本仿真启动 ===")
+    with viewer.launch_passive(model, data) as v:
+        while v.is_running():
+            mujoco.mj_step(model, data)
+            v.sync()
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file

From be9b7ac633ea7a42620f69995cdcc9a59568973c Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Mon, 22 Dec 2025 20:48:27 +0800
Subject: [PATCH 07/19] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=88=9D=E5=A7=8B?=
 =?UTF-8?q?=E4=BB=A3=E7=A0=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 250 ++++++++++++++++++++++++++++++
 1 file changed, 250 insertions(+)
 create mode 100644 src/Robot_arm_motion/robot_arm.py

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
new file mode 100644
index 0000000000..ec35bf8ad0
--- /dev/null
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -0,0 +1,250 @@
+import mujoco
+import mujoco.viewer as viewer
+import numpy as np
+import time
+import sys
+import os
+
+# ========== 路径适配 ==========
+SCENE_PATH = os.path.join(os.path.dirname(__file__),
+                          "mujoco_menagerie-main",
+                          "franka_emika_panda",
+                          "grab_scene.xml")
+
+if not os.path.exists(SCENE_PATH):
+    print(f"❌ 场景文件不存在：{SCENE_PATH}")
+    sys.exit(1)
+
+# ========== 智能抓取控制器 ==========
+class PandaAutoGrab:
+    def __init__(self):
+        self.model = mujoco.MjModel.from_xml_path(SCENE_PATH)
+        self.data = mujoco.MjData(self.model)
+        self.viewer = None
+        self.running = True
+        self.step_counter = 0
+        self.current_phase = 0
+        self.grab_complete = False
+
+        # 机械臂参数
+        self.ee_body_id = self.model.body("hand").id
+        self.joint_names = [f"joint{i}" for i in range(1, 8)]
+        self.joint_ids = [self.model.joint(name).id for name in self.joint_names]
+        self.gripper_joint_names = ["finger_joint1", "finger_joint2"]
+
+        # 雅克比矩阵
+        self.jacp = np.zeros((3, self.model.nv))
+        self.jacr = np.zeros((3, self.model.nv))
+
+        # 抓取参数
+        self.cube_body_id = self.model.body("cube").id
+        self.target_place_pos = np.array([0.3, 0.0, 0.1])
+        self.gripper_open_pos = 0.04
+        self.gripper_close_pos = 0.005
+        self.safe_lift_height = 0.15
+        self.grab_height = 0.05
+
+        # 打印模型信息
+        print("="*50)
+        print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
+        print("📌 模型Joint列表：", [self.model.joint(i).name for i in range(min(self.model.njnt, 10))])
+        print("="*50)
+
+    def get_ee_pos(self):
+        """获取末端执行器位置"""
+        return self.data.xpos[self.ee_body_id].copy()
+
+    def get_cube_pos(self):
+        """获取立方体位置"""
+        return self.data.xpos[self.cube_body_id].copy()
+
+    def _compute_jacobian(self):
+        """计算雅克比矩阵"""
+        mujoco.mj_jac(self.model, self.data, self.jacp, self.jacr, self.get_ee_pos(), self.ee_body_id)
+        return self.jacp[:, self.joint_ids]
+
+    def _move_step(self, target, tol=0.003, speed=0.3):
+        """单步移动控制（修复维度匹配问题）"""
+        ee_pos = self.get_ee_pos()
+        error = target - ee_pos
+        error_norm = np.linalg.norm(error)
+
+        if error_norm < tol:
+            return True  # 到达目标
+
+        # 计算雅克比矩阵
+        jacobian = self._compute_jacobian()  # 3×7
+
+        # ========== 修正：正确的阻尼伪逆计算 ==========
+        # 方法1：使用正则化参数的伪逆（推荐）
+        lambda_ = 0.01  # 阻尼系数
+        jacobian_pinv = jacobian.T @ np.linalg.inv(jacobian @ jacobian.T + lambda_ * np.eye(3))
+
+        # 方法2：若方法1仍报错，可改用numpy伪逆（自动处理维度）
+        # jacobian_pinv = np.linalg.pinv(jacobian, rcond=1e-3)
+
+        # 关节速度指令
+        joint_vel_cmd = speed * jacobian_pinv @ error
+        joint_vel_cmd = np.clip(joint_vel_cmd, -0.5, 0.5)  # 速度限制
+
+        # PD力矩计算
+        torque = np.zeros(7)
+        for i in range(7):
+            angle_error = joint_vel_cmd[i] * 0.1
+            torque[i] = 250 * angle_error - 100 * self.data.qvel[self.joint_ids[i]]
+            torque[i] = np.clip(torque[i], -20, 20)
+
+        # 设置关节力矩
+        for i in range(7):
+            self.data.ctrl[self.joint_ids[i]] = torque[i]
+
+        return False
+
+    def _gripper_step(self, pos):
+        """单步夹爪控制"""
+        for j_name in self.gripper_joint_names:
+            j_id = self.model.joint(j_name).id
+            self.data.ctrl[j_id] = pos
+        return True
+
+    def _grab_phase_machine(self):
+        """抓取状态机"""
+        if self.current_phase == 0:
+            # 阶段0：移动到初始位置
+            if self._move_step(np.array([0.4, 0.0, 0.2])):
+                print("\n✅ 到达初始位置")
+                self.current_phase = 1
+                self.step_counter = 0
+
+        elif self.current_phase == 1:
+            # 阶段1：获取立方体位置
+            self.cube_pos = self.get_cube_pos()
+            print(f"\n🎯 识别到立方体位置：{np.round(self.cube_pos, 3)}")
+            self.current_phase = 2
+
+        elif self.current_phase == 2:
+            # 阶段2：移动到立方体上方
+            if self._move_step(self.cube_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4):
+                print("\n✅ 到达立方体上方")
+                self.current_phase = 3
+                self.step_counter = 0
+
+        elif self.current_phase == 3:
+            # 阶段3：打开夹爪
+            if self.step_counter == 0:
+                self._gripper_step(self.gripper_open_pos)
+                print("\n✋ 打开夹爪")
+            if self.step_counter > 100:  # 等待夹爪动作
+                self.current_phase = 4
+                self.step_counter = 0
+            self.step_counter += 1
+
+        elif self.current_phase == 4:
+            # 阶段4：下降抓取
+            if self._move_step(self.cube_pos + np.array([0, 0, self.grab_height]), speed=0.2):
+                print("\n✅ 下降到抓取高度")
+                self.current_phase = 5
+                self.step_counter = 0
+
+        elif self.current_phase == 5:
+            # 阶段5：闭合夹爪
+            if self.step_counter == 0:
+                self._gripper_step(self.gripper_close_pos)
+                print("\n🤏 闭合夹爪抓取")
+            if self.step_counter > 100:
+                self.current_phase = 6
+                self.step_counter = 0
+            self.step_counter += 1
+
+        elif self.current_phase == 6:
+            # 阶段6：抬升立方体
+            if self._move_step(self.cube_pos + np.array([0, 0, self.safe_lift_height + 0.05]), speed=0.3):
+                print("\n✅ 抬升立方体")
+                self.current_phase = 7
+                self.step_counter = 0
+
+        elif self.current_phase == 7:
+            # 阶段7：移动到放置点上方
+            if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4):
+                print("\n✅ 到达放置点上方")
+                self.current_phase = 8
+                self.step_counter = 0
+
+        elif self.current_phase == 8:
+            # 阶段8：下降放置
+            if self._move_step(self.target_place_pos + np.array([0, 0, self.grab_height]), speed=0.2):
+                print("\n✅ 下降到放置高度")
+                self.current_phase = 9
+                self.step_counter = 0
+
+        elif self.current_phase == 9:
+            # 阶段9：释放立方体
+            if self.step_counter == 0:
+                self._gripper_step(self.gripper_open_pos)
+                print("\n🫳 释放立方体")
+            if self.step_counter > 100:
+                self.current_phase = 10
+                self.step_counter = 0
+            self.step_counter += 1
+
+        elif self.current_phase == 10:
+            # 阶段10：撤离机械臂
+            if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.3):
+                print("\n✅ 撤离机械臂")
+                self.current_phase = 11
+                self.step_counter = 0
+
+        elif self.current_phase == 11:
+            # 阶段11：返回初始位置
+            if self._move_step(np.array([0.4, 0.0, 0.2]), speed=0.4):
+                print("\n✅ 返回初始位置")
+                self.current_phase = 12
+
+        elif self.current_phase == 12:
+            # 阶段12：抓取完成
+            if not self.grab_complete:
+                print("\n" + "="*50)
+                print("✅ 智能抓取任务完成！")
+                print("="*50)
+                self.grab_complete = True
+
+    def run(self):
+        """单线程仿真主循环"""
+        # 初始化Viewer
+        self.viewer = viewer.launch_passive(self.model, self.data)
+        self.viewer.cam.azimuth = 70
+        self.viewer.cam.elevation = -25
+        self.viewer.cam.distance = 1.8
+        self.viewer.cam.lookat = np.array([0.4, 0.0, 0.1])
+
+        print("\n🚀 仿真已启动，开始自动抓取...")
+        print("💡 关闭Viewer窗口可退出程序")
+
+        # 单线程主循环
+        while self.viewer.is_running():
+            if self.running and not self.grab_complete:
+                self._grab_phase_machine()
+            else:
+                # 抓取完成后归零力矩
+                for i in range(7):
+                    self.data.ctrl[self.joint_ids[i]] = 0
+
+            mujoco.mj_step(self.model, self.data)
+            self.viewer.sync()
+            time.sleep(1/200)
+
+        # 清理
+        self.running = False
+        self.viewer.close()
+        print("\n👋 仿真结束")
+
+# ========== 主函数 ==========
+if __name__ == "__main__":
+    try:
+        panda = PandaAutoGrab()
+        panda.run()
+    except Exception as e:
+        print(f"\n❌ 程序错误：{e}")
+        import traceback
+        traceback.print_exc()
+        sys.exit(1)
\ No newline at end of file

From 88c69ac97dabb69545c3cf08792dee38d66fcaaf Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Tue, 23 Dec 2025 17:18:38 +0800
Subject: [PATCH 08/19] =?UTF-8?q?=E6=8F=90=E5=8F=96PD=E6=8E=A7=E5=88=B6?=
 =?UTF-8?q?=E5=8F=82=E6=95=B0=E4=B8=BA=E7=B1=BB=E5=86=85=E5=B8=B8=E9=87=8F?=
 =?UTF-8?q?=E5=B9=B6=E6=B7=BB=E5=8A=A0=E7=B1=BB=E5=88=9D=E5=A7=8B=E5=8C=96?=
 =?UTF-8?q?=E6=96=87=E6=A1=A3=E5=AD=97=E7=AC=A6=E4=B8=B2?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 16 +++++++++++-----
 1 file changed, 11 insertions(+), 5 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index 213f40b563..31cbafb06c 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -5,7 +5,6 @@
 import sys
 import os
 
-# 【优化1】添加文件头注释，标注项目信息
 """
 Franka Panda 机械臂自动抓取仿真 v1.1
 基于MuJoCo实现的基础抓取控制器
@@ -24,6 +23,7 @@
 # ========== 智能抓取控制器 ==========
 class PandaAutoGrab:
     def __init__(self):
+        """初始化Franka Panda机械臂抓取控制器，加载模型和初始化参数"""
         self.model = mujoco.MjModel.from_xml_path(SCENE_PATH)
         self.data = mujoco.MjData(self.model)
         self.viewer = None
@@ -50,6 +50,11 @@ def __init__(self):
         self.safe_lift_height = 0.15
         self.grab_height = 0.05
 
+        # 【优化1】提取PD控制参数为类内常量
+        self.PD_KP = 250  # 比例增益
+        self.PD_KD = 100  # 微分增益
+        self.TORQUE_LIMIT = 20  # 力矩限制
+
         # 打印模型信息
         print("="*50)
         print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
@@ -97,8 +102,9 @@ def _move_step(self, target, tol=0.003, speed=0.3):
         torque = np.zeros(7)
         for i in range(7):
             angle_error = joint_vel_cmd[i] * 0.1
-            torque[i] = 250 * angle_error - 100 * self.data.qvel[self.joint_ids[i]]
-            torque[i] = np.clip(torque[i], -20, 20)
+            # 【优化2】使用类内常量替代硬编码的PD参数
+            torque[i] = self.PD_KP * angle_error - self.PD_KD * self.data.qvel[self.joint_ids[i]]
+            torque[i] = np.clip(torque[i], -self.TORQUE_LIMIT, self.TORQUE_LIMIT)
 
         # 设置关节力矩
         for i in range(7):
@@ -226,11 +232,11 @@ def run(self):
         print("\n🚀 仿真已启动，开始自动抓取...")
         print("💡 关闭Viewer窗口可退出程序")
 
-        # 【优化2】提取休眠时间为常量，便于后续调整
+        # 提取休眠时间为常量，便于后续调整
         SIMULATION_SLEEP = 1/200
 
         # 单线程主循环
-        # 【优化3】添加KeyboardInterrupt捕获，支持Ctrl+C优雅退出
+        # 添加KeyboardInterrupt捕获，支持Ctrl+C优雅退出
         try:
             while self.viewer.is_running():
                 if self.running and not self.grab_complete:

From 9517f2eb08d166b7a16e6aaaad59d4db2587886a Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Wed, 24 Dec 2025 13:58:50 +0800
Subject: [PATCH 09/19] =?UTF-8?q?=E6=8F=90=E5=8F=96=E9=9B=85=E5=85=8B?=
 =?UTF-8?q?=E6=AF=94=E9=98=BB=E5=B0=BC=E7=B3=BB=E6=95=B0=E4=B8=BA=E5=B8=B8?=
 =?UTF-8?q?=E9=87=8F=E5=B9=B6=E5=AE=8C=E5=96=84=5Fcompute=5Fjacobian?=
 =?UTF-8?q?=E6=96=87=E6=A1=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 29 +++++++++++++++++++----------
 1 file changed, 19 insertions(+), 10 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index 31cbafb06c..d6626d88f2 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -20,6 +20,7 @@
     print(f"❌ 场景文件不存在：{SCENE_PATH}")
     sys.exit(1)
 
+
 # ========== 智能抓取控制器 ==========
 class PandaAutoGrab:
     def __init__(self):
@@ -50,16 +51,19 @@ def __init__(self):
         self.safe_lift_height = 0.15
         self.grab_height = 0.05
 
-        # 【优化1】提取PD控制参数为类内常量
+        # PD控制参数
         self.PD_KP = 250  # 比例增益
         self.PD_KD = 100  # 微分增益
         self.TORQUE_LIMIT = 20  # 力矩限制
 
+        # 【优化1】提取雅克比伪逆的阻尼系数为类内常量
+        self.JACOBIAN_DAMPING = 0.01  # 雅克比伪逆的阻尼系数
+
         # 打印模型信息
-        print("="*50)
+        print("=" * 50)
         print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
         print("📌 模型Joint列表：", [self.model.joint(i).name for i in range(min(self.model.njnt, 10))])
-        print("="*50)
+        print("=" * 50)
 
     def get_ee_pos(self):
         """获取末端执行器位置"""
@@ -70,7 +74,11 @@ def get_cube_pos(self):
         return self.data.xpos[self.cube_body_id].copy()
 
     def _compute_jacobian(self):
-        """计算雅克比矩阵"""
+        """计算末端执行器的位置雅克比矩阵
+
+        Returns:
+            np.ndarray: 3×7的位置雅克比矩阵（仅包含机械臂7个关节的分量）
+        """
         mujoco.mj_jac(self.model, self.data, self.jacp, self.jacr, self.get_ee_pos(), self.ee_body_id)
         return self.jacp[:, self.joint_ids]
 
@@ -88,8 +96,8 @@ def _move_step(self, target, tol=0.003, speed=0.3):
 
         # ========== 修正：正确的阻尼伪逆计算 ==========
         # 方法1：使用正则化参数的伪逆（推荐）
-        lambda_ = 0.01  # 阻尼系数
-        jacobian_pinv = jacobian.T @ np.linalg.inv(jacobian @ jacobian.T + lambda_ * np.eye(3))
+        # 【优化2】使用类内常量替代硬编码的阻尼系数
+        jacobian_pinv = jacobian.T @ np.linalg.inv(jacobian @ jacobian.T + self.JACOBIAN_DAMPING * np.eye(3))
 
         # 方法2：若方法1仍报错，可改用numpy伪逆（自动处理维度）
         # jacobian_pinv = np.linalg.pinv(jacobian, rcond=1e-3)
@@ -102,7 +110,6 @@ def _move_step(self, target, tol=0.003, speed=0.3):
         torque = np.zeros(7)
         for i in range(7):
             angle_error = joint_vel_cmd[i] * 0.1
-            # 【优化2】使用类内常量替代硬编码的PD参数
             torque[i] = self.PD_KP * angle_error - self.PD_KD * self.data.qvel[self.joint_ids[i]]
             torque[i] = np.clip(torque[i], -self.TORQUE_LIMIT, self.TORQUE_LIMIT)
 
@@ -215,9 +222,9 @@ def _grab_phase_machine(self):
         elif self.current_phase == 12:
             # 阶段12：抓取完成
             if not self.grab_complete:
-                print("\n" + "="*50)
+                print("\n" + "=" * 50)
                 print("✅ 智能抓取任务完成！")
-                print("="*50)
+                print("=" * 50)
                 self.grab_complete = True
 
     def run(self):
@@ -233,7 +240,7 @@ def run(self):
         print("💡 关闭Viewer窗口可退出程序")
 
         # 提取休眠时间为常量，便于后续调整
-        SIMULATION_SLEEP = 1/200
+        SIMULATION_SLEEP = 1 / 200
 
         # 单线程主循环
         # 添加KeyboardInterrupt捕获，支持Ctrl+C优雅退出
@@ -257,6 +264,7 @@ def run(self):
         self.viewer.close()
         print("\n👋 仿真结束")
 
+
 # ========== 主函数 ==========
 if __name__ == "__main__":
     try:
@@ -265,5 +273,6 @@ def run(self):
     except Exception as e:
         print(f"\n❌ 程序错误：{e}")
         import traceback
+
         traceback.print_exc()
         sys.exit(1)
\ No newline at end of file

From 1cf25b092101f6e5ee10497f3ede8185b6822168 Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Wed, 24 Dec 2025 19:32:10 +0800
Subject: [PATCH 10/19] =?UTF-8?q?=E6=8F=90=E5=8F=96=E5=85=B3=E8=8A=82?=
 =?UTF-8?q?=E9=80=9F=E5=BA=A6=E9=99=90=E5=88=B6=E5=B8=B8=E9=87=8F=E5=B9=B6?=
 =?UTF-8?q?=E5=AE=8C=E5=96=84=5Fmove=5Fstep=E6=96=87=E6=A1=A3=E5=AD=97?=
 =?UTF-8?q?=E7=AC=A6=E4=B8=B2?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 20 ++++++++++++++++----
 1 file changed, 16 insertions(+), 4 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index d6626d88f2..77f4e6183c 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -56,9 +56,12 @@ def __init__(self):
         self.PD_KD = 100  # 微分增益
         self.TORQUE_LIMIT = 20  # 力矩限制
 
-        # 【优化1】提取雅克比伪逆的阻尼系数为类内常量
+        # 雅克比伪逆参数
         self.JACOBIAN_DAMPING = 0.01  # 雅克比伪逆的阻尼系数
 
+        # 【优化1】提取关节速度限制为类内常量
+        self.JOINT_VEL_LIMIT = 0.5  # 关节速度上限
+
         # 打印模型信息
         print("=" * 50)
         print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
@@ -83,7 +86,16 @@ def _compute_jacobian(self):
         return self.jacp[:, self.joint_ids]
 
     def _move_step(self, target, tol=0.003, speed=0.3):
-        """单步移动控制（修复维度匹配问题）"""
+        """单步位置控制：基于雅克比伪逆实现末端执行器的位置跟踪
+
+        Args:
+            target (np.ndarray): 末端执行器的目标位置，形状为(3,)的三维坐标[x, y, z]
+            tol (float): 位置误差容忍阈值，当实际位置与目标位置的欧氏距离小于该值时，认为到达目标
+            speed (float): 移动速度系数，控制机械臂的运动速度
+
+        Returns:
+            bool: 若到达目标位置返回True，否则返回False
+        """
         ee_pos = self.get_ee_pos()
         error = target - ee_pos
         error_norm = np.linalg.norm(error)
@@ -96,7 +108,6 @@ def _move_step(self, target, tol=0.003, speed=0.3):
 
         # ========== 修正：正确的阻尼伪逆计算 ==========
         # 方法1：使用正则化参数的伪逆（推荐）
-        # 【优化2】使用类内常量替代硬编码的阻尼系数
         jacobian_pinv = jacobian.T @ np.linalg.inv(jacobian @ jacobian.T + self.JACOBIAN_DAMPING * np.eye(3))
 
         # 方法2：若方法1仍报错，可改用numpy伪逆（自动处理维度）
@@ -104,7 +115,8 @@ def _move_step(self, target, tol=0.003, speed=0.3):
 
         # 关节速度指令
         joint_vel_cmd = speed * jacobian_pinv @ error
-        joint_vel_cmd = np.clip(joint_vel_cmd, -0.5, 0.5)  # 速度限制
+        # 【优化2】使用类内常量替代硬编码的关节速度限制
+        joint_vel_cmd = np.clip(joint_vel_cmd, -self.JOINT_VEL_LIMIT, self.JOINT_VEL_LIMIT)
 
         # PD力矩计算
         torque = np.zeros(7)

From 0d4223a762e4c5f5a3c58a85281b91a48a1b6200 Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Wed, 24 Dec 2025 21:31:06 +0800
Subject: [PATCH 11/19] =?UTF-8?q?=E6=8F=90=E5=8F=96=E4=BD=8D=E7=BD=AE?=
 =?UTF-8?q?=E8=AF=AF=E5=B7=AE=E9=98=88=E5=80=BC=E5=B8=B8=E9=87=8F=E5=B9=B6?=
 =?UTF-8?q?=E5=AE=8C=E5=96=84=5Fgripper=5Fstep=E6=96=87=E6=A1=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 22 ++++++++++++----------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index de173de815..e9377ebed5 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -52,7 +52,6 @@ def __init__(self):
         self.grab_height = 0.05
 
         # PD控制参数
-        # 【优化1】提取PD控制参数为类内常量
         self.PD_KP = 250  # 比例增益
         self.PD_KD = 100  # 微分增益
         self.TORQUE_LIMIT = 20  # 力矩限制
@@ -60,9 +59,12 @@ def __init__(self):
         # 雅克比伪逆参数
         self.JACOBIAN_DAMPING = 0.01  # 雅克比伪逆的阻尼系数
 
-        # 【优化1】提取关节速度限制为类内常量
+        # 关节速度参数
         self.JOINT_VEL_LIMIT = 0.5  # 关节速度上限
 
+        # 【优化1】提取位置误差容忍阈值为类内常量
+        self.POS_TOLERANCE = 0.003  # 末端执行器位置误差容忍阈值
+
         # 打印模型信息
         print("=" * 50)
         print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
@@ -86,12 +88,11 @@ def _compute_jacobian(self):
         mujoco.mj_jac(self.model, self.data, self.jacp, self.jacr, self.get_ee_pos(), self.ee_body_id)
         return self.jacp[:, self.joint_ids]
 
-    def _move_step(self, target, tol=0.003, speed=0.3):
+    def _move_step(self, target, speed=0.3):
         """单步位置控制：基于雅克比伪逆实现末端执行器的位置跟踪
 
         Args:
             target (np.ndarray): 末端执行器的目标位置，形状为(3,)的三维坐标[x, y, z]
-            tol (float): 位置误差容忍阈值，当实际位置与目标位置的欧氏距离小于该值时，认为到达目标
             speed (float): 移动速度系数，控制机械臂的运动速度
 
         Returns:
@@ -101,7 +102,8 @@ def _move_step(self, target, tol=0.003, speed=0.3):
         error = target - ee_pos
         error_norm = np.linalg.norm(error)
 
-        if error_norm < tol:
+        # 【优化2】使用类内常量替代硬编码的位置误差阈值
+        if error_norm < self.POS_TOLERANCE:
             return True  # 到达目标
 
         # 计算雅克比矩阵
@@ -116,14 +118,12 @@ def _move_step(self, target, tol=0.003, speed=0.3):
 
         # 关节速度指令
         joint_vel_cmd = speed * jacobian_pinv @ error
-        # 【优化2】使用类内常量替代硬编码的关节速度限制
         joint_vel_cmd = np.clip(joint_vel_cmd, -self.JOINT_VEL_LIMIT, self.JOINT_VEL_LIMIT)
 
         # PD力矩计算
         torque = np.zeros(7)
         for i in range(7):
             angle_error = joint_vel_cmd[i] * 0.1
-            # 【优化2】使用类内常量替代硬编码的PD参数
             torque[i] = self.PD_KP * angle_error - self.PD_KD * self.data.qvel[self.joint_ids[i]]
             torque[i] = np.clip(torque[i], -self.TORQUE_LIMIT, self.TORQUE_LIMIT)
 
@@ -134,11 +134,14 @@ def _move_step(self, target, tol=0.003, speed=0.3):
         return False
 
     def _gripper_step(self, pos):
-        """单步夹爪控制"""
+        """单步夹爪位置控制，设置夹爪的目标开合位置
+
+        Args:
+            pos (float): 夹爪目标位置，0.04为完全打开，0.005为闭合抓取
+        """
         for j_name in self.gripper_joint_names:
             j_id = self.model.joint(j_name).id
             self.data.ctrl[j_id] = pos
-        return True
 
     def _grab_phase_machine(self):
         """抓取状态机"""
@@ -255,7 +258,6 @@ def run(self):
 
         # 提取休眠时间为常量，便于后续调整
         SIMULATION_SLEEP = 1 / 200
-        SIMULATION_SLEEP = 1/200
 
         # 单线程主循环
         # 添加KeyboardInterrupt捕获，支持Ctrl+C优雅退出

From 697865ecad6e3d1a009544c981ccaeedf47b663e Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Thu, 25 Dec 2025 12:41:18 +0800
Subject: [PATCH 12/19] =?UTF-8?q?=E6=8F=90=E5=8F=96=E5=A4=B9=E7=88=AA?=
 =?UTF-8?q?=E7=AD=89=E5=BE=85=E6=AD=A5=E6=95=B0=E5=B8=B8=E9=87=8F=E5=B9=B6?=
 =?UTF-8?q?=E5=AE=8C=E5=96=84=5Fgrab=5Fphase=5Fmachine=E6=96=87=E6=A1=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 21 +++++++++++++++------
 1 file changed, 15 insertions(+), 6 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index e9377ebed5..974441299e 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -62,9 +62,12 @@ def __init__(self):
         # 关节速度参数
         self.JOINT_VEL_LIMIT = 0.5  # 关节速度上限
 
-        # 【优化1】提取位置误差容忍阈值为类内常量
+        # 位置控制参数
         self.POS_TOLERANCE = 0.003  # 末端执行器位置误差容忍阈值
 
+        # 【优化1】提取夹爪等待步数为类内常量
+        self.GRIPPER_WAIT_STEPS = 100  # 夹爪动作完成所需的等待步数
+
         # 打印模型信息
         print("=" * 50)
         print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
@@ -102,7 +105,6 @@ def _move_step(self, target, speed=0.3):
         error = target - ee_pos
         error_norm = np.linalg.norm(error)
 
-        # 【优化2】使用类内常量替代硬编码的位置误差阈值
         if error_norm < self.POS_TOLERANCE:
             return True  # 到达目标
 
@@ -144,7 +146,11 @@ def _gripper_step(self, pos):
             self.data.ctrl[j_id] = pos
 
     def _grab_phase_machine(self):
-        """抓取状态机"""
+        """抓取状态机：按阶段执行机械臂的抓取、移动、放置等一系列动作
+
+        状态机分为12个阶段，从初始位置移动→识别立方体→抓取→放置→返回，
+        每个阶段完成后自动切换到下一个阶段，直到抓取任务完成。
+        """
         if self.current_phase == 0:
             # 阶段0：移动到初始位置
             if self._move_step(np.array([0.4, 0.0, 0.2])):
@@ -170,7 +176,8 @@ def _grab_phase_machine(self):
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n✋ 打开夹爪")
-            if self.step_counter > 100:  # 等待夹爪动作
+            # 【优化2】使用类内常量替代硬编码的等待步数
+            if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = 4
                 self.step_counter = 0
             self.step_counter += 1
@@ -187,7 +194,8 @@ def _grab_phase_machine(self):
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_close_pos)
                 print("\n🤏 闭合夹爪抓取")
-            if self.step_counter > 100:
+            # 【优化2】使用类内常量替代硬编码的等待步数
+            if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = 6
                 self.step_counter = 0
             self.step_counter += 1
@@ -218,7 +226,8 @@ def _grab_phase_machine(self):
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n🫳 释放立方体")
-            if self.step_counter > 100:
+            # 【优化2】使用类内常量替代硬编码的等待步数
+            if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = 10
                 self.step_counter = 0
             self.step_counter += 1

From c9d1524d4e44090fafda777165d2191a044826e3 Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Thu, 25 Dec 2025 20:26:26 +0800
Subject: [PATCH 13/19] =?UTF-8?q?=E6=8F=90=E5=8F=96=E5=88=9D=E5=A7=8B?=
 =?UTF-8?q?=E4=BD=8D=E7=BD=AE=E5=B8=B8=E9=87=8F=E5=B9=B6=E6=B7=BB=E5=8A=A0?=
 =?UTF-8?q?=E4=BD=8D=E7=BD=AE=E6=96=B9=E6=B3=95=E7=9A=84=E7=B1=BB=E5=9E=8B?=
 =?UTF-8?q?=E6=B3=A8=E8=A7=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 30 ++++++++++++++++++++----------
 1 file changed, 20 insertions(+), 10 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index 974441299e..a9cfa76723 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -65,21 +65,32 @@ def __init__(self):
         # 位置控制参数
         self.POS_TOLERANCE = 0.003  # 末端执行器位置误差容忍阈值
 
-        # 【优化1】提取夹爪等待步数为类内常量
+        # 夹爪控制参数
         self.GRIPPER_WAIT_STEPS = 100  # 夹爪动作完成所需的等待步数
 
+        # 【优化1】提取机械臂初始位置为类内常量
+        self.INIT_EE_POS = np.array([0.4, 0.0, 0.2])  # 末端执行器初始目标位置
+
         # 打印模型信息
         print("=" * 50)
         print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
         print("📌 模型Joint列表：", [self.model.joint(i).name for i in range(min(self.model.njnt, 10))])
         print("=" * 50)
 
-    def get_ee_pos(self):
-        """获取末端执行器位置"""
+    def get_ee_pos(self) -> np.ndarray:
+        """获取末端执行器位置
+
+        Returns:
+            np.ndarray: 末端执行器的三维位置坐标[x, y, z]
+        """
         return self.data.xpos[self.ee_body_id].copy()
 
-    def get_cube_pos(self):
-        """获取立方体位置"""
+    def get_cube_pos(self) -> np.ndarray:
+        """获取立方体位置
+
+        Returns:
+            np.ndarray: 立方体的三维位置坐标[x, y, z]
+        """
         return self.data.xpos[self.cube_body_id].copy()
 
     def _compute_jacobian(self):
@@ -153,7 +164,8 @@ def _grab_phase_machine(self):
         """
         if self.current_phase == 0:
             # 阶段0：移动到初始位置
-            if self._move_step(np.array([0.4, 0.0, 0.2])):
+            # 【优化2】使用类内常量替代硬编码的初始位置
+            if self._move_step(self.INIT_EE_POS):
                 print("\n✅ 到达初始位置")
                 self.current_phase = 1
                 self.step_counter = 0
@@ -176,7 +188,6 @@ def _grab_phase_machine(self):
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n✋ 打开夹爪")
-            # 【优化2】使用类内常量替代硬编码的等待步数
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = 4
                 self.step_counter = 0
@@ -194,7 +205,6 @@ def _grab_phase_machine(self):
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_close_pos)
                 print("\n🤏 闭合夹爪抓取")
-            # 【优化2】使用类内常量替代硬编码的等待步数
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = 6
                 self.step_counter = 0
@@ -226,7 +236,6 @@ def _grab_phase_machine(self):
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n🫳 释放立方体")
-            # 【优化2】使用类内常量替代硬编码的等待步数
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = 10
                 self.step_counter = 0
@@ -241,7 +250,8 @@ def _grab_phase_machine(self):
 
         elif self.current_phase == 11:
             # 阶段11：返回初始位置
-            if self._move_step(np.array([0.4, 0.0, 0.2]), speed=0.4):
+            # 【优化2】使用类内常量替代硬编码的初始位置
+            if self._move_step(self.INIT_EE_POS, speed=0.4):
                 print("\n✅ 返回初始位置")
                 self.current_phase = 12
 

From 5b43246f070f2e224a1b2c6b829fe97aafea2c7b Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Fri, 26 Dec 2025 15:09:10 +0800
Subject: [PATCH 14/19] =?UTF-8?q?=E6=8F=90=E5=8F=96=E6=8A=AC=E5=8D=87?=
 =?UTF-8?q?=E9=AB=98=E5=BA=A6=E5=A2=9E=E9=87=8F=E5=B8=B8=E9=87=8F=E5=B9=B6?=
 =?UTF-8?q?=E5=AE=8C=E5=96=84=5Fcompute=5Fjacobian=E7=B1=BB=E5=9E=8B?=
 =?UTF-8?q?=E6=B3=A8=E8=A7=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index a9cfa76723..548c07660f 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -68,9 +68,12 @@ def __init__(self):
         # 夹爪控制参数
         self.GRIPPER_WAIT_STEPS = 100  # 夹爪动作完成所需的等待步数
 
-        # 【优化1】提取机械臂初始位置为类内常量
+        # 位置坐标参数
         self.INIT_EE_POS = np.array([0.4, 0.0, 0.2])  # 末端执行器初始目标位置
 
+        # 【优化1】提取抬升高度增量为类内常量
+        self.LIFT_HEIGHT_INCREMENT = 0.05  # 抓取后额外抬升的高度增量
+
         # 打印模型信息
         print("=" * 50)
         print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
@@ -93,7 +96,7 @@ def get_cube_pos(self) -> np.ndarray:
         """
         return self.data.xpos[self.cube_body_id].copy()
 
-    def _compute_jacobian(self):
+    def _compute_jacobian(self) -> np.ndarray:
         """计算末端执行器的位置雅克比矩阵
 
         Returns:
@@ -164,7 +167,6 @@ def _grab_phase_machine(self):
         """
         if self.current_phase == 0:
             # 阶段0：移动到初始位置
-            # 【优化2】使用类内常量替代硬编码的初始位置
             if self._move_step(self.INIT_EE_POS):
                 print("\n✅ 到达初始位置")
                 self.current_phase = 1
@@ -212,7 +214,9 @@ def _grab_phase_machine(self):
 
         elif self.current_phase == 6:
             # 阶段6：抬升立方体
-            if self._move_step(self.cube_pos + np.array([0, 0, self.safe_lift_height + 0.05]), speed=0.3):
+            # 【优化2】使用类内常量替代硬编码的抬升增量
+            lift_target = self.cube_pos + np.array([0, 0, self.safe_lift_height + self.LIFT_HEIGHT_INCREMENT])
+            if self._move_step(lift_target, speed=0.3):
                 print("\n✅ 抬升立方体")
                 self.current_phase = 7
                 self.step_counter = 0
@@ -250,7 +254,6 @@ def _grab_phase_machine(self):
 
         elif self.current_phase == 11:
             # 阶段11：返回初始位置
-            # 【优化2】使用类内常量替代硬编码的初始位置
             if self._move_step(self.INIT_EE_POS, speed=0.4):
                 print("\n✅ 返回初始位置")
                 self.current_phase = 12

From 9682b40be78410c6d584f49b4f7879df05d11a25 Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Fri, 26 Dec 2025 15:57:30 +0800
Subject: [PATCH 15/19] =?UTF-8?q?=E6=8F=90=E5=8F=96=E7=9B=B8=E6=9C=BA?=
 =?UTF-8?q?=E5=8F=82=E6=95=B0=E5=B8=B8=E9=87=8F=E5=B9=B6=E5=AE=8C=E5=96=84?=
 =?UTF-8?q?=5Fmove=5Fstep=E7=B1=BB=E5=9E=8B=E6=B3=A8=E8=A7=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 20 ++++++++++++--------
 1 file changed, 12 insertions(+), 8 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index 548c07660f..b6bc64b457 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -70,10 +70,14 @@ def __init__(self):
 
         # 位置坐标参数
         self.INIT_EE_POS = np.array([0.4, 0.0, 0.2])  # 末端执行器初始目标位置
-
-        # 【优化1】提取抬升高度增量为类内常量
         self.LIFT_HEIGHT_INCREMENT = 0.05  # 抓取后额外抬升的高度增量
 
+        # 【优化1】提取相机视角参数为类内常量
+        self.CAM_AZIMUTH = 70  # 相机方位角
+        self.CAM_ELEVATION = -25  # 相机仰角
+        self.CAM_DISTANCE = 1.8  # 相机距离
+        self.CAM_LOOKAT = np.array([0.4, 0.0, 0.1])  # 相机注视点
+
         # 打印模型信息
         print("=" * 50)
         print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
@@ -105,7 +109,7 @@ def _compute_jacobian(self) -> np.ndarray:
         mujoco.mj_jac(self.model, self.data, self.jacp, self.jacr, self.get_ee_pos(), self.ee_body_id)
         return self.jacp[:, self.joint_ids]
 
-    def _move_step(self, target, speed=0.3):
+    def _move_step(self, target: np.ndarray, speed: float = 0.3) -> bool:
         """单步位置控制：基于雅克比伪逆实现末端执行器的位置跟踪
 
         Args:
@@ -214,7 +218,6 @@ def _grab_phase_machine(self):
 
         elif self.current_phase == 6:
             # 阶段6：抬升立方体
-            # 【优化2】使用类内常量替代硬编码的抬升增量
             lift_target = self.cube_pos + np.array([0, 0, self.safe_lift_height + self.LIFT_HEIGHT_INCREMENT])
             if self._move_step(lift_target, speed=0.3):
                 print("\n✅ 抬升立方体")
@@ -270,10 +273,11 @@ def run(self):
         """单线程仿真主循环"""
         # 初始化Viewer
         self.viewer = viewer.launch_passive(self.model, self.data)
-        self.viewer.cam.azimuth = 70
-        self.viewer.cam.elevation = -25
-        self.viewer.cam.distance = 1.8
-        self.viewer.cam.lookat = np.array([0.4, 0.0, 0.1])
+        # 【优化2】使用类内常量替代硬编码的相机参数
+        self.viewer.cam.azimuth = self.CAM_AZIMUTH
+        self.viewer.cam.elevation = self.CAM_ELEVATION
+        self.viewer.cam.distance = self.CAM_DISTANCE
+        self.viewer.cam.lookat = self.CAM_LOOKAT
 
         print("\n🚀 仿真已启动，开始自动抓取...")
         print("💡 关闭Viewer窗口可退出程序")

From ea8b777ef8562a99968faad30ab1a7a0c2ed4421 Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Sat, 27 Dec 2025 15:35:17 +0800
Subject: [PATCH 16/19] =?UTF-8?q?=E6=8F=90=E5=8D=87=E4=BB=BF=E7=9C=9F?=
 =?UTF-8?q?=E4=BC=91=E7=9C=A0=E6=97=B6=E9=97=B4=E4=B8=BA=E5=B8=B8=E9=87=8F?=
 =?UTF-8?q?=E5=B9=B6=E5=AE=8C=E5=96=84=5Fgripper=5Fstep=E7=B1=BB=E5=9E=8B?=
 =?UTF-8?q?=E6=B3=A8=E8=A7=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index b6bc64b457..882ce9dc20 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -72,12 +72,15 @@ def __init__(self):
         self.INIT_EE_POS = np.array([0.4, 0.0, 0.2])  # 末端执行器初始目标位置
         self.LIFT_HEIGHT_INCREMENT = 0.05  # 抓取后额外抬升的高度增量
 
-        # 【优化1】提取相机视角参数为类内常量
+        # 相机视角参数
         self.CAM_AZIMUTH = 70  # 相机方位角
         self.CAM_ELEVATION = -25  # 相机仰角
         self.CAM_DISTANCE = 1.8  # 相机距离
         self.CAM_LOOKAT = np.array([0.4, 0.0, 0.1])  # 相机注视点
 
+        # 【优化1】将仿真休眠时间提升为类内常量
+        self.SIMULATION_SLEEP = 1 / 200  # 仿真循环的休眠时间
+
         # 打印模型信息
         print("=" * 50)
         print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
@@ -153,7 +156,7 @@ def _move_step(self, target: np.ndarray, speed: float = 0.3) -> bool:
 
         return False
 
-    def _gripper_step(self, pos):
+    def _gripper_step(self, pos: float) -> None:
         """单步夹爪位置控制，设置夹爪的目标开合位置
 
         Args:
@@ -273,7 +276,6 @@ def run(self):
         """单线程仿真主循环"""
         # 初始化Viewer
         self.viewer = viewer.launch_passive(self.model, self.data)
-        # 【优化2】使用类内常量替代硬编码的相机参数
         self.viewer.cam.azimuth = self.CAM_AZIMUTH
         self.viewer.cam.elevation = self.CAM_ELEVATION
         self.viewer.cam.distance = self.CAM_DISTANCE
@@ -282,9 +284,6 @@ def run(self):
         print("\n🚀 仿真已启动，开始自动抓取...")
         print("💡 关闭Viewer窗口可退出程序")
 
-        # 提取休眠时间为常量，便于后续调整
-        SIMULATION_SLEEP = 1 / 200
-
         # 单线程主循环
         # 添加KeyboardInterrupt捕获，支持Ctrl+C优雅退出
         try:
@@ -298,7 +297,8 @@ def run(self):
 
                 mujoco.mj_step(self.model, self.data)
                 self.viewer.sync()
-                time.sleep(SIMULATION_SLEEP)
+                # 【优化2】使用类内常量替代局部变量
+                time.sleep(self.SIMULATION_SLEEP)
         except KeyboardInterrupt:
             print("\n⚠️ 检测到Ctrl+C，正在退出仿真...")
 

From 79f45e43e489925667dc5e8788871ba0afdc4752 Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Sat, 27 Dec 2025 17:03:27 +0800
Subject: [PATCH 17/19] =?UTF-8?q?=E9=87=8D=E6=9E=84=E7=9B=B8=E6=9C=BA?=
 =?UTF-8?q?=E5=88=9D=E5=A7=8B=E5=8C=96=E5=B9=B6=E5=AE=8C=E5=96=84=5Fgrab?=
 =?UTF-8?q?=5Fphase=5Fmachine=E7=B1=BB=E5=9E=8B=E6=B3=A8=E8=A7=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 49 +++++++++----------------------
 1 file changed, 14 insertions(+), 35 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index 882ce9dc20..a5177aa7aa 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -78,7 +78,7 @@ def __init__(self):
         self.CAM_DISTANCE = 1.8  # 相机距离
         self.CAM_LOOKAT = np.array([0.4, 0.0, 0.1])  # 相机注视点
 
-        # 【优化1】将仿真休眠时间提升为类内常量
+        # 仿真控制参数
         self.SIMULATION_SLEEP = 1 / 200  # 仿真循环的休眠时间
 
         # 打印模型信息
@@ -129,28 +129,18 @@ def _move_step(self, target: np.ndarray, speed: float = 0.3) -> bool:
         if error_norm < self.POS_TOLERANCE:
             return True  # 到达目标
 
-        # 计算雅克比矩阵
-        jacobian = self._compute_jacobian()  # 3×7
-
-        # ========== 修正：正确的阻尼伪逆计算 ==========
-        # 方法1：使用正则化参数的伪逆（推荐）
+        jacobian = self._compute_jacobian()
         jacobian_pinv = jacobian.T @ np.linalg.inv(jacobian @ jacobian.T + self.JACOBIAN_DAMPING * np.eye(3))
 
-        # 方法2：若方法1仍报错，可改用numpy伪逆（自动处理维度）
-        # jacobian_pinv = np.linalg.pinv(jacobian, rcond=1e-3)
-
-        # 关节速度指令
         joint_vel_cmd = speed * jacobian_pinv @ error
         joint_vel_cmd = np.clip(joint_vel_cmd, -self.JOINT_VEL_LIMIT, self.JOINT_VEL_LIMIT)
 
-        # PD力矩计算
         torque = np.zeros(7)
         for i in range(7):
             angle_error = joint_vel_cmd[i] * 0.1
             torque[i] = self.PD_KP * angle_error - self.PD_KD * self.data.qvel[self.joint_ids[i]]
             torque[i] = np.clip(torque[i], -self.TORQUE_LIMIT, self.TORQUE_LIMIT)
 
-        # 设置关节力矩
         for i in range(7):
             self.data.ctrl[self.joint_ids[i]] = torque[i]
 
@@ -166,34 +156,30 @@ def _gripper_step(self, pos: float) -> None:
             j_id = self.model.joint(j_name).id
             self.data.ctrl[j_id] = pos
 
-    def _grab_phase_machine(self):
+    def _grab_phase_machine(self) -> None:
         """抓取状态机：按阶段执行机械臂的抓取、移动、放置等一系列动作
 
         状态机分为12个阶段，从初始位置移动→识别立方体→抓取→放置→返回，
         每个阶段完成后自动切换到下一个阶段，直到抓取任务完成。
         """
         if self.current_phase == 0:
-            # 阶段0：移动到初始位置
             if self._move_step(self.INIT_EE_POS):
                 print("\n✅ 到达初始位置")
                 self.current_phase = 1
                 self.step_counter = 0
 
         elif self.current_phase == 1:
-            # 阶段1：获取立方体位置
             self.cube_pos = self.get_cube_pos()
             print(f"\n🎯 识别到立方体位置：{np.round(self.cube_pos, 3)}")
             self.current_phase = 2
 
         elif self.current_phase == 2:
-            # 阶段2：移动到立方体上方
             if self._move_step(self.cube_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4):
                 print("\n✅ 到达立方体上方")
                 self.current_phase = 3
                 self.step_counter = 0
 
         elif self.current_phase == 3:
-            # 阶段3：打开夹爪
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n✋ 打开夹爪")
@@ -203,14 +189,12 @@ def _grab_phase_machine(self):
             self.step_counter += 1
 
         elif self.current_phase == 4:
-            # 阶段4：下降抓取
             if self._move_step(self.cube_pos + np.array([0, 0, self.grab_height]), speed=0.2):
                 print("\n✅ 下降到抓取高度")
                 self.current_phase = 5
                 self.step_counter = 0
 
         elif self.current_phase == 5:
-            # 阶段5：闭合夹爪
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_close_pos)
                 print("\n🤏 闭合夹爪抓取")
@@ -220,7 +204,6 @@ def _grab_phase_machine(self):
             self.step_counter += 1
 
         elif self.current_phase == 6:
-            # 阶段6：抬升立方体
             lift_target = self.cube_pos + np.array([0, 0, self.safe_lift_height + self.LIFT_HEIGHT_INCREMENT])
             if self._move_step(lift_target, speed=0.3):
                 print("\n✅ 抬升立方体")
@@ -228,21 +211,18 @@ def _grab_phase_machine(self):
                 self.step_counter = 0
 
         elif self.current_phase == 7:
-            # 阶段7：移动到放置点上方
             if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4):
                 print("\n✅ 到达放置点上方")
                 self.current_phase = 8
                 self.step_counter = 0
 
         elif self.current_phase == 8:
-            # 阶段8：下降放置
             if self._move_step(self.target_place_pos + np.array([0, 0, self.grab_height]), speed=0.2):
                 print("\n✅ 下降到放置高度")
                 self.current_phase = 9
                 self.step_counter = 0
 
         elif self.current_phase == 9:
-            # 阶段9：释放立方体
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n🫳 释放立方体")
@@ -252,57 +232,56 @@ def _grab_phase_machine(self):
             self.step_counter += 1
 
         elif self.current_phase == 10:
-            # 阶段10：撤离机械臂
             if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.3):
                 print("\n✅ 撤离机械臂")
                 self.current_phase = 11
                 self.step_counter = 0
 
         elif self.current_phase == 11:
-            # 阶段11：返回初始位置
             if self._move_step(self.INIT_EE_POS, speed=0.4):
                 print("\n✅ 返回初始位置")
                 self.current_phase = 12
 
         elif self.current_phase == 12:
-            # 阶段12：抓取完成
             if not self.grab_complete:
                 print("\n" + "=" * 50)
                 print("✅ 智能抓取任务完成！")
                 print("=" * 50)
                 self.grab_complete = True
 
-    def run(self):
-        """单线程仿真主循环"""
-        # 初始化Viewer
-        self.viewer = viewer.launch_passive(self.model, self.data)
+    # 【优化1】新增方法：集中初始化相机参数
+    def _init_camera(self) -> None:
+        """初始化Viewer的相机视角"""
         self.viewer.cam.azimuth = self.CAM_AZIMUTH
         self.viewer.cam.elevation = self.CAM_ELEVATION
         self.viewer.cam.distance = self.CAM_DISTANCE
         self.viewer.cam.lookat = self.CAM_LOOKAT
 
+    def run(self):
+        """单线程仿真主循环"""
+        # 初始化Viewer
+        self.viewer = viewer.launch_passive(self.model, self.data)
+
+        # 【优化2】调用新方法初始化相机
+        self._init_camera()
+
         print("\n🚀 仿真已启动，开始自动抓取...")
         print("💡 关闭Viewer窗口可退出程序")
 
-        # 单线程主循环
-        # 添加KeyboardInterrupt捕获，支持Ctrl+C优雅退出
         try:
             while self.viewer.is_running():
                 if self.running and not self.grab_complete:
                     self._grab_phase_machine()
                 else:
-                    # 抓取完成后归零力矩
                     for i in range(7):
                         self.data.ctrl[self.joint_ids[i]] = 0
 
                 mujoco.mj_step(self.model, self.data)
                 self.viewer.sync()
-                # 【优化2】使用类内常量替代局部变量
                 time.sleep(self.SIMULATION_SLEEP)
         except KeyboardInterrupt:
             print("\n⚠️ 检测到Ctrl+C，正在退出仿真...")
 
-        # 清理
         self.running = False
         self.viewer.close()
         print("\n👋 仿真结束")

From 80e02870fc631acc7596f4460a80a267613a11c8 Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Sat, 27 Dec 2025 17:28:29 +0800
Subject: [PATCH 18/19] =?UTF-8?q?=E4=B8=BA=E7=8A=B6=E6=80=81=E6=9C=BA?=
 =?UTF-8?q?=E9=98=B6=E6=AE=B5=E5=AE=9A=E4=B9=89=E5=85=B7=E5=90=8D=E5=B8=B8?=
 =?UTF-8?q?=E9=87=8F=EF=BC=8C=E6=8F=90=E5=8D=87=E5=8F=AF=E8=AF=BB=E6=80=A7?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 70 ++++++++++++++++++-------------
 1 file changed, 41 insertions(+), 29 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index a5177aa7aa..c66639816d 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -23,6 +23,21 @@
 
 # ========== 智能抓取控制器 ==========
 class PandaAutoGrab:
+    # 【优化1】定义状态机阶段的具名常量，提升代码可读性
+    PHASE_MOVE_TO_INIT = 0
+    PHASE_DETECT_CUBE = 1
+    PHASE_MOVE_TO_CUBE_ABOVE = 2
+    PHASE_OPEN_GRIPPER = 3
+    PHASE_MOVE_TO_GRAB_HEIGHT = 4
+    PHASE_CLOSE_GRIPPER = 5
+    PHASE_LIFT_CUBE = 6
+    PHASE_MOVE_TO_PLACE_ABOVE = 7
+    PHASE_MOVE_TO_PLACE_HEIGHT = 8
+    PHASE_RELEASE_CUBE = 9
+    PHASE_MOVE_BACK_FROM_PLACE = 10
+    PHASE_MOVE_BACK_TO_INIT = 11
+    PHASE_FINISHED = 12
+
     def __init__(self):
         """初始化Franka Panda机械臂抓取控制器，加载模型和初始化参数"""
         self.model = mujoco.MjModel.from_xml_path(SCENE_PATH)
@@ -162,94 +177,94 @@ def _grab_phase_machine(self) -> None:
         状态机分为12个阶段，从初始位置移动→识别立方体→抓取→放置→返回，
         每个阶段完成后自动切换到下一个阶段，直到抓取任务完成。
         """
-        if self.current_phase == 0:
+        # 【优化2】使用具名常量替代数字，使逻辑更清晰
+        if self.current_phase == self.PHASE_MOVE_TO_INIT:
             if self._move_step(self.INIT_EE_POS):
                 print("\n✅ 到达初始位置")
-                self.current_phase = 1
+                self.current_phase = self.PHASE_DETECT_CUBE
                 self.step_counter = 0
 
-        elif self.current_phase == 1:
+        elif self.current_phase == self.PHASE_DETECT_CUBE:
             self.cube_pos = self.get_cube_pos()
             print(f"\n🎯 识别到立方体位置：{np.round(self.cube_pos, 3)}")
-            self.current_phase = 2
+            self.current_phase = self.PHASE_MOVE_TO_CUBE_ABOVE
 
-        elif self.current_phase == 2:
+        elif self.current_phase == self.PHASE_MOVE_TO_CUBE_ABOVE:
             if self._move_step(self.cube_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4):
                 print("\n✅ 到达立方体上方")
-                self.current_phase = 3
+                self.current_phase = self.PHASE_OPEN_GRIPPER
                 self.step_counter = 0
 
-        elif self.current_phase == 3:
+        elif self.current_phase == self.PHASE_OPEN_GRIPPER:
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n✋ 打开夹爪")
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
-                self.current_phase = 4
+                self.current_phase = self.PHASE_MOVE_TO_GRAB_HEIGHT
                 self.step_counter = 0
             self.step_counter += 1
 
-        elif self.current_phase == 4:
+        elif self.current_phase == self.PHASE_MOVE_TO_GRAB_HEIGHT:
             if self._move_step(self.cube_pos + np.array([0, 0, self.grab_height]), speed=0.2):
                 print("\n✅ 下降到抓取高度")
-                self.current_phase = 5
+                self.current_phase = self.PHASE_CLOSE_GRIPPER
                 self.step_counter = 0
 
-        elif self.current_phase == 5:
+        elif self.current_phase == self.PHASE_CLOSE_GRIPPER:
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_close_pos)
                 print("\n🤏 闭合夹爪抓取")
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
-                self.current_phase = 6
+                self.current_phase = self.PHASE_LIFT_CUBE
                 self.step_counter = 0
             self.step_counter += 1
 
-        elif self.current_phase == 6:
+        elif self.current_phase == self.PHASE_LIFT_CUBE:
             lift_target = self.cube_pos + np.array([0, 0, self.safe_lift_height + self.LIFT_HEIGHT_INCREMENT])
             if self._move_step(lift_target, speed=0.3):
                 print("\n✅ 抬升立方体")
-                self.current_phase = 7
+                self.current_phase = self.PHASE_MOVE_TO_PLACE_ABOVE
                 self.step_counter = 0
 
-        elif self.current_phase == 7:
+        elif self.current_phase == self.PHASE_MOVE_TO_PLACE_ABOVE:
             if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4):
                 print("\n✅ 到达放置点上方")
-                self.current_phase = 8
+                self.current_phase = self.PHASE_MOVE_TO_PLACE_HEIGHT
                 self.step_counter = 0
 
-        elif self.current_phase == 8:
+        elif self.current_phase == self.PHASE_MOVE_TO_PLACE_HEIGHT:
             if self._move_step(self.target_place_pos + np.array([0, 0, self.grab_height]), speed=0.2):
                 print("\n✅ 下降到放置高度")
-                self.current_phase = 9
+                self.current_phase = self.PHASE_RELEASE_CUBE
                 self.step_counter = 0
 
-        elif self.current_phase == 9:
+        elif self.current_phase == self.PHASE_RELEASE_CUBE:
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
                 print("\n🫳 释放立方体")
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
-                self.current_phase = 10
+                self.current_phase = self.PHASE_MOVE_BACK_FROM_PLACE
                 self.step_counter = 0
             self.step_counter += 1
 
-        elif self.current_phase == 10:
+        elif self.current_phase == self.PHASE_MOVE_BACK_FROM_PLACE:
             if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.3):
                 print("\n✅ 撤离机械臂")
-                self.current_phase = 11
+                self.current_phase = self.PHASE_MOVE_BACK_TO_INIT
                 self.step_counter = 0
 
-        elif self.current_phase == 11:
+        elif self.current_phase == self.PHASE_MOVE_BACK_TO_INIT:
             if self._move_step(self.INIT_EE_POS, speed=0.4):
                 print("\n✅ 返回初始位置")
-                self.current_phase = 12
+                self.current_phase = self.PHASE_FINISHED
 
-        elif self.current_phase == 12:
+        elif self.current_phase == self.PHASE_FINISHED:
             if not self.grab_complete:
                 print("\n" + "=" * 50)
                 print("✅ 智能抓取任务完成！")
                 print("=" * 50)
                 self.grab_complete = True
 
-    # 【优化1】新增方法：集中初始化相机参数
     def _init_camera(self) -> None:
         """初始化Viewer的相机视角"""
         self.viewer.cam.azimuth = self.CAM_AZIMUTH
@@ -259,10 +274,7 @@ def _init_camera(self) -> None:
 
     def run(self):
         """单线程仿真主循环"""
-        # 初始化Viewer
         self.viewer = viewer.launch_passive(self.model, self.data)
-
-        # 【优化2】调用新方法初始化相机
         self._init_camera()
 
         print("\n🚀 仿真已启动，开始自动抓取...")

From fa83184cbeb7ccd436a07842e4a2df242fcd44e2 Mon Sep 17 00:00:00 2001
From: dengzhuo0119 <daoyuedz@qq.com>
Date: Sun, 28 Dec 2025 12:21:53 +0800
Subject: [PATCH 19/19] =?UTF-8?q?=E5=BC=95=E5=85=A5logging,=20=E5=A2=9E?=
 =?UTF-8?q?=E5=BC=BA=E9=94=99=E8=AF=AF=E5=A4=84=E7=90=86=E5=B9=B6=E6=B7=BB?=
 =?UTF-8?q?=E5=8A=A0=E7=B1=BB=E6=96=87=E6=A1=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robot_arm_motion/robot_arm.py | 104 ++++++++++++++++++------------
 1 file changed, 64 insertions(+), 40 deletions(-)

diff --git a/src/Robot_arm_motion/robot_arm.py b/src/Robot_arm_motion/robot_arm.py
index c66639816d..809d02e913 100644
--- a/src/Robot_arm_motion/robot_arm.py
+++ b/src/Robot_arm_motion/robot_arm.py
@@ -4,6 +4,10 @@
 import time
 import sys
 import os
+import logging  # 【优化1】引入logging模块
+
+# 配置logging，使其输出到控制台并包含时间和日志级别
+logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')
 
 """
 Franka Panda 机械臂自动抓取仿真 v1.1
@@ -16,14 +20,29 @@
                           "franka_emika_panda",
                           "grab_scene.xml")
 
-if not os.path.exists(SCENE_PATH):
-    print(f"❌ 场景文件不存在：{SCENE_PATH}")
-    sys.exit(1)
-
 
 # ========== 智能抓取控制器 ==========
 class PandaAutoGrab:
-    # 【优化1】定义状态机阶段的具名常量，提升代码可读性
+    """
+    【优化3】添加类级文档字符串
+    Franka Panda 机械臂智能抓取控制器 (基于MuJoCo)
+
+    该类实现了一个完整的、基于视觉的机械臂抓取和放置任务。
+    它通过一个状态机来编排一系列动作，并使用基于雅可比伪逆的操作空间控制
+    来精确地移动机械臂末端执行器。
+
+    核心算法:
+    - **状态机**: 采用阶段式状态机 (`_grab_phase_machine`) 来管理抓取流程。
+    - **运动学控制**: 使用雅可比矩阵的伪逆将末端执行器的笛卡尔空间速度
+      指令转换为关节空间的速度指令，实现高精度的位置跟踪。
+    - **PD控制器**: 在关节空间使用PD控制器将速度指令转换为力矩输出。
+
+    主要属性:
+    - model, data: MuJoCo的核心数据结构，分别表示模型和数据。
+    - current_phase: 当前状态机所处的阶段。
+    - 各类参数: 如PD增益、速度限制、位置容差等，均定义为类属性以便调整。
+    """
+    # 状态机阶段常量
     PHASE_MOVE_TO_INIT = 0
     PHASE_DETECT_CUBE = 1
     PHASE_MOVE_TO_CUBE_ABOVE = 2
@@ -40,14 +59,27 @@ class PandaAutoGrab:
 
     def __init__(self):
         """初始化Franka Panda机械臂抓取控制器，加载模型和初始化参数"""
-        self.model = mujoco.MjModel.from_xml_path(SCENE_PATH)
-        self.data = mujoco.MjData(self.model)
+        self.model = None
+        self.data = None
         self.viewer = None
         self.running = True
         self.step_counter = 0
         self.current_phase = 0
         self.grab_complete = False
 
+        # 尝试加载模型文件
+        try:
+            self.model = mujoco.MjModel.from_xml_path(SCENE_PATH)
+            self.data = mujoco.MjData(self.model)
+        except FileNotFoundError:
+            logging.error(f"场景文件不存在，请检查路径: {SCENE_PATH}")
+            sys.exit(1)
+        # 【优化2】增强错误处理，捕获MuJoCo特定的致命错误
+        except mujoco.FatalError as e:
+            logging.error(f"加载MJCF模型时发生致命错误: {e}")
+            logging.error("请检查XML文件的语法和引用的资源（如mesh, texture）是否正确。")
+            sys.exit(1)
+
         # 机械臂参数
         self.ee_body_id = self.model.body("hand").id
         self.joint_names = [f"joint{i}" for i in range(1, 8)]
@@ -97,10 +129,10 @@ def __init__(self):
         self.SIMULATION_SLEEP = 1 / 200  # 仿真循环的休眠时间
 
         # 打印模型信息
-        print("=" * 50)
-        print("📌 模型Body列表：", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
-        print("📌 模型Joint列表：", [self.model.joint(i).name for i in range(min(self.model.njnt, 10))])
-        print("=" * 50)
+        logging.info("=" * 50)
+        logging.info("📌 模型Body列表: %s", [self.model.body(i).name for i in range(min(self.model.nbody, 10))])
+        logging.info("📌 模型Joint列表: %s", [self.model.joint(i).name for i in range(min(self.model.njnt, 10))])
+        logging.info("=" * 50)
 
     def get_ee_pos(self) -> np.ndarray:
         """获取末端执行器位置
@@ -172,33 +204,28 @@ def _gripper_step(self, pos: float) -> None:
             self.data.ctrl[j_id] = pos
 
     def _grab_phase_machine(self) -> None:
-        """抓取状态机：按阶段执行机械臂的抓取、移动、放置等一系列动作
-
-        状态机分为12个阶段，从初始位置移动→识别立方体→抓取→放置→返回，
-        每个阶段完成后自动切换到下一个阶段，直到抓取任务完成。
-        """
-        # 【优化2】使用具名常量替代数字，使逻辑更清晰
+        """抓取状态机：按阶段执行机械臂的抓取、移动、放置等一系列动作"""
         if self.current_phase == self.PHASE_MOVE_TO_INIT:
             if self._move_step(self.INIT_EE_POS):
-                print("\n✅ 到达初始位置")
+                logging.info("✅ 到达初始位置")
                 self.current_phase = self.PHASE_DETECT_CUBE
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_DETECT_CUBE:
             self.cube_pos = self.get_cube_pos()
-            print(f"\n🎯 识别到立方体位置：{np.round(self.cube_pos, 3)}")
+            logging.info("🎯 识别到立方体位置: %s", np.round(self.cube_pos, 3))
             self.current_phase = self.PHASE_MOVE_TO_CUBE_ABOVE
 
         elif self.current_phase == self.PHASE_MOVE_TO_CUBE_ABOVE:
             if self._move_step(self.cube_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4):
-                print("\n✅ 到达立方体上方")
+                logging.info("✅ 到达立方体上方")
                 self.current_phase = self.PHASE_OPEN_GRIPPER
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_OPEN_GRIPPER:
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
-                print("\n✋ 打开夹爪")
+                logging.info("✋ 打开夹爪")
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = self.PHASE_MOVE_TO_GRAB_HEIGHT
                 self.step_counter = 0
@@ -206,14 +233,14 @@ def _grab_phase_machine(self) -> None:
 
         elif self.current_phase == self.PHASE_MOVE_TO_GRAB_HEIGHT:
             if self._move_step(self.cube_pos + np.array([0, 0, self.grab_height]), speed=0.2):
-                print("\n✅ 下降到抓取高度")
+                logging.info("✅ 下降到抓取高度")
                 self.current_phase = self.PHASE_CLOSE_GRIPPER
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_CLOSE_GRIPPER:
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_close_pos)
-                print("\n🤏 闭合夹爪抓取")
+                logging.info("🤏 闭合夹爪抓取")
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = self.PHASE_LIFT_CUBE
                 self.step_counter = 0
@@ -222,26 +249,26 @@ def _grab_phase_machine(self) -> None:
         elif self.current_phase == self.PHASE_LIFT_CUBE:
             lift_target = self.cube_pos + np.array([0, 0, self.safe_lift_height + self.LIFT_HEIGHT_INCREMENT])
             if self._move_step(lift_target, speed=0.3):
-                print("\n✅ 抬升立方体")
+                logging.info("✅ 抬升立方体")
                 self.current_phase = self.PHASE_MOVE_TO_PLACE_ABOVE
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_MOVE_TO_PLACE_ABOVE:
             if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.4):
-                print("\n✅ 到达放置点上方")
+                logging.info("✅ 到达放置点上方")
                 self.current_phase = self.PHASE_MOVE_TO_PLACE_HEIGHT
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_MOVE_TO_PLACE_HEIGHT:
             if self._move_step(self.target_place_pos + np.array([0, 0, self.grab_height]), speed=0.2):
-                print("\n✅ 下降到放置高度")
+                logging.info("✅ 下降到放置高度")
                 self.current_phase = self.PHASE_RELEASE_CUBE
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_RELEASE_CUBE:
             if self.step_counter == 0:
                 self._gripper_step(self.gripper_open_pos)
-                print("\n🫳 释放立方体")
+                logging.info("🫳 释放立方体")
             if self.step_counter > self.GRIPPER_WAIT_STEPS:
                 self.current_phase = self.PHASE_MOVE_BACK_FROM_PLACE
                 self.step_counter = 0
@@ -249,20 +276,20 @@ def _grab_phase_machine(self) -> None:
 
         elif self.current_phase == self.PHASE_MOVE_BACK_FROM_PLACE:
             if self._move_step(self.target_place_pos + np.array([0, 0, self.safe_lift_height]), speed=0.3):
-                print("\n✅ 撤离机械臂")
+                logging.info("✅ 撤离机械臂")
                 self.current_phase = self.PHASE_MOVE_BACK_TO_INIT
                 self.step_counter = 0
 
         elif self.current_phase == self.PHASE_MOVE_BACK_TO_INIT:
             if self._move_step(self.INIT_EE_POS, speed=0.4):
-                print("\n✅ 返回初始位置")
+                logging.info("✅ 返回初始位置")
                 self.current_phase = self.PHASE_FINISHED
 
         elif self.current_phase == self.PHASE_FINISHED:
             if not self.grab_complete:
-                print("\n" + "=" * 50)
-                print("✅ 智能抓取任务完成！")
-                print("=" * 50)
+                logging.info("=" * 50)
+                logging.info("✅ 智能抓取任务完成！")
+                logging.info("=" * 50)
                 self.grab_complete = True
 
     def _init_camera(self) -> None:
@@ -277,8 +304,8 @@ def run(self):
         self.viewer = viewer.launch_passive(self.model, self.data)
         self._init_camera()
 
-        print("\n🚀 仿真已启动，开始自动抓取...")
-        print("💡 关闭Viewer窗口可退出程序")
+        logging.info("🚀 仿真已启动，开始自动抓取...")
+        logging.info("💡 关闭Viewer窗口可退出程序")
 
         try:
             while self.viewer.is_running():
@@ -292,11 +319,11 @@ def run(self):
                 self.viewer.sync()
                 time.sleep(self.SIMULATION_SLEEP)
         except KeyboardInterrupt:
-            print("\n⚠️ 检测到Ctrl+C，正在退出仿真...")
+            logging.warning("⚠️ 检测到Ctrl+C，正在退出仿真...")
 
         self.running = False
         self.viewer.close()
-        print("\n👋 仿真结束")
+        logging.info("👋 仿真结束")
 
 
 # ========== 主函数 ==========
@@ -305,8 +332,5 @@ def run(self):
         panda = PandaAutoGrab()
         panda.run()
     except Exception as e:
-        print(f"\n❌ 程序错误：{e}")
-        import traceback
-
-        traceback.print_exc()
+        logging.error(f"❌ 程序发生未处理的错误: {e}", exc_info=True)
         sys.exit(1)
\ No newline at end of file