Shoulder range of motion: drive-mode angular limits in the Box3D fork

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

scripts/SixDofSpike.cs

@@ -4,7 +4,8 @@ using Godot;
 
 // Headless regression for the forked addon's Generic6DOFJoint3D → Box3D mapping
 // (ball = b3SphericalJoint with cone/twist limits + torque-capped motor; drive = b3MotorJoint;
-// all-locked = weld). Run first whenever the addon binary changes.
+// all-locked = weld; drive + angular limits = b3MotorJoint with the fork's swing-ellipse/twist limits,
+// the shoulder). Run first whenever the addon binary changes.
 // godot-mono --headless --fixed-fps 90 --path . scenes/sixdof_spike.tscn
 public partial class SixDofSpike : Node3D
 {
@@ -18,6 +19,11 @@ public partial class SixDofSpike : Node3D
     private const float WeakMuscle = 1.5f;
     private const float MuscleKp = 20f;
     private const float MuscleMaxSpin = 8f;
+    // Limited drive (the shoulder): asymmetric swing bounds about joint Y / Z, twist about X.
+    private static readonly float LimLowerY = Mathf.DegToRad(-20f), LimUpperY = Mathf.DegToRad(60f);
+    private static readonly float LimLowerZ = Mathf.DegToRad(-40f), LimUpperZ = Mathf.DegToRad(100f);
+    private static readonly float LimTwist = Mathf.DegToRad(45f);
+    private const float SocketKp = 30f, SocketForce = 2000f;
 
     private readonly List<(string Name, Action<float> Tick, float Duration, Func<bool> Finish)> _phases = new();
     private int _phase = -1;
@@ -37,6 +43,9 @@ public partial class SixDofSpike : Node3D
         _phases.Add(("hand pushing into a wall pushes the anchor body away (reaction)", DriveBlockedTick, 1.5f, DriveBlockedFinish));
         _phases.Add(("all-locked config is rigid", WeldTick, 1.5f, WeldFinish));
         _phases.Add(("live flag toggling stays finite", ToggleTick, 1f, ToggleFinish));
+        _phases.Add(("limited drive holds the asymmetric swing ellipse + twist under hard torque", LimitedDriveTorqueTick, 4f, LimitedDriveTorqueFinish));
+        _phases.Add(("limited drive: muscle reaches targets inside the range, stops at the bound outside it", LimitedDriveMuscleTick, 6f, LimitedDriveMuscleFinish));
+        _phases.Add(("limited drive: the linear socket drive still moves the joint (shrug)", LimitedDriveSocketTick, 1.5f, LimitedDriveSocketFinish));
         Next();
     }
 
@@ -210,6 +219,39 @@ public partial class SixDofSpike : Node3D
                 _box = MakeBox(new Vector3(0.1f, 0.4f, 0.1f), 1f, new Vector3(0, 2.8f, 0));
                 _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j => ForAxes((f, p) => f(Generic6DofJoint3D.Flag.EnableAngularLimit, false), j));
                 break;
+            case 10:
+            case 11:
+            case 12:
+                // An upper arm (2 kg, 0.3 m) hanging from a static chest through a shoulder-style DRIVE:
+                // linear velocity motor = socket, angular velocity motor = muscle, angular limits on.
+                // Joint X points down the arm (twist axis), joint Y = world +X, joint Z = world +Z.
+                _anchor = MakeStatic(Vector3.One * 0.1f, new Vector3(0, 3, 0));
+                _box = MakeBox(new Vector3(0.08f, 0.3f, 0.08f), 2f, new Vector3(0, 2.85f, 0));
+                _joint = MakeJoint(_anchor, _box, new Vector3(0, 3, 0), j =>
+                {
+                    j.Basis = new Basis(Vector3.Forward, Mathf.Pi / 2f);
+                    ForAxes((f, p) =>
+                    {
+                        f(Generic6DofJoint3D.Flag.EnableLinearLimit, false);
+                        f(Generic6DofJoint3D.Flag.EnableAngularLimit, true);
+                        f(Generic6DofJoint3D.Flag.EnableLinearMotor, true);
+                        f(Generic6DofJoint3D.Flag.EnableMotor, true);
+                        p(Generic6DofJoint3D.Param.LinearMotorForceLimit, SocketForce);
+                        p(Generic6DofJoint3D.Param.AngularMotorForceLimit, phase == 11 ? 30f : 0.5f);
+                    }, j);
+                    j.SetParamX(Generic6DofJoint3D.Param.AngularLowerLimit, -LimTwist);
+                    j.SetParamX(Generic6DofJoint3D.Param.AngularUpperLimit, LimTwist);
+                    j.SetParamY(Generic6DofJoint3D.Param.AngularLowerLimit, LimLowerY);
+                    j.SetParamY(Generic6DofJoint3D.Param.AngularUpperLimit, LimUpperY);
+                    j.SetParamZ(Generic6DofJoint3D.Param.AngularLowerLimit, LimLowerZ);
+                    j.SetParamZ(Generic6DofJoint3D.Param.AngularUpperLimit, LimUpperZ);
+                });
+                _overshootDeg = -999f;
+                _maxTwist = 0f;
+                _reach = Vector4.Zero;
+                _muscleStage = -1;
+                _stageErr.Clear();
+                break;
         }
     }
 
