#!/usr/bin/env python3
"""机械臂控制 ROS 节点（独立版本）

完全独立，不依赖 udp_control.py
包含所有必要的运动学和控制代码
"""

import json
import math
import socket
import time
from dataclasses import dataclass
from pathlib import Path
from typing import Optional, List

import rclpy
from rclpy.node import Node

# 导入自定义消息
from arm_control_msgs.srv import (
    MoveJoints,
    MovePose,
    GetPose,
    SetGripper
)
from arm_control_msgs.msg import (
    JointState,
    TCPPose
)

# ============================================================================
# 常量定义
# ============================================================================

DEFAULT_UDP_IP = "192.168.4.1"
DEFAULT_UDP_PORT = 8888

DEFAULT_HEIGHT_MIN = -290
DEFAULT_HEIGHT_MAX = 0
DEFAULT_JOINT_MIN = -180
DEFAULT_JOINT_MAX = 180
DEFAULT_J2_MIN = -110
DEFAULT_J2_MAX = 115
DEFAULT_J3_MIN = -120
DEFAULT_J3_MAX = 145
DEFAULT_J4_MIN = -90
DEFAULT_J4_MAX = 130

J5_OPEN = 81
J5_CLOSED = -100
DEFAULT_FIXED_J5 = J5_OPEN

# Z4 值根据夹爪状态变化
# J5 = -100 (闭合) → 夹爪朝上 (UP) → z4 = -100
# J5 = 81 (张开) → 夹爪朝下 (DOWN) → z4 = 55
Z4_UP = -100      # 夹爪朝上（J5=-100，闭合）
Z4_DOWN = 55      # 夹爪朝下（J5=81，张开）

GRIP_ANGLE = -5
RELEASE_ANGLE = 80
DEFAULT_FIXED_J6 = RELEASE_ANGLE

DEFAULT_ZERO_J2 = 3
DEFAULT_ZERO_J3 = 7
DEFAULT_ZERO_J4 = 25

DEFAULT_L1 = 125.0
DEFAULT_L2 = 125.0
DEFAULT_X4 = 110.0
DEFAULT_Z4 = 80.0  # 仅用于配置默认值，实际使用动态 z4

DEFAULT_INTERP_DURATION = 1.0
DEFAULT_INTERP_RATE = 20.0

STATE_FILE = Path.home() / ".ros" / "udp_control_state.json"

# ============================================================================
# 数据类定义
# ============================================================================

class ArmControlError(ValueError):
    """机械臂控制错误"""
    pass


@dataclass(frozen=True)
class ArmGeometry:
    """机械臂几何参数"""
    l1: float
    l2: float
    x4: float
    z4: float


@dataclass(frozen=True)
class ArmLimits:
    """关节限位"""
    height_min: int = DEFAULT_HEIGHT_MIN
    height_max: int = DEFAULT_HEIGHT_MAX
    joint_min: int = DEFAULT_JOINT_MIN
    joint_max: int = DEFAULT_JOINT_MAX
    j2_min: int = DEFAULT_J2_MIN
    j2_max: int = DEFAULT_J2_MAX
    j3_min: int = DEFAULT_J3_MIN
    j3_max: int = DEFAULT_J3_MAX
    j4_min: int = DEFAULT_J4_MIN
    j4_max: int = DEFAULT_J4_MAX


@dataclass(frozen=True)
class ArmZeroOffsets:
    """零点偏移"""
    j2: int = DEFAULT_ZERO_J2
    j3: int = DEFAULT_ZERO_J3
    j4: int = DEFAULT_ZERO_J4


@dataclass(frozen=True)
class ArmJointState:
    """关节状态"""
    height: int
    j2: int
    j3: int
    j4: int
    j5: int = DEFAULT_FIXED_J5
    j6: int = DEFAULT_FIXED_J6

    def to_udp_message(self) -> bytes:
        """转换为 UDP 消息"""
        return (
            f"JXB:{self.height}:{self.j2}:{self.j3}:{self.j4}:"
            f"{self.j5}:{self.j6}:0:0:EZHY\n"
        ).encode("utf-8")


