Body step 2.2: fix the 2.1 headset instabilities

Spike now runs real input (90 Hz sample-and-hold tracking + jitter under 144 Hz
physics), which reproduced the playtest. Root causes fixed:
- PoseFilter/RagdollDriver: timestamped extrapolation of tracking, one tick
  pipeline shared by PlayerRig and the spike
- low-pass all feed-forwards (ground control amplified jitter by body mass)
- arm muscles back to local targets (physical-frame aim fed chest rotation back)
- structural hand strength only when braced or holding
- no free-path view chasing while grounded (8 m crouch slide); stick hold-back
- explicit inertia for the Legs body (shape-less tuck tumbled the pelvis)
- world grab: Box3D shape queries return a null collider, use collider_id
- new crouch-hold phases; world-grab detection asserted

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

scripts/RagdollSpike.cs

@@ -23,8 +23,6 @@ public partial class RagdollSpike : Node3D
     private Transform3D _origin = Transform3D.Identity;
     private Transform3D _headLocal, _leftLocal, _rightLocal;
     private Vector3 _stickVelocity;
-    private Vector3 _prevL, _prevR;
-    private bool _needBuild;
 
     // Measurements (reset per phase).
     private float _maxEyeErr, _maxHandErr, _endHandErr, _maxSpeed, _minCamZ, _minEyeZ, _maxElbowErr;
@@ -32,13 +30,49 @@ public partial class RagdollSpike : Node3D
     private int _snapsAtStart;
     private float _measureFrom;
 
