Issue converting Rigidbody physics to Physicsbody physics

[iron sage]

Hi could anyone help me with a physics issue? I'm currently in the process of migrating some code that worked with game objects and rigid bodies over to ECS and the Unity.Physics package but so far I'm having issues because the ECS physics body seems to be behaving slightly differently to a normal Rigidbody.

The video attached shows the issue I'm having with a PD controller script I wrote. The 2 cubes (representing the player's hands) should rotate with the input from the controller, and they do until the rotation reaches about 90 degrees on any axis at which point they wobble like crazy.

https://youtu.be/5D2bN7xL5us?si=6h1a7wJxGG9QzF_a&t=755 this is the video I'm taking the code from which I've tested works fine with rigid bodies. The only difference I can see between the rigid body version and the physics body version is how I'm applying the torque. With Rigidbodies I would do this rb.AddTorque(torque, ForceMode.Acceleration and with ECS physics body I do velocity.ValueRW.Angular += torque * deltaTime

Here is my code for the PD controller:

 public Vector3 CalculatePIDRotation(
    float frequency,
    float damping,
    Quaternion currentRotation,
    Quaternion targetRotation,
    Vector3 currentAngularVelocity,
    float deltaTime)
    {
      float kp = Mathf.Pow(6f * frequency, 2) * 0.25f;
      float kd = 4.5f * frequency * damping;
      float g = 1 / (1 + kd * deltaTime + kp * deltaTime * deltaTime);
      float ksg = kp * g;
      float kdg = (kd + kp * deltaTime) * g;
      
      Quaternion q = targetRotation * Quaternion.Inverse(currentRotation);
      if (q.w < 0)
      {
        q.x = -q.x;
        q.y = -q.y;
        q.z = -q.z;
        q.w = -q.w;
      }
      
      q.ToAngleAxis(out float angle, out Vector3 axis);
      axis.Normalize();
      axis *= Mathf.Deg2Rad;
      Vector3 torque = ksg * axis * angle + -currentAngularVelocity * kdg;
      return torque;
    }

And here is my physics system for applying the torque:

  [UpdateInGroup(typeof(PhysicsSystemGroup))]
  [UpdateBefore(typeof(PhysicsInitializeGroup))]
  public partial class PredictedPhysicalControllerSystem : SystemBase
  {
    private IPIDService _pidService;

    protected override void OnCreate()
    {
      EntityQueryBuilder entityQuery = new EntityQueryBuilder(Allocator.Temp)
        .WithAll<
          PhysicsVelocity,
          LocalTransform,
          PhysicalControllerComponent,
          PlayerRigReferenceComponent,
          Simulate>();

      RequireForUpdate<NetworkTime>();
      RequireForUpdate(entityQuery.Build(this));
      _pidService = new PIDService();
    }

    protected override void OnUpdate()
    {
      float deltaTime = SystemAPI.Time.fixedDeltaTime;
      NetworkTime networkTime = SystemAPI.GetSingleton<NetworkTime>();
      NetworkTick tick = networkTime.ServerTick;

      foreach (var (velocity, transform, controller, playerReference) in SystemAPI
                 .Query<
                   RefRW<PhysicsVelocity>,
                   RefRW<LocalTransform>,
                   RefRO<PhysicalControllerComponent>,
                   RefRO<PlayerRigReferenceComponent>>()
                 .WithAll<Simulate>())
      {
        GetTargetData(
          tick,
          playerReference.ValueRO,
          controller.ValueRO,
          out float3 targetPosition,
          out quaternion targetRotation, // Returns the Quaternion rotation from my InputActionAsset that maps to the controller's rotation in local space
          out float3 targetLinearVelocity);

        float3 torque = _pidService.CalculatePIDRotation(
          controller.ValueRO.AngularFrequency,
          controller.ValueRO.AngularDamping,
          transform.ValueRO.Rotation,
          targetRotation,
          velocity.ValueRO.Angular,
          deltaTime);

        velocity.ValueRW.Angular += torque * deltaTime;
      }
    }
  }

Any help is very much appreciated. Thankyou. Also feel free to DM me