@dataclass(frozen=True)
class ArmPose:
    """TCP 位姿"""
    x: float
    y: float
    z: float
    phi_deg: float


@dataclass(frozen=True)
class ArmMathState:
    """数学坐标系的关节状态"""
    d1: float
    theta2_deg: float
    theta3_deg: float
    theta4_deg: float


# ============================================================================
# 运动学函数
# ============================================================================

def clamp_int(value: float, lower: int, upper: int, name: str) -> int:
    """限幅并转换为整数"""
    rounded = int(round(value))
    if rounded < lower or rounded > upper:
        raise ArmControlError(f"{name}={rounded} 超出范围 [{lower}, {upper}]")
    return rounded


def normalize_angle_deg(angle_deg: float) -> float:
    """角度归一化到 [-180, 180)"""
    normalized = (angle_deg + 180.0) % 360.0 - 180.0
    if normalized == -180.0 and angle_deg > 0:
        return 180.0
    return normalized


def resolve_z4_from_j5(j5: int) -> float:
    """根据 J5 状态确定 z4 值

    - J5 = -100 (闭合): 夹爪朝上，z4 = -100mm
    - J5 = 81 (张开): 夹爪朝下，z4 = 55mm
    """
    return Z4_UP if j5 < 0 else Z4_DOWN


def resolve_j5_from_z(z: float) -> int:
    """根据目标 z 坐标自动选择夹爪状态

    - z > -55: 使用朝上状态 (J5=-100, z4=-100)
    - z <= -55: 使用朝下状态 (J5=81, z4=55)
    """
    return J5_CLOSED if z > -55 else J5_OPEN


def forward_kinematics(geometry: ArmGeometry, state: ArmMathState, z4: float) -> ArmPose:
    """正运动学：关节角度 → TCP 位姿

    Args:
        geometry: 机械臂几何参数
        state: 数学坐标系的关节状态
        z4: J4 到 TCP 的 Z 偏移（根据 J5 状态确定）
    """
    theta2 = math.radians(state.theta2_deg)
    theta3 = math.radians(state.theta3_deg)
    theta4 = math.radians(state.theta4_deg)
    phi = theta2 + theta3 + theta4

    j4_center_x = (
        geometry.l1 * math.cos(theta2) +
        geometry.l2 * math.cos(theta2 + theta3)
    )
    j4_center_y = (
        geometry.l1 * math.sin(theta2) +
        geometry.l2 * math.sin(theta2 + theta3)
    )

    x = j4_center_x + geometry.x4 * math.cos(phi)
    y = j4_center_y + geometry.x4 * math.sin(phi)
    z = state.d1 - z4  # 使用动态 z4

    return ArmPose(x=x, y=y, z=z, phi_deg=math.degrees(phi))


def inverse_kinematics(
    geometry: ArmGeometry,
    pose: ArmPose,
    limits: ArmLimits,
    elbow_up: bool,
    j5: int,
    j6: int,
    z4: float,
) -> ArmMathState:
    """逆运动学：TCP 位姿 → 关节角度

    Args:
        geometry: 机械臂几何参数
        pose: 目标 TCP 位姿
        limits: 关节限位
        elbow_up: 肘部朝上/朝下
        j5: J5 角度
        j6: J6 角度
        z4: J4 到 TCP 的 Z 偏移（根据 J5 状态确定）
    """
    # 计算 J4 中心位置
    phi = math.radians(pose.phi_deg)
    j4_x = pose.x - geometry.x4 * math.cos(phi)
    j4_y = pose.y - geometry.x4 * math.sin(phi)
    j4_z = pose.z + z4  # 使用动态 z4

    d1 = j4_z
    
    # 计算平面距离
    r2 = j4_x * j4_x + j4_y * j4_y
    if r2 < 1e-9:
        raise ArmControlError("目标点过于接近奇异点")
    
    # 计算 theta3
    denom = 2.0 * geometry.l1 * geometry.l2
    if abs(denom) < 1e-9:
        raise ArmControlError("几何参数无效")
    
    c3 = (r2 - geometry.l1 * geometry.l1 - geometry.l2 * geometry.l2) / denom
    if c3 < -1.0 - 1e-9 or c3 > 1.0 + 1e-9:
        reach = math.sqrt(r2)
        raise ArmControlError(f"目标超出工作空间，距离={reach:.3f}mm")
    c3 = max(-1.0, min(1.0, c3))
    
    s3_abs = math.sqrt(max(0.0, 1.0 - c3 * c3))
    s3 = -s3_abs if elbow_up else s3_abs
    
    theta3 = math.atan2(s3, c3)
    theta2 = math.atan2(j4_y, j4_x) - math.atan2(
        geometry.l2 * s3,
        geometry.l1 + geometry.l2 * c3,
    )
    theta4 = phi - theta2 - theta3
    
    return ArmMathState(
        d1=d1,
        theta2_deg=normalize_angle_deg(math.degrees(theta2)),
        theta3_deg=normalize_angle_deg(math.degrees(theta3)),
        theta4_deg=normalize_angle_deg(math.degrees(theta4)),
    )