-    private Transform3D Camera => _origin * _headLocal;
-    private Transform3D Left => _origin * _leftLocal;
-    private Transform3D Right => _origin * _rightLocal;
+    // What the ragdoll sees. REAL-INPUT mode (default; `-- ideal` disables it) reproduces the headset:
+    // tracking poses update at the 90 Hz display rate (sample-and-hold between 144 Hz physics ticks)
+    // with a little jitter — the smooth per-tick input of the ideal mode hid the 2.1 instabilities.
+    private Transform3D Camera => _origin * Tracked(0, _headLocal);
+    private Transform3D Left => _origin * Tracked(1, _leftLocal);
+    private Transform3D Right => _origin * Tracked(2, _rightLocal);
+
+    private bool _realInput = true, _noiseOn = true, _holdOn = true;
+    private const float TrackingHz = 90f;
+    private readonly Transform3D[] _held = new Transform3D[3];
+    private readonly long[] _heldFrame = { -1, -1, -1 };
+    private readonly RandomNumberGenerator _noise = new() { Seed = 3 };
+    private float _clock;
+
+    private Transform3D Tracked(int i, Transform3D truth)
+    {
+        if (!_realInput)
+            return truth;
+        long frame = _holdOn ? (long)(_clock * TrackingHz) : (long)(_clock * 100000f);
+        if (frame != _heldFrame[i])
+        {
+            _heldFrame[i] = frame;
+            float p = _noiseOn ? 0.0005f : 0f, r = _noiseOn ? Mathf.DegToRad(0.1f) : 0f;   // pessimistic for inside-out tracking
+            Vector3 dp = new(_noise.Randfn(0f, p), _noise.Randfn(0f, p), _noise.Randfn(0f, p));
+            Basis dr = Basis.FromEuler(new Vector3(_noise.Randfn(0f, r), _noise.Randfn(0f, r), _noise.Randfn(0f, r)));
+            _held[i] = new Transform3D(dr * truth.Basis, truth.Origin + dp);
+        }
+        return _held[i];
+    }
 
     public override void _Ready()
     {
         _profile = BodyProfile.Estimate(EyeHeight);
+        foreach (string arg in OS.GetCmdlineUserArgs())
+        {
+            if (arg == "ideal")
+                _realInput = false;
+            if (arg == "nonoise")
+                _noiseOn = false;
+            if (arg == "nohold")
+                _holdOn = false;
+        }
+        GD.Print($"RAGDOLL: input mode {(_realInput ? "REAL (90 Hz tracking, jitter)" : "ideal")} at {Engine.PhysicsTicksPerSecond} Hz physics");
         AddChild(MakeStatic(new Vector3(40f, 1f, 40f), new Vector3(0f, -0.5f, 0f), 29));
         _ragdoll = new PlayerRagdoll { Name = "Ragdoll" };
         AddChild(_ragdoll);
@@ -65,6 +99,8 @@ public partial class RagdollSpike : Node3D
         _phases.Add(("two-handed pull-up on a grabbed bar", () => SetupBarHang(), t => TickPullUp(both: true), 3.5f, () => FinishPullUp(true)));
         _phases.Add(("one-handed pull-up is weaker", () => SetupBarHang(), t => TickPullUp(both: false), 3.5f, () => FinishPullUp(false)));
         _phases.Add(("crawls through a 0.6 m tunnel", SetupTunnel, TickCrawl, 7f, FinishCrawl));
+        _phases.Add(("holds a crouch (eyes 1.0 m) without sliding or spinning", Rest, t => TickCrouchHold(1.0f), 5f, FinishCrouchHold));
+        _phases.Add(("holds a deep crouch (eyes 0.7 m) without sliding or spinning", Rest, t => TickCrouchHold(0.7f), 5f, FinishCrouchHold));
         _phases.Add(("walks off a 1 m ledge and lands", SetupLedgeTop, _ => _stickVelocity = _t > 0.5f && _t < 2f ? new Vector3(0f, 0f, -1.2f) : Vector3.Zero, 4f, FinishDrop));
         _phases.Add(("walks up an 18° ramp", () => SetupRamp(18f), _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 4f, () => FinishRamp(true)));
         _phases.Add(("can't walk up a 40° ramp", () => SetupRamp(40f), _ => _stickVelocity = new Vector3(0f, 0f, -1.2f), 4f, () => FinishRamp(false)));
@@ -79,6 +115,7 @@ public partial class RagdollSpike : Node3D
             return;
         float dt = (float)delta;
         _t += dt;
+        _clock += dt;
         _phases[_phase].Tick(dt);
         if (_phase > 0)
             Drive(dt);
@@ -116,24 +153,34 @@ public partial class RagdollSpike : Node3D
     // Exactly PlayerRig's tick order.
     private void Drive(float dt)
     {
-        _origin.Origin += _stickVelocity * dt;
-        if (_ragdoll.Built && !_needBuild)
-            _origin.Origin += _ragdoll.OriginCorrection(Camera.Origin, _origin.Origin.Y, dt);
-        _solver.Solve(_profile, Camera, Left, Right, _origin.Origin.Y, dt);
-        if (_needBuild)
-        {
-            _ragdoll.Build(_profile, _solver, Left, Right);
-            _needBuild = false;
-            _prevL = Left.Origin;
-            _prevR = Right.Origin;
-        }
-        Vector3 lv = (Left.Origin - _prevL) / dt, rv = (Right.Origin - _prevR) / dt;
-        _prevL = Left.Origin;
-        _prevR = Right.Origin;
-        _ragdoll.Step(_solver, Left, Right, lv, rv, _origin.Origin.Y, dt);
+        _driver ??= new RagdollDriver { Solver = _solver, Ragdoll = _ragdoll };
+        Vector3 stick = _ragdoll.Built && !_needBuild ? PlayerRig.HoldBack(_stickVelocity, _ragdoll.HeldBack) : _stickVelocity;
+        _origin.Origin += stick * dt;
+        _driver.Tick(_profile, ref _origin, Tracked(0, _headLocal), Tracked(1, _leftLocal), Tracked(2, _rightLocal), _stickVelocity, dt);
         Measure();
     }
 
+    private RagdollDriver _driver;
+    private bool _needBuild
+    {
+        get => _driver == null || _driver.RebuildRequested;
+        set
+        {
+            _driver ??= new RagdollDriver { Solver = _solver, Ragdoll = _ragdoll };
+            _driver.RebuildRequested = value;
+            if (value)
+            {
+                // New phase: resample tracking now (a held sample from the last phase would build
+                // the body where the previous phase left the head).
+                for (int i = 0; i < 3; i++)
+                    _heldFrame[i] = -1;
+                _driver.Head.Reset();
+                _driver.Left.Reset();
+                _driver.Right.Reset();
+            }
+        }
+    }
+
     private void Measure()
     {
         foreach (RigidBody3D b in _ragdoll.Bodies)
@@ -224,8 +271,10 @@ public partial class RagdollSpike : Node3D
 
     private bool FinishStand()
     {
+        float core = Mathf.Max(_ragdoll.Pelvis.LinearVelocity.Length(), Mathf.Max(_ragdoll.Chest.LinearVelocity.Length(), _ragdoll.Head.LinearVelocity.Length()));
+        GD.Print($"RAGDOLL:   body core speed at rest {core:F3} m/s (limit 0.03)");
         GD.Print($"RAGDOLL:   eye error max {_maxEyeErr:F3} m, hand error max {_maxHandErr:F3} m, elbow error max {Mathf.RadToDeg(_maxElbowErr):F1}°, end speed {_maxSpeed:F3} m/s, grounded {_ragdoll.Grounded}, snaps {NewSnaps}, pelvis y {_ragdoll.Pelvis.GlobalPosition.Y:F3}");
-        return _finite && NewSnaps == 0 && _ragdoll.Grounded && _maxEyeErr < 0.03f && _maxHandErr < 0.03f && _maxSpeed < 0.05f && _maxElbowErr < Mathf.DegToRad(8f);
+        return _finite && NewSnaps == 0 && _ragdoll.Grounded && _maxEyeErr < 0.03f && _maxHandErr < 0.03f && core < 0.03f && _maxSpeed < 0.15f && _maxElbowErr < Mathf.DegToRad(8f);
     }
 
     private readonly List<string> _reachLog = new();
@@ -435,8 +484,6 @@ public partial class RagdollSpike : Node3D
             Transform3D delta = _origin * before.AffineInverse();
             _ragdoll.TeleportBy(delta);
             _solver.Shift(delta);
-            _prevL = delta * _prevL;
-            _prevR = delta * _prevR;
             _teleported = true;
         }
         if (!_turned && _t > 0.9f)
@@ -446,8 +493,6 @@ public partial class RagdollSpike : Node3D
             _origin = rot * _origin;
             _ragdoll.TeleportBy(rot);
             _solver.Shift(rot);
-            _prevL = rot * _prevL;
-            _prevR = rot * _prevR;
             _turned = true;
         }
     }
