From c2b70306b8ec637312f7985c88f04d87a76a5955 Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Fri, 19 Dec 2025 18:38:24 +0800
Subject: [PATCH 01/13] =?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/Mechanical_arm_grasping/README.md | 1 +
 src/Mechanical_arm_grasping/main.py   | 0
 2 files changed, 1 insertion(+)
 create mode 100644 src/Mechanical_arm_grasping/README.md
 create mode 100644 src/Mechanical_arm_grasping/main.py

diff --git a/src/Mechanical_arm_grasping/README.md b/src/Mechanical_arm_grasping/README.md
new file mode 100644
index 0000000000..e4f1315851
--- /dev/null
+++ b/src/Mechanical_arm_grasping/README.md
@@ -0,0 +1 @@
+机械臂抓取系统 
\ No newline at end of file
diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
new file mode 100644
index 0000000000..e69de29bb2

From ef8b457878cba7016a6e7ae176e3cfa5d80ba1ed Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Sun, 21 Dec 2025 21:46:25 +0800
Subject: [PATCH 02/13] =?UTF-8?q?=E6=B7=BB=E5=8A=A0.gitignore=EF=BC=8C?=
 =?UTF-8?q?=E5=BF=BD=E7=95=A5=E6=97=A0=E7=94=A8=E6=9C=AC=E5=9C=B0=E6=96=87?=
 =?UTF-8?q?=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Mechanical_arm_grasping/.gitgnore | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 src/Mechanical_arm_grasping/.gitgnore

diff --git a/src/Mechanical_arm_grasping/.gitgnore b/src/Mechanical_arm_grasping/.gitgnore
new file mode 100644
index 0000000000..e69de29bb2

From 7563e69fcedded95a65b5ce195ded4ea59a4e172 Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Sun, 21 Dec 2025 21:51:36 +0800
Subject: [PATCH 03/13] =?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/Mechanical_arm_grasping/.gitgnore |  0
 src/Mechanical_arm_grasping/main.py   | 28 +++++++++++++++++++++++++++
 2 files changed, 28 insertions(+)
 delete mode 100644 src/Mechanical_arm_grasping/.gitgnore

diff --git a/src/Mechanical_arm_grasping/.gitgnore b/src/Mechanical_arm_grasping/.gitgnore
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
index e69de29bb2..ccc567aa29 100644
--- a/src/Mechanical_arm_grasping/main.py
+++ b/src/Mechanical_arm_grasping/main.py
@@ -0,0 +1,28 @@
+# 代码1：超声波传感器目标位置检测（简单版）
+import time
+
+def ultrasonic_sensor_simulation():
+    """模拟超声波传感器，返回目标与机械臂的距离（单位：cm）"""
+    # 随机生成合理距离（模拟真实传感器检测结果）
+    import random
+    target_distance = random.uniform(5, 50)  # 目标距离5-50cm
+    print(f"【超声波传感器检测】目标距离：{target_distance:.2f} cm")
+    return target_distance
+
+def judge_target_position(distance):
+    """根据距离判断目标位置状态"""
+    if distance < 15:
+        return "近距离（可抓取）"
+    elif 15 <= distance <= 30:
+        return "中距离（需伸展）"
+    else:
+        return "远距离（超出范围）"
+
+# 主程序
+if __name__ == "__main__":
+    print("=== 机械臂目标检测（简单版）===")
+    for i in range(3):  # 检测3次
+        distance = ultrasonic_sensor_simulation()
+        position_state = judge_target_position(distance)
+        print(f"第{i+1}次检测结果：{position_state}\n")
+        time.sleep(1)  # 间隔1秒检测
\ No newline at end of file

From 4795836eb517401f38e9b4ca2db25a4252966a31 Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Mon, 22 Dec 2025 18:14:52 +0800
Subject: [PATCH 04/13] =?UTF-8?q?=E6=8E=A7=E5=88=B6=E6=9C=BA=E6=A2=B0?=
 =?UTF-8?q?=E8=87=82=E5=8D=87=E9=99=8D=E6=A8=A1=E6=8B=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Mechanical_arm_grasping/main.py | 117 +++++++++++++++++++++-------
 1 file changed, 90 insertions(+), 27 deletions(-)

diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
index ccc567aa29..33afe57051 100644
--- a/src/Mechanical_arm_grasping/main.py
+++ b/src/Mechanical_arm_grasping/main.py
@@ -1,28 +1,91 @@
-# 代码1：超声波传感器目标位置检测（简单版）
-import time
-
-def ultrasonic_sensor_simulation():
-    """模拟超声波传感器，返回目标与机械臂的距离（单位：cm）"""
-    # 随机生成合理距离（模拟真实传感器检测结果）
-    import random
-    target_distance = random.uniform(5, 50)  # 目标距离5-50cm
-    print(f"【超声波传感器检测】目标距离：{target_distance:.2f} cm")
-    return target_distance
-
-def judge_target_position(distance):
-    """根据距离判断目标位置状态"""
-    if distance < 15:
-        return "近距离（可抓取）"
-    elif 15 <= distance <= 30:
-        return "中距离（需伸展）"
+import pygame
+import sys
+import math  # 导入Python内置math库，用于角度弧度转换
+
+# 初始化pygame
+pygame.init()
+
+# 窗口配置
+WINDOW_WIDTH = 400
+WINDOW_HEIGHT = 500
+screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
+pygame.display.set_caption("机械臂关节升降模拟")
+
+# 颜色定义
+BLACK = (0, 0, 0)
+WHITE = (255, 255, 255)
+BLUE = (0, 128, 255)
+RED = (255, 50, 50)
+
+# 机械臂参数
+BASE_X = WINDOW_WIDTH // 2  # 机械臂底座x坐标
+BASE_Y = WINDOW_HEIGHT - 50  # 机械臂底座y坐标
+JOINT_LENGTH = 200  # 关节臂长度（影响升降范围）
+joint_angle = 90    # 关节初始角度（90度为垂直向下，0度为水平向左，180度为水平向右）
+speed = 1           # 角度变化速度（控制升降快慢）
+
+def draw_arm(screen, base_x, base_y, angle, length):
+    """绘制机械臂模型"""
+    # 计算关节末端坐标（通过角度转换，使用math库的radians方法）
+    # 三角函数计算：y轴向下为正，需调整角度映射
+    rad = math.radians(angle)  # 修复：替换为math.radians
+    joint_end_x = base_x + length * math.cos(rad)  # 修复：使用math.cos
+    joint_end_y = base_y - length * math.sin(rad)  # 修复：使用math.sin，减号实现y轴向上为高度增加
+
+    # 绘制底座
+    pygame.draw.circle(screen, BLUE, (base_x, base_y), 20)
+    # 绘制关节臂（底座到关节末端）
+    pygame.draw.line(screen, RED, (base_x, base_y), (joint_end_x, joint_end_y), 8)
+    # 绘制关节末端
+    pygame.draw.circle(screen, BLACK, (int(joint_end_x), int(joint_end_y)), 10)
+    # 绘制高度提示文字
+    height = int(BASE_Y - joint_end_y)  # 计算当前高度（像素值，近似对应实际高度）
+    font = pygame.font.SysFont(None, 24)
+    text = font.render(f"关节高度：{height}px", True, BLACK)
+    screen.blit(text, (10, 10))
+
+# 主循环
+clock = pygame.time.Clock()
+is_running = True
+move_direction = "up"  # 初始运动方向：上升
+
+while is_running:
+    # 控制帧率
+    clock.tick(60)
+    # 填充背景色
+    screen.fill(WHITE)
+
+    # 事件处理（关闭窗口、按键控制）
+    for event in pygame.event.get():
+        if event.type == pygame.QUIT:
+            is_running = False
+        # 按键控制：上箭头上升，下箭头下降
+        if event.type == pygame.KEYDOWN:
+            if event.key == pygame.K_UP:
+                move_direction = "up"
+            if event.key == pygame.K_DOWN:
+                move_direction = "down"
+
+    # 更新关节角度（控制升降）
+    if move_direction == "up":
+        # 角度增大：关节上升（最大角度170度，避免超出窗口）
+        if joint_angle < 170:
+            joint_angle += speed
+        else:
+            move_direction = "down"  # 到达上限后自动下降
     else:
-        return "远距离（超出范围）"
-
-# 主程序
-if __name__ == "__main__":
-    print("=== 机械臂目标检测（简单版）===")
-    for i in range(3):  # 检测3次
-        distance = ultrasonic_sensor_simulation()
-        position_state = judge_target_position(distance)
-        print(f"第{i+1}次检测结果：{position_state}\n")
-        time.sleep(1)  # 间隔1秒检测
\ No newline at end of file
+        # 角度减小：关节下降（最小角度10度，避免超出窗口）
+        if joint_angle > 10:
+            joint_angle -= speed
+        else:
+            move_direction = "up"  # 到达下限后自动上升
+
+    # 绘制机械臂
+    draw_arm(screen, BASE_X, BASE_Y, joint_angle, JOINT_LENGTH)
+
+    # 更新窗口显示
+    pygame.display.flip()
+
+# 退出程序
+pygame.quit()
+sys.exit()
\ No newline at end of file

From 0d1343a14572ebab2ef4e01eb395fe526393a31d Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Mon, 22 Dec 2025 20:48:23 +0800
Subject: [PATCH 05/13] =?UTF-8?q?=E5=88=9D=E5=A7=8B=E6=9C=BA=E6=A2=B0?=
 =?UTF-8?q?=E8=87=82=E5=8D=87=E9=99=8D=E6=8E=A7=E5=88=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Mechanical_arm_grasping/main.py | 161 ++++++++++++++--------------
 1 file changed, 82 insertions(+), 79 deletions(-)

diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
index 33afe57051..683b104bbb 100644
--- a/src/Mechanical_arm_grasping/main.py
+++ b/src/Mechanical_arm_grasping/main.py
@@ -1,91 +1,94 @@
-import pygame
-import sys
-import math  # 导入Python内置math库，用于角度弧度转换
+import pybullet as p
+import pybullet_data
+import time
+import numpy as np
 
-# 初始化pygame
-pygame.init()
 
-# 窗口配置
-WINDOW_WIDTH = 400
-WINDOW_HEIGHT = 500
-screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
-pygame.display.set_caption("机械臂关节升降模拟")
+class ArmElevatorControllerPyBullet:
+    def __init__(self):
+        # 连接PyBullet模拟器（GUI模式，显示界面）
+        self.physics_client = p.connect(p.GUI)
+        # 设置模型搜索路径（关键：确保能找到内置模型）
+        p.setAdditionalSearchPath(pybullet_data.getDataPath())
+        # 关闭重力（避免机械臂倾倒，专注升降控制；若需要真实物理效果可开启）
+        p.setGravity(0, 0, 0)
 
-# 颜色定义
-BLACK = (0, 0, 0)
-WHITE = (255, 255, 255)
-BLUE = (0, 128, 255)
-RED = (255, 50, 50)
+        # 加载地面和KUKA IIWA机械臂（内置模型，必存在，无需额外配置）
+        self.plane_id = p.loadURDF("plane.urdf")  # 加载地面
+        # 机械臂初始位姿：坐标(0,0,0)，姿态（无旋转）
+        self.arm_id = p.loadURDF(
+            "kuka_iiwa/model.urdf",
+            basePosition=[0, 0, 0],
+            baseOrientation=p.getQuaternionFromEuler([0, 0, 0])
+        )
 
-# 机械臂参数
-BASE_X = WINDOW_WIDTH // 2  # 机械臂底座x坐标
-BASE_Y = WINDOW_HEIGHT - 50  # 机械臂底座y坐标
-JOINT_LENGTH = 200  # 关节臂长度（影响升降范围）
-joint_angle = 90    # 关节初始角度（90度为垂直向下，0度为水平向左，180度为水平向右）
-speed = 1           # 角度变化速度（控制升降快慢）
+        # 定义升降关节：选择KUKA IIWA的第1个关节（索引0，可实现垂直方向升降/旋转，适配升降逻辑）
+        self.elevator_joint_index = 0
+        # 获取关节信息（限位、当前位置）
+        joint_info = p.getJointInfo(self.arm_id, self.elevator_joint_index)
+        self.joint_min = joint_info[8]  # 关节运动下限
+        self.joint_max = joint_info[9]  # 关节运动上限
+        self.current_pos = p.getJointState(self.arm_id, self.elevator_joint_index)[0]  # 当前位置
 
-def draw_arm(screen, base_x, base_y, angle, length):
-    """绘制机械臂模型"""
-    # 计算关节末端坐标（通过角度转换，使用math库的radians方法）
-    # 三角函数计算：y轴向下为正，需调整角度映射
-    rad = math.radians(angle)  # 修复：替换为math.radians
-    joint_end_x = base_x + length * math.cos(rad)  # 修复：使用math.cos
-    joint_end_y = base_y - length * math.sin(rad)  # 修复：使用math.sin，减号实现y轴向上为高度增加
+        # 打印关节初始化信息
+        print(f"升降关节初始化完成：")
+        print(f"关节索引：{self.elevator_joint_index}")
+        print(f"当前位置：{self.current_pos:.3f}")
+        print(f"运动范围：[{self.joint_min:.3f}, {self.joint_max:.3f}]")
 
-    # 绘制底座
-    pygame.draw.circle(screen, BLUE, (base_x, base_y), 20)
-    # 绘制关节臂（底座到关节末端）
-    pygame.draw.line(screen, RED, (base_x, base_y), (joint_end_x, joint_end_y), 8)
-    # 绘制关节末端
-    pygame.draw.circle(screen, BLACK, (int(joint_end_x), int(joint_end_y)), 10)
-    # 绘制高度提示文字
-    height = int(BASE_Y - joint_end_y)  # 计算当前高度（像素值，近似对应实际高度）
-    font = pygame.font.SysFont(None, 24)
-    text = font.render(f"关节高度：{height}px", True, BLACK)
-    screen.blit(text, (10, 10))
+    def move_elevator(self, target_pos, speed=0.05):
+        """
+        驱动升降关节运动到目标位置
+        :param target_pos: 目标位置（需在关节限位范围内）
+        :param speed: 运动速度（正数，越小越慢）
+        """
+        # 校验目标位置合法性
+        if target_pos < self.joint_min or target_pos > self.joint_max:
+            raise ValueError(f"目标位置超出关节范围！允许范围：[{self.joint_min:.3f}, {self.joint_max:.3f}]")
 
-# 主循环
-clock = pygame.time.Clock()
-is_running = True
-move_direction = "up"  # 初始运动方向：上升
+        print(f"\n开始升降运动：当前位置 {self.current_pos:.3f} → 目标位置 {target_pos:.3f}")
+        # 循环控制，直到接近目标位置（误差小于0.001）
+        while abs(self.current_pos - target_pos) > 0.001:
+            # 计算运动步长（根据目标位置判断升降方向）
+            step = speed if target_pos > self.current_pos else -speed
+            # 更新当前位置（防止超出限位）
+            self.current_pos = np.clip(self.current_pos + step, self.joint_min, self.joint_max)
+            # 发送位置指令给关节（位置控制模式）
+            p.setJointMotorControl2(
+                bodyUniqueId=self.arm_id,
+                jointIndex=self.elevator_joint_index,
+                controlMode=p.POSITION_CONTROL,
+                targetPosition=self.current_pos
+            )
+            # 步进物理仿真（更新场景状态）
+            p.stepSimulation()
+            # 小幅延时，模拟真实运动节奏
+            time.sleep(0.01)
+            # 获取模拟器中关节的实际位置（反馈同步）
+            self.current_pos = p.getJointState(self.arm_id, self.elevator_joint_index)[0]
+            # 实时刷新显示当前位置
+            print(f"实时位置：{self.current_pos:.3f}", end='\r')
 
-while is_running:
-    # 控制帧率
-    clock.tick(60)
-    # 填充背景色
-    screen.fill(WHITE)
+        print(f"\n升降运动完成！最终位置：{self.current_pos:.3f}")
 
-    # 事件处理（关闭窗口、按键控制）
-    for event in pygame.event.get():
-        if event.type == pygame.QUIT:
-            is_running = False
-        # 按键控制：上箭头上升，下箭头下降
-        if event.type == pygame.KEYDOWN:
-            if event.key == pygame.K_UP:
-                move_direction = "up"
-            if event.key == pygame.K_DOWN:
-                move_direction = "down"
+    def disconnect(self):
+        """断开与PyBullet模拟器的连接"""
+        p.disconnect(self.physics_client)
+        print("\n已断开与PyBullet模拟器的连接")
 
-    # 更新关节角度（控制升降）
-    if move_direction == "up":
-        # 角度增大：关节上升（最大角度170度，避免超出窗口）
-        if joint_angle < 170:
-            joint_angle += speed
-        else:
-            move_direction = "down"  # 到达上限后自动下降
-    else:
-        # 角度减小：关节下降（最小角度10度，避免超出窗口）
-        if joint_angle > 10:
-            joint_angle -= speed
-        else:
-            move_direction = "up"  # 到达下限后自动上升
 
-    # 绘制机械臂
-    draw_arm(screen, BASE_X, BASE_Y, joint_angle, JOINT_LENGTH)
+# ------------------- 主执行程序 -------------------
+if __name__ == "__main__":
+    # 1. 初始化机械臂升降控制器
+    arm_controller = ArmElevatorControllerPyBullet()
 
-    # 更新窗口显示
-    pygame.display.flip()
-
-# 退出程序
-pygame.quit()
-sys.exit()
\ No newline at end of file
+    try:
+        # 2. 执行升降动作序列
+        arm_controller.move_elevator(target_pos=arm_controller.joint_max * 0.6, speed=0.03)  # 上升（接近上限）
+        time.sleep(1)  # 停顿1秒
+        arm_controller.move_elevator(target_pos=arm_controller.joint_min * 0.6, speed=0.02)  # 下降（接近下限）
+        time.sleep(1)  # 停顿1秒
+        arm_controller.move_elevator(target_pos=0)  # 回到初始中间位置
+    finally:
+        # 3. 无论是否出错，最终断开连接
+        arm_controller.disconnect()
\ No newline at end of file

From 723a4e7345709911c29fe42e91673e07bea97d24 Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Tue, 23 Dec 2025 20:12:51 +0800
Subject: [PATCH 06/13] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E5=85=B3?=
 =?UTF-8?q?=E8=8A=82=E4=BC=B8=E5=B1=95=E4=BB=A3=E7=A0=81=E4=BC=98=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Mechanical_arm_grasping/main.py | 227 ++++++++++++++++++++--------
 1 file changed, 166 insertions(+), 61 deletions(-)

diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
index 683b104bbb..3c2f4a522b 100644
--- a/src/Mechanical_arm_grasping/main.py
+++ b/src/Mechanical_arm_grasping/main.py
@@ -2,93 +2,198 @@
 import pybullet_data
 import time
 import numpy as np
+from typing import Optional, Tuple
 
