외골격 로봇 표면 근전도(sEMG) 실시간 의도 인식 & 1,000Hz 0-Alloc C# 힐형(Hill-type) 근육 모델 직렬 탄성 액추에이터(SEA) 보행 보조 서보 엔진

using System;
using System.Runtime.CompilerServices;
using System.Numerics;

namespace ZeliDesk.Stem.Biomechatronics
{
    /// 

    /// 1,000Hz 외골격 로봇 sEMG 근육 모델 역산 및 직렬 탄성 액추에이터(SEA) 보행 서보
    /// GC 무할당 스택 메모리 0-Alloc 구현체
    /// 
    public ref struct ExoskeletonSeaServo
    {
        // 생체 로봇 물리 상수
        private const float SeaTorsionalStiffnessNmRad = 120.0f; // SEA 비틀림 스프링 상수
        private const float MaxAssistTorqueNm = 45.0f;          // 무릎 관절 최대 보조 토크
        private const float IsometricMaxForceN = 1200.0f;       // 대퇴사두근 최대 근력 F0
        private const float GearRatio = 50.0f;                  // 감속비

        // 서보 제어 상태
        public float MuscleActivation;
        public float EstimatedBiologicalTorqueNm;
        public float MeasuredSeaSpringTorqueNm;
        public float CommandedMotorCurrentA;
        public float GaitPhaseProgressPercent;          // 보행 주기 진행도 (0~100%)
        public bool IsStancePhase;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Initialize()
        {
            MuscleActivation = 0.05f;
            EstimatedBiologicalTorqueNm = 0.0f;
            MeasuredSeaSpringTorqueNm = 0.0f;
            CommandedMotorCurrentA = 0.0f;
            GaitPhaseProgressPercent = 0.0f;
            IsStancePhase = true;
        }

        /// 

        /// 1ms 제어 주기: sEMG 원신호 및 SEA 광학 엔코더 처짐각으로부터 보행 보조 모터 전류 제어
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void StepExoskeletonControl(
            float rawSemgMv, 
            float jointAngleDeg, 
            float motorAngleDeg, 
            float groundReactionForceN,
            out float commandedCurrentAmp, 
            out float currentJointTorqueNm, 
            out float assistanceLevelPercent)
        {
            // 1. sEMG 대역 통과 및 전파 정류 포락선 추출 (Zero-Alloc 스택 연산)
            float rectified = MathF.Abs(rawSemgMv);
            // 1차 IIR 저역 통과 필터 (컷오프 6Hz 상당)
            MuscleActivation = MuscleActivation * 0.96f + rectified * 0.04f;
            MuscleActivation = MathF.Min(1.0f, MathF.Max(0.0f, MuscleActivation));

            // 2. 힐형 근육 모델 관절 토크 역산 (모멘트 암 r = 0.045m 가정)
            const float MuscleMomentArmM = 0.045f;
            EstimatedBiologicalTorqueNm = MuscleActivation * IsometricMaxForceN * MuscleMomentArmM;

            // 3. 지면 반력(GRF) 기반 보행 주기 판별 (입각기 vs 유각기)
            IsStancePhase = groundReactionForceN > 50.0f;

            // 4. SEA 스프링 처짐각(Deflection Angle) 및 실측 토크 계측
            float motorEquivalentJointDeg = motorAngleDeg / GearRatio;
            float springDeflectionRad = (motorEquivalentJointDeg - jointAngleDeg) * (MathF.PI / 180.0f);
            MeasuredSeaSpringTorqueNm = springDeflectionRad * SeaTorsionalStiffnessNmRad;

            // 5. 1,000Hz 가상 임피던스 추종 폐루프 제어기
            // 생체 생성 토크의 40%를 외골격이 보조
            float targetExoTorqueNm = EstimatedBiologicalTorqueNm * 0.40f;
            targetExoTorqueNm = MathF.Min(MaxAssistTorqueNm, targetExoTorqueNm);

            // 토크 오차 피드백
            float torqueError = targetExoTorqueNm - MeasuredSeaSpringTorqueNm;
            const float KpTorque = 0.25f; // A per Nm
            CommandedMotorCurrentA = targetExoTorqueNm * 0.08f + torqueError * KpTorque;

            // 모터 전류 제한 (-15A ~ +15A)
            CommandedMotorCurrentA = MathF.Max(-15.0f, MathF.Min(15.0f, CommandedMotorCurrentA));

            commandedCurrentAmp = CommandedMotorCurrentA;
            currentJointTorqueNm = MeasuredSeaSpringTorqueNm;
            assistanceLevelPercent = (targetExoTorqueNm / MaxAssistTorqueNm) * 100.0f;
        }
    }
}