From 67cf20b1c4a499b56776c29241d1deddc084dae0 Mon Sep 17 00:00:00 2001
From: FallenSigh <fallensigh@gmail.com>
Date: Tue, 16 Jun 2026 22:12:55 +0800
Subject: [PATCH] =?UTF-8?q?feat(arm=5Fcontrol):=20=E6=B7=BB=E5=8A=A0?=
 =?UTF-8?q?=E5=90=AF=E5=8A=A8=E8=87=AA=E5=8A=A8=E5=A4=8D=E4=BD=8D=E5=8A=9F?=
 =?UTF-8?q?=E8=83=BD=E5=B9=B6=E4=BF=AE=E5=A4=8D=20UDP=20=E5=91=BD=E4=BB=A4?=
 =?UTF-8?q?=E8=B0=83=E7=94=A8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-openagent)

Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>
---
 ros2/src/udp_teleop/config/arm_control.yaml   |  8 ++
 ros2/src/udp_teleop/udp_teleop/arm_control.py | 73 +++++++++++++++++++
 2 files changed, 81 insertions(+)

diff --git a/ros2/src/udp_teleop/config/arm_control.yaml b/ros2/src/udp_teleop/config/arm_control.yaml
index fe3de57..2288a39 100644
--- a/ros2/src/udp_teleop/config/arm_control.yaml
+++ b/ros2/src/udp_teleop/config/arm_control.yaml
@@ -34,5 +34,13 @@ arm_control:
     default_rate: 100.0      # 插值频率 (Hz)
     use_state_cache: true   # 是否使用状态缓存
 
+    # 启动复位
+    auto_reset_on_startup: true  # 启动时是否自动复位
+    reset_x: 200.0               # 复位位置 X (mm)
+    reset_y: 0.0                 # 复位位置 Y (mm)
+    reset_z: -120.0                # 复位位置 Z (mm)
+    reset_phi: 0.0               # 复位朝向 (度)
+    reset_duration: 3.0          # 复位运动时长 (秒)
+
     # 状态发布频率
     publish_rate: 10.0      # Hz
diff --git a/ros2/src/udp_teleop/udp_teleop/arm_control.py b/ros2/src/udp_teleop/udp_teleop/arm_control.py
index 24defdc..96279da 100644
--- a/ros2/src/udp_teleop/udp_teleop/arm_control.py
+++ b/ros2/src/udp_teleop/udp_teleop/arm_control.py
@@ -393,6 +393,12 @@ class ArmControlNode(Node):
                 ('default_rate', DEFAULT_INTERP_RATE),
                 ('use_state_cache', True),
                 ('publish_rate', 10.0),
+                ('auto_reset_on_startup', True),
+                ('reset_x', 200.0),
+                ('reset_y', 0.0),
+                ('reset_z', 10.0),
+                ('reset_phi', 0.0),
+                ('reset_duration', 3.0),
             ]
         )
 
@@ -401,6 +407,14 @@ class ArmControlNode(Node):
         self.udp_port = self.get_parameter('udp_port').value
         self.publish_rate = self.get_parameter('publish_rate').value
 
+        # 复位参数
+        self.auto_reset_on_startup = self.get_parameter('auto_reset_on_startup').value
+        self.reset_x = self.get_parameter('reset_x').value
+        self.reset_y = self.get_parameter('reset_y').value
+        self.reset_z = self.get_parameter('reset_z').value
+        self.reset_phi = self.get_parameter('reset_phi').value
+        self.reset_duration = self.get_parameter('reset_duration').value
+
         # 机械臂几何参数
         self.geometry = ArmGeometry(
             l1=self.get_parameter('l1').value,
@@ -485,6 +499,65 @@ class ArmControlNode(Node):
         self.get_logger().info(f'机械臂控制节点已启动')
         self.get_logger().info(f'UDP 目标: {self.udp_ip}:{self.udp_port}')
 
+        # 自动复位
+        if self.auto_reset_on_startup:
+            self.get_logger().info('执行启动复位...')
+            import threading
+            threading.Thread(target=self._auto_reset, daemon=True).start()
+
+    def _auto_reset(self):
+        """自动复位线程"""
+        import time
+        time.sleep(1.0)  # 等待节点完全启动
+
+        try:
+            target_pose = ArmPose(
+                x=self.reset_x,
+                y=self.reset_y,
+                z=self.reset_z,
+                phi_deg=self.reset_phi
+            )
+
+            # 根据目标 z 坐标自动选择 J5 和 z4
+            j5 = resolve_j5_from_z(target_pose.z)
+            j6 = DEFAULT_FIXED_J6
+            z4 = resolve_z4_from_j5(j5)
+
+            # 逆运动学
+            math_state = inverse_kinematics(
+                geometry=self.geometry,
+                pose=target_pose,
+                limits=self.limits,
+                elbow_up=False,
+                j5=j5,
+                j6=j6,
+                z4=z4,
+            )
+
+            # 转换为命令状态
+            target_state = math_to_command_state(
+                math_state, self.zero_offsets, self.limits, j5, j6
+            )
+
+            # 生成插值轨迹
+            steps = max(1, int(self.reset_duration * self.default_rate))
+            trajectory = interpolate_command_states(
+                self.current_state, target_state, steps
+            )
+
+            # 执行运动
+            self.get_logger().info(f'复位到: ({self.reset_x:.1f}, {self.reset_y:.1f}, {self.reset_z:.1f}), phi={self.reset_phi:.1f}°')
+            for state in trajectory:
+                self.send_udp_commands([state], 0.0)
+                self.current_state = state
+                self.save_state()
+                time.sleep(1.0 / self.default_rate)
+
+            self.get_logger().info('✓ 复位完成')
+
+        except Exception as e:
+            self.get_logger().error(f'复位失败: {e}')
+
     def load_state(self):
         """从缓存加载状态"""
         if not self.use_state_cache or not STATE_FILE.exists():