+# ------------------- 配置常量（便于统一修改） -------------------
+SIMULATION_GRAVITY: Tuple[float, float, float] = (0, 0, 0)  # 仿真重力，(0,0,-9.8)为真实重力
+ARM_MODEL_PATH: str = "kuka_iiwa/model.urdf"  # 机械臂模型路径
+ARM_BASE_POSITION: Tuple[float, float, float] = (0, 0, 0)  # 机械臂初始位置
+ARM_BASE_ORIENTATION: Tuple[float, float, float] = (0, 0, 0)  # 机械臂初始姿态（欧拉角）
+ELEVATOR_JOINT_INDEX: int = 0  # 升降关节索引
+MOVE_SPEED_DEFAULT: float = 0.03  # 默认升降速度
+POSITION_TOLERANCE: float = 0.001  # 位置误差容忍度（到达该误差即认为运动完成）
+DELAY_STEP: float = 0.01  # 仿真步进延时
+
+
+class ArmElevatorController:
+    """机械臂升降关节控制器（面向对象封装，职责单一）"""
 
-class ArmElevatorControllerPyBullet:
     def __init__(self):
-        # 连接PyBullet模拟器（GUI模式，显示界面）
-        self.physics_client = p.connect(p.GUI)
-        # 设置模型搜索路径（关键：确保能找到内置模型）
-        p.setAdditionalSearchPath(pybullet_data.getDataPath())
-        # 关闭重力（避免机械臂倾倒，专注升降控制；若需要真实物理效果可开启）
-        p.setGravity(0, 0, 0)
-
-        # 加载地面和KUKA IIWA机械臂（内置模型，必存在，无需额外配置）
-        self.plane_id = p.loadURDF("plane.urdf")  # 加载地面
-        # 机械臂初始位姿：坐标(0,0,0)，姿态（无旋转）
-        self.arm_id = p.loadURDF(
-            "kuka_iiwa/model.urdf",
-            basePosition=[0, 0, 0],
-            baseOrientation=p.getQuaternionFromEuler([0, 0, 0])
-        )
-
-        # 定义升降关节：选择KUKA IIWA的第1个关节（索引0，可实现垂直方向升降/旋转，适配升降逻辑）
-        self.elevator_joint_index = 0
-        # 获取关节信息（限位、当前位置）
+        """初始化模拟器连接、机械臂模型和关节信息"""
+        self.physics_client: Optional[int] = None
+        self.arm_id: Optional[int] = None
+        self.plane_id: Optional[int] = None
+
+        # 关节相关参数
+        self.elevator_joint_index: int = ELEVATOR_JOINT_INDEX
+        self.joint_min: float = 0.0
+        self.joint_max: float = 0.0
+        self.current_pos: float = 0.0
+
+        # 初始化流程
+        self._connect_simulator()
+        self._load_scene()
+        self._init_joint_info()
+        self._print_init_info()
+
+    def _connect_simulator(self) -> None:
+        """私有方法：连接PyBullet模拟器（封装初始化细节）"""
+        try:
+            self.physics_client = p.connect(p.GUI)
+            p.setAdditionalSearchPath(pybullet_data.getDataPath())
+            p.setGravity(*SIMULATION_GRAVITY)
+            print("✅ 成功连接PyBullet模拟器")
+        except Exception as e:
+            raise RuntimeError(f"❌ 连接模拟器失败：{str(e)}")
+
+    def _load_scene(self) -> None:
+        """私有方法：加载地面和机械臂模型（封装场景加载逻辑）"""
+        try:
+            # 加载地面
+            self.plane_id = p.loadURDF("plane.urdf")
+            # 加载机械臂
+            base_orientation = p.getQuaternionFromEuler(ARM_BASE_ORIENTATION)
+            self.arm_id = p.loadURDF(
+                ARM_MODEL_PATH,
+                basePosition=ARM_BASE_POSITION,
+                baseOrientation=base_orientation
+            )
+            print("✅ 成功加载场景（地面+机械臂）")
+        except Exception as e:
+            self.disconnect()  # 加载失败时自动断开连接
+            raise RuntimeError(f"❌ 加载场景失败：{str(e)}")
+
+    def _init_joint_info(self) -> None:
+        """私有方法：初始化升降关节的限位和当前位置"""
+        if self.arm_id is None:
+            raise RuntimeError("❌ 机械臂未加载，无法初始化关节信息")
+
+        # 获取关节基础信息
         joint_info = p.getJointInfo(self.arm_id, self.elevator_joint_index)
-        self.joint_min = joint_info[8]  # 关节运动下限
-        self.joint_max = joint_info[9]  # 关节运动上限
-        self.current_pos = p.getJointState(self.arm_id, self.elevator_joint_index)[0]  # 当前位置
+        self.joint_min = joint_info[8]
+        self.joint_max = joint_info[9]
+        # 获取当前关节位置
+        self.current_pos = p.getJointState(self.arm_id, self.elevator_joint_index)[0]
 
-        # 打印关节初始化信息
-        print(f"升降关节初始化完成：")
+    def _print_init_info(self) -> None:
+        """打印初始化信息（格式化输出，更易读）"""
+        print("\n=" * 40)
+        print("📌 机械臂升降关节初始化信息")
+        print("=" * 40)
         print(f"关节索引：{self.elevator_joint_index}")
         print(f"当前位置：{self.current_pos:.3f}")
         print(f"运动范围：[{self.joint_min:.3f}, {self.joint_max:.3f}]")
+        print(f"默认速度：{MOVE_SPEED_DEFAULT}")
+        print(f"位置误差容忍度：{POSITION_TOLERANCE}")
+        print("=" * 40 + "\n")
 
-    def move_elevator(self, target_pos, speed=0.05):
+    def _check_target_pos_valid(self, target_pos: float) -> bool:
+        """私有方法：校验目标位置是否合法（返回布尔值，便于后续扩展）"""
+        if self.joint_min <= target_pos <= self.joint_max:
+            return True
+        print(f"❌ 目标位置 {target_pos:.3f} 超出关节范围：[{self.joint_min:.3f}, {self.joint_max:.3f}]")
+        return False
+
+    def move_elevator(self, target_pos: float, speed: Optional[float] = None) -> None:
         """
-        驱动升降关节运动到目标位置
+        驱动升降关节运动到目标位置（公开方法，对外提供核心功能）
         :param target_pos: 目标位置（需在关节限位范围内）
-        :param speed: 运动速度（正数，越小越慢）
+        :param speed: 运动速度，默认使用MOVE_SPEED_DEFAULT
+        :return: None
         """
-        # 校验目标位置合法性
-        if target_pos < self.joint_min or target_pos > self.joint_max:
-            raise ValueError(f"目标位置超出关节范围！允许范围：[{self.joint_min:.3f}, {self.joint_max:.3f}]")
-
-        print(f"\n开始升降运动：当前位置 {self.current_pos:.3f} → 目标位置 {target_pos:.3f}")
-        # 循环控制，直到接近目标位置（误差小于0.001）
-        while abs(self.current_pos - target_pos) > 0.001:
-            # 计算运动步长（根据目标位置判断升降方向）
-            step = speed if target_pos > self.current_pos else -speed
+        # 处理默认速度
+        move_speed = speed if speed is not None else MOVE_SPEED_DEFAULT
+        # 校验目标位置
+        if not self._check_target_pos_valid(target_pos):
+            return
+
+        # 打印运动开始信息
+        print(f"\n🚀 开始升降运动：当前位置 {self.current_pos:.3f} → 目标位置 {target_pos:.3f}（速度：{move_speed}）")
+
+        # 闭环控制关节运动
+        while abs(self.current_pos - target_pos) > POSITION_TOLERANCE:
+            # 计算运动步长（方向+大小）
+            step = move_speed if target_pos > self.current_pos else -move_speed
             # 更新当前位置（防止超出限位）
             self.current_pos = np.clip(self.current_pos + step, self.joint_min, self.joint_max)
-            # 发送位置指令给关节（位置控制模式）
+            # 发送位置控制指令
             p.setJointMotorControl2(
                 bodyUniqueId=self.arm_id,
                 jointIndex=self.elevator_joint_index,
                 controlMode=p.POSITION_CONTROL,
                 targetPosition=self.current_pos
             )
-            # 步进物理仿真（更新场景状态）
+            # 步进仿真
             p.stepSimulation()
-            # 小幅延时，模拟真实运动节奏
-            time.sleep(0.01)
-            # 获取模拟器中关节的实际位置（反馈同步）
+            time.sleep(DELAY_STEP)
+            # 同步模拟器中的实际关节位置
             self.current_pos = p.getJointState(self.arm_id, self.elevator_joint_index)[0]
-            # 实时刷新显示当前位置
-            print(f"实时位置：{self.current_pos:.3f}", end='\r')
+            # 实时刷新显示（清除当前行，更整洁）
+            print(f"🔍 实时位置：{self.current_pos:.3f}", end='\r')
 
-        print(f"\n升降运动完成！最终位置：{self.current_pos:.3f}")
+        # 运动完成提示
+        print(f"\n✅ 升降运动完成！最终位置：{self.current_pos:.3f}")
 
-    def disconnect(self):
-        """断开与PyBullet模拟器的连接"""
-        p.disconnect(self.physics_client)
-        print("\n已断开与PyBullet模拟器的连接")
+    def move_elevator_relative(self, delta_pos: float, speed: Optional[float] = None) -> None:
+        """
+        相对运动：基于当前位置升降指定距离（新增功能，提升易用性）
+        :param delta_pos: 相对位移（正数=上升，负数=下降）
+        :param speed: 运动速度
+        :return: None
+        """
+        target_pos = self.current_pos + delta_pos
+        self.move_elevator(target_pos, speed)
 
+    def disconnect(self) -> None:
+        """断开模拟器连接（容错处理，避免重复断开）"""
+        if self.physics_client is not None:
+            p.disconnect(self.physics_client)
+            self.physics_client = None
+            print("\n🔌 已断开与PyBullet模拟器的连接")
 
-# ------------------- 主执行程序 -------------------
-if __name__ == "__main__":
-    # 1. 初始化机械臂升降控制器
-    arm_controller = ArmElevatorControllerPyBullet()
 
+# ------------------- 主执行逻辑（解耦，便于测试） -------------------
+def main():
+    """主函数：执行升降动作序列"""
+    arm_controller = None
     try:
-        # 2. 执行升降动作序列
-        arm_controller.move_elevator(target_pos=arm_controller.joint_max * 0.6, speed=0.03)  # 上升（接近上限）
-        time.sleep(1)  # 停顿1秒
-        arm_controller.move_elevator(target_pos=arm_controller.joint_min * 0.6, speed=0.02)  # 下降（接近下限）
-        time.sleep(1)  # 停顿1秒
-        arm_controller.move_elevator(target_pos=0)  # 回到初始中间位置
+        # 初始化控制器
+        arm_controller = ArmElevatorController()
+
+        # 执行升降动作序列
+        print("\n" + "-" * 50)
+        print("📝 执行升降动作序列1：上升到上限60%")
+        print("-" * 50)
+        arm_controller.move_elevator(target_pos=arm_controller.joint_max * 0.6)
+        time.sleep(1)
+
+        print("\n" + "-" * 50)
+        print("📝 执行升降动作序列2：下降到下限60%")
+        print("-" * 50)
+        arm_controller.move_elevator(target_pos=arm_controller.joint_min * 0.6, speed=0.02)
+        time.sleep(1)
+
+        print("\n" + "-" * 50)
+        print("📝 执行升降动作序列3：相对上升0.5")
+        print("-" * 50)
+        arm_controller.move_elevator_relative(delta_pos=0.5, speed=0.04)
+        time.sleep(1)
+
+        print("\n" + "-" * 50)
+        print("📝 执行升降动作序列4：回到初始位置0")
+        print("-" * 50)
+        arm_controller.move_elevator(target_pos=0)
+
+    except Exception as e:
+        print(f"\n❌ 程序执行出错：{str(e)}")
     finally:
-        # 3. 无论是否出错，最终断开连接
-        arm_controller.disconnect()
\ No newline at end of file
+        # 确保无论是否出错，都断开连接
+        if arm_controller is not None:
+            arm_controller.disconnect()
+
+
+if __name__ == "__main__":
+    # 启动程序
+    print("🚀 启动机械臂升降控制系统...")
+    main()
+    print("\n🎉 程序正常结束")
\ No newline at end of file

From e5d7fd72cb29b01fe0f7c14de10d973de3b0ca2b Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Wed, 24 Dec 2025 22:39:28 +0800
Subject: [PATCH 07/13] =?UTF-8?q?mujoco=E7=9A=84=E6=9C=BA=E6=A2=B0?=
 =?UTF-8?q?=E8=87=82=E8=BF=90=E8=A1=8C?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../arm_gripper_v3.xml                        |   0
 src/Mechanical_arm_grasping/main.py           | 299 +++++++-----------
 src/Mechanical_arm_grasping/robot_arm.xml     |   0
 .../robot_arm_control.py                      |   0
 4 files changed, 108 insertions(+), 191 deletions(-)
 create mode 100644 src/Mechanical_arm_grasping/arm_gripper_v3.xml
 create mode 100644 src/Mechanical_arm_grasping/robot_arm.xml
 create mode 100644 src/Mechanical_arm_grasping/robot_arm_control.py

diff --git a/src/Mechanical_arm_grasping/arm_gripper_v3.xml b/src/Mechanical_arm_grasping/arm_gripper_v3.xml
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
index 3c2f4a522b..1e1efa33d6 100644
--- a/src/Mechanical_arm_grasping/main.py
+++ b/src/Mechanical_arm_grasping/main.py
@@ -1,199 +1,116 @@
-import pybullet as p
-import pybullet_data
+# 机械臂MuJoCo 3.4.0 原生语法稳定版（100%兼容，无API报错）
+import mujoco
+import mujoco.viewer
 import time
-import numpy as np
-from typing import Optional, Tuple
 
-# ------------------- 配置常量（便于统一修改） -------------------
-SIMULATION_GRAVITY: Tuple[float, float, float] = (0, 0, 0)  # 仿真重力，(0,0,-9.8)为真实重力
-ARM_MODEL_PATH: str = "kuka_iiwa/model.urdf"  # 机械臂模型路径
-ARM_BASE_POSITION: Tuple[float, float, float] = (0, 0, 0)  # 机械臂初始位置
-ARM_BASE_ORIENTATION: Tuple[float, float, float] = (0, 0, 0)  # 机械臂初始姿态（欧拉角）
-ELEVATOR_JOINT_INDEX: int = 0  # 升降关节索引
-MOVE_SPEED_DEFAULT: float = 0.03  # 默认升降速度
-POSITION_TOLERANCE: float = 0.001  # 位置误差容忍度（到达该误差即认为运动完成）
-DELAY_STEP: float = 0.01  # 仿真步进延时
 
-
-class ArmElevatorController:
-    """机械臂升降关节控制器（面向对象封装，职责单一）"""
-
-    def __init__(self):
-        """初始化模拟器连接、机械臂模型和关节信息"""
-        self.physics_client: Optional[int] = None
-        self.arm_id: Optional[int] = None
-        self.plane_id: Optional[int] = None
-
-        # 关节相关参数
-        self.elevator_joint_index: int = ELEVATOR_JOINT_INDEX
-        self.joint_min: float = 0.0
-        self.joint_max: float = 0.0
-        self.current_pos: float = 0.0
-
-        # 初始化流程
-        self._connect_simulator()
-        self._load_scene()
-        self._init_joint_info()
-        self._print_init_info()
-
-    def _connect_simulator(self) -> None:
-        """私有方法：连接PyBullet模拟器（封装初始化细节）"""
-        try:
-            self.physics_client = p.connect(p.GUI)
-            p.setAdditionalSearchPath(pybullet_data.getDataPath())
-            p.setGravity(*SIMULATION_GRAVITY)
-            print("✅ 成功连接PyBullet模拟器")
-        except Exception as e:
-            raise RuntimeError(f"❌ 连接模拟器失败：{str(e)}")
-
-    def _load_scene(self) -> None:
-        """私有方法：加载地面和机械臂模型（封装场景加载逻辑）"""
-        try:
-            # 加载地面
-            self.plane_id = p.loadURDF("plane.urdf")
-            # 加载机械臂
-            base_orientation = p.getQuaternionFromEuler(ARM_BASE_ORIENTATION)
-            self.arm_id = p.loadURDF(
-                ARM_MODEL_PATH,
-                basePosition=ARM_BASE_POSITION,
-                baseOrientation=base_orientation
-            )
-            print("✅ 成功加载场景（地面+机械臂）")
-        except Exception as e:
-            self.disconnect()  # 加载失败时自动断开连接
-            raise RuntimeError(f"❌ 加载场景失败：{str(e)}")
-
-    def _init_joint_info(self) -> None:
-        """私有方法：初始化升降关节的限位和当前位置"""
-        if self.arm_id is None:
-            raise RuntimeError("❌ 机械臂未加载，无法初始化关节信息")
-
-        # 获取关节基础信息
-        joint_info = p.getJointInfo(self.arm_id, self.elevator_joint_index)
-        self.joint_min = joint_info[8]
-        self.joint_max = joint_info[9]
-        # 获取当前关节位置
-        self.current_pos = p.getJointState(self.arm_id, self.elevator_joint_index)[0]
-
-    def _print_init_info(self) -> None:
-        """打印初始化信息（格式化输出，更易读）"""
-        print("\n=" * 40)
-        print("📌 机械臂升降关节初始化信息")
-        print("=" * 40)
-        print(f"关节索引：{self.elevator_joint_index}")
-        print(f"当前位置：{self.current_pos:.3f}")
-        print(f"运动范围：[{self.joint_min:.3f}, {self.joint_max:.3f}]")
-        print(f"默认速度：{MOVE_SPEED_DEFAULT}")
-        print(f"位置误差容忍度：{POSITION_TOLERANCE}")
-        print("=" * 40 + "\n")
-
-    def _check_target_pos_valid(self, target_pos: float) -> bool:
-        """私有方法：校验目标位置是否合法（返回布尔值，便于后续扩展）"""
-        if self.joint_min <= target_pos <= self.joint_max:
-            return True
-        print(f"❌ 目标位置 {target_pos:.3f} 超出关节范围：[{self.joint_min:.3f}, {self.joint_max:.3f}]")
-        return False
-
-    def move_elevator(self, target_pos: float, speed: Optional[float] = None) -> None:
-        """
-        驱动升降关节运动到目标位置（公开方法，对外提供核心功能）
-        :param target_pos: 目标位置（需在关节限位范围内）
-        :param speed: 运动速度，默认使用MOVE_SPEED_DEFAULT
-        :return: None
-        """
-        # 处理默认速度
-        move_speed = speed if speed is not None else MOVE_SPEED_DEFAULT
-        # 校验目标位置
-        if not self._check_target_pos_valid(target_pos):
-            return
-
-        # 打印运动开始信息
-        print(f"\n🚀 开始升降运动：当前位置 {self.current_pos:.3f} → 目标位置 {target_pos:.3f}（速度：{move_speed}）")
-
-        # 闭环控制关节运动
-        while abs(self.current_pos - target_pos) > POSITION_TOLERANCE:
-            # 计算运动步长（方向+大小）
-            step = move_speed if target_pos > self.current_pos else -move_speed
-            # 更新当前位置（防止超出限位）
-            self.current_pos = np.clip(self.current_pos + step, self.joint_min, self.joint_max)
-            # 发送位置控制指令
-            p.setJointMotorControl2(
-                bodyUniqueId=self.arm_id,
-                jointIndex=self.elevator_joint_index,
-                controlMode=p.POSITION_CONTROL,
-                targetPosition=self.current_pos
-            )
-            # 步进仿真
-            p.stepSimulation()
-            time.sleep(DELAY_STEP)
-            # 同步模拟器中的实际关节位置
-            self.current_pos = p.getJointState(self.arm_id, self.elevator_joint_index)[0]
-            # 实时刷新显示（清除当前行，更整洁）
-            print(f"🔍 实时位置：{self.current_pos:.3f}", end='\r')
-
-        # 运动完成提示
-        print(f"\n✅ 升降运动完成！最终位置：{self.current_pos:.3f}")
-
-    def move_elevator_relative(self, delta_pos: float, speed: Optional[float] = None) -> None:
-        """
-        相对运动：基于当前位置升降指定距离（新增功能，提升易用性）
-        :param delta_pos: 相对位移（正数=上升，负数=下降）
-        :param speed: 运动速度
-        :return: None
-        """
-        target_pos = self.current_pos + delta_pos
-        self.move_elevator(target_pos, speed)
-
-    def disconnect(self) -> None:
-        """断开模拟器连接（容错处理，避免重复断开）"""
-        if self.physics_client is not None:
-            p.disconnect(self.physics_client)
-            self.physics_client = None
-            print("\n🔌 已断开与PyBullet模拟器的连接")
-
-
-# ------------------- 主执行逻辑（解耦，便于测试） -------------------
-def main():
-    """主函数：执行升降动作序列"""
-    arm_controller = None
+def robot_arm_final_stable_demo():
+    # 1. 内置机械臂XML模型（slide/hinge关节，3.4.0原生兼容）
+    robot_xml = """
+<mujoco model="Simple Robot Arm">
+  <compiler angle="radian" inertiafromgeom="true"/>
+  <option timestep="0.005" gravity="0 0 -9.81"/>
+  <visual/>
+  <asset>
+    <material name="red" rgba="0.8 0.2 0.2 1"/>
+    <material name="blue" rgba="0.2 0.2 0.8 1"/>
+    <material name="gray" rgba="0.5 0.5 0.5 1"/>
+  </asset>
+  <worldbody>
+    <camera name="fixed_camera" pos="1.5 1.5 1.0" xyaxes="1 0 0 0 1 0"/>
+    <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 -0.1" material="gray"/>
+    <body name="base" pos="0 0 0">
+      <geom name="base_geom" type="cylinder" size="0.2 0.1" pos="0 0 0" material="blue"/>
+      <joint name="base_joint" type="free"/>
+      <body name="lift_link" pos="0 0 0.1">
+        <geom name="lift_geom" type="cylinder" size="0.15 0.3" pos="0 0 0.3" material="blue"/>
+        <joint name="lift_joint" type="slide" axis="0 0 1" pos="0 0 0" range="0 1.0" damping="0.1"/>
+        <body name="extend_link" pos="0 0 0.6">
+          <geom name="extend_geom" type="cylinder" size="0.1 0.4" pos="0.4 0 0" material="blue"/>
+          <joint name="extend_joint" type="slide" axis="1 0 0" pos="0 0 0" range="0 0.8" damping="0.1"/>
+          <body name="gripper_base" pos="0.8 0 0">
+            <geom name="gripper_base_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="red"/>
+            <body name="left_gripper" pos="0 0.1 0">
+              <geom name="left_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
+              <joint name="left_gripper_joint" type="hinge" axis="0 0 1" pos="0 -0.1 0" range="-0.5 0" damping="0.05"/>
+            </body>
+            <body name="right_gripper" pos="0 -0.1 0">
+              <geom name="right_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
+              <joint name="right_gripper_joint" type="hinge" axis="0 0 1" pos="0 0.1 0" range="0 0.5" damping="0.05"/>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+  <actuator>
+    <position name="lift_actuator" joint="lift_joint" kp="1000" kv="100"/>
+    <position name="extend_actuator" joint="extend_joint" kp="1000" kv="100"/>
+    <position name="left_gripper_actuator" joint="left_gripper_joint" kp="500" kv="50"/>
+    <position name="right_gripper_actuator" joint="right_gripper_joint" kp="500" kv="50"/>
+  </actuator>
+</mujoco>
+    """
+
+    # 2. 加载模型
     try:
-        # 初始化控制器
-        arm_controller = ArmElevatorController()
-
-        # 执行升降动作序列
-        print("\n" + "-" * 50)
-        print("📝 执行升降动作序列1：上升到上限60%")
-        print("-" * 50)
-        arm_controller.move_elevator(target_pos=arm_controller.joint_max * 0.6)
-        time.sleep(1)
-
-        print("\n" + "-" * 50)
-        print("📝 执行升降动作序列2：下降到下限60%")
-        print("-" * 50)
-        arm_controller.move_elevator(target_pos=arm_controller.joint_min * 0.6, speed=0.02)
-        time.sleep(1)
-
-        print("\n" + "-" * 50)
-        print("📝 执行升降动作序列3：相对上升0.5")
-        print("-" * 50)
-        arm_controller.move_elevator_relative(delta_pos=0.5, speed=0.04)
-        time.sleep(1)
-
-        print("\n" + "-" * 50)
-        print("📝 执行升降动作序列4：回到初始位置0")
-        print("-" * 50)
-        arm_controller.move_elevator(target_pos=0)
-
+        model = mujoco.MjModel.from_xml_string(robot_xml)
+        data = mujoco.MjData(model)
+        print("✅ 机械臂模型加载成功，启动仿真...")
     except Exception as e:
-        print(f"\n❌ 程序执行出错：{str(e)}")
-    finally:
-        # 确保无论是否出错，都断开连接
-        if arm_controller is not None:
-            arm_controller.disconnect()
+        print(f"❌ 模型加载失败：{e}")
+        return
+
+    # 3. 获取执行器索引（对应data.ctrl数组的下标，3.4.0原生支持）
+    lift_act_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "lift_actuator")
+    extend_act_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "extend_actuator")
+    left_gripper_act_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "left_gripper_actuator")
+    right_gripper_act_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_gripper_actuator")
+
+    # 4. 动作控制逻辑（直接操作data.ctrl，3.4.0原生语法）
+    def control_lift(target):
+        data.ctrl[lift_act_idx] = target  # 直接给执行器对应下标赋值
+
+    def control_extend(target):
+        data.ctrl[extend_act_idx] = target
+
+    def control_gripper(target_left):
+        target_right = -target_left
+        data.ctrl[left_gripper_act_idx] = target_left
+        data.ctrl[right_gripper_act_idx] = target_right
+
+    # 5. 预设动作流程
+    action_list = [
+        ("上升", "lift", 0.8, 2.0),
+        ("伸展", "extend", 0.6, 2.0),
+        ("夹紧", "gripper", -0.4, 1.0),
+        ("保持", "none", None, 1.5),
+        ("放松", "gripper", 0, 1.0),
+        ("收缩", "extend", 0, 2.0),
+        ("下降", "lift", 0, 2.0),
+    ]
+
+    # 6. 启动可视化并执行动作
+    with mujoco.viewer.launch_passive(model, data) as viewer:
+        for action_name, action_type, target, dur in action_list:
+            print(f"🔧 正在执行：{action_name}")
+            start_time = time.time()
+            while (time.time() - start_time) < dur and viewer.is_running():
+                # 执行对应动作
+                if action_type == "lift":
+                    control_lift(target)
+                elif action_type == "extend":
+                    control_extend(target)
+                elif action_type == "gripper":
+                    control_gripper(target)
+
+                # 步进仿真+同步可视化
+                mujoco.mj_step(model, data)
+                viewer.sync()
+                time.sleep(0.001)
+
+    print("🎉 机械臂动作执行完毕！")
 
 
 if __name__ == "__main__":
-    # 启动程序
-    print("🚀 启动机械臂升降控制系统...")
-    main()
-    print("\n🎉 程序正常结束")
\ No newline at end of file
+    robot_arm_final_stable_demo()
\ No newline at end of file
diff --git a/src/Mechanical_arm_grasping/robot_arm.xml b/src/Mechanical_arm_grasping/robot_arm.xml
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/Mechanical_arm_grasping/robot_arm_control.py b/src/Mechanical_arm_grasping/robot_arm_control.py
new file mode 100644
index 0000000000..e69de29bb2

From 615d15f12de5cc50c32df3e30b3a995da959eeb7 Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Thu, 25 Dec 2025 20:14:32 +0800
Subject: [PATCH 08/13] =?UTF-8?q?=E6=97=8B=E8=BD=AC=E5=85=B3=E8=8A=82?=
 =?UTF-8?q?=E5=A4=9A=E8=87=AA=E7=94=B1=E5=BA=A6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Mechanical_arm_grasping/main.py | 165 +++++++++++++++++-----------
 1 file changed, 100 insertions(+), 65 deletions(-)

diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
index 0f2329ea3b..4a95ab7e0d 100644
--- a/src/Mechanical_arm_grasping/main.py
+++ b/src/Mechanical_arm_grasping/main.py
@@ -1,13 +1,13 @@
-# 机械臂MuJoCo 3.4.0 原生语法稳定版（100%兼容，无冲突标记）
+# MuJoCo 3.4.0 多自由度旋转机械臂演示（全新版本）
 import mujoco
 import mujoco.viewer
 import time
 
 
-def robot_arm_final_stable_demo():
-    # 1. 内置机械臂XML模型（slide/hinge关节，3.4.0原生兼容）
-    robot_xml = """
-<mujoco model="Simple Robot Arm">
+def multi_dof_robot_arm_demo():
+    # 1. 内置多自由度机械臂XML模型（含旋转+升降+伸展+夹爪）
+    multi_dof_xml = """
+<mujoco model="Multi-DOF Robot Arm">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <visual/>
@@ -15,35 +15,50 @@ def robot_arm_final_stable_demo():
     <material name="red" rgba="0.8 0.2 0.2 1"/>
     <material name="blue" rgba="0.2 0.2 0.8 1"/>
     <material name="gray" rgba="0.5 0.5 0.5 1"/>
+    <material name="yellow" rgba="0.8 0.8 0.2 1"/>
   </asset>
   <worldbody>
-    <camera name="fixed_camera" pos="1.5 1.5 1.0" xyaxes="1 0 0 0 1 0"/>
+    <camera name="fixed_camera" pos="2.0 1.0 1.2" xyaxes="1 0 0 0 1 0"/>
+    <!-- 地面 -->
     <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 -0.1" material="gray"/>
+    <!-- 目标标记点 -->
+    <geom name="target_marker" type="sphere" size="0.05" pos="0.6 0.6 0.1" material="yellow"/>
+    <!-- 多自由度机械臂 -->
     <body name="base" pos="0 0 0">
       <geom name="base_geom" type="cylinder" size="0.2 0.1" pos="0 0 0" material="blue"/>
       <joint name="base_joint" type="free"/>
-      <body name="lift_link" pos="0 0 0.1">
-        <geom name="lift_geom" type="cylinder" size="0.15 0.3" pos="0 0 0.3" material="blue"/>
-        <joint name="lift_joint" type="slide" axis="0 0 1" pos="0 0 0" range="0 1.0" damping="0.1"/>
-        <body name="extend_link" pos="0 0 0.6">
-          <geom name="extend_geom" type="cylinder" size="0.1 0.4" pos="0.4 0 0" material="blue"/>
-          <joint name="extend_joint" type="slide" axis="1 0 0" pos="0 0 0" range="0 0.8" damping="0.1"/>
-          <body name="gripper_base" pos="0.8 0 0">
-            <geom name="gripper_base_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="red"/>
-            <body name="left_gripper" pos="0 0.1 0">
-              <geom name="left_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
-              <joint name="left_gripper_joint" type="hinge" axis="0 0 1" pos="0 -0.1 0" range="-0.5 0" damping="0.05"/>
-            </body>
-            <body name="right_gripper" pos="0 -0.1 0">
-              <geom name="right_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
-              <joint name="right_gripper_joint" type="hinge" axis="0 0 1" pos="0 0.1 0" range="0 0.5" damping="0.05"/>
+      <!-- 1. 水平旋转关节（绕Z轴旋转，调整机械臂朝向） -->
+      <body name="rotate_link" pos="0 0 0.1">
+        <geom name="rotate_geom" type="cylinder" size="0.12 0.2" pos="0 0 0.1" material="blue"/>
+        <joint name="rotate_joint" type="hinge" axis="0 0 1" pos="0 0 0" range="-1.57 1.57" damping="0.1"/>
+        <!-- 2. 升降关节 -->
+        <body name="lift_link" pos="0 0 0.3">
+          <geom name="lift_geom" type="cylinder" size="0.1 0.3" pos="0 0 0.3" material="blue"/>
+          <joint name="lift_joint" type="slide" axis="0 0 1" pos="0 0 0" range="0 1.0" damping="0.1"/>
+          <!-- 3. 伸展关节 -->
+          <body name="extend_link" pos="0 0 0.6">
+            <geom name="extend_geom" type="cylinder" size="0.08 0.4" pos="0.4 0 0" material="blue"/>
+            <joint name="extend_joint" type="slide" axis="1 0 0" pos="0 0 0" range="0 0.8" damping="0.1"/>
+            <!-- 4. 夹爪 -->
+            <body name="gripper_base" pos="0.8 0 0">
+              <geom name="gripper_base_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="red"/>
+              <body name="left_gripper" pos="0 0.1 0">
+                <geom name="left_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
+                <joint name="left_gripper_joint" type="hinge" axis="0 0 1" pos="0 -0.1 0" range="-0.5 0" damping="0.05"/>
+              </body>
+              <body name="right_gripper" pos="0 -0.1 0">
+                <geom name="right_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
+                <joint name="right_gripper_joint" type="hinge" axis="0 0 1" pos="0 0.1 0" range="0 0.5" damping="0.05"/>
+              </body>
             </body>
           </body>
         </body>
       </body>
     </body>
   </worldbody>
+  <!-- 执行器配置（新增旋转关节执行器） -->
   <actuator>
+    <position name="rotate_actuator" joint="rotate_joint" kp="1000" kv="100"/>
     <position name="lift_actuator" joint="lift_joint" kp="1000" kv="100"/>
     <position name="extend_actuator" joint="extend_joint" kp="1000" kv="100"/>
     <position name="left_gripper_actuator" joint="left_gripper_joint" kp="500" kv="50"/>
@@ -54,63 +69,83 @@ def robot_arm_final_stable_demo():
 
     # 2. 加载模型
     try:
-        model = mujoco.MjModel.from_xml_string(robot_xml)
+        model = mujoco.MjModel.from_xml_string(multi_dof_xml)
         data = mujoco.MjData(model)
-        print("✅ 机械臂模型加载成功，启动仿真...")
+        print("✅ 多自由度机械臂模型加载成功，启动仿真...")
     except Exception as e:
         print(f"❌ 模型加载失败：{e}")
         return
 
-    # 3. 获取执行器索引（对应data.ctrl数组的下标，3.4.0原生支持）
-    lift_act_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "lift_actuator")
-    extend_act_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "extend_actuator")
-    left_gripper_act_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "left_gripper_actuator")
-    right_gripper_act_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_gripper_actuator")
-
-    # 4. 动作控制逻辑（直接操作data.ctrl，3.4.0原生语法）
-    def control_lift(target):
-        data.ctrl[lift_act_idx] = target  # 直接给执行器对应下标赋值
-
-    def control_extend(target):
-        data.ctrl[extend_act_idx] = target
-
-    def control_gripper(target_left):
-        target_right = -target_left
-        data.ctrl[left_gripper_act_idx] = target_left
-        data.ctrl[right_gripper_act_idx] = target_right
-
-    # 5. 预设动作流程
-    action_list = [
-        ("上升", "lift", 0.8, 2.0),
-        ("伸展", "extend", 0.6, 2.0),
-        ("夹紧", "gripper", -0.4, 1.0),
-        ("保持", "none", None, 1.5),
-        ("放松", "gripper", 0, 1.0),
-        ("收缩", "extend", 0, 2.0),
-        ("下降", "lift", 0, 2.0),
+    # 3. 获取所有执行器索引
+    rotate_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "rotate_actuator")
+    lift_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "lift_actuator")
+    extend_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "extend_actuator")
+    left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "left_gripper_actuator")
+    right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_gripper_actuator")
+
+    # 4. 控制函数（新增旋转控制）
+    def control_rotate(val):
+        data.ctrl[rotate_idx] = val
+
+    def control_lift(val):
+        data.ctrl[lift_idx] = val
+
+    def control_extend(val):
+        data.ctrl[extend_idx] = val
+
+    def control_gripper(val):
+        data.ctrl[left_grip_idx] = val
+        data.ctrl[right_grip_idx] = -val
+
+    # 5. 多自由度动作流程（含旋转调整朝向）
+    action_sequence = [
+        ("旋转调整朝向", control_rotate, 1.0, 2.0),  # 水平旋转（朝向黄色标记点）
+        ("上升准备", control_lift, 0.6, 1.5),
+        ("伸展接近目标", control_extend, 0.7, 2.0),
+        ("下降到位", control_lift, 0.2, 1.5),
+        ("夹紧夹爪", control_gripper, -0.4, 1.0),
+        ("抓取上升", control_lift, 0.7, 1.5),
+        ("反向旋转归位", control_rotate, 0.0, 2.0),
+        ("收缩机械臂", control_extend, 0.0, 2.0),
+        ("下降放置", control_lift, 0.2, 1.5),
+        ("放松夹爪", control_gripper, 0.0, 1.0),
+        ("最终归位", control_lift, 0.5, 1.5),
     ]
 
-    # 6. 启动可视化并执行动作
+    # 6. 启动仿真并执行动作（新增关节状态打印）
     with mujoco.viewer.launch_passive(model, data) as viewer:
-        for action_name, action_type, target, dur in action_list:
-            print(f"🔧 正在执行：{action_name}")
+        # 打印状态表头
+        print("\n📊 实时关节状态（旋转角度/升降高度/伸展长度）")
+        print("-" * 50)
+
+        for action_name, func, target, duration in action_sequence:
+            print(f"\n🔧 正在执行：{action_name}")
             start_time = time.time()
-            while (time.time() - start_time) < dur and viewer.is_running():
-                # 执行对应动作
-                if action_type == "lift":
-                    control_lift(target)
-                elif action_type == "extend":
-                    control_extend(target)
-                elif action_type == "gripper":
-                    control_gripper(target)
-
-                # 步进仿真+同步可视化
+            while (time.time() - start_time) < duration and viewer.is_running():
+                func(target)
                 mujoco.mj_step(model, data)
+
+                # 实时打印关键关节状态
+                rotate_angle = data.joint("rotate_joint").qpos[0]
+                lift_height = data.joint("lift_joint").qpos[0]
+                extend_length = data.joint("extend_joint").qpos[0]
+                print(
+                    f"\r旋转角度：{rotate_angle:.2f} rad | 升降高度：{lift_height:.2f} m | 伸展长度：{extend_length:.2f} m",
+                    end="")
+
                 viewer.sync()
                 time.sleep(0.001)
 