@@ -358,6 +400,146 @@ public partial class SixDofSpike : Node3D
         return _finite && _maxSwing < 2f && _maxPivotSep < PivotLimit;
     }
 
+    // ---- limited drive (shoulder) ----
+    private float _overshootDeg;
+    private Vector4 _reach;      // max +Y, max -Y, max +Z, max -Z swing components reached (deg)
+    private int _muscleStage;
+    private readonly List<string> _stageErr = new();
+    private bool _stagesOk = true;
+
+    // Swing rotation vector (joint Y/Z components, rad) taking frame A's X onto frame B's X, and the
+    // remaining twist about X (rad).
+    private void SwingTwist(out Vector2 swing, out float twist)
+    {
+        Quaternion rel = Rel();
+        Vector3 b = rel * Vector3.Right;
+        float angle = Vector3.Right.AngleTo(b);
+        Vector3 axis = Vector3.Right.Cross(b);
+        swing = axis.LengthSquared() > 1e-12f ? new Vector2(axis.Y, axis.Z).Normalized() * angle : Vector2.Zero;
+        Quaternion swingQ = axis.LengthSquared() > 1e-12f ? new Quaternion(axis.Normalized(), angle) : Quaternion.Identity;
+        Quaternion t = swingQ.Inverse() * rel;
+        twist = 2f * Mathf.Atan2(t.X, t.W);
+        if (twist > Mathf.Pi) twist -= Mathf.Tau;
+        if (twist < -Mathf.Pi) twist += Mathf.Tau;
+    }
+
+    // Radius of the quadrant ellipse in the swing direction (same formula as the fork).
+    private static float SwingLimit(Vector2 dir)
+    {
+        float a = dir.X >= 0f ? LimUpperY : -LimLowerY, b = dir.Y >= 0f ? LimUpperZ : -LimLowerZ;
+        float u = dir.X / a, v = dir.Y / b;
+        return 1f / Mathf.Sqrt(u * u + v * v);
+    }
+
+    // Socket: linear velocity motor toward the pivot (+ a moving offset), like PlayerRagdoll.ShoulderDrive.
+    private void SocketDrive(Vector3 offset)
+    {
+        Vector3 err = _pivotWorld + offset - _box.GlobalTransform * _pivotLocalB;
+        Vector3 v = FrameA().Inverse() * (err * SocketKp);
+        _joint.SetParamX(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.X);
+        _joint.SetParamY(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Y);
+        _joint.SetParamZ(Generic6DofJoint3D.Param.LinearMotorTargetVelocity, v.Z);
+    }
+
+    private void Muscle(Quaternion target)
+    {
+        Vector3 err = BodyMath.RotationError(target, Rel());
+        Vector3 w = (err * MuscleKp).LimitLength(MuscleMaxSpin);
+        _joint.SetParamX(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.X);
+        _joint.SetParamY(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Y);
+        _joint.SetParamZ(Generic6DofJoint3D.Param.AngularMotorTargetVelocity, w.Z);
+    }
+
+    private void TrackLimits()
+    {
+        TrackFinite();
+        SwingTwist(out Vector2 s, out float tw);
+        if (s.Length() > Mathf.DegToRad(2f))
+            _overshootDeg = Mathf.Max(_overshootDeg, Mathf.RadToDeg(s.Length() - SwingLimit(s.Normalized())));
+        _maxTwist = Mathf.Max(_maxTwist, Mathf.RadToDeg(Mathf.Abs(tw)));
+        _reach = new Vector4(Mathf.Max(_reach.X, Mathf.RadToDeg(s.X)), Mathf.Max(_reach.Y, -Mathf.RadToDeg(s.X)),
+            Mathf.Max(_reach.Z, Mathf.RadToDeg(s.Y)), Mathf.Max(_reach.W, -Mathf.RadToDeg(s.Y)));
+        _maxPivotSep = Mathf.Max(_maxPivotSep, PivotSep());
+    }
+
+    private void LimitedDriveTorqueTick(float dt)
+    {
+        SocketDrive(Vector3.Zero);
+        Muscle(Rel());  // weak (0.5 N·m) muscle holding nothing in particular
+        // A hard torque sweeping all swing directions (joint Y/Z) plus an alternating twist torque.
+        float phi = _t * 3f;
+        Vector3 local = new Vector3(6f * Mathf.Sign(Mathf.Sin(_t * 5f)), 25f * Mathf.Cos(phi), 25f * Mathf.Sin(phi));
+        _box.ApplyTorque(FrameA() * local);
+        TrackLimits();
+    }
+
+    private bool LimitedDriveTorqueFinish()
+    {
+        GD.Print($"6DOF:   25 N·m sweep: swing overshoot max {_overshootDeg:F1} deg (limit {LimitToleranceDeg}); reached +Y {_reach.X:F0}/60, -Y {_reach.Y:F0}/20, +Z {_reach.Z:F0}/100, -Z {_reach.W:F0}/40 deg; twist max {_maxTwist:F1} deg (limit 45); socket {_maxPivotSep:F4} m");
+        bool reached = _reach.X > 50f && _reach.Y > 14f && _reach.Z > 85f && _reach.W > 32f;
+        return _finite && _overshootDeg < LimitToleranceDeg && reached && _maxTwist < Mathf.RadToDeg(LimTwist) + LimitToleranceDeg && _maxPivotSep < PivotLimit;
+    }
+
+    // Targets (swing Y, swing Z, twist, deg): inside, inside, inside, then one beyond the +Y bound.
+    private static readonly Vector3[] MuscleTargets = { new(40f, 0f, 0f), new(0f, 80f, 0f), new(-15f, -30f, 30f), new(85f, 0f, 0f) };
+
+    private static Quaternion SwingTwistQuat(Vector3 deg)
+    {
+        Vector3 rv = new(0f, Mathf.DegToRad(deg.X), Mathf.DegToRad(deg.Y));
+        Quaternion swing = rv.LengthSquared() > 1e-12f ? new Quaternion(rv.Normalized(), rv.Length()) : Quaternion.Identity;
+        return swing * new Quaternion(Vector3.Right, Mathf.DegToRad(deg.Z));
+    }
+
+    private void LimitedDriveMuscleTick(float dt)
+    {
+        SocketDrive(Vector3.Zero);
+        int stage = Mathf.Min((int)(_t / 1.5f), MuscleTargets.Length - 1);
+        Quaternion target = SwingTwistQuat(MuscleTargets[stage]);
+        Muscle(target);
+        TrackLimits();
+        if (_t >= (stage + 1) * 1.5f - dt * 1.5f && _muscleStage < stage)
+        {
+            _muscleStage = stage;
+            SwingTwist(out Vector2 s, out float tw);
+            Vector3 got = new(Mathf.RadToDeg(s.X), Mathf.RadToDeg(s.Y), Mathf.RadToDeg(tw));
+            if (stage < 3)
+            {
+                float err = Mathf.RadToDeg(BodyMath.RotationError(target, Rel()).Length());
+                _stageErr.Add($"target {MuscleTargets[stage]} → {got.X:F0},{got.Y:F0},{got.Z:F0} err {err:F1}");
+                _stagesOk &= err < 3f;
+            }
+            else
+            {
+                _stageErr.Add($"target {MuscleTargets[stage]} (beyond +Y 60) → swing Y {got.X:F1}");
+                _stagesOk &= got.X < 60f + LimitToleranceDeg && got.X > 55f;
+            }
+        }
+    }
+
+    private bool LimitedDriveMuscleFinish()
+    {
+        GD.Print($"6DOF:   30 N·m muscle: {string.Join("; ", _stageErr)}; overshoot max {_overshootDeg:F1} deg; socket {_maxPivotSep:F4} m");
+        return _finite && _stagesOk && _stageErr.Count == MuscleTargets.Length && _overshootDeg < LimitToleranceDeg && _maxPivotSep < PivotLimit;
+    }
+
+    private Vector3 ShrugOffset => new Vector3(0f, 0.05f, -0.03f) * Mathf.Min(_t / 0.5f, 1f);
+
+    private void LimitedDriveSocketTick(float dt)
+    {
+        SocketDrive(ShrugOffset);
+        Muscle(Quaternion.Identity);
+        TrackFinite();
+        SwingTwist(out Vector2 s, out _);
+        _maxSwing = Mathf.Max(_maxSwing, Mathf.RadToDeg(s.Length()));
+    }
+
+    private bool LimitedDriveSocketFinish()
+    {
+        float err = (_box.GlobalTransform * _pivotLocalB).DistanceTo(_pivotWorld + ShrugOffset);
+        GD.Print($"6DOF:   socket moved {ShrugOffset.Length():F3} m: tracking error {err:F4} m (limit 0.005), arm swing max {_maxSwing:F1} deg");
+        return _finite && err < 0.005f && _maxSwing < 5f;
+    }
+
     private int _toggles;
 
     private void ToggleTick(float dt)