@@ -552,6 +597,18 @@ public partial class RagdollSpike : Node3D
         _leftLocal = both ? new Transform3D(grip, new Vector3(-0.22f, y, BarHangZ + 0.03f)) : new Transform3D(Basis.Identity, new Vector3(-0.2f, 1.0f, -0.3f));
         if (!_barGrabbed && t > 0.6f && _ragdoll.Built)
         {
+            // The game's own world-grab detection must find the bar under the palm.
+            if (both)
+            {
+                for (int side = 0; side < 2; side++)
+                {
+                    RigidBody3D hand = _ragdoll.Hand(side);
+                    Vector3 palm = PropGrabber.PalmPoint(hand, side);
+                    StaticBody3D found = PropGrabber.WorldUnderPalm(palm, hand.GetWorld3D()) ?? PropGrabber.WorldUnderPalm(hand.GlobalPosition, hand.GetWorld3D());
+                    _grabDetect &= found == _bar;
+                    GD.Print($"RAGDOLL:     world grab detection side {side}: palm {palm}, bar {_bar.GlobalPosition}, found {(found == null ? "nothing" : found.Name)}");
+                }
+            }
             _barGrabbed = true;
             _barPelvisStart = _ragdoll.Pelvis.GlobalPosition.Y;
             for (int side = both ? 0 : 1; side < 2; side++)
@@ -567,6 +624,7 @@ public partial class RagdollSpike : Node3D
     }
 
     private float _twoHandRise;