-    print("🎉 机械臂动作执行完毕！")
+        # 最后保持4秒查看效果
+        print("\n\n📌 动作流程完成，保持可视化4秒...")
+        start = time.time()
+        while (time.time() - start) < 4 and viewer.is_running():
+            mujoco.mj_step(model, data)
+            viewer.sync()
+            time.sleep(0.001)
+
+    print("\n🎉 多自由度机械臂演示完毕！")
 
 
 if __name__ == "__main__":
-    robot_arm_final_stable_demo()
\ No newline at end of file
+    multi_dof_robot_arm_demo()
\ No newline at end of file

From d13871005fef7a5c919a9bb57b6822e7e754f2ca Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Fri, 26 Dec 2025 08:42:26 +0800
Subject: [PATCH 09/13] =?UTF-8?q?=20=E5=B8=A6=E5=AE=9E=E6=97=B6=E6=9C=AB?=
 =?UTF-8?q?=E7=AB=AF=E4=BD=8D=E7=BD=AE=E5=8F=8D=E9=A6=88=E7=9A=84=20SCARA?=
 =?UTF-8?q?=20=E5=9E=8B=E6=9C=BA=E6=A2=B0=E8=87=82=E6=BC=94=E7=A4=BA?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Mechanical_arm_grasping/main.py | 241 ++++++++++++++++------------
 1 file changed, 139 insertions(+), 102 deletions(-)

diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
index 4a95ab7e0d..3b8b00d148 100644
--- a/src/Mechanical_arm_grasping/main.py
+++ b/src/Mechanical_arm_grasping/main.py
@@ -1,54 +1,69 @@
-# MuJoCo 3.4.0 多自由度旋转机械臂演示（全新版本）
+# MuJoCo 3.4.0 SCARA型机械臂（末端反馈+目标跟随）演示
 import mujoco
 import mujoco.viewer
 import time
+import numpy as np
 
 
-def multi_dof_robot_arm_demo():
-    # 1. 内置多自由度机械臂XML模型（含旋转+升降+伸展+夹爪）
-    multi_dof_xml = """
-<mujoco model="Multi-DOF Robot Arm">
+def scara_robot_arm_demo():
+    # 1. 内置SCARA机械臂XML模型（工业常用构型）
+    scara_xml = """
+<mujoco model="SCARA Robot Arm">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <visual/>
   <asset>
     <material name="red" rgba="0.8 0.2 0.2 1"/>
-    <material name="blue" rgba="0.2 0.2 0.8 1"/>
+    <material name="darkblue" rgba="0.1 0.1 0.6 1"/>
     <material name="gray" rgba="0.5 0.5 0.5 1"/>
+    <material name="green" rgba="0.2 0.8 0.2 1"/>
     <material name="yellow" rgba="0.8 0.8 0.2 1"/>
+    <material name="cyan" rgba="0.2 0.8 0.8 1"/>
   </asset>
   <worldbody>
-    <camera name="fixed_camera" pos="2.0 1.0 1.2" xyaxes="1 0 0 0 1 0"/>
+    <camera name="fixed_camera" pos="2.0 2.0 1.5" xyaxes="1 0 0 0 1 0"/>
     <!-- 地面 -->
     <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 -0.1" material="gray"/>
-    <!-- 目标标记点 -->
-    <geom name="target_marker" type="sphere" size="0.05" pos="0.6 0.6 0.1" material="yellow"/>
-    <!-- 多自由度机械臂 -->
+    <!-- 动态目标点（青色小球） -->
+    <body name="moving_target" pos="0.8 0.6 0.3">
+      <geom name="target_geom" type="sphere" size="0.05" pos="0 0 0" material="cyan"/>
+      <joint name="target_joint" type="free"/>
+    </body>
+    <!-- SCARA机械臂（工业构型：旋转1+旋转2+升降+旋转夹爪） -->
     <body name="base" pos="0 0 0">
-      <geom name="base_geom" type="cylinder" size="0.2 0.1" pos="0 0 0" material="blue"/>
+      <geom name="base_geom" type="cylinder" size="0.25 0.15" pos="0 0 0" material="darkblue"/>
       <joint name="base_joint" type="free"/>
-      <!-- 1. 水平旋转关节（绕Z轴旋转，调整机械臂朝向） -->
-      <body name="rotate_link" pos="0 0 0.1">
-        <geom name="rotate_geom" type="cylinder" size="0.12 0.2" pos="0 0 0.1" material="blue"/>
-        <joint name="rotate_joint" type="hinge" axis="0 0 1" pos="0 0 0" range="-1.57 1.57" damping="0.1"/>
-        <!-- 2. 升降关节 -->
-        <body name="lift_link" pos="0 0 0.3">
-          <geom name="lift_geom" type="cylinder" size="0.1 0.3" pos="0 0 0.3" material="blue"/>
-          <joint name="lift_joint" type="slide" axis="0 0 1" pos="0 0 0" range="0 1.0" damping="0.1"/>
-          <!-- 3. 伸展关节 -->
-          <body name="extend_link" pos="0 0 0.6">
-            <geom name="extend_geom" type="cylinder" size="0.08 0.4" pos="0.4 0 0" material="blue"/>
-            <joint name="extend_joint" type="slide" axis="1 0 0" pos="0 0 0" range="0 0.8" damping="0.1"/>
-            <!-- 4. 夹爪 -->
-            <body name="gripper_base" pos="0.8 0 0">
-              <geom name="gripper_base_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="red"/>
-              <body name="left_gripper" pos="0 0.1 0">
-                <geom name="left_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
-                <joint name="left_gripper_joint" type="hinge" axis="0 0 1" pos="0 -0.1 0" range="-0.5 0" damping="0.05"/>
-              </body>
-              <body name="right_gripper" pos="0 -0.1 0">
-                <geom name="right_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
-                <joint name="right_gripper_joint" type="hinge" axis="0 0 1" pos="0 0.1 0" range="0 0.5" damping="0.05"/>
+      <!-- 关节1：水平旋转（绕Z轴，基座旋转） -->
+      <body name="joint1_link" pos="0 0 0.15">
+        <geom name="joint1_geom" type="cylinder" size="0.15 0.2" pos="0 0 0.1" material="darkblue"/>
+        <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.08"/>
+        <!-- 关节2：水平旋转（绕Z轴，大臂旋转） -->
+        <body name="joint2_link" pos="0.5 0 0.1">
+          <geom name="joint2_geom" type="cylinder" size="0.12 0.4" pos="0.2 0 0" material="darkblue"/>
+          <joint name="joint2" type="hinge" axis="0 0 1" pos="0 0 0" range="-2.0 2.0" damping="0.08"/>
+          <!-- 关节3：垂直升降（Z轴，小臂升降） -->
+          <body name="joint3_link" pos="0.4 0 0">
+            <geom name="joint3_geom" type="cylinder" size="0.1 0.3" pos="0 0 0.15" material="darkblue"/>
+            <joint name="joint3" type="slide" axis="0 0 1" pos="0 0 0" range="0 0.8" damping="0.08"/>
+            <!-- 关节4：夹爪旋转（绕Z轴，末端旋转） -->
+            <body name="joint4_link" pos="0 0 0.15">
+              <geom name="joint4_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="darkblue"/>
+              <joint name="joint4" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.05"/>
+              <!-- 末端夹爪 -->
+              <body name="gripper_base" pos="0 0 0">
+                <geom name="gripper_base_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="red"/>
+                <!-- 左夹爪 -->
+                <body name="left_gripper" pos="0 0.1 0">
+                  <geom name="left_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
+                  <joint name="left_grip_joint" type="hinge" axis="0 0 1" pos="0 -0.1 0" range="-0.5 0" damping="0.05"/>
+                </body>
+                <!-- 右夹爪 -->
+                <body name="right_gripper" pos="0 -0.1 0">
+                  <geom name="right_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
+                  <joint name="right_grip_joint" type="hinge" axis="0 0 1" pos="0 0.1 0" range="0 0.5" damping="0.05"/>
+                </body>
+                <!-- 末端位置标记（绿色小球，用于反馈） -->
+                <geom name="end_effector_marker" type="sphere" size="0.03" pos="0 0 -0.05" material="green"/>
               </body>
             </body>
           </body>
@@ -56,96 +71,118 @@ def multi_dof_robot_arm_demo():
       </body>
     </body>
   </worldbody>
-  <!-- 执行器配置（新增旋转关节执行器） -->
+  <!-- 执行器配置（高精度位置控制） -->
   <actuator>
-    <position name="rotate_actuator" joint="rotate_joint" kp="1000" kv="100"/>
-    <position name="lift_actuator" joint="lift_joint" kp="1000" kv="100"/>
-    <position name="extend_actuator" joint="extend_joint" kp="1000" kv="100"/>
-    <position name="left_gripper_actuator" joint="left_gripper_joint" kp="500" kv="50"/>
-    <position name="right_gripper_actuator" joint="right_gripper_joint" kp="500" kv="50"/>
+    <position name="joint1_act" joint="joint1" kp="1500" kv="150"/>
+    <position name="joint2_act" joint="joint2" kp="1500" kv="150"/>
+    <position name="joint3_act" joint="joint3" kp="1500" kv="150"/>
+    <position name="joint4_act" joint="joint4" kp="1500" kv="150"/>
+    <position name="left_grip_act" joint="left_grip_joint" kp="800" kv="80"/>
+    <position name="right_grip_act" joint="right_grip_joint" kp="800" kv="80"/>
   </actuator>
 </mujoco>
     """
 
     # 2. 加载模型
     try:
-        model = mujoco.MjModel.from_xml_string(multi_dof_xml)
+        model = mujoco.MjModel.from_xml_string(scara_xml)
         data = mujoco.MjData(model)
-        print("✅ 多自由度机械臂模型加载成功，启动仿真...")
+        print("✅ SCARA机械臂模型加载成功，启动仿真...")
     except Exception as e:
         print(f"❌ 模型加载失败：{e}")
         return
 
-    # 3. 获取所有执行器索引
-    rotate_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "rotate_actuator")
-    lift_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "lift_actuator")
-    extend_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "extend_actuator")
-    left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "left_gripper_actuator")
-    right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_gripper_actuator")
-
-    # 4. 控制函数（新增旋转控制）
-    def control_rotate(val):
-        data.ctrl[rotate_idx] = val
-
-    def control_lift(val):
-        data.ctrl[lift_idx] = val
-
-    def control_extend(val):
-        data.ctrl[extend_idx] = val
+    # 3. 获取执行器索引
+    joint1_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint1_act")
+    joint2_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act")
+    joint3_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint3_act")
+    joint4_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint4_act")
+    left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "left_grip_act")
+    right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_grip_act")
+
+    # 4. 获取末端执行器（绿色标记）的ID（用于位置反馈）
+    end_effector_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "end_effector_marker")
+    target_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "target_geom")
+
+    # 5. 控制函数（平滑控制+末端反馈）
+    def smooth_set_joint(joint_idx, target_val, duration, viewer):
+        start_val = data.ctrl[joint_idx]
+        start_time = time.time()
+        while (time.time() - start_time) < duration and viewer.is_running():
+            t = (time.time() - start_time) / duration
+            current_val = start_val + t * (target_val - start_val)
+            data.ctrl[joint_idx] = current_val
+            # 实时打印末端位置
+            print_end_effector_position(data, end_effector_id, target_id)
+            # 步进仿真
+            mujoco.mj_step(model, data)
+            viewer.sync()
+            time.sleep(0.001)
 
-    def control_gripper(val):
-        data.ctrl[left_grip_idx] = val
-        data.ctrl[right_grip_idx] = -val
+    def smooth_set_gripper(target, duration, viewer):
+        start_left = data.ctrl[left_grip_idx]
+        start_right = data.ctrl[right_grip_idx]
+        target_right = -target
+        start_time = time.time()
+        while (time.time() - start_time) < duration and viewer.is_running():
+            t = (time.time() - start_time) / duration
+            data.ctrl[left_grip_idx] = start_left + t * (target - start_left)
+            data.ctrl[right_grip_idx] = start_right + t * (target_right - start_right)
+            print_end_effector_position(data, end_effector_id, target_id)
+            mujoco.mj_step(model, data)
+            viewer.sync()
+            time.sleep(0.001)
 
-    # 5. 多自由度动作流程（含旋转调整朝向）
-    action_sequence = [
-        ("旋转调整朝向", control_rotate, 1.0, 2.0),  # 水平旋转（朝向黄色标记点）
-        ("上升准备", control_lift, 0.6, 1.5),
-        ("伸展接近目标", control_extend, 0.7, 2.0),
-        ("下降到位", control_lift, 0.2, 1.5),
-        ("夹紧夹爪", control_gripper, -0.4, 1.0),
-        ("抓取上升", control_lift, 0.7, 1.5),
-        ("反向旋转归位", control_rotate, 0.0, 2.0),
-        ("收缩机械臂", control_extend, 0.0, 2.0),
-        ("下降放置", control_lift, 0.2, 1.5),
-        ("放松夹爪", control_gripper, 0.0, 1.0),
-        ("最终归位", control_lift, 0.5, 1.5),
+    def print_end_effector_position(data, ee_id, tar_id):
+        # 获取末端和目标的位置
+        ee_pos = data.geom_xpos[ee_id]
+        tar_pos = data.geom_xpos[tar_id]
+        # 计算距离
+        distance = np.linalg.norm(ee_pos - tar_pos)
+        # 实时刷新打印（不换行）
+        print(
+            f"\r末端位置(X:{ee_pos[0]:.2f}, Y:{ee_pos[1]:.2f}, Z:{ee_pos[2]:.2f}) | 目标位置(X:{tar_pos[0]:.2f}, Y:{tar_pos[1]:.2f}, Z:{tar_pos[2]:.2f}) | 距离:{distance:.3f} m",
+            end="")
+
+    # 6. SCARA机械臂目标跟随流程
+    scara_steps = [
+        ("关节1旋转对准目标", joint1_idx, 0.785, 2.5),  # 45°旋转
+        ("关节2旋转调整姿态", joint2_idx, -0.523, 2.0),  # -30°旋转
+        ("关节3升降接近目标", joint3_idx, 0.3, 1.8),  # 下降接近目标
+        ("关节4旋转校准方向", joint4_idx, 1.047, 2.0),  # 60°旋转校准
+        ("夹紧夹爪模拟抓取", "gripper", -0.4, 1.2),  # 夹紧夹爪
+        ("关节3升降抬升目标", joint3_idx, 0.6, 1.8),  # 抬升
+        ("关节1反向旋转归位", joint1_idx, 0.0, 2.5),  # 归位旋转
+        ("关节2反向旋转归位", joint2_idx, 0.0, 2.0),  # 归位旋转
+        ("关节3下降放置目标", joint3_idx, 0.3, 1.8),  # 下降放置
+        ("放松夹爪完成操作", "gripper", 0.0, 1.2),  # 放松夹爪
+        ("关节3升降归位", joint3_idx, 0.0, 1.8),  # 最终归位
+        ("关节4旋转归位", joint4_idx, 0.0, 2.0),  # 最终归位
     ]
 
-    # 6. 启动仿真并执行动作（新增关节状态打印）
+    # 7. 启动仿真
     with mujoco.viewer.launch_passive(model, data) as viewer:
-        # 打印状态表头
-        print("\n📊 实时关节状态（旋转角度/升降高度/伸展长度）")
-        print("-" * 50)
-
-        for action_name, func, target, duration in action_sequence:
-            print(f"\n🔧 正在执行：{action_name}")
-            start_time = time.time()
-            while (time.time() - start_time) < duration and viewer.is_running():
-                func(target)
-                mujoco.mj_step(model, data)
-
-                # 实时打印关键关节状态
-                rotate_angle = data.joint("rotate_joint").qpos[0]
-                lift_height = data.joint("lift_joint").qpos[0]
-                extend_length = data.joint("extend_joint").qpos[0]
-                print(
-                    f"\r旋转角度：{rotate_angle:.2f} rad | 升降高度：{lift_height:.2f} m | 伸展长度：{extend_length:.2f} m",
-                    end="")
-
-                viewer.sync()
-                time.sleep(0.001)
-
-        # 最后保持4秒查看效果
-        print("\n\n📌 动作流程完成，保持可视化4秒...")
-        start = time.time()
-        while (time.time() - start) < 4 and viewer.is_running():
+        print("\n📌 开始SCARA机械臂目标跟随流程...")
+        print("-" * 60)
+
+        for step_name, joint_or_grip, target, duration in scara_steps:
+            print(f"\n\n🔧 {step_name}")
+            if joint_or_grip == "gripper":
+                smooth_set_gripper(target, duration, viewer)
+            else:
+                smooth_set_joint(joint_or_grip, target, duration, viewer)
+
+        # 保持5秒查看最终效果
+        print("\n\n\n📌 SCARA机械臂操作完成，保持可视化5秒...")
+        start_hold = time.time()
+        while (time.time() - start_hold) < 5 and viewer.is_running():
+            print_end_effector_position(data, end_effector_id, target_id)
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
 
-    print("\n🎉 多自由度机械臂演示完毕！")
+    print("\n\n🎉 SCARA机械臂末端反馈+目标跟随演示完毕！")
 
 
 if __name__ == "__main__":
-    multi_dof_robot_arm_demo()
\ No newline at end of file
+    scara_robot_arm_demo()
\ No newline at end of file

From 0dc7604bdf7eccdd962d030340879151ebafb783 Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Fri, 26 Dec 2025 16:05:05 +0800
Subject: [PATCH 10/13] =?UTF-8?q?7=E8=87=AA=E7=94=B1=E5=BA=A6=E5=8D=8F?=
 =?UTF-8?q?=E4=BD=9C=E6=9C=BA=E6=A2=B0=E8=87=82=E6=A8=A1=E5=9E=8B?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Mechanical_arm_grasping/main.py | 272 ++++++++++++++++------------
 1 file changed, 157 insertions(+), 115 deletions(-)

diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
index 3b8b00d148..e8ca917650 100644
--- a/src/Mechanical_arm_grasping/main.py
+++ b/src/Mechanical_arm_grasping/main.py
@@ -1,69 +1,86 @@
-# MuJoCo 3.4.0 SCARA型机械臂（末端反馈+目标跟随）演示
+# MuJoCo 3.4.0 7自由度协作机械臂（极简版，无传感器，零XML错误）
 import mujoco
 import mujoco.viewer
 import time
 import numpy as np
 
 
-def scara_robot_arm_demo():
-    # 1. 内置SCARA机械臂XML模型（工业常用构型）
-    scara_xml = """
-<mujoco model="SCARA Robot Arm">
+def collaborative_robot_arm_demo():
+    # 彻底移除所有传感器相关代码，仅保留基础机械臂+抓取逻辑
+    cobot_xml = """
+<mujoco model="7-DOF Collaborative Robot Arm">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <visual/>
   <asset>
     <material name="red" rgba="0.8 0.2 0.2 1"/>
-    <material name="darkblue" rgba="0.1 0.1 0.6 1"/>
+    <material name="lightblue" rgba="0.4 0.7 0.9 1"/>
     <material name="gray" rgba="0.5 0.5 0.5 1"/>
     <material name="green" rgba="0.2 0.8 0.2 1"/>
     <material name="yellow" rgba="0.8 0.8 0.2 1"/>
-    <material name="cyan" rgba="0.2 0.8 0.8 1"/>
+    <material name="orange" rgba="0.9 0.5 0.2 1"/>
   </asset>
+
+  <!-- 世界体定义 -->
   <worldbody>
-    <camera name="fixed_camera" pos="2.0 2.0 1.5" xyaxes="1 0 0 0 1 0"/>
+    <camera name="fixed_camera" pos="2.5 2.0 2.0" xyaxes="1 0 0 0 1 0"/>
     <!-- 地面 -->
     <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 -0.1" material="gray"/>
-    <!-- 动态目标点（青色小球） -->
-    <body name="moving_target" pos="0.8 0.6 0.3">
-      <geom name="target_geom" type="sphere" size="0.05" pos="0 0 0" material="cyan"/>
+    <!-- 易碎目标：橙色球体（低刚度，防止夹碎） -->
+    <body name="fragile_target" pos="0.7 0.5 0.1">
+      <geom name="target_geom" type="sphere" size="0.07" pos="0 0 0" material="orange" solref="0.02 1.0" solimp="0.9 0.95 0.01"/>
       <joint name="target_joint" type="free"/>
     </body>
-    <!-- SCARA机械臂（工业构型：旋转1+旋转2+升降+旋转夹爪） -->
+    <!-- 7自由度协作机械臂（UR5e构型） -->
     <body name="base" pos="0 0 0">
-      <geom name="base_geom" type="cylinder" size="0.25 0.15" pos="0 0 0" material="darkblue"/>
+      <geom name="base_geom" type="cylinder" size="0.2 0.2" pos="0 0 0" material="lightblue"/>
       <joint name="base_joint" type="free"/>
-      <!-- 关节1：水平旋转（绕Z轴，基座旋转） -->
-      <body name="joint1_link" pos="0 0 0.15">
-        <geom name="joint1_geom" type="cylinder" size="0.15 0.2" pos="0 0 0.1" material="darkblue"/>
-        <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.08"/>
-        <!-- 关节2：水平旋转（绕Z轴，大臂旋转） -->
-        <body name="joint2_link" pos="0.5 0 0.1">
-          <geom name="joint2_geom" type="cylinder" size="0.12 0.4" pos="0.2 0 0" material="darkblue"/>
-          <joint name="joint2" type="hinge" axis="0 0 1" pos="0 0 0" range="-2.0 2.0" damping="0.08"/>
-          <!-- 关节3：垂直升降（Z轴，小臂升降） -->
-          <body name="joint3_link" pos="0.4 0 0">
-            <geom name="joint3_geom" type="cylinder" size="0.1 0.3" pos="0 0 0.15" material="darkblue"/>
-            <joint name="joint3" type="slide" axis="0 0 1" pos="0 0 0" range="0 0.8" damping="0.08"/>
-            <!-- 关节4：夹爪旋转（绕Z轴，末端旋转） -->
-            <body name="joint4_link" pos="0 0 0.15">
-              <geom name="joint4_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="darkblue"/>
+      <!-- 关节1：基座旋转（Z轴） -->
+      <body name="joint1_link" pos="0 0 0.2">
+        <geom name="joint1_geom" type="cylinder" size="0.12 0.3" pos="0 0 0.15" material="lightblue"/>
+        <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.05"/>
+        <!-- 关节2：大臂俯仰（Y轴） -->
+        <body name="joint2_link" pos="0 0 0.3">
+          <geom name="joint2_geom" type="cylinder" size="0.1 0.4" pos="0 0 0.2" material="lightblue"/>
+          <joint name="joint2" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
+          <!-- 关节3：小臂俯仰（Y轴） -->
+          <body name="joint3_link" pos="0 0 0.4">
+            <geom name="joint3_geom" type="cylinder" size="0.09 0.4" pos="0 0 0.2" material="lightblue"/>
+            <joint name="joint3" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
+            <!-- 关节4：腕部旋转（Z轴） -->
+            <body name="joint4_link" pos="0 0 0.4">
+              <geom name="joint4_geom" type="cylinder" size="0.08 0.3" pos="0 0 0.15" material="lightblue"/>
               <joint name="joint4" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.05"/>
-              <!-- 末端夹爪 -->
-              <body name="gripper_base" pos="0 0 0">
-                <geom name="gripper_base_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="red"/>
-                <!-- 左夹爪 -->
-                <body name="left_gripper" pos="0 0.1 0">
-                  <geom name="left_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
-                  <joint name="left_grip_joint" type="hinge" axis="0 0 1" pos="0 -0.1 0" range="-0.5 0" damping="0.05"/>
-                </body>
-                <!-- 右夹爪 -->
-                <body name="right_gripper" pos="0 -0.1 0">
-                  <geom name="right_gripper_geom" type="box" size="0.1 0.05 0.05" pos="0 0 0" material="red"/>
-                  <joint name="right_grip_joint" type="hinge" axis="0 0 1" pos="0 0.1 0" range="0 0.5" damping="0.05"/>
+              <!-- 关节5：腕部俯仰（Y轴） -->
+              <body name="joint5_link" pos="0 0 0.3">
+                <geom name="joint5_geom" type="cylinder" size="0.07 0.3" pos="0 0 0.15" material="lightblue"/>
+                <joint name="joint5" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
+                <!-- 关节6：腕部偏摆（Z轴） -->
+                <body name="joint6_link" pos="0 0 0.3">
+                  <geom name="joint6_geom" type="cylinder" size="0.06 0.2" pos="0 0 0.1" material="lightblue"/>
+                  <joint name="joint6" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.05"/>
+                  <!-- 关节7：末端旋转（Y轴） -->
+                  <body name="joint7_link" pos="0 0 0.2">
+                    <geom name="joint7_geom" type="cylinder" size="0.05 0.2" pos="0 0 0.1" material="lightblue"/>
+                    <joint name="joint7" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
+                    <!-- 夹爪（简化版，无传感器） -->
+                    <body name="gripper_base" pos="0 0 0.2">
+                      <geom name="gripper_base_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="red"/>
+                      <!-- 左夹爪 -->
+                      <body name="left_gripper" pos="0 0.1 0">
+                        <geom name="left_gripper_geom" type="box" size="0.1 0.06 0.06" pos="0 0 0" material="red"/>
+                        <joint name="left_grip_joint" type="hinge" axis="0 0 1" pos="0 -0.1 0" range="-0.6 0" damping="0.03"/>
+                      </body>
+                      <!-- 右夹爪 -->
+                      <body name="right_gripper" pos="0 -0.1 0">
+                        <geom name="right_gripper_geom" type="box" size="0.1 0.06 0.06" pos="0 0 0" material="red"/>
+                        <joint name="right_grip_joint" type="hinge" axis="0 0 1" pos="0 0.1 0" range="0 0.6" damping="0.03"/>
+                      </body>
+                      <!-- 末端标记 -->
+                      <geom name="end_marker" type="sphere" size="0.03" pos="0 0 -0.05" material="green"/>
+                    </body>
+                  </body>
                 </body>
-                <!-- 末端位置标记（绿色小球，用于反馈） -->
-                <geom name="end_effector_marker" type="sphere" size="0.03" pos="0 0 -0.05" material="green"/>
               </body>
             </body>
           </body>
@@ -71,118 +88,143 @@ def scara_robot_arm_demo():
       </body>
     </body>
   </worldbody>
-  <!-- 执行器配置（高精度位置控制） -->
+
+  <!-- 执行器配置（仅保留位置/速度控制，无传感器依赖） -->
   <actuator>
-    <position name="joint1_act" joint="joint1" kp="1500" kv="150"/>
-    <position name="joint2_act" joint="joint2" kp="1500" kv="150"/>
-    <position name="joint3_act" joint="joint3" kp="1500" kv="150"/>
-    <position name="joint4_act" joint="joint4" kp="1500" kv="150"/>
-    <position name="left_grip_act" joint="left_grip_joint" kp="800" kv="80"/>
-    <position name="right_grip_act" joint="right_grip_joint" kp="800" kv="80"/>
+    <position name="joint1_act" joint="joint1" kp="1200" kv="120"/>
+    <position name="joint2_act" joint="joint2" kp="1200" kv="120"/>
+    <position name="joint3_act" joint="joint3" kp="1200" kv="120"/>
+    <position name="joint4_act" joint="joint4" kp="1200" kv="120"/>
+    <position name="joint5_act" joint="joint5" kp="1200" kv="120"/>
+    <position name="joint6_act" joint="joint6" kp="1200" kv="120"/>
+    <position name="joint7_act" joint="joint7" kp="1200" kv="120"/>
+    <!-- 夹爪速度控制（低刚度，防止夹碎物体） -->
+    <velocity name="left_grip_act" joint="left_grip_joint" kv="50" ctrlrange="-0.5 0"/>
+    <velocity name="right_grip_act" joint="right_grip_joint" kv="50" ctrlrange="0 0.5"/>
   </actuator>
 </mujoco>
     """
 