def command_to_math_state(
    command_state: ArmJointState,
    zero_offsets: ArmZeroOffsets,
) -> ArmMathState:
    """命令状态 → 数学状态"""
    return ArmMathState(
        d1=command_state.height,
        theta2_deg=command_state.j2 - zero_offsets.j2,
        theta3_deg=command_state.j3 - zero_offsets.j3,
        theta4_deg=command_state.j4 - zero_offsets.j4,
    )


def math_to_command_state(
    math_state: ArmMathState,
    zero_offsets: ArmZeroOffsets,
    limits: ArmLimits,
    j5: int,
    j6: int,
) -> ArmJointState:
    """数学状态 → 命令状态"""
    return ArmJointState(
        height=clamp_int(
            round(math_state.d1),
            limits.height_min,
            limits.height_max,
            "height",
        ),
        j2=clamp_int(
            math_state.theta2_deg + zero_offsets.j2,
            limits.j2_min,
            limits.j2_max,
            "J2",
        ),
        j3=clamp_int(
            math_state.theta3_deg + zero_offsets.j3,
            limits.j3_min,
            limits.j3_max,
            "J3",
        ),
        j4=clamp_int(
            math_state.theta4_deg + zero_offsets.j4,
            limits.j4_min,
            limits.j4_max,
            "J4",
        ),
        j5=clamp_int(j5, limits.joint_min, limits.joint_max, "J5"),
        j6=clamp_int(j6, limits.joint_min, limits.joint_max, "J6"),
    )


def interpolate_command_states(
    start: ArmJointState,
    end: ArmJointState,
    steps: int,
) -> List[ArmJointState]:
    """关节空间插值"""
    if steps <= 1:
        return [end]
    
    states = []
    for step_index in range(1, steps + 1):
        t = step_index / steps
        states.append(
            ArmJointState(
                height=int(round(start.height + (end.height - start.height) * t)),
                j2=int(round(start.j2 + (end.j2 - start.j2) * t)),
                j3=int(round(start.j3 + (end.j3 - start.j3) * t)),
                j4=int(round(start.j4 + (end.j4 - start.j4) * t)),
                j5=int(round(start.j5 + (end.j5 - start.j5) * t)),
                j6=int(round(start.j6 + (end.j6 - start.j6) * t)),
            )
        )
    return states


# ============================================================================
# ROS 节点
# ============================================================================