+    private bool _grabDetect = true;
 
     private bool FinishPullUp(bool both)
     {
@@ -576,7 +634,8 @@ public partial class RagdollSpike : Node3D
         if (both)
         {
             _twoHandRise = _maxPelvisRise;
-            return _finite && NewSnaps == 0 && _maxPelvisRise > 0.25f;
+            GD.Print($"RAGDOLL:   world grab detection {(_grabDetect ? "found the bar" : "FAILED")}");
+            return _finite && NewSnaps == 0 && _maxPelvisRise > 0.25f && _grabDetect;
         }
         return _finite && NewSnaps == 0 && _maxPelvisRise < _twoHandRise * 0.6f;
     }
@@ -630,6 +689,42 @@ public partial class RagdollSpike : Node3D
             && _ragdoll.Eye.DistanceTo(Camera.Origin) < 0.1f && _ragdoll.Pelvis.GlobalPosition.Z > -3f;
     }
 
+    private Vector3 _crouchPelvis;
+    private float _crouchYaw, _crouchDrift, _crouchSpin;
+
+    // Squat down over 1.5 s (looking a bit down, hands in front of the knees) and hold.
+    private void TickCrouchHold(float eye)
+    {
+        float k = Mathf.SmoothStep(0f, 1f, Mathf.Min(_t / 1.5f, 1f));
+        float y = Mathf.Lerp(EyeHeight, eye, k);
+        _headLocal = new Transform3D(new Basis(Vector3.Right, -0.3f * k), new Vector3(0f, y, -0.1f * k));
+        _leftLocal = new Transform3D(Basis.Identity, new Vector3(-0.2f, Mathf.Lerp(1.0f, y - 0.6f, k), -0.35f));
+        _rightLocal = new Transform3D(Basis.Identity, new Vector3(0.2f, Mathf.Lerp(1.0f, y - 0.6f, k), -0.35f));
+        _measureFrom = float.MaxValue;
+        if (!_ragdoll.Built || _needBuild)
+            return;
+        Vector3 f = -_ragdoll.Pelvis.GlobalTransform.Basis.Z;
+        float yaw = Mathf.Atan2(-f.X, -f.Z);
+        if (_t < 2f)
+        {
+            _crouchPelvis = _ragdoll.Pelvis.GlobalPosition;
+            _crouchYaw = yaw;
+            _crouchDrift = _crouchSpin = 0f;
+            return;
+        }
+        Vector3 d = _ragdoll.Pelvis.GlobalPosition - _crouchPelvis;
+        d.Y = 0f;
+        _crouchDrift = Mathf.Max(_crouchDrift, d.Length());
+        _crouchSpin = Mathf.Max(_crouchSpin, Mathf.Abs(Mathf.AngleDifference(_crouchYaw, yaw)));
+    }
+
+    private bool FinishCrouchHold()
+    {
+        GD.Print($"RAGDOLL:   crouch hold: pelvis drift {_crouchDrift:F3} m, yaw change {Mathf.RadToDeg(_crouchSpin):F1}°, eye error {_ragdoll.Eye.DistanceTo(Camera.Origin):F3}, origin {_origin.Origin}, snaps {NewSnaps}");
+        return _finite && NewSnaps == 0 && _crouchDrift < 0.05f && _crouchSpin < Mathf.DegToRad(5f) && _origin.Origin.Length() < 0.05f
+            && _ragdoll.Eye.DistanceTo(Camera.Origin) < 0.05f;
+    }
+
     // A slope rising away (−Z) from ObstacleFront up to 1 m, then a platform.
     private void SetupRamp(float degrees)
     {