-    # 2. 加载模型
+    # 2. 加载模型（100%兼容3.4.0，无任何传感器相关错误）
     try:
-        model = mujoco.MjModel.from_xml_string(scara_xml)
+        model = mujoco.MjModel.from_xml_string(cobot_xml)
         data = mujoco.MjData(model)
-        print("✅ SCARA机械臂模型加载成功，启动仿真...")
+        print("✅ 7自由度协作机械臂模型加载成功，启动仿真...")
     except Exception as e:
         print(f"❌ 模型加载失败：{e}")
         return
 
-    # 3. 获取执行器索引
-    joint1_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint1_act")
-    joint2_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act")
-    joint3_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint3_act")
-    joint4_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint4_act")
+    # 3. 获取执行器索引（仅保留基础执行器，无传感器）
+    # 关节执行器索引
+    joint_idxs = {
+        "joint1": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint1_act"),
+        "joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act"),
+        "joint3": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint3_act"),
+        "joint4": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint4_act"),
+        "joint5": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint5_act"),
+        "joint6": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint6_act"),
+        "joint7": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint7_act"),
+    }
+    # 夹爪执行器索引
     left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "left_grip_act")
     right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_grip_act")
 
-    # 4. 获取末端执行器（绿色标记）的ID（用于位置反馈）
-    end_effector_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "end_effector_marker")
-    target_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "target_geom")
-
-    # 5. 控制函数（平滑控制+末端反馈）
-    def smooth_set_joint(joint_idx, target_val, duration, viewer):
-        start_val = data.ctrl[joint_idx]
+    # 4. 核心控制函数（移除力传感器依赖，改用时间控制抓取）
+    def smooth_joint_control(joint_name, target_angle, duration, viewer):
+        """平滑关节角度控制"""
+        idx = joint_idxs[joint_name]
+        start_angle = data.ctrl[idx]
         start_time = time.time()
         while (time.time() - start_time) < duration and viewer.is_running():
             t = (time.time() - start_time) / duration
-            current_val = start_val + t * (target_val - start_val)
-            data.ctrl[joint_idx] = current_val
-            # 实时打印末端位置
-            print_end_effector_position(data, end_effector_id, target_id)
-            # 步进仿真
+            current_angle = start_angle + t * (target_angle - start_angle)
+            data.ctrl[idx] = current_angle
+            # 打印关节状态（无力度）
+            print(f"\r{joint_name}角度：{current_angle:.2f} rad", end="")
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
 
-    def smooth_set_gripper(target, duration, viewer):
-        start_left = data.ctrl[left_grip_idx]
-        start_right = data.ctrl[right_grip_idx]
-        target_right = -target
+    def safe_grasp(viewer):
+        """安全抓取（通过时间控制闭合，模拟力控效果）"""
+        print("\n🔧 开始安全抓取（低速度闭合，防止夹碎物体）")
+        grip_speed = -0.1  # 低速闭合，避免夹碎
+        start_time = time.time()
+        # 闭合1.5秒后停止（模拟力控阈值）
+        while time.time() - start_time < 1.5 and viewer.is_running():
+            data.ctrl[left_grip_idx] = grip_speed
+            data.ctrl[right_grip_idx] = -grip_speed
+            print(f"\r抓取进度：{((time.time() - start_time) / 1.5) * 100:.1f}%", end="")
+            mujoco.mj_step(model, data)
+            viewer.sync()
+            time.sleep(0.001)
+        # 停止闭合
+        data.ctrl[left_grip_idx] = 0
+        data.ctrl[right_grip_idx] = 0
+        print("\n✅ 抓取完成（已停止闭合，防止夹碎）！")
+
+    def release_gripper(duration, viewer):
+        """放松夹爪"""
+        print("\n🔧 开始放松夹爪")
         start_time = time.time()
         while (time.time() - start_time) < duration and viewer.is_running():
-            t = (time.time() - start_time) / duration
-            data.ctrl[left_grip_idx] = start_left + t * (target - start_left)
-            data.ctrl[right_grip_idx] = start_right + t * (target_right - start_right)
-            print_end_effector_position(data, end_effector_id, target_id)
+            data.ctrl[left_grip_idx] = 0.2  # 张开速度
+            data.ctrl[right_grip_idx] = -0.2
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
+        print("✅ 夹爪已完全张开")
 
