Stop link transform drift on repeated SetDOFValues with bodyTransform

[Puttichai]

Summary

KinBody::SetDOFValues(joints, bodyTransform, checklimits) previously set link[0] to baseLinkTransform directly and then propagated a relative tbase = baseLinkTransform * link[0].inverse() to link[i > 0]. Because link[0] was always exact, tbase carried the same FP epsilon on every call and that epsilon was re-applied to link[i > 0] each time, so link transforms drift on repeated identical calls, including the no-op case where the same values are passed back in.

This affects callers that round-trip configuration values for a body, notably SetConfigurationValues and any environment-synchronization path that mirrors a body's state into a clone every cycle. The drift is small per call but compounds.

The fix delegates to _SetTransformNoPostProcess, which applies the same tapply to all links including link[0]. Link[0] then absorbs the same epsilon as the other links, so on the next call tapply ~ I − epsilon and the drift cancels rather than compounds.

Notes

• For DOF > 0 bodies, link[0] is no longer set to baseLinkTransform exactly (it now goes through tapply * link[0]_old, picking up one ULP). The error is bounded (that is, it does not compound) and the subsequent SetDOFValues(vJointValues, checklimits) recomputes link[i>0] via forward kinematics.
• No API change. No behavior change for single-link bodies beyond the bounded one-ULP shift noted above.

Related links

• Unit tests https://tiny.mujin.co.jp/e4vee
• Issue page https://tiny.mujin.co.jp/iqggf

[kanbouchou]

// For DOF>0 bodies the subsequent SetDOFValues recomputes link transforms via FK and overwrites this loop's output, so the change only affects 0-DOF multi-link bodies.

I think "the change" may make sense in the PR context where we are seeing old and new code side-by-side. However, once merged into production, this comment may not be clear what "the cahnge" refers to. Can you rephrase to something which makes sense without knowing the old code?

[Puttichai]

Thank you for checking. I updated the comment.

[kanbouchou]

Thanks, looks good to me.

[rdiankov]

To tell you the truth, it looks like _SetTransformNoPostProcess has the drift problem since now links[0] essentially get assigned (baseLinkTransform * links[0].transform.inverse() * links[0].transform), rather than just baseLinkTransform.
Something is weird here.. do you have an example of this in action?

[Puttichai]

@rdiankov Yes, and that's also noted in the description.

do you have an example of this in action?

Yes. You can find an example on the issue page https://tiny.mujin.co.jp/iqggf
Another working example would be the test case itself (https://tiny.mujin.co.jp/e4vee)