class ArmControlNode(Node):
    """机械臂控制 ROS 节点"""

    def __init__(self):
        super().__init__('arm_control')

        # 声明参数
        self.declare_parameters(
            namespace='',
            parameters=[
                ('udp_ip', DEFAULT_UDP_IP),
                ('udp_port', DEFAULT_UDP_PORT),
                ('l1', DEFAULT_L1),
                ('l2', DEFAULT_L2),
                ('x4', DEFAULT_X4),
                ('z4', DEFAULT_Z4),
                ('height_min', DEFAULT_HEIGHT_MIN),
                ('height_max', DEFAULT_HEIGHT_MAX),
                ('j2_min', DEFAULT_J2_MIN),
                ('j2_max', DEFAULT_J2_MAX),
                ('j3_min', DEFAULT_J3_MIN),
                ('j3_max', DEFAULT_J3_MAX),
                ('j4_min', DEFAULT_J4_MIN),
                ('j4_max', DEFAULT_J4_MAX),
                ('joint_min', DEFAULT_JOINT_MIN),
                ('joint_max', DEFAULT_JOINT_MAX),
                ('zero_j2', DEFAULT_ZERO_J2),
                ('zero_j3', DEFAULT_ZERO_J3),
                ('zero_j4', DEFAULT_ZERO_J4),
                ('default_duration', DEFAULT_INTERP_DURATION),
                ('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),
            ]
        )

        # 获取参数
        self.udp_ip = self.get_parameter('udp_ip').value
        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,
            l2=self.get_parameter('l2').value,
            x4=self.get_parameter('x4').value,
            z4=self.get_parameter('z4').value,
        )

        # 关节限位
        self.limits = ArmLimits(
            height_min=self.get_parameter('height_min').value,
            height_max=self.get_parameter('height_max').value,
            j2_min=self.get_parameter('j2_min').value,
            j2_max=self.get_parameter('j2_max').value,
            j3_min=self.get_parameter('j3_min').value,
            j3_max=self.get_parameter('j3_max').value,
            j4_min=self.get_parameter('j4_min').value,
            j4_max=self.get_parameter('j4_max').value,
            joint_min=self.get_parameter('joint_min').value,
            joint_max=self.get_parameter('joint_max').value,
        )

        # 零点偏移
        self.zero_offsets = ArmZeroOffsets(
            j2=self.get_parameter('zero_j2').value,
            j3=self.get_parameter('zero_j3').value,
            j4=self.get_parameter('zero_j4').value,
        )

        # 默认插值参数
        self.default_duration = self.get_parameter('default_duration').value
        self.default_rate = self.get_parameter('default_rate').value
        self.use_state_cache = self.get_parameter('use_state_cache').value

        # 当前状态
        self.current_state: Optional[ArmJointState] = None
        self.load_state()

        # UDP socket
        self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

        # 创建服务
        self.srv_move_joints = self.create_service(
            MoveJoints,
            'arm_control/move_joints',
            self.handle_move_joints
        )
        self.srv_move_pose = self.create_service(
            MovePose,
            'arm_control/move_pose',
            self.handle_move_pose
        )
        self.srv_get_pose = self.create_service(
            GetPose,
            'arm_control/get_pose',
            self.handle_get_pose
        )
        self.srv_set_gripper = self.create_service(
            SetGripper,
            'arm_control/set_gripper',
            self.handle_set_gripper
        )

        # 创建发布者
        self.pub_joint_states = self.create_publisher(
            JointState,
            'arm_control/joint_states',
            10
        )
        self.pub_tcp_pose = self.create_publisher(
            TCPPose,
            'arm_control/tcp_pose',
            10
        )

        # 创建定时器发布状态
        self.timer = self.create_timer(
            1.0 / self.publish_rate,
            self.publish_state
        )

        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():
            self.current_state = ArmJointState(
                height=0,
                j2=self.zero_offsets.j2,
                j3=self.zero_offsets.j3,
                j4=self.zero_offsets.j4,
                j5=DEFAULT_FIXED_J5,
                j6=DEFAULT_FIXED_J6,
            )
            return

        try:
            data = json.loads(STATE_FILE.read_text())
            self.current_state = ArmJointState(
                height=data['height'],
                j2=data['j2'],
                j3=data['j3'],
                j4=data['j4'],
                j5=data.get('j5', DEFAULT_FIXED_J5),
                j6=data.get('j6', DEFAULT_FIXED_J6),
            )
            self.get_logger().info(f'从缓存加载状态')
        except Exception as e:
            self.get_logger().warn(f'加载状态失败: {e}，使用默认状态')
            self.current_state = ArmJointState(
                height=0,
                j2=self.zero_offsets.j2,
                j3=self.zero_offsets.j3,
                j4=self.zero_offsets.j4,
                j5=DEFAULT_FIXED_J5,
                j6=DEFAULT_FIXED_J6,
            )

    def save_state(self):
        """保存状态到缓存"""
        if not self.use_state_cache or self.current_state is None:
            return

        try:
            STATE_FILE.parent.mkdir(parents=True, exist_ok=True)
            data = {
                'height': self.current_state.height,
                'j2': self.current_state.j2,
                'j3': self.current_state.j3,
                'j4': self.current_state.j4,
                'j5': self.current_state.j5,
                'j6': self.current_state.j6,
            }
            STATE_FILE.write_text(json.dumps(data, indent=2))
        except Exception as e:
            self.get_logger().warn(f'保存状态失败: {e}')

    def send_udp_commands(
        self,
        states: List[ArmJointState],
        duration: float
    ) -> bool:
        """发送 UDP 命令序列"""
        if not states:
            return True

        delay = duration / len(states) if len(states) > 1 and duration > 0.0 else 0.0

        try:
            for i, state in enumerate(states):
                msg = state.to_udp_message()
                self.udp_socket.sendto(msg, (self.udp_ip, self.udp_port))

                if delay > 0.0 and i < len(states) - 1:
                    time.sleep(delay)

            self.current_state = states[-1]
            self.save_state()
            return True

        except Exception as e:
            self.get_logger().error(f'发送 UDP 命令失败: {e}')
            return False


    def handle_move_joints(self, request, response):
        """处理关节空间运动服务"""
        try:
            target_state = ArmJointState(
                height=clamp_int(request.height, self.limits.height_min, self.limits.height_max, 'height'),
                j2=clamp_int(request.j2, self.limits.j2_min, self.limits.j2_max, 'j2'),
                j3=clamp_int(request.j3, self.limits.j3_min, self.limits.j3_max, 'j3'),
                j4=clamp_int(request.j4, self.limits.j4_min, self.limits.j4_max, 'j4'),
                j5=clamp_int(request.j5, self.limits.joint_min, self.limits.joint_max, 'j5'),
                j6=clamp_int(request.j6, self.limits.joint_min, self.limits.joint_max, 'j6'),
            )

            duration = request.duration if request.duration > 0 else self.default_duration
            steps = max(1, int(math.ceil(duration * self.default_rate)))

            path = interpolate_command_states(self.current_state, target_state, steps)
            success = self.send_udp_commands(path, duration)

            response.success = success
            response.message = "运动完成" if success else "运动失败"
            self.get_logger().info(f'关节运动 -> {response.message}')

        except Exception as e:
            response.success = False
            response.message = f'错误: {str(e)}'
            self.get_logger().error(f'关节运动失败: {e}')

        return response

    def handle_move_pose(self, request, response):
        """处理笛卡尔空间运动服务"""
        try:
            target_pose = ArmPose(
                x=request.x,
                y=request.y,
                z=request.z,
                phi_deg=request.phi
            )

            # 解析 z4：优先级 up/down > 自动选择
            if request.up:
                z4 = Z4_UP      # 夹爪朝上，z4=-100
            elif request.down:
                z4 = Z4_DOWN    # 夹爪朝下，z4=55
            else:
                # 自动选择：根据目标 z 坐标
                j5_auto = resolve_j5_from_z(target_pose.z)
                z4 = resolve_z4_from_j5(j5_auto)

            # 解析夹爪开合（J6：grip/release）
            if request.grip:
                j6 = GRIP_ANGLE
            elif request.release:
                j6 = RELEASE_ANGLE
            else:
                j6 = self.current_state.j6

            # 根据 z4 反推 J5（用于 UDP 命令）
            j5 = J5_CLOSED if z4 == Z4_UP else J5_OPEN

            # 逆运动学
            math_state = inverse_kinematics(
                geometry=self.geometry,
                pose=target_pose,
                limits=self.limits,
                elbow_up=request.elbow_up,
                j5=j5,
                j6=j6,
                z4=z4,
            )

            # 转换为命令状态
            target_state = math_to_command_state(
                math_state,
                self.zero_offsets,
                self.limits,
                j5=j5,
                j6=j6,
            )

            duration = request.duration if request.duration > 0 else self.default_duration
            steps = max(1, int(math.ceil(duration * self.default_rate)))

            path = interpolate_command_states(self.current_state, target_state, steps)
            success = self.send_udp_commands(path, duration)

            response.success = success
            response.message = "运动完成" if success else "运动失败"
            self.get_logger().info(
                f'位姿运动: ({request.x:.1f}, {request.y:.1f}, {request.z:.1f}, {request.phi:.1f}) '
                f'-> {response.message}'
            )

        except Exception as e:
            response.success = False
            response.message = f'错误: {str(e)}'
            self.get_logger().error(f'位姿运动失败: {e}')

        return response

    def handle_get_pose(self, request, response):
        """处理查询位姿服务"""
        try:
            if self.current_state is None:
                response.success = False
                response.message = "当前状态未初始化"
                return response

            math_state = command_to_math_state(self.current_state, self.zero_offsets)

            # 根据当前 J5 状态确定 z4
            z4 = resolve_z4_from_j5(self.current_state.j5)
            pose = forward_kinematics(self.geometry, math_state, z4)

            response.success = True
            response.x = float(pose.x)
            response.y = float(pose.y)
            response.z = float(pose.z)
            response.phi = float(pose.phi_deg)
            response.height = int(self.current_state.height)
            response.j2 = int(self.current_state.j2)
            response.j3 = int(self.current_state.j3)
            response.j4 = int(self.current_state.j4)
            response.j5 = int(self.current_state.j5)
            response.j6 = int(self.current_state.j6)

        except Exception as e:
            response.success = False
            response.message = f'错误: {str(e)}'
            self.get_logger().error(f'查询位姿失败: {e}')

        return response

    def handle_set_gripper(self, request, response):
        """处理夹爪控制服务"""
        try:
            if request.gripper_state == SetGripper.Request.GRIPPER_OPEN:
                j5 = J5_OPEN
            elif request.gripper_state == SetGripper.Request.GRIPPER_CLOSED:
                j5 = J5_CLOSED
            else:
                j5 = self.current_state.j5

            if request.grip:
                j6 = GRIP_ANGLE
            elif request.release:
                j6 = RELEASE_ANGLE
            else:
                j6 = self.current_state.j6

            target_state = ArmJointState(
                height=self.current_state.height,
                j2=self.current_state.j2,
                j3=self.current_state.j3,
                j4=self.current_state.j4,
                j5=j5,
                j6=j6,
            )

            success = self.send_udp_commands([target_state], 0.0)

            response.success = success
            response.message = "夹爪控制完成" if success else "夹爪控制失败"
            self.get_logger().info(f'夹爪控制: j5={j5}, j6={j6} -> {response.message}')

        except Exception as e:
            response.success = False
            response.message = f'错误: {str(e)}'
            self.get_logger().error(f'夹爪控制失败: {e}')

        return response

    def publish_state(self):
        """定时发布状态"""
        if self.current_state is None:
            return

        try:
            # 发布关节状态
            joint_msg = JointState()
            joint_msg.header.stamp = self.get_clock().now().to_msg()
            joint_msg.height = self.current_state.height
            joint_msg.j2 = self.current_state.j2
            joint_msg.j3 = self.current_state.j3
            joint_msg.j4 = self.current_state.j4
            joint_msg.j5 = self.current_state.j5
            joint_msg.j6 = self.current_state.j6
            self.pub_joint_states.publish(joint_msg)

            # 计算并发布 TCP 位姿
            math_state = command_to_math_state(self.current_state, self.zero_offsets)
            z4 = resolve_z4_from_j5(self.current_state.j5)
            pose = forward_kinematics(self.geometry, math_state, z4)

            pose_msg = TCPPose()
            pose_msg.header.stamp = self.get_clock().now().to_msg()
            pose_msg.x = float(pose.x)
            pose_msg.y = float(pose.y)
            pose_msg.z = float(pose.z)
            pose_msg.phi = float(pose.phi_deg)
            self.pub_tcp_pose.publish(pose_msg)

        except Exception as e:
            self.get_logger().error(f'发布状态失败: {e}')

    def destroy_node(self):
        """节点销毁时的清理"""
        self.udp_socket.close()
        super().destroy_node()


def main(args=None):
    rclpy.init(args=args)
    node = ArmControlNode()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