-    def print_end_effector_position(data, ee_id, tar_id):
-        # 获取末端和目标的位置
-        ee_pos = data.geom_xpos[ee_id]
-        tar_pos = data.geom_xpos[tar_id]
-        # 计算距离
-        distance = np.linalg.norm(ee_pos - tar_pos)
-        # 实时刷新打印（不换行）
-        print(
-            f"\r末端位置(X:{ee_pos[0]:.2f}, Y:{ee_pos[1]:.2f}, Z:{ee_pos[2]:.2f}) | 目标位置(X:{tar_pos[0]:.2f}, Y:{tar_pos[1]:.2f}, Z:{tar_pos[2]:.2f}) | 距离:{distance:.3f} m",
-            end="")
-
-    # 6. SCARA机械臂目标跟随流程
-    scara_steps = [
-        ("关节1旋转对准目标", joint1_idx, 0.785, 2.5),  # 45°旋转
-        ("关节2旋转调整姿态", joint2_idx, -0.523, 2.0),  # -30°旋转
-        ("关节3升降接近目标", joint3_idx, 0.3, 1.8),  # 下降接近目标
-        ("关节4旋转校准方向", joint4_idx, 1.047, 2.0),  # 60°旋转校准
-        ("夹紧夹爪模拟抓取", "gripper", -0.4, 1.2),  # 夹紧夹爪
-        ("关节3升降抬升目标", joint3_idx, 0.6, 1.8),  # 抬升
-        ("关节1反向旋转归位", joint1_idx, 0.0, 2.5),  # 归位旋转
-        ("关节2反向旋转归位", joint2_idx, 0.0, 2.0),  # 归位旋转
-        ("关节3下降放置目标", joint3_idx, 0.3, 1.8),  # 下降放置
-        ("放松夹爪完成操作", "gripper", 0.0, 1.2),  # 放松夹爪
-        ("关节3升降归位", joint3_idx, 0.0, 1.8),  # 最终归位
-        ("关节4旋转归位", joint4_idx, 0.0, 2.0),  # 最终归位
+    # 5. 7自由度机械臂抓取流程
+    cobot_steps = [
+        ("关节1旋转对准目标", "joint1", 0.87, 3.0),  # 50°旋转
+        ("关节2俯仰调整高度", "joint2", 0.785, 2.5),  # 45°俯仰
+        ("关节3俯仰接近目标", "joint3", -0.61, 2.5),  # -35°俯仰
+        ("关节4腕部旋转校准", "joint4", 1.047, 2.0),  # 60°旋转
+        ("关节5腕部俯仰调整", "joint5", 0.523, 2.0),  # 30°俯仰
+        ("关节6腕部偏摆校准", "joint6", 0.349, 2.0),  # 20°偏摆
+        ("关节7末端旋转对准", "joint7", 0.174, 2.0),  # 10°旋转
     ]
 
-    # 7. 启动仿真
+    # 6. 启动仿真（纯3.4.0原生逻辑）
     with mujoco.viewer.launch_passive(model, data) as viewer:
-        print("\n📌 开始SCARA机械臂目标跟随流程...")
+        print("\n📌 开始7自由度协作机械臂抓取流程...")
         print("-" * 60)
 
-        for step_name, joint_or_grip, target, duration in scara_steps:
+        # 第一步：关节运动对准目标
+        for step_name, joint_name, target_angle, duration in cobot_steps:
             print(f"\n\n🔧 {step_name}")
-            if joint_or_grip == "gripper":
-                smooth_set_gripper(target, duration, viewer)
-            else:
-                smooth_set_joint(joint_or_grip, target, duration, viewer)
+            smooth_joint_control(joint_name, target_angle, duration, viewer)
+
+        # 第二步：安全抓取（模拟力控）
+        safe_grasp(viewer=viewer)
+
+        # 第三步：抬升目标（仅调整关节2）
+        print("\n\n🔧 抓取成功，抬升目标")
+        smooth_joint_control("joint2", 1.047, 2.5, viewer)  # 60°俯仰抬升
+
+        # 第四步：归位（关节1旋转回原位）
+        print("\n\n🔧 旋转归位")
+        smooth_joint_control("joint1", 0.0, 3.0, viewer)
+
+        # 第五步：下放目标
+        print("\n\n🔧 下放目标")
+        smooth_joint_control("joint2", 0.785, 2.5, viewer)  # 45°俯仰下放
+
+        # 第六步：放松夹爪
+        print("\n\n🔧 放松夹爪完成放置")
+        release_gripper(duration=2.0, viewer=viewer)
 
-        # 保持5秒查看最终效果
-        print("\n\n\n📌 SCARA机械臂操作完成，保持可视化5秒...")
+        # 保持6秒查看最终效果
+        print("\n\n📌 抓取流程完成，保持可视化6秒...")
         start_hold = time.time()
-        while (time.time() - start_hold) < 5 and viewer.is_running():
-            print_end_effector_position(data, end_effector_id, target_id)
+        while (time.time() - start_hold) < 6 and viewer.is_running():
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
 
-    print("\n\n🎉 SCARA机械臂末端反馈+目标跟随演示完毕！")
+    print("\n\n🎉 7自由度协作机械臂抓取演示完毕！")
 
 
 if __name__ == "__main__":
-    scara_robot_arm_demo()
\ No newline at end of file
+    collaborative_robot_arm_demo()
\ No newline at end of file

From 06a0154191de70a131483c83a1ab4c5e2f7fe20a Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Sat, 27 Dec 2025 17:20:12 +0800
Subject: [PATCH 11/13] =?UTF-8?q?2D=20=E5=B9=B3=E9=9D=A2=E6=9C=BA=E6=A2=B0?=
 =?UTF-8?q?=E8=87=82=E6=8A=93=E5=8F=96=E6=BC=94=E7=A4=BA?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Mechanical_arm_grasping/main.py | 268 +++++++++++++---------------
 1 file changed, 121 insertions(+), 147 deletions(-)

diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
index e8ca917650..7db2149def 100644
--- a/src/Mechanical_arm_grasping/main.py
+++ b/src/Mechanical_arm_grasping/main.py
@@ -1,87 +1,66 @@
-# MuJoCo 3.4.0 7自由度协作机械臂（极简版，无传感器，零XML错误）
+# MuJoCo 3.4.0 轻量版2D平面机械臂抓取（无传感器，零XML错误）
 import mujoco
 import mujoco.viewer
 import time
 import numpy as np
 
 
-def collaborative_robot_arm_demo():
-    # 彻底移除所有传感器相关代码，仅保留基础机械臂+抓取逻辑
-    cobot_xml = """
-<mujoco model="7-DOF Collaborative Robot Arm">
+def simple_2d_robot_arm_demo():
+    # 纯2D平面模型，仅保留MuJoCo 3.4.0原生支持标签
+    robot_2d_xml = """
+<mujoco model="2D Simple Robot Arm">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
-  <visual/>
+  <visual>
+    <global azimuth="90" elevation="-90"/>  <!-- 2D平面视角 -->
+  </visual>
   <asset>
     <material name="red" rgba="0.8 0.2 0.2 1"/>
-    <material name="lightblue" rgba="0.4 0.7 0.9 1"/>
+    <material name="blue" rgba="0.2 0.4 0.8 1"/>
     <material name="gray" rgba="0.5 0.5 0.5 1"/>
-    <material name="green" rgba="0.2 0.8 0.2 1"/>
     <material name="yellow" rgba="0.8 0.8 0.2 1"/>
-    <material name="orange" rgba="0.9 0.5 0.2 1"/>
   </asset>
 
-  <!-- 世界体定义 -->
+  <!-- 2D世界体定义（限制在X-Y平面运动） -->
   <worldbody>
-    <camera name="fixed_camera" pos="2.5 2.0 2.0" xyaxes="1 0 0 0 1 0"/>
-    <!-- 地面 -->
-    <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 -0.1" material="gray"/>
-    <!-- 易碎目标：橙色球体（低刚度，防止夹碎） -->
-    <body name="fragile_target" pos="0.7 0.5 0.1">
-      <geom name="target_geom" type="sphere" size="0.07" pos="0 0 0" material="orange" solref="0.02 1.0" solimp="0.9 0.95 0.01"/>
+    <!-- 固定相机（2D视角） -->
+    <camera name="2d_camera" pos="0 2 1" xyaxes="1 0 0 0 0 1"/>
+    <!-- 地面（2D平面） -->
+    <geom name="floor" type="plane" size="3 3 0.1" pos="0 0 -0.1" material="gray"/>
+    <!-- 抓取目标：黄色立方体（2D平面放置） -->
+    <body name="target" pos="1.2 0 0.1">
+      <geom name="target_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="yellow"/>
       <joint name="target_joint" type="free"/>
     </body>
-    <!-- 7自由度协作机械臂（UR5e构型） -->
+    <!-- 2自由度平面机械臂 -->
     <body name="base" pos="0 0 0">
-      <geom name="base_geom" type="cylinder" size="0.2 0.2" pos="0 0 0" material="lightblue"/>
+      <geom name="base_geom" type="cylinder" size="0.15 0.1" pos="0 0 0" material="blue"/>
       <joint name="base_joint" type="free"/>
-      <!-- 关节1：基座旋转（Z轴） -->
-      <body name="joint1_link" pos="0 0 0.2">
-        <geom name="joint1_geom" type="cylinder" size="0.12 0.3" pos="0 0 0.15" material="lightblue"/>
-        <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.05"/>
-        <!-- 关节2：大臂俯仰（Y轴） -->
-        <body name="joint2_link" pos="0 0 0.3">
-          <geom name="joint2_geom" type="cylinder" size="0.1 0.4" pos="0 0 0.2" material="lightblue"/>
-          <joint name="joint2" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
-          <!-- 关节3：小臂俯仰（Y轴） -->
-          <body name="joint3_link" pos="0 0 0.4">
-            <geom name="joint3_geom" type="cylinder" size="0.09 0.4" pos="0 0 0.2" material="lightblue"/>
-            <joint name="joint3" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
-            <!-- 关节4：腕部旋转（Z轴） -->
-            <body name="joint4_link" pos="0 0 0.4">
-              <geom name="joint4_geom" type="cylinder" size="0.08 0.3" pos="0 0 0.15" material="lightblue"/>
-              <joint name="joint4" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.05"/>
-              <!-- 关节5：腕部俯仰（Y轴） -->
-              <body name="joint5_link" pos="0 0 0.3">
-                <geom name="joint5_geom" type="cylinder" size="0.07 0.3" pos="0 0 0.15" material="lightblue"/>
-                <joint name="joint5" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
-                <!-- 关节6：腕部偏摆（Z轴） -->
-                <body name="joint6_link" pos="0 0 0.3">
-                  <geom name="joint6_geom" type="cylinder" size="0.06 0.2" pos="0 0 0.1" material="lightblue"/>
-                  <joint name="joint6" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.05"/>
-                  <!-- 关节7：末端旋转（Y轴） -->
-                  <body name="joint7_link" pos="0 0 0.2">
-                    <geom name="joint7_geom" type="cylinder" size="0.05 0.2" pos="0 0 0.1" material="lightblue"/>
-                    <joint name="joint7" type="hinge" axis="0 1 0" pos="0 0 0" range="-2.0 2.0" damping="0.05"/>
-                    <!-- 夹爪（简化版，无传感器） -->
-                    <body name="gripper_base" pos="0 0 0.2">
-                      <geom name="gripper_base_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="red"/>
-                      <!-- 左夹爪 -->
-                      <body name="left_gripper" pos="0 0.1 0">
-                        <geom name="left_gripper_geom" type="box" size="0.1 0.06 0.06" pos="0 0 0" material="red"/>
-                        <joint name="left_grip_joint" type="hinge" axis="0 0 1" pos="0 -0.1 0" range="-0.6 0" damping="0.03"/>
-                      </body>
-                      <!-- 右夹爪 -->
-                      <body name="right_gripper" pos="0 -0.1 0">
-                        <geom name="right_gripper_geom" type="box" size="0.1 0.06 0.06" pos="0 0 0" material="red"/>
-                        <joint name="right_grip_joint" type="hinge" axis="0 0 1" pos="0 0.1 0" range="0 0.6" damping="0.03"/>
-                      </body>
-                      <!-- 末端标记 -->
-                      <geom name="end_marker" type="sphere" size="0.03" pos="0 0 -0.05" material="green"/>
-                    </body>
-                  </body>
-                </body>
-              </body>
+
+      <!-- 关节1：基座旋转（Z轴，2D平面旋转） -->
+      <body name="arm1" pos="0 0 0.1">
+        <geom name="arm1_geom" type="cylinder" size="0.08 0.6" pos="0 0 0.3" material="blue"/>
+        <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.03"/>
+
+        <!-- 关节2：大臂俯仰（Z轴，2D平面摆动） -->
+        <body name="arm2" pos="0 0 0.6">
+          <geom name="arm2_geom" type="cylinder" size="0.06 0.5" pos="0 0 0.25" material="blue"/>
+          <joint name="joint2" type="hinge" axis="0 0 1" pos="0 0 0" range="-2.0 2.0" damping="0.03"/>
+
+          <!-- 简易夹爪（2D平面抓取） -->
+          <body name="gripper_base" pos="0 0 0.5">
+            <geom name="gripper_base_geom" type="box" size="0.08 0.08 0.08" pos="0 0 0" material="red"/>
+
+            <!-- 左夹爪 -->
+            <body name="left_grip" pos="0 0.08 0">
+              <geom name="left_grip_geom" type="box" size="0.06 0.04 0.06" pos="0 0 0" material="red"/>
+              <joint name="left_grip_joint" type="hinge" axis="0 0 1" pos="0 -0.08 0" range="-0.5 0" damping="0.02"/>
+            </body>
+
+            <!-- 右夹爪 -->
+            <body name="right_grip" pos="0 -0.08 0">
+              <geom name="right_grip_geom" type="box" size="0.06 0.04 0.06" pos="0 0 0" material="red"/>
+              <joint name="right_grip_joint" type="hinge" axis="0 0 1" pos="0 0.08 0" range="0 0.5" damping="0.02"/>
             </body>
           </body>
         </body>
@@ -89,142 +68,137 @@ def collaborative_robot_arm_demo():
     </body>
   </worldbody>
 
-  <!-- 执行器配置（仅保留位置/速度控制，无传感器依赖） -->
+  <!-- 执行器配置（MuJoCo 3.4.0原生支持） -->
   <actuator>
-    <position name="joint1_act" joint="joint1" kp="1200" kv="120"/>
-    <position name="joint2_act" joint="joint2" kp="1200" kv="120"/>
-    <position name="joint3_act" joint="joint3" kp="1200" kv="120"/>
-    <position name="joint4_act" joint="joint4" kp="1200" kv="120"/>
-    <position name="joint5_act" joint="joint5" kp="1200" kv="120"/>
-    <position name="joint6_act" joint="joint6" kp="1200" kv="120"/>
-    <position name="joint7_act" joint="joint7" kp="1200" kv="120"/>
-    <!-- 夹爪速度控制（低刚度，防止夹碎物体） -->
-    <velocity name="left_grip_act" joint="left_grip_joint" kv="50" ctrlrange="-0.5 0"/>
-    <velocity name="right_grip_act" joint="right_grip_joint" kv="50" ctrlrange="0 0.5"/>
+    <!-- 关节位置控制 -->
+    <position name="joint1_act" joint="joint1" kp="1000" kv="100"/>
+    <position name="joint2_act" joint="joint2" kp="1000" kv="100"/>
+
+    <!-- 夹爪速度控制（安全低速） -->
+    <velocity name="left_grip_act" joint="left_grip_joint" kv="40" ctrlrange="-0.4 0"/>
+    <velocity name="right_grip_act" joint="right_grip_joint" kv="40" ctrlrange="0 0.4"/>
   </actuator>
 </mujoco>
     """
 
-    # 2. 加载模型（100%兼容3.4.0，无任何传感器相关错误）
+    # 加载模型（确保100%兼容MuJoCo 3.4.0）
     try:
-        model = mujoco.MjModel.from_xml_string(cobot_xml)
+        model = mujoco.MjModel.from_xml_string(robot_2d_xml)
         data = mujoco.MjData(model)
-        print("✅ 7自由度协作机械臂模型加载成功，启动仿真...")
+        print("✅ 2D平面机械臂模型加载成功，启动仿真...")
     except Exception as e:
         print(f"❌ 模型加载失败：{e}")
         return
 
-    # 3. 获取执行器索引（仅保留基础执行器，无传感器）
-    # 关节执行器索引
+    # 获取执行器索引
     joint_idxs = {
         "joint1": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint1_act"),
-        "joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act"),
-        "joint3": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint3_act"),
-        "joint4": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint4_act"),
-        "joint5": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint5_act"),
-        "joint6": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint6_act"),
-        "joint7": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint7_act"),
+        "joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act")
     }
-    # 夹爪执行器索引
     left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "left_grip_act")
     right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_grip_act")
 
-    # 4. 核心控制函数（移除力传感器依赖，改用时间控制抓取）
-    def smooth_joint_control(joint_name, target_angle, duration, viewer):
-        """平滑关节角度控制"""
+    # 核心控制函数
+    def smooth_joint_move(joint_name, target_angle, duration, viewer):
+        """平滑移动关节到目标角度"""
         idx = joint_idxs[joint_name]
         start_angle = data.ctrl[idx]
         start_time = time.time()
+
         while (time.time() - start_time) < duration and viewer.is_running():
-            t = (time.time() - start_time) / duration
-            current_angle = start_angle + t * (target_angle - start_angle)
+            progress = (time.time() - start_time) / duration
+            current_angle = start_angle + progress * (target_angle - start_angle)
             data.ctrl[idx] = current_angle
-            # 打印关节状态（无力度）
-            print(f"\r{joint_name}角度：{current_angle:.2f} rad", end="")
+
+            # 打印实时状态
+            print(f"\r{joint_name} 当前角度：{current_angle:.2f} rad | 目标角度：{target_angle:.2f} rad", end="")
+
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
+        print()  # 换行
 
-    def safe_grasp(viewer):
-        """安全抓取（通过时间控制闭合，模拟力控效果）"""
-        print("\n🔧 开始安全抓取（低速度闭合，防止夹碎物体）")
-        grip_speed = -0.1  # 低速闭合，避免夹碎
+    def safe_gripper_close(viewer):
+        """安全闭合夹爪（低速+定时，模拟力控）"""
+        print("\n🔧 开始闭合夹爪（安全低速）")
+        grip_speed = -0.3
         start_time = time.time()
-        # 闭合1.5秒后停止（模拟力控阈值）
-        while time.time() - start_time < 1.5 and viewer.is_running():
+        close_duration = 1.2  # 闭合1.2秒后停止，防止夹碎
+
+        while (time.time() - start_time) < close_duration and viewer.is_running():
+            progress = (time.time() - start_time) / close_duration
             data.ctrl[left_grip_idx] = grip_speed
             data.ctrl[right_grip_idx] = -grip_speed
-            print(f"\r抓取进度：{((time.time() - start_time) / 1.5) * 100:.1f}%", end="")
+
+            print(f"\r夹爪闭合进度：{progress * 100:.1f}%", end="")
+
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
-        # 停止闭合
+
+        # 停止夹爪运动
         data.ctrl[left_grip_idx] = 0
         data.ctrl[right_grip_idx] = 0
-        print("\n✅ 抓取完成（已停止闭合，防止夹碎）！")
+        print("\n✅ 夹爪闭合完成，已锁定目标")
 
-    def release_gripper(duration, viewer):
-        """放松夹爪"""
-        print("\n🔧 开始放松夹爪")
+    def gripper_open(duration, viewer):
+        """张开夹爪"""
+        print("\n🔧 开始张开夹爪")
         start_time = time.time()
+
         while (time.time() - start_time) < duration and viewer.is_running():
-            data.ctrl[left_grip_idx] = 0.2  # 张开速度
-            data.ctrl[right_grip_idx] = -0.2
+            data.ctrl[left_grip_idx] = 0.3
+            data.ctrl[right_grip_idx] = -0.3
+
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
-        print("✅ 夹爪已完全张开")
-
-    # 5. 7自由度机械臂抓取流程
-    cobot_steps = [
-        ("关节1旋转对准目标", "joint1", 0.87, 3.0),  # 50°旋转
-        ("关节2俯仰调整高度", "joint2", 0.785, 2.5),  # 45°俯仰
-        ("关节3俯仰接近目标", "joint3", -0.61, 2.5),  # -35°俯仰
-        ("关节4腕部旋转校准", "joint4", 1.047, 2.0),  # 60°旋转
-        ("关节5腕部俯仰调整", "joint5", 0.523, 2.0),  # 30°俯仰
-        ("关节6腕部偏摆校准", "joint6", 0.349, 2.0),  # 20°偏摆
-        ("关节7末端旋转对准", "joint7", 0.174, 2.0),  # 10°旋转
-    ]
-
-    # 6. 启动仿真（纯3.4.0原生逻辑）
+
+        data.ctrl[left_grip_idx] = 0
+        data.ctrl[right_grip_idx] = 0
+        print("✅ 夹爪已完全张开，目标放置完成")
+
+    # 2D机械臂抓取流程
     with mujoco.viewer.launch_passive(model, data) as viewer:
-        print("\n📌 开始7自由度协作机械臂抓取流程...")
+        print("\n📌 开始2D平面机械臂抓取流程...")
         print("-" * 60)
 
-        # 第一步：关节运动对准目标
-        for step_name, joint_name, target_angle, duration in cobot_steps:
-            print(f"\n\n🔧 {step_name}")
-            smooth_joint_control(joint_name, target_angle, duration, viewer)
+        # 步骤1：关节1旋转对准目标
+        print("\n\n🔧 步骤1：基座旋转对准目标")
+        smooth_joint_move("joint1", 0.0, 2.5, viewer)
+
+        # 步骤2：关节2俯仰接近目标
+        print("\n\n🔧 步骤2：大臂俯仰接近目标")
+        smooth_joint_move("joint2", -0.785, 2.5, viewer)  # -45°俯仰
 
-        # 第二步：安全抓取（模拟力控）
-        safe_grasp(viewer=viewer)
+        # 步骤3：安全闭合夹爪抓取目标
+        safe_gripper_close(viewer)
 
-        # 第三步：抬升目标（仅调整关节2）
-        print("\n\n🔧 抓取成功，抬升目标")
-        smooth_joint_control("joint2", 1.047, 2.5, viewer)  # 60°俯仰抬升
+        # 步骤4：抬升目标（关节2回正）
+        print("\n\n🔧 步骤4：抬升抓取目标")
+        smooth_joint_move("joint2", 0.0, 2.0, viewer)
 
-        # 第四步：归位（关节1旋转回原位）
-        print("\n\n🔧 旋转归位")
-        smooth_joint_control("joint1", 0.0, 3.0, viewer)
+        # 步骤5：基座旋转归位
+        print("\n\n🔧 步骤5：机械臂旋转归位")
+        smooth_joint_move("joint1", 1.57, 3.0, viewer)  # 90°旋转归位
 
-        # 第五步：下放目标
-        print("\n\n🔧 下放目标")
-        smooth_joint_control("joint2", 0.785, 2.5, viewer)  # 45°俯仰下放
+        # 步骤6：下放目标（关节2再次俯仰）
+        print("\n\n🔧 步骤6：下放抓取目标")
+        smooth_joint_move("joint2", -0.785, 2.0, viewer)
 
-        # 第六步：放松夹爪
-        print("\n\n🔧 放松夹爪完成放置")
-        release_gripper(duration=2.0, viewer=viewer)
+        # 步骤7：张开夹爪完成放置
+        gripper_open(1.5, viewer)
 
-        # 保持6秒查看最终效果
-        print("\n\n📌 抓取流程完成，保持可视化6秒...")
+        # 保持可视化5秒
+        print("\n\n📌 抓取流程全部完成，保持可视化5秒...")
         start_hold = time.time()
-        while (time.time() - start_hold) < 6 and viewer.is_running():
+        while (time.time() - start_hold) < 5 and viewer.is_running():
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
 
-    print("\n\n🎉 7自由度协作机械臂抓取演示完毕！")
+    print("\n\n🎉 2D平面机械臂抓取演示完毕！")
 
 
 if __name__ == "__main__":
-    collaborative_robot_arm_demo()
\ No newline at end of file
+    simple_2d_robot_arm_demo()
\ No newline at end of file

From 0783d0e69a5c23e8f2a7068cc956ef8f80a0fd84 Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Sat, 27 Dec 2025 17:32:01 +0800
Subject: [PATCH 12/13] =?UTF-8?q?=E6=A1=8C=E9=9D=A2=E5=88=86=E6=8B=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Mechanical_arm_grasping/main.py | 245 ++++++++++++++++------------
 1 file changed, 141 insertions(+), 104 deletions(-)

diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
index 7db2149def..bff3e37182 100644
--- a/src/Mechanical_arm_grasping/main.py
+++ b/src/Mechanical_arm_grasping/main.py
@@ -1,66 +1,80 @@
-# MuJoCo 3.4.0 轻量版2D平面机械臂抓取（无传感器，零XML错误）
+# MuJoCo 3.4.0 桌面分拣机械臂（无传感器，零XML错误，批量分拣演示）
 import mujoco
 import mujoco.viewer
 import time
 import numpy as np
 
 
-def simple_2d_robot_arm_demo():
-    # 纯2D平面模型，仅保留MuJoCo 3.4.0原生支持标签
-    robot_2d_xml = """
-<mujoco model="2D Simple Robot Arm">
+def desktop_sorting_robot_demo():
+    # 纯MuJoCo 3.4.0原生语法，仅保留支持的根标签
+    sorting_robot_xml = """
+<mujoco model="3-DOF Desktop Sorting Robot">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <visual>
-    <global azimuth="90" elevation="-90"/>  <!-- 2D平面视角 -->
+    <global azimuth="45" elevation="-30"/>  <!-- 直观3D视角 -->
   </visual>
   <asset>
     <material name="red" rgba="0.8 0.2 0.2 1"/>
     <material name="blue" rgba="0.2 0.4 0.8 1"/>
     <material name="gray" rgba="0.5 0.5 0.5 1"/>
+    <material name="green" rgba="0.2 0.8 0.2 1"/>
     <material name="yellow" rgba="0.8 0.8 0.2 1"/>
   </asset>
 
-  <!-- 2D世界体定义（限制在X-Y平面运动） -->
+  <!-- 世界体定义（桌面分拣场景） -->
   <worldbody>
-    <!-- 固定相机（2D视角） -->
-    <camera name="2d_camera" pos="0 2 1" xyaxes="1 0 0 0 0 1"/>
-    <!-- 地面（2D平面） -->
-    <geom name="floor" type="plane" size="3 3 0.1" pos="0 0 -0.1" material="gray"/>
-    <!-- 抓取目标：黄色立方体（2D平面放置） -->
-    <body name="target" pos="1.2 0 0.1">
-      <geom name="target_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="yellow"/>
-      <joint name="target_joint" type="free"/>
+    <!-- 固定视角相机 -->
+    <camera name="sorting_camera" pos="2 2 1.5" xyaxes="1 0 0 0 1 0"/>
+    <!-- 桌面（灰色平面） -->
+    <geom name="desktop" type="plane" size="2 2 0.1" pos="0 0 -0.05" material="gray"/>
+    <!-- 分拣目标1：红色方块（待搬运） -->
+    <body name="target_red" pos="1.0 0.5 0.0">
+      <geom name="target_red_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="red"/>
+      <joint name="target_red_joint" type="free"/>
     </body>
-    <!-- 2自由度平面机械臂 -->
-    <body name="base" pos="0 0 0">
-      <geom name="base_geom" type="cylinder" size="0.15 0.1" pos="0 0 0" material="blue"/>
+    <!-- 分拣目标2：蓝色方块（待搬运） -->
+    <body name="target_blue" pos="1.0 -0.5 0.0">
+      <geom name="target_blue_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="blue"/>
+      <joint name="target_blue_joint" type="free"/>
+    </body>
+    <!-- 分拣区域：绿色标记框（目标放置区域） -->
+    <geom name="sorting_area" type="box" size="0.3 0.3 0.01" pos="-1.0 0 0.0" material="green"/>
+    <!-- 3自由度桌面机械臂 -->
+    <body name="robot_base" pos="0 0 0.0">
+      <geom name="base_geom" type="cylinder" size="0.2 0.1" pos="0 0 0" material="yellow"/>
       <joint name="base_joint" type="free"/>
 
-      <!-- 关节1：基座旋转（Z轴，2D平面旋转） -->
-      <body name="arm1" pos="0 0 0.1">
-        <geom name="arm1_geom" type="cylinder" size="0.08 0.6" pos="0 0 0.3" material="blue"/>
-        <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.03"/>
-
-        <!-- 关节2：大臂俯仰（Z轴，2D平面摆动） -->
-        <body name="arm2" pos="0 0 0.6">
-          <geom name="arm2_geom" type="cylinder" size="0.06 0.5" pos="0 0 0.25" material="blue"/>
-          <joint name="joint2" type="hinge" axis="0 0 1" pos="0 0 0" range="-2.0 2.0" damping="0.03"/>
-
-          <!-- 简易夹爪（2D平面抓取） -->
-          <body name="gripper_base" pos="0 0 0.5">
-            <geom name="gripper_base_geom" type="box" size="0.08 0.08 0.08" pos="0 0 0" material="red"/>
-
-            <!-- 左夹爪 -->
-            <body name="left_grip" pos="0 0.08 0">
-              <geom name="left_grip_geom" type="box" size="0.06 0.04 0.06" pos="0 0 0" material="red"/>
-              <joint name="left_grip_joint" type="hinge" axis="0 0 1" pos="0 -0.08 0" range="-0.5 0" damping="0.02"/>
-            </body>
-
-            <!-- 右夹爪 -->
-            <body name="right_grip" pos="0 -0.08 0">
-              <geom name="right_grip_geom" type="box" size="0.06 0.04 0.06" pos="0 0 0" material="red"/>
-              <joint name="right_grip_joint" type="hinge" axis="0 0 1" pos="0 0.08 0" range="0 0.5" damping="0.02"/>
+      <!-- 关节1：基座旋转（Z轴，360°旋转） -->
+      <body name="arm_main" pos="0 0 0.1">
+        <geom name="arm_main_geom" type="cylinder" size="0.09 0.7" pos="0 0 0.35" material="yellow"/>
+        <joint name="joint1_rotate" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.04"/>
+
+        <!-- 关节2：大臂俯仰（Y轴，调整高度） -->
+        <body name="arm_secondary" pos="0 0 0.7">
+          <geom name="arm_secondary_geom" type="cylinder" size="0.07 0.6" pos="0 0 0.3" material="yellow"/>
+          <joint name="joint2_pitch" type="hinge" axis="0 1 0" pos="0 0 0" range="-1.5 1.5" damping="0.04"/>
+
+          <!-- 关节3：小臂伸缩（X轴，调整抓取距离） -->
+          <body name="arm_telescope" pos="0 0 0.6">
+            <geom name="arm_telescope_geom" type="cylinder" size="0.05 0.4" pos="0.2 0 0" material="yellow"/>
+            <joint name="joint3_telescope" type="slide" axis="1 0 0" pos="0 0 0" range="0 0.4" damping="0.04"/>
+
+            <!-- 分拣夹爪（简易平行夹爪） -->
+            <body name="gripper_core" pos="0.4 0 0">
+              <geom name="gripper_core_geom" type="box" size="0.08 0.08 0.08" pos="0 0 0" material="red"/>
+
+              <!-- 左夹爪 -->
+              <body name="gripper_left" pos="0 0.08 0">
+                <geom name="gripper_left_geom" type="box" size="0.06 0.05 0.06" pos="0 0 0" material="red"/>
+                <joint name="gripper_left_joint" type="hinge" axis="0 0 1" pos="0 -0.08 0" range="-0.4 0" damping="0.02"/>
+              </body>
+
+              <!-- 右夹爪 -->
+              <body name="gripper_right" pos="0 -0.08 0">
+                <geom name="gripper_right_geom" type="box" size="0.06 0.05 0.06" pos="0 0 0" material="red"/>
+                <joint name="gripper_right_joint" type="hinge" axis="0 0 1" pos="0 0.08 0" range="0 0.4" damping="0.02"/>
+              </body>
             </body>
           </body>
         </body>
@@ -68,62 +82,64 @@ def simple_2d_robot_arm_demo():
     </body>
   </worldbody>
 
-  <!-- 执行器配置（MuJoCo 3.4.0原生支持） -->
+  <!-- 执行器配置（MuJoCo 3.4.0 100%兼容） -->
   <actuator>
-    <!-- 关节位置控制 -->
-    <position name="joint1_act" joint="joint1" kp="1000" kv="100"/>
-    <position name="joint2_act" joint="joint2" kp="1000" kv="100"/>
-
-    <!-- 夹爪速度控制（安全低速） -->
-    <velocity name="left_grip_act" joint="left_grip_joint" kv="40" ctrlrange="-0.4 0"/>
-    <velocity name="right_grip_act" joint="right_grip_joint" kv="40" ctrlrange="0 0.4"/>
+    <!-- 3个关节的位置控制（高精度定位） -->
+    <position name="joint1_act" joint="joint1_rotate" kp="1200" kv="110"/>
+    <position name="joint2_act" joint="joint2_pitch" kp="1200" kv="110"/>
+    <position name="joint3_act" joint="joint3_telescope" kp="1200" kv="110"/>
+
+    <!-- 夹爪的速度控制（软接触，防止损坏目标） -->
+    <velocity name="gripper_left_act" joint="gripper_left_joint" kv="45" ctrlrange="-0.35 0"/>
+    <velocity name="gripper_right_act" joint="gripper_right_joint" kv="45" ctrlrange="0 0.35"/>
   </actuator>
 </mujoco>
     """
 
-    # 加载模型（确保100%兼容MuJoCo 3.4.0）
+    # 加载模型（确保无XML错误，适配MuJoCo 3.4.0）
     try:
-        model = mujoco.MjModel.from_xml_string(robot_2d_xml)
+        model = mujoco.MjModel.from_xml_string(sorting_robot_xml)
         data = mujoco.MjData(model)
-        print("✅ 2D平面机械臂模型加载成功，启动仿真...")
+        print("✅ 桌面分拣机械臂模型加载成功，启动仿真...")
     except Exception as e:
         print(f"❌ 模型加载失败：{e}")
         return
 
-    # 获取执行器索引
+    # 获取执行器索引（原生API，无兼容问题）
     joint_idxs = {
         "joint1": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint1_act"),
-        "joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act")
+        "joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act"),
+        "joint3": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint3_act")
     }
