Body Stage 1 collapse root-caused + fixed: NaN in torso servo math (GetAngle = 2*acos(W) at W=1.0000001), not solver stacking
- BodyMath.RotationError: atan2-based shortest-arc error (no NaN, no GetAxis gain collapse inside 0.16 deg); used by PhysicsTorso orientation servo and PhysicsHands rotation lock - BodyMath.ResetIfNonFinite escape hatch on torso + hands - PhysicsHands.Drive extracted (static) so spikes run the real hand chase - CollapseSpike: deterministic repro (slow yaw drift across a parked chest, 7/40 NaN on old code) + solver-stacking scenarios (seated chest in table, two-hand-welded 50 kg anvil, real PropShop) — 49/49 clean after fix, x3; all other spikes PASS x3 - Failure brief RESOLUTION section; HANDOFF/AGENTS updated Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
FoxyzandClaude Opus 5.5 committed
1 parent
ce1e0e3b0c
commit
cb518999e5
9 files changed
+466
-47
No files matched your search
@@ -0,0 +1,328 @@
|
||||
using System.Collections.Generic;
|
||||
using Godot;
|
||||
|
||||
// Headless regression for the Stage-1 playtest collapse (2026-10-01). Two families:
|
||||
// - Stacking: seated chest overlapping the static table, 50 kg anvil rigid-welded to two
|
||||
// chase hands, the logged grab sequence (L, R two-handed, L release, L re-grab), the real
|
||||
// PropShop with the chest plowing through the props. Did NOT reproduce the NaN — kept as
|
||||
// the guard for the solver-stacking hypothesis.
|
||||
// - Yaw drift: head still while the torso yaw target drifts slowly (0.2 deg/s) across the
|
||||
// body's yaw — the actual root cause. The old servo's error quaternion hit W = 1.0000001
|
||||
// there, GetAngle() = 2*acos(W) went NaN, and AngularVelocity += NaN killed the chest.
|
||||
// Run: godot-mono --headless --fixed-fps 90 --path . scenes/collapse_spike.tscn
|
||||
public partial class CollapseSpike : Node3D
|
||||
{
|
||||
private const float PalmGrabOffset = 0.045f;
|
||||
private const float RunawaySpeed = 50f;
|
||||
private const int DriftYawCount = 40;
|
||||
private static readonly Vector3 HandSize = new(0.035f, 0.09f, 0.12f);
|
||||
private static readonly Vector3 AnvilSize = new(0.28f, 0.16f, 0.13f);
|
||||
|
||||
private struct Scenario
|
||||
{
|
||||
public string Name;
|
||||
public Vector3 Head;
|
||||
public float Duration;
|
||||
public bool Anvil;
|
||||
public bool Table;
|
||||
public bool Props;
|
||||
public bool Sweep;
|
||||
public bool Still;
|
||||
public float Yaw;
|
||||
public float YawDrift;
|
||||
}
|
||||
|
||||
private readonly List<Scenario> _scenarios = new();
|
||||
private int _index = -1;
|
||||
private float _t;
|
||||
private int _step;
|
||||
private Node3D _world;
|
||||
private Node3D _head;
|
||||
private PhysicsTorso _torso;
|
||||
private readonly RigidBody3D[] _hands = new RigidBody3D[2];
|
||||
private readonly Node3D[] _targets = new Node3D[2];
|
||||
private readonly ConeTwistJoint3D[] _welds = new ConeTwistJoint3D[2];
|
||||
private readonly Vector3[] _targetBase = new Vector3[2];
|
||||
private RigidBody3D _anvil;
|
||||
private bool _failed;
|
||||
private float _maxTorsoSpeed;
|
||||
private float _maxTorsoSpin;
|
||||
private float _maxPropSpeed;
|
||||
private float _maxYawErrDeg;
|
||||
private readonly HashSet<string> _done = new();
|
||||
private readonly List<string> _results = new();
|
||||
private int _failCount;
|
||||
private readonly RandomNumberGenerator _rng = new() { Seed = 1302 };
|
||||
|
||||
public override void _Ready()
|
||||
{
|
||||
// Seated head 1.01 m (the playtest calibration); chest center z = head z + 0.12.
|
||||
// Table front edge is z = -0.2, chest half-depth 0.065: overlap starts at head z < -0.255.
|
||||
Add("standing, no table overlap, anvil", new(0.2f, 1.65f, 0.3f), 6f, anvil: true, table: true);
|
||||
Add("seated, chest clear of table, anvil", new(0.2f, 1.01f, 0.05f), 6f, anvil: true, table: true);
|
||||
Add("seated, chest 5 cm into table, no props", new(0.2f, 1.01f, -0.31f), 6f, table: true);
|
||||
Add("seated, chest 15 cm into table, no props", new(0.2f, 1.01f, -0.41f), 6f, table: true);
|
||||
Add("seated, chest 5 cm into table, anvil", new(0.2f, 1.01f, -0.31f), 6f, anvil: true, table: true);
|
||||
Add("seated, chest 15 cm into table, anvil", new(0.2f, 1.01f, -0.41f), 6f, anvil: true, table: true);
|
||||
Add("standing, anvil pressed into chest, no table", new(0.2f, 1.65f, 0.3f), 6f, anvil: true);
|
||||
Add("seated, real PropShop, chest sweeps through props, anvil", new(0.2f, 1.01f, -0.31f), 6f, anvil: true, props: true, sweep: true);
|
||||
Add("seated, real PropShop, chest sweeps through props, no grabs", new(0.2f, 1.01f, -0.31f), 6f, props: true, sweep: true);
|
||||
for (int i = 0; i < DriftYawCount; i++)
|
||||
{
|
||||
float yaw = -3f + i * 0.1537f;
|
||||
_scenarios.Add(new Scenario
|
||||
{
|
||||
Name = $"seated still, yaw {yaw:F2} drifting 0.2 deg/s, anvil held",
|
||||
Head = new(0.2f, 1.01f, 0.05f), Duration = 2.5f, Anvil = true, Table = true,
|
||||
Still = true, Yaw = yaw, YawDrift = 0.0035f,
|
||||
});
|
||||
}
|
||||
NextScenario();
|
||||
}
|
||||
|
||||
private void Add(string name, Vector3 head, float duration, bool anvil = false, bool table = false, bool props = false, bool sweep = false)
|
||||
{
|
||||
_scenarios.Add(new Scenario { Name = name, Head = head, Duration = duration, Anvil = anvil, Table = table || props, Props = props, Sweep = sweep });
|
||||
}
|
||||
|
||||
private void NextScenario()
|
||||
{
|
||||
if (_world != null)
|
||||
{
|
||||
_world.QueueFree();
|
||||
_world = null;
|
||||
}
|
||||
_index++;
|
||||
if (_index >= _scenarios.Count)
|
||||
{
|
||||
foreach (string line in _results)
|
||||
GD.Print(line);
|
||||
GD.Print($"COLLAPSE: {_scenarios.Count - _failCount}/{_scenarios.Count} scenarios clean");
|
||||
GD.Print($"COLLAPSE: overall {(_failCount == 0 ? "PASS — no NaN/runaway in any scenario" : "FAIL — see above")}");
|
||||
GetTree().Quit();
|
||||
return;
|
||||
}
|
||||
_t = 0f;
|
||||
_step = 0;
|
||||
_failed = false;
|
||||
_maxTorsoSpeed = 0f;
|
||||
_maxTorsoSpin = 0f;
|
||||
_maxPropSpeed = 0f;
|
||||
_maxYawErrDeg = 0f;
|
||||
_done.Clear();
|
||||
_welds[0] = _welds[1] = null;
|
||||
Build(_scenarios[_index]);
|
||||
}
|
||||
|
||||
private void Build(Scenario s)
|
||||
{
|
||||
_world = new Node3D();
|
||||
AddChild(_world);
|
||||
|
||||
StaticBody3D floor = new() { CollisionMask = 0b11101 };
|
||||
floor.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = new Vector3(10f, 0.1f, 10f) }, Position = new Vector3(0f, -0.05f, 0f) });
|
||||
_world.AddChild(floor);
|
||||
|
||||
if (s.Props)
|
||||
{
|
||||
_world.AddChild(new PropShop());
|
||||
}
|
||||
else if (s.Table)
|
||||
{
|
||||
StaticBody3D table = new() { CollisionMask = 0b11101 };
|
||||
table.AddChild(new CollisionShape3D { Position = new Vector3(0f, 0.725f, -0.6f), Shape = new BoxShape3D { Size = new Vector3(1.4f, 0.05f, 0.8f) } });
|
||||
foreach (Vector3 o in new Vector3[] { new(-0.62f, 0, 0.32f), new(0.62f, 0, 0.32f), new(-0.62f, 0, -0.32f), new(0.62f, 0, -0.32f) })
|
||||
table.AddChild(new CollisionShape3D { Position = new Vector3(o.X, 0.3625f, -0.6f + o.Z), Shape = new BoxShape3D { Size = new Vector3(0.06f, 0.725f, 0.06f) } });
|
||||
_world.AddChild(table);
|
||||
}
|
||||
|
||||
_head = new Node3D { Position = s.Head };
|
||||
_world.AddChild(_head);
|
||||
_torso = new PhysicsTorso { HeadSource = _head };
|
||||
_world.AddChild(_torso);
|
||||
Vector3 chest = s.Head + new Vector3(0f, -0.42f, 0.12f);
|
||||
_torso.Body.GlobalPosition = chest;
|
||||
|
||||
Vector3 anvilPos = s.Table ? new Vector3(s.Head.X, 0.83f, Mathf.Min(chest.Z - 0.2f, -0.3f)) : chest + new Vector3(0f, 0f, -0.2f);
|
||||
_anvil = null;
|
||||
if (s.Props)
|
||||
{
|
||||
foreach (Node n in GetTree().GetNodesInGroup("prop"))
|
||||
if (n is RigidBody3D rb && rb.Name.ToString().StartsWith("Anvil"))
|
||||
_anvil = rb;
|
||||
anvilPos = _anvil.GlobalPosition;
|
||||
if (!s.Anvil)
|
||||
_anvil = null;
|
||||
}
|
||||
else if (s.Anvil)
|
||||
{
|
||||
_anvil = new RigidBody3D
|
||||
{
|
||||
Name = "Anvil",
|
||||
Mass = 50f,
|
||||
Position = anvilPos,
|
||||
CollisionMask = 0b11101,
|
||||
GravityScale = s.Table ? 1f : 0f,
|
||||
PhysicsMaterialOverride = new PhysicsMaterial { Bounce = 0f, Friction = 0.95f },
|
||||
};
|
||||
_anvil.AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = AnvilSize } });
|
||||
_world.AddChild(_anvil);
|
||||
}
|
||||
|
||||
for (int h = 0; h < 2; h++)
|
||||
{
|
||||
float side = h == 0 ? -1f : 1f;
|
||||
// Palm offset: left +X, right -X — the hand sits just outside the anvil's X face.
|
||||
Vector3 handPos = anvilPos + new Vector3(side * (AnvilSize.X * 0.5f + HandSize.X * 0.5f + 0.005f), 0f, 0f);
|
||||
_hands[h] = new RigidBody3D
|
||||
{
|
||||
Mass = 0.4f,
|
||||
GravityScale = 0f,
|
||||
ContinuousCd = true,
|
||||
Position = handPos,
|
||||
CollisionLayer = 0b1000,
|
||||
CollisionMask = 0b1001,
|
||||
};
|
||||
_hands[h].AddChild(new CollisionShape3D { Shape = new BoxShape3D { Size = HandSize } });
|
||||
_world.AddChild(_hands[h]);
|
||||
_targets[h] = new Node3D { Position = handPos };
|
||||
_world.AddChild(_targets[h]);
|
||||
_targetBase[h] = handPos;
|
||||
}
|
||||
}
|
||||
|
||||
public override void _PhysicsProcess(double delta)
|
||||
{
|
||||
if (_world == null)
|
||||
return;
|
||||
float dt = (float)delta;
|
||||
_t += dt;
|
||||
_step++;
|
||||
Scenario s = _scenarios[_index];
|
||||
|
||||
// Controllers: settle, grab L, grab R, haul the anvil toward the chest and down into the
|
||||
// table edge with a wobble, release L, re-grab L (the logged sequence), keep hauling.
|
||||
Vector3 haul = Vector3.Zero;
|
||||
if (_t > 1.0f && !s.Still)
|
||||
{
|
||||
float k = Mathf.Min((_t - 1.0f) / 1.0f, 1f);
|
||||
haul = new Vector3(0f, -0.05f, 0.35f) * k
|
||||
+ new Vector3(0.04f * Mathf.Sin(9f * _t), 0.03f * Mathf.Sin(7f * _t), 0.05f * Mathf.Sin(5f * _t));
|
||||
}
|
||||
for (int h = 0; h < 2; h++)
|
||||
_targets[h].GlobalPosition = _targetBase[h] + haul;
|
||||
|
||||
if (_anvil != null)
|
||||
{
|
||||
Once("grabL", _t > 0.4f, () => Grab(0));
|
||||
Once("grabR", _t > 0.7f, () => Grab(1));
|
||||
if (!s.Still)
|
||||
{
|
||||
Once("releaseL", _t > 2.6f, () => Release(0));
|
||||
Once("regrabL", _t > 3.2f, () => Grab(0));
|
||||
}
|
||||
}
|
||||
|
||||
for (int h = 0; h < 2; h++)
|
||||
PhysicsHands.Drive(_hands[h], _targets[h].GlobalTransform, dt);
|
||||
|
||||
Vector3 sway = s.Still ? Vector3.Zero : s.Sweep
|
||||
? new Vector3(-0.2f + 0.55f * Mathf.Sin(0.9f * _t), 0f, -0.12f + 0.14f * Mathf.Sin(1.7f * _t))
|
||||
: new Vector3(0.03f * Mathf.Sin(2f * _t), 0f, 0.04f * Mathf.Sin(1.3f * _t));
|
||||
// Still: yaw parked, then drifting slowly across the body's yaw from t = 1 s.
|
||||
// Moving: arbitrary base yaw plus sub-millidegree tracker noise.
|
||||
float yaw = s.Still ? s.Yaw + s.YawDrift * Mathf.Max(_t - 2f, -1f) : 0.37f + 1e-5f * _rng.Randfn();
|
||||
_head.GlobalTransform = new Transform3D(new Basis(new Quaternion(Vector3.Up, yaw)), s.Head + sway);
|
||||
|
||||
if (!_failed)
|
||||
Monitor(s);
|
||||
|
||||
if (_t >= s.Duration)
|
||||
{
|
||||
bool ok = !_failed;
|
||||
if (!ok)
|
||||
_failCount++;
|
||||
if (!ok || !s.Still)
|
||||
_results.Add($"COLLAPSE: [{(ok ? "PASS" : "FAIL")}] {s.Name}: torso |v| max {_maxTorsoSpeed:F2} m/s, |w| max {_maxTorsoSpin:F2} rad/s, props |v| max {_maxPropSpeed:F2} m/s, torso err {_torso.Body.GlobalPosition.DistanceTo(_torso.TargetPosition):F3} m");
|
||||
else
|
||||
_results.Add($"COLLAPSE: [PASS] {s.Name}: yaw error max after settle {_maxYawErrDeg:F3} deg");
|
||||
NextScenario();
|
||||
}
|
||||
}
|
||||
|
||||
private void Monitor(Scenario s)
|
||||
{
|
||||
RigidBody3D torso = _torso.Body;
|
||||
Check("torso", torso);
|
||||
Check("handL", _hands[0]);
|
||||
Check("handR", _hands[1]);
|
||||
if (_anvil != null)
|
||||
Check("anvil", _anvil);
|
||||
foreach (Node n in GetTree().GetNodesInGroup("prop"))
|
||||
{
|
||||
if (n is not RigidBody3D prop || prop.IsQueuedForDeletion())
|
||||
continue;
|
||||
Check($"prop '{prop.Name}'", prop);
|
||||
_maxPropSpeed = Mathf.Max(_maxPropSpeed, prop.LinearVelocity.Length());
|
||||
if (prop.LinearVelocity.Length() > RunawaySpeed)
|
||||
Fail($"prop '{prop.Name}' runaway {prop.LinearVelocity.Length():F1} m/s");
|
||||
}
|
||||
if (_failed)
|
||||
return;
|
||||
_maxTorsoSpeed = Mathf.Max(_maxTorsoSpeed, torso.LinearVelocity.Length());
|
||||
_maxTorsoSpin = Mathf.Max(_maxTorsoSpin, torso.AngularVelocity.Length());
|
||||
if (torso.LinearVelocity.Length() > RunawaySpeed || (_anvil != null && _anvil.LinearVelocity.Length() > RunawaySpeed))
|
||||
Fail($"runaway speed (torso {torso.LinearVelocity.Length():F1}, anvil {_anvil?.LinearVelocity.Length() ?? 0f:F1})");
|
||||
if (s.Still && _t > 1.5f)
|
||||
{
|
||||
Vector3 bodyFwd = -torso.GlobalTransform.Basis.Z;
|
||||
Vector3 headFwd = -_head.GlobalTransform.Basis.Z;
|
||||
float err = Mathf.RadToDeg(Mathf.Atan2(bodyFwd.Cross(headFwd).Y, bodyFwd.Dot(headFwd)));
|
||||
_maxYawErrDeg = Mathf.Max(_maxYawErrDeg, Mathf.Abs(err));
|
||||
}
|
||||
}
|
||||
|
||||
private void Check(string name, RigidBody3D body)
|
||||
{
|
||||
if (!body.GlobalTransform.IsFinite() || !body.LinearVelocity.IsFinite() || !body.AngularVelocity.IsFinite())
|
||||
Fail($"{name} non-finite: pos {body.GlobalPosition} v {body.LinearVelocity} w {body.AngularVelocity}");
|
||||
}
|
||||
|
||||
private void Fail(string why)
|
||||
{
|
||||
if (_failed)
|
||||
return;
|
||||
_failed = true;
|
||||
GD.Print($"COLLAPSE: FIRST ANOMALY in '{_scenarios[_index].Name}' t={_t:F3}s step {_step}: {why}");
|
||||
}
|
||||
|
||||
private void Once(string key, bool cond, System.Action action)
|
||||
{
|
||||
if (cond && _done.Add(key))
|
||||
action();
|
||||
}
|
||||
|
||||
private void Grab(int h)
|
||||
{
|
||||
RigidBody3D hand = _hands[h];
|
||||
Vector3 pivot = hand.GlobalPosition + hand.GlobalTransform.Basis * new Vector3(h == 0 ? PalmGrabOffset : -PalmGrabOffset, 0f, 0f);
|
||||
ConeTwistJoint3D weld = new()
|
||||
{
|
||||
ExcludeNodesFromCollision = true,
|
||||
Position = pivot,
|
||||
NodeA = hand.GetPath(),
|
||||
NodeB = _anvil.GetPath(),
|
||||
SwingSpan = 0f,
|
||||
TwistSpan = 0f,
|
||||
Bias = 0f,
|
||||
};
|
||||
_world.AddChild(weld);
|
||||
_welds[h] = weld;
|
||||
}
|
||||
|
||||
private void Release(int h)
|
||||
{
|
||||
_welds[h]?.QueueFree();
|
||||
_welds[h] = null;
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user