Fix Frame Issue in mjCBody::AccumulateInertia #3027
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Fixes #2982
Issue
When fusestatic="true" is enabled, child body inertias are incorrectly accumulated when the child body has a rotation relative to the parent. The bug was introduced in commit 7932b4b.
The problem occurs in mjCBody::AccumulateInertia where the child body's inertial orientation is not properly transformed to the parent's coordinate frame before combining inertias.
Solution
Changed line 2266 in
src/user/user_objects.cc
to use the transformed quaternion other_iquat instead of the untransformed other->iquat.
Before:
double iquat[2][4] = {
{result->iquat[0], result->iquat[1], result->iquat[2], result->iquat[3]},
{other->iquat[0], other->iquat[1], other->iquat[2], other->iquat[3]} // Untransformed
};
After:
double iquat[2][4] = {
{result->iquat[0], result->iquat[1], result->iquat[2], result->iquat[3]},
{other_iquat[0], other_iquat[1], other_iquat[2], other_iquat[3]} // Properly transformed
};