Faulty ID result for QuaternionFloating joints

[lieskjur]

Hi, I came across a strange issue. If I connect a body who's moment of inertia is not expressed at it's center of mass using a QuaternionFloating joint the inverse dynamics calculation returns a faulty result.

I put together the simplest example I could think of for demonstration. Also tried an analogous test on the QuaternionSpherical joint which doesn't exhibit the same issue, which I find interesting.

I know this issue wouldn't generally arise as calculating joint torques for a floating joint is rather peculiar but I do believe I have a legitimate use-case. At the moment I know of some work-arounds if need be but I'll try searching for the cause myself once I'm done with core code for my thesis.