-    left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "left_grip_act")
-    right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "right_grip_act")
+    left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "gripper_left_act")
+    right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "gripper_right_act")
 
-    # 核心控制函数
-    def smooth_joint_move(joint_name, target_angle, duration, viewer):
-        """平滑移动关节到目标角度"""
+    # 核心控制函数（分段式动作，精准可控）
+    def precise_joint_control(joint_name, target_val, duration, viewer):
+        """精准控制关节移动/伸缩到目标值"""
         idx = joint_idxs[joint_name]
-        start_angle = data.ctrl[idx]
+        start_val = data.ctrl[idx]
         start_time = time.time()
 
         while (time.time() - start_time) < duration and viewer.is_running():
             progress = (time.time() - start_time) / duration
-            current_angle = start_angle + progress * (target_angle - start_angle)
-            data.ctrl[idx] = current_angle
+            current_val = start_val + progress * (target_val - start_val)
+            data.ctrl[idx] = current_val
 
-            # 打印实时状态
-            print(f"\r{joint_name} 当前角度：{current_angle:.2f} rad | 目标角度：{target_angle:.2f} rad", end="")
+            # 实时打印动作状态
+            print(f"\r{joint_name} 当前值：{current_val:.2f} | 目标值：{target_val:.2f}", end="")
 
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
         print()  # 换行
 
-    def safe_gripper_close(viewer):
-        """安全闭合夹爪（低速+定时，模拟力控）"""
-        print("\n🔧 开始闭合夹爪（安全低速）")
+    def soft_gripper_close(viewer, target_name):
+        """软接触闭合夹爪（低速+定时，保护目标物体）"""
+        print(f"\n🔧 开始闭合夹爪，抓取{target_name}")
         grip_speed = -0.3
+        close_duration = 1.0  # 短时间闭合，避免过夹
         start_time = time.time()
-        close_duration = 1.2  # 闭合1.2秒后停止，防止夹碎
 
         while (time.time() - start_time) < close_duration and viewer.is_running():
             progress = (time.time() - start_time) / close_duration
@@ -136,17 +152,18 @@ def safe_gripper_close(viewer):
             viewer.sync()
             time.sleep(0.001)
 
-        # 停止夹爪运动
+        # 锁定夹爪，保持抓取状态
         data.ctrl[left_grip_idx] = 0
         data.ctrl[right_grip_idx] = 0
-        print("\n✅ 夹爪闭合完成，已锁定目标")
+        print(f"\n✅ {target_name} 抓取完成，夹爪已锁定")
 
-    def gripper_open(duration, viewer):
-        """张开夹爪"""
-        print("\n🔧 开始张开夹爪")
+    def gripper_open_full(viewer, target_name):
+        """完全张开夹爪，放置目标物体"""
+        print(f"\n🔧 开始张开夹爪，放置{target_name}")
+        open_duration = 0.8
         start_time = time.time()
 
-        while (time.time() - start_time) < duration and viewer.is_running():
+        while (time.time() - start_time) < open_duration and viewer.is_running():
             data.ctrl[left_grip_idx] = 0.3
             data.ctrl[right_grip_idx] = -0.3
 
@@ -154,51 +171,71 @@ def gripper_open(duration, viewer):
             viewer.sync()
             time.sleep(0.001)
 
+        # 复位夹爪控制
         data.ctrl[left_grip_idx] = 0
         data.ctrl[right_grip_idx] = 0
-        print("✅ 夹爪已完全张开，目标放置完成")
+        print(f"✅ {target_name} 放置完成，夹爪已复位")
 
-    # 2D机械臂抓取流程
-    with mujoco.viewer.launch_passive(model, data) as viewer:
-        print("\n📌 开始2D平面机械臂抓取流程...")
-        print("-" * 60)
+    def sort_single_target(viewer, target_name, joint1_angle, joint3_extend):
+        """分拣单个目标物体的完整流程"""
+        # 步骤1：旋转基座对准目标
+        print(f"\n\n===== 开始分拣 {target_name} =====")
+        print("\n🔧 步骤1：旋转基座对准目标")
+        precise_joint_control("joint1", joint1_angle, 2.5, viewer)
+
+        # 步骤2：俯仰大臂降低高度
+        print("\n🔧 步骤2：降低机械臂高度，接近目标")
+        precise_joint_control("joint2", -0.6, 2.0, viewer)
+
+        # 步骤3：伸缩小臂靠近目标
+        print("\n🔧 步骤3：伸缩小臂，对准目标中心")
+        precise_joint_control("joint3", joint3_extend, 2.0, viewer)
 
-        # 步骤1：关节1旋转对准目标
-        print("\n\n🔧 步骤1：基座旋转对准目标")
-        smooth_joint_move("joint1", 0.0, 2.5, viewer)
+        # 步骤4：软接触闭合夹爪，抓取目标
+        soft_gripper_close(viewer, target_name)
 
-        # 步骤2：关节2俯仰接近目标
-        print("\n\n🔧 步骤2：大臂俯仰接近目标")
-        smooth_joint_move("joint2", -0.785, 2.5, viewer)  # -45°俯仰
+        # 步骤5：抬升机械臂，脱离桌面
+        print("\n🔧 步骤5：抬升目标，脱离桌面")
+        precise_joint_control("joint2", 0.0, 1.8, viewer)
 
-        # 步骤3：安全闭合夹爪抓取目标
-        safe_gripper_close(viewer)
+        # 步骤6：旋转基座，对准分拣区域
+        print("\n🔧 步骤6：旋转机械臂，对准绿色分拣区域")
+        precise_joint_control("joint1", 3.14, 3.0, viewer)
 
-        # 步骤4：抬升目标（关节2回正）
-        print("\n\n🔧 步骤4：抬升抓取目标")
-        smooth_joint_move("joint2", 0.0, 2.0, viewer)
+        # 步骤7：降低高度，准备放置目标
+        print("\n🔧 步骤7：降低目标高度，接近分拣区域")
+        precise_joint_control("joint2", -0.6, 1.8, viewer)
 
-        # 步骤5：基座旋转归位
-        print("\n\n🔧 步骤5：机械臂旋转归位")
-        smooth_joint_move("joint1", 1.57, 3.0, viewer)  # 90°旋转归位
+        # 步骤8：张开夹爪，放置目标
+        gripper_open_full(viewer, target_name)
+
+        # 步骤9：复位机械臂，准备下一个目标
+        print("\n🔧 步骤9：复位机械臂，准备分拣下一个目标")
+        precise_joint_control("joint2", 0.0, 1.5, viewer)
+        precise_joint_control("joint3", 0.0, 1.5, viewer)
+        precise_joint_control("joint1", 0.0, 2.0, viewer)
+
+    # 启动完整分拣流程
+    with mujoco.viewer.launch_passive(model, data) as viewer:
+        print("\n📌 开始桌面机械臂双目标分拣流程...")
+        print("-" * 60)
 
-        # 步骤6：下放目标（关节2再次俯仰）
-        print("\n\n🔧 步骤6：下放抓取目标")
-        smooth_joint_move("joint2", -0.785, 2.0, viewer)
+        # 第一阶段：分拣红色方块
+        sort_single_target(viewer, "红色方块", 0.0, 0.35)
 
-        # 步骤7：张开夹爪完成放置
-        gripper_open(1.5, viewer)
+        # 第二阶段：分拣蓝色方块（调整关节参数，对准蓝色目标）
+        sort_single_target(viewer, "蓝色方块", -0.5, 0.35)
 
-        # 保持可视化5秒
-        print("\n\n📌 抓取流程全部完成，保持可视化5秒...")
+        # 保持可视化，查看分拣结果
+        print("\n\n📌 双目标分拣流程全部完成，保持可视化6秒...")
         start_hold = time.time()
-        while (time.time() - start_hold) < 5 and viewer.is_running():
+        while (time.time() - start_hold) < 6 and viewer.is_running():
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
 
-    print("\n\n🎉 2D平面机械臂抓取演示完毕！")
+    print("\n\n🎉 桌面分拣机械臂双目标分拣演示完毕！")
 
 
 if __name__ == "__main__":
-    simple_2d_robot_arm_demo()
\ No newline at end of file
+    desktop_sorting_robot_demo()
\ No newline at end of file

From 5f2a80ad1abd847773f2f184b49c649672b7faad Mon Sep 17 00:00:00 2001
From: gongjh0916 <2748130352@qq.com>
Date: Sun, 28 Dec 2025 12:45:27 +0800
Subject: [PATCH 13/13] =?UTF-8?q?=20=E5=B8=A6=E8=87=AA=E5=8A=A8=E5=A4=8D?=
 =?UTF-8?q?=E4=BD=8D=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Mechanical_arm_grasping/main.py | 272 ++++++++++++++--------------
 1 file changed, 139 insertions(+), 133 deletions(-)

diff --git a/src/Mechanical_arm_grasping/main.py b/src/Mechanical_arm_grasping/main.py
index bff3e37182..c311683975 100644
--- a/src/Mechanical_arm_grasping/main.py
+++ b/src/Mechanical_arm_grasping/main.py
@@ -1,79 +1,74 @@
-# MuJoCo 3.4.0 桌面分拣机械臂（无传感器，零XML错误，批量分拣演示）
+# MuJoCo 3.4.0 带自动复位的3自由度机械臂精准取放（无传感器，零XML错误）
 import mujoco
 import mujoco.viewer
 import time
 import numpy as np
 
 
-def desktop_sorting_robot_demo():
-    # 纯MuJoCo 3.4.0原生语法，仅保留支持的根标签
-    sorting_robot_xml = """
-<mujoco model="3-DOF Desktop Sorting Robot">
+def robot_arm_auto_reset_demo():
+    # 纯MuJoCo 3.4.0原生语法，无任何高版本扩展标签
+    robot_xml = """
+<mujoco model="3-DOF Robot Arm with Auto Reset">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <visual>
-    <global azimuth="45" elevation="-30"/>  <!-- 直观3D视角 -->
+    <global azimuth="30" elevation="-25"/>  <!-- 清晰3D视角 -->
   </visual>
   <asset>
     <material name="red" rgba="0.8 0.2 0.2 1"/>
