양자 터널링 복합재(QTC) 압저항 센싱 & 1,000Hz 0-Alloc C# MUM-T 로봇 그리퍼 촉각 서보 제어기

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Stem.Robotics
{
    /// <summary>
    /// QTC 촉각 그리퍼 텔레메트리 구조체 (16-byte 스택 전용)
    /// </summary>
    public readonly ref struct QtcGripperState
    {
        public readonly float ContactForceN;    // 추정 접촉 수직항력 (N)
        public readonly float ResistanceOhm;    // 복조된 QTC 센서 저항 (Ohm)
        public readonly float SlipIndex;        // 미끄러짐 위험도 지수
        public readonly float CommandedTorqueNm;// 그리퍼 모터 지령 토크 (Nm)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public QtcGripperState(float force, float r, float slip, float torque)
        {
            ContactForceN = force;
            ResistanceOhm = r;
            SlipIndex = slip;
            CommandedTorqueNm = torque;
        }
    }

    /// <summary>
    /// 1,000Hz Zero-Allocation QTC 양자 터널링 로봇 촉각 서보 엔진
    /// </summary>
    public ref struct QtcGripperServoEngine
    {
        private readonly float _targetForceN;       // 목표 파지 수직항력 (N)
        private readonly float _frictionCoeff;      // 표면 마찰계수 추정치
        private float _lastForceN;                  // 직전 수직항력
        private float _lastCommandTorque;           // 직전 모터 토크 지령

        private const float OpenResistance = 1e9f;  // 무부하 개방 저항 (1 Giga-ohm)
        private const float SlipThreshold = 15.0f;  // 슬립 판정 임계 미분치 (N/s)
        private const float MaxTorqueLimitNm = 5.0f;// 그리퍼 모터 최대 토크

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public QtcGripperServoEngine(float targetForce, float frictionCoeff)
        {
            _targetForceN = targetForce;
            _frictionCoeff = frictionCoeff;
            _lastForceN = 0.0f;
            _lastCommandTorque = 0.0f;
        }

        /// <summary>
        /// 1ms 주기(1,000Hz) QTC 전압 계측치 기반 저항 해독, 슬립 방어 및 토크 지령 산출 (0-Alloc)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public QtcGripperState Update1Khz(float sensorVoltageV, float biasVoltageV, float dtSeconds)
        {
            // 1) 전압 분배기로부터 QTC 센서 저항 역산 (V_out = V_bias * R_ref / (R_ref + R_qtc))
            const float referenceResistor = 10000.0f; // 10 kOhm
            float rQtc = sensorVoltageV > 0.001f 
                ? referenceResistor * ((biasVoltageV / sensorVoltageV) - 1.0f) 
                : OpenResistance;
            rQtc = Math.Clamp(rQtc, 0.1f, OpenResistance);

            // 2) Simmons 터널링 모델 기반 접촉 하중 (N) 역산 (대수 역함수 근사)
            // ln(R_open / R) = kappa * F^gamma
            float logRatio = MathF.Log(OpenResistance / rQtc);
            float estimatedForceN = logRatio > 0.01f ? MathF.Pow(logRatio * 0.45f, 1.4f) : 0.0f;

            // 3) 순간 하중 변화율(dF/dt) 분석을 통한 슬립(미끄러짐) 감지
            float forceRate = (estimatedForceN - _lastForceN) / dtSeconds;
            _lastForceN = estimatedForceN;

            float slipIndicator = 0.0f;
            float forceBoost = 0.0f;
            if (forceRate < -SlipThreshold) // 갑작스러운 하중 감소 = 미끄러짐 개시
            {
                slipIndicator = MathF.Abs(forceRate);
                forceBoost = slipIndicator * 0.05f; // 즉각적 파지력 부스팅
            }

            // 4) 임피던스 컴플라이언스 모터 지령 토크 계산
            float forceError = (_targetForceN + forceBoost) - estimatedForceN;
            float targetTorque = (forceError * 0.12f);
            targetTorque = Math.Clamp(targetTorque, 0.0f, MaxTorqueLimitNm);

            _lastCommandTorque += (targetTorque - _lastCommandTorque) * 0.35f;

            return new QtcGripperState(estimatedForceN, rQtc, slipIndicator, _lastCommandTorque);
        }
    }
}