Fix body-link transform update

[superfashi]

Currently the body transform is incorrectly updated inside KinBody::UpdateFromKinBodyInfo due to an early change to _baseLinkInBodyTransform, which resulted in a wrong link-body transformation state. This resulted in body transform changed in an unexpected way. This MR fixes this issue.

[woswos]

Pipeline ID: 797811

[superfashi]

Before the update, $L = B \times BL$. However, $B = L \times BL^{-1}$ is actually recorded.

If we just update $B$ to be $B'$, we actually want to update $L' = B' \times BL = L \times BL^{-1}$.

However, if this line exists, the code preemptively updated the internal state of $BL$, so you lose the original $BL$ which you needed for the calculation of $L'$.

I can be fairly sure that originally this bug is discovered, and instead of fixing the right place, an extra Transform bodyTransform = info._transform * _invBaseLinkInBodyTransform; is introduced in L6399 to "fix" the calculation. However this fix fails to cover the other case when the transform of base link isn't identity.

[rdiankov]

THanks!