-    <material name="blue" rgba="0.2 0.4 0.8 1"/>
+    <material name="yellow" rgba="0.8 0.7 0.2 1"/>
     <material name="gray" rgba="0.5 0.5 0.5 1"/>
+    <material name="blue" rgba="0.2 0.4 0.8 1"/>
     <material name="green" rgba="0.2 0.8 0.2 1"/>
-    <material name="yellow" rgba="0.8 0.8 0.2 1"/>
   </asset>
 
-  <!-- 世界体定义（桌面分拣场景） -->
+  <!-- 世界体定义 -->
   <worldbody>
-    <!-- 固定视角相机 -->
-    <camera name="sorting_camera" pos="2 2 1.5" xyaxes="1 0 0 0 1 0"/>
-    <!-- 桌面（灰色平面） -->
-    <geom name="desktop" type="plane" size="2 2 0.1" pos="0 0 -0.05" material="gray"/>
-    <!-- 分拣目标1：红色方块（待搬运） -->
-    <body name="target_red" pos="1.0 0.5 0.0">
-      <geom name="target_red_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="red"/>
-      <joint name="target_red_joint" type="free"/>
+    <!-- 固定监控相机 -->
+    <camera name="monitor_camera" pos="1.8 1.8 1.2" xyaxes="1 0 0 0 1 0"/>
+    <!-- 工作平台 -->
+    <geom name="workbench" type="plane" size="2 2 0.1" pos="0 0 -0.05" material="gray"/>
+    <!-- 待抓取目标：蓝色球体（易抓取，不易滚落） -->
+    <body name="target_ball" pos="0.9 0.6 0.0">
+      <geom name="target_geom" type="sphere" size="0.08" pos="0 0 0" material="blue"/>
+      <joint name="target_joint" type="free"/>
     </body>
-    <!-- 分拣目标2：蓝色方块（待搬运） -->
-    <body name="target_blue" pos="1.0 -0.5 0.0">
-      <geom name="target_blue_geom" type="box" size="0.1 0.1 0.1" pos="0 0 0" material="blue"/>
-      <joint name="target_blue_joint" type="free"/>
-    </body>
-    <!-- 分拣区域：绿色标记框（目标放置区域） -->
-    <geom name="sorting_area" type="box" size="0.3 0.3 0.01" pos="-1.0 0 0.0" material="green"/>
-    <!-- 3自由度桌面机械臂 -->
+    <!-- 目标放置区域：绿色圆形标记 -->
+    <geom name="place_area" type="cylinder" size="0.15 0.01" pos="-0.9 0.6 0.0" material="green"/>
+    <!-- 3自由度机械臂 -->
     <body name="robot_base" pos="0 0 0.0">
-      <geom name="base_geom" type="cylinder" size="0.2 0.1" pos="0 0 0" material="yellow"/>
+      <geom name="base_geom" type="cylinder" size="0.18 0.1" pos="0 0 0" material="yellow"/>
       <joint name="base_joint" type="free"/>
 
-      <!-- 关节1：基座旋转（Z轴，360°旋转） -->
-      <body name="arm_main" pos="0 0 0.1">
-        <geom name="arm_main_geom" type="cylinder" size="0.09 0.7" pos="0 0 0.35" material="yellow"/>
-        <joint name="joint1_rotate" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.04"/>
+      <!-- 关节1：基座旋转（Z轴） -->
+      <body name="arm_1" pos="0 0 0.1">
+        <geom name="arm1_geom" type="cylinder" size="0.08 0.6" pos="0 0 0.3" material="yellow"/>
+        <joint name="joint1_rotate" type="hinge" axis="0 0 1" pos="0 0 0" range="-3.14 3.14" damping="0.03"/>
 
-        <!-- 关节2：大臂俯仰（Y轴，调整高度） -->
-        <body name="arm_secondary" pos="0 0 0.7">
-          <geom name="arm_secondary_geom" type="cylinder" size="0.07 0.6" pos="0 0 0.3" material="yellow"/>
-          <joint name="joint2_pitch" type="hinge" axis="0 1 0" pos="0 0 0" range="-1.5 1.5" damping="0.04"/>
+        <!-- 关节2：大臂俯仰（Y轴） -->
+        <body name="arm_2" pos="0 0 0.6">
+          <geom name="arm2_geom" type="cylinder" size="0.07 0.5" pos="0 0 0.25" material="yellow"/>
+          <joint name="joint2_pitch" type="hinge" axis="0 1 0" pos="0 0 0" range="-1.5 1.5" damping="0.03"/>
 
-          <!-- 关节3：小臂伸缩（X轴，调整抓取距离） -->
-          <body name="arm_telescope" pos="0 0 0.6">
-            <geom name="arm_telescope_geom" type="cylinder" size="0.05 0.4" pos="0.2 0 0" material="yellow"/>
-            <joint name="joint3_telescope" type="slide" axis="1 0 0" pos="0 0 0" range="0 0.4" damping="0.04"/>
+          <!-- 关节3：小臂伸缩（X轴） -->
+          <body name="arm_3" pos="0 0 0.5">
+            <geom name="arm3_geom" type="cylinder" size="0.06 0.4" pos="0.2 0 0" material="yellow"/>
+            <joint name="joint3_telescope" type="slide" axis="1 0 0" pos="0 0 0" range="0 0.4" damping="0.03"/>
 
-            <!-- 分拣夹爪（简易平行夹爪） -->
-            <body name="gripper_core" pos="0.4 0 0">
-              <geom name="gripper_core_geom" type="box" size="0.08 0.08 0.08" pos="0 0 0" material="red"/>
+            <!-- 平行夹爪 -->
+            <body name="gripper_base" pos="0.4 0 0">
+              <geom name="gripper_base_geom" type="box" size="0.07 0.07 0.07" pos="0 0 0" material="red"/>
 
               <!-- 左夹爪 -->
-              <body name="gripper_left" pos="0 0.08 0">
-                <geom name="gripper_left_geom" type="box" size="0.06 0.05 0.06" pos="0 0 0" material="red"/>
-                <joint name="gripper_left_joint" type="hinge" axis="0 0 1" pos="0 -0.08 0" range="-0.4 0" damping="0.02"/>
+              <body name="gripper_left" pos="0 0.07 0">
+                <geom name="gripper_left_geom" type="box" size="0.05 0.04 0.05" pos="0 0 0" material="red"/>
+                <joint name="gripper_left_joint" type="hinge" axis="0 0 1" pos="0 -0.07 0" range="-0.4 0" damping="0.02"/>
               </body>
 
               <!-- 右夹爪 -->
-              <body name="gripper_right" pos="0 -0.08 0">
-                <geom name="gripper_right_geom" type="box" size="0.06 0.05 0.06" pos="0 0 0" material="red"/>
-                <joint name="gripper_right_joint" type="hinge" axis="0 0 1" pos="0 0.08 0" range="0 0.4" damping="0.02"/>
+              <body name="gripper_right" pos="0 -0.07 0">
+                <geom name="gripper_right_geom" type="box" size="0.05 0.04 0.05" pos="0 0 0" material="red"/>
+                <joint name="gripper_right_joint" type="hinge" axis="0 0 1" pos="0 0.07 0" range="0 0.4" damping="0.02"/>
               </body>
             </body>
           </body>
@@ -82,30 +77,30 @@ def desktop_sorting_robot_demo():
     </body>
   </worldbody>
 
-  <!-- 执行器配置（MuJoCo 3.4.0 100%兼容） -->
+  <!-- 执行器配置（MuJoCo 3.4.0 完全兼容） -->
   <actuator>
-    <!-- 3个关节的位置控制（高精度定位） -->
-    <position name="joint1_act" joint="joint1_rotate" kp="1200" kv="110"/>
-    <position name="joint2_act" joint="joint2_pitch" kp="1200" kv="110"/>
-    <position name="joint3_act" joint="joint3_telescope" kp="1200" kv="110"/>
-
-    <!-- 夹爪的速度控制（软接触，防止损坏目标） -->
-    <velocity name="gripper_left_act" joint="gripper_left_joint" kv="45" ctrlrange="-0.35 0"/>
-    <velocity name="gripper_right_act" joint="gripper_right_joint" kv="45" ctrlrange="0 0.35"/>
+    <!-- 关节位置控制（高精度） -->
+    <position name="joint1_act" joint="joint1_rotate" kp="1100" kv="100"/>
+    <position name="joint2_act" joint="joint2_pitch" kp="1100" kv="100"/>
+    <position name="joint3_act" joint="joint3_telescope" kp="1100" kv="100"/>
+
+    <!-- 夹爪速度控制（软接触，防损坏） -->
+    <velocity name="gripper_left_act" joint="gripper_left_joint" kv="40" ctrlrange="-0.3 0"/>
+    <velocity name="gripper_right_act" joint="gripper_right_joint" kv="40" ctrlrange="0 0.3"/>
   </actuator>
 </mujoco>
     """
 
-    # 加载模型（确保无XML错误，适配MuJoCo 3.4.0）
+    # 加载模型（确保零XML错误）
     try:
-        model = mujoco.MjModel.from_xml_string(sorting_robot_xml)
+        model = mujoco.MjModel.from_xml_string(robot_xml)
         data = mujoco.MjData(model)
-        print("✅ 桌面分拣机械臂模型加载成功，启动仿真...")
+        print("✅ 3自由度机械臂模型加载成功，启动仿真...")
     except Exception as e:
         print(f"❌ 模型加载失败：{e}")
         return
 
-    # 获取执行器索引（原生API，无兼容问题）
+    # 获取执行器索引
     joint_idxs = {
         "joint1": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint1_act"),
         "joint2": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "joint2_act"),
@@ -114,9 +109,10 @@ def desktop_sorting_robot_demo():
     left_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "gripper_left_act")
     right_grip_idx = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "gripper_right_act")
 
-    # 核心控制函数（分段式动作，精准可控）
-    def precise_joint_control(joint_name, target_val, duration, viewer):
-        """精准控制关节移动/伸缩到目标值"""
+    # ---------------------- 模块化功能函数 ----------------------
+    def joint_move(joint_name, target_val, duration, viewer, step_desc):
+        """单关节精准移动"""
+        print(f"\n🔧 {step_desc}")
         idx = joint_idxs[joint_name]
         start_val = data.ctrl[idx]
         start_time = time.time()
@@ -126,19 +122,18 @@ def precise_joint_control(joint_name, target_val, duration, viewer):
             current_val = start_val + progress * (target_val - start_val)
             data.ctrl[idx] = current_val
 
-            # 实时打印动作状态
-            print(f"\r{joint_name} 当前值：{current_val:.2f} | 目标值：{target_val:.2f}", end="")
-
+            print(f"\r{joint_name} 进度：{progress * 100:.1f}% | 当前值：{current_val:.2f}", end="")
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
         print()  # 换行
+        return True
 
-    def soft_gripper_close(viewer, target_name):
-        """软接触闭合夹爪（低速+定时，保护目标物体）"""
-        print(f"\n🔧 开始闭合夹爪，抓取{target_name}")
-        grip_speed = -0.3
-        close_duration = 1.0  # 短时间闭合，避免过夹
+    def gripper_close(viewer, desc="目标"):
+        """软接触闭合夹爪"""
+        print(f"\n🔧 闭合夹爪抓取{desc}")
+        grip_speed = -0.25
+        close_duration = 1.0
         start_time = time.time()
 
         while (time.time() - start_time) < close_duration and viewer.is_running():
@@ -147,95 +142,106 @@ def soft_gripper_close(viewer, target_name):
             data.ctrl[right_grip_idx] = -grip_speed
 
             print(f"\r夹爪闭合进度：{progress * 100:.1f}%", end="")
-
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
 
-        # 锁定夹爪，保持抓取状态
         data.ctrl[left_grip_idx] = 0
         data.ctrl[right_grip_idx] = 0
-        print(f"\n✅ {target_name} 抓取完成，夹爪已锁定")
+        print(f"\n✅ {desc} 抓取完成，夹爪锁定")
+        return True
 
-    def gripper_open_full(viewer, target_name):
-        """完全张开夹爪，放置目标物体"""
-        print(f"\n🔧 开始张开夹爪，放置{target_name}")
+    def gripper_open(viewer, desc="目标"):
+        """张开夹爪放置目标"""
+        print(f"\n🔧 张开夹爪放置{desc}")
         open_duration = 0.8
         start_time = time.time()
 
         while (time.time() - start_time) < open_duration and viewer.is_running():
-            data.ctrl[left_grip_idx] = 0.3
-            data.ctrl[right_grip_idx] = -0.3
-
+            data.ctrl[left_grip_idx] = 0.25
+            data.ctrl[right_grip_idx] = -0.25
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
 
-        # 复位夹爪控制
         data.ctrl[left_grip_idx] = 0
         data.ctrl[right_grip_idx] = 0
-        print(f"✅ {target_name} 放置完成，夹爪已复位")
-
-    def sort_single_target(viewer, target_name, joint1_angle, joint3_extend):
-        """分拣单个目标物体的完整流程"""
-        # 步骤1：旋转基座对准目标
-        print(f"\n\n===== 开始分拣 {target_name} =====")
-        print("\n🔧 步骤1：旋转基座对准目标")
-        precise_joint_control("joint1", joint1_angle, 2.5, viewer)
-
-        # 步骤2：俯仰大臂降低高度
-        print("\n🔧 步骤2：降低机械臂高度，接近目标")
-        precise_joint_control("joint2", -0.6, 2.0, viewer)
-
-        # 步骤3：伸缩小臂靠近目标
-        print("\n🔧 步骤3：伸缩小臂，对准目标中心")
-        precise_joint_control("joint3", joint3_extend, 2.0, viewer)
-
-        # 步骤4：软接触闭合夹爪，抓取目标
-        soft_gripper_close(viewer, target_name)
-
-        # 步骤5：抬升机械臂，脱离桌面
-        print("\n🔧 步骤5：抬升目标，脱离桌面")
-        precise_joint_control("joint2", 0.0, 1.8, viewer)
-
-        # 步骤6：旋转基座，对准分拣区域
-        print("\n🔧 步骤6：旋转机械臂，对准绿色分拣区域")
-        precise_joint_control("joint1", 3.14, 3.0, viewer)
-
-        # 步骤7：降低高度，准备放置目标
-        print("\n🔧 步骤7：降低目标高度，接近分拣区域")
-        precise_joint_control("joint2", -0.6, 1.8, viewer)
-
-        # 步骤8：张开夹爪，放置目标
-        gripper_open_full(viewer, target_name)
-
-        # 步骤9：复位机械臂，准备下一个目标
-        print("\n🔧 步骤9：复位机械臂，准备分拣下一个目标")
-        precise_joint_control("joint2", 0.0, 1.5, viewer)
-        precise_joint_control("joint3", 0.0, 1.5, viewer)
-        precise_joint_control("joint1", 0.0, 2.0, viewer)
-
-    # 启动完整分拣流程
+        print(f"✅ {desc} 放置完成，夹爪复位")
+        return True
+
+    def robot_auto_reset(viewer):
+        """机械臂自动复位到初始位置"""
+        print("\n\n🔧 开始机械臂自动复位")
+        # 步骤1：抬升大臂
+        joint_move("joint2", 0.0, 1.5, viewer, "复位：抬升大臂")
+        # 步骤2：收缩小臂
+        joint_move("joint3", 0.0, 1.5, viewer, "复位：收缩小臂")
+        # 步骤3：旋转基座回正
+        joint_move("joint1", 0.0, 2.0, viewer, "复位：基座回正")
+        print("✅ 机械臂已完成自动复位，准备下一次抓取")
+        return True
+
+    def grab_and_place(viewer, retry_max=2):
+        """完整取放流程（含自动重试）"""
+        retry_count = 0
+        success = False
+
+        while retry_count < retry_max and not success:
+            print(f"\n\n===== 开始第 {retry_count + 1} 次抓取尝试 =====")
+            try:
+                # 阶段1：对准目标
+                joint_move("joint1", 0.0, 2.0, viewer, "步骤1：旋转基座对准蓝色目标")
+                joint_move("joint2", -0.7, 2.0, viewer, "步骤2：俯仰大臂接近目标")
+                joint_move("joint3", 0.35, 2.0, viewer, "步骤3：伸缩小臂对准目标")
+
+                # 阶段2：抓取目标
+                gripper_close(viewer, "蓝色球体")
+
+                # 阶段3：抬升并转移目标
+                joint_move("joint2", 0.0, 1.5, viewer, "步骤4：抬升目标脱离平台")
+                joint_move("joint1", 3.14, 2.5, viewer, "步骤5：旋转基座对准绿色放置区域")
+                joint_move("joint2", -0.7, 1.5, viewer, "步骤6：降低目标接近放置区域")
+
+                # 阶段4：放置目标
+                gripper_open(viewer, "蓝色球体")
+
+                # 抓取成功，退出重试循环
+                success = True
+                print("\n\n🎉 第 {retry_count+1} 次抓取尝试成功！")
+            except Exception as e:
+                retry_count += 1
+                print(f"\n❌ 第 {retry_count} 次抓取失败：{e}，准备重试...")
+                robot_auto_reset(viewer)
+
+        if not success:
+            print(f"\n❌ 已达到最大重试次数（{retry_max}次），抓取失败")
+        return success
+
+    # ---------------------- 启动主流程 ----------------------
     with mujoco.viewer.launch_passive(model, data) as viewer:
-        print("\n📌 开始桌面机械臂双目标分拣流程...")
+        print("\n📌 开始带自动复位的机械臂精准取放流程...")
         print("-" * 60)
 
-        # 第一阶段：分拣红色方块
-        sort_single_target(viewer, "红色方块", 0.0, 0.35)
+        # 执行完整取放流程
+        grab_success = grab_and_place(viewer)
 
-        # 第二阶段：分拣蓝色方块（调整关节参数，对准蓝色目标）
-        sort_single_target(viewer, "蓝色方块", -0.5, 0.35)
+        # 无论成功与否，最终执行自动复位
+        if grab_success:
+            robot_auto_reset(viewer)
+        else:
+            print("\n🔧 强制执行机械臂自动复位")
+            robot_auto_reset(viewer)
 
-        # 保持可视化，查看分拣结果
-        print("\n\n📌 双目标分拣流程全部完成，保持可视化6秒...")
+        # 保持可视化查看结果
+        print("\n\n📌 流程结束，保持可视化5秒...")
         start_hold = time.time()
-        while (time.time() - start_hold) < 6 and viewer.is_running():
+        while (time.time() - start_hold) < 5 and viewer.is_running():
             mujoco.mj_step(model, data)
             viewer.sync()
             time.sleep(0.001)
 
-    print("\n\n🎉 桌面分拣机械臂双目标分拣演示完毕！")
+    print("\n\n🎉 3自由度机械臂自动复位取放演示完毕！")
 
 
 if __name__ == "__main__":
-    desktop_sorting_robot_demo()
\ No newline at end of file
+    robot_arm_auto_reset_demo()
\ No newline at end of file