From 545f867d4d512c6686e787ab0b8a99ffe7a93111 Mon Sep 17 00:00:00 2001 From: Puttichai Date: Tue, 26 May 2026 11:00:53 +0900 Subject: [PATCH 1/2] fix: stop link transform drift on repeated SetDOFValues with bodyTransform (cherry picked from commit e1882aa233077885c27a4cff4163690124e6a79c) --- src/libopenrave/kinbody.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 65924c9b79..1820e59cb3 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -2193,15 +2193,16 @@ void KinBody::SetDOFValues(const std::vector& vJointValues, const Transfo if( _veclinks.size() == 0 ) { return; } - Transform baseLinkTransform = bodyTransform * _baseLinkInBodyTransform; - Transform tbase = baseLinkTransform*_veclinks.at(0)->GetTransform().inverse(); - _veclinks.at(0)->SetTransform(baseLinkTransform); - - // apply the relative transformation to all links!! (needed for passive joints) - for(size_t i = 1; i < _veclinks.size(); ++i) { - _veclinks[i]->SetTransform(tbase*_veclinks[i]->GetTransform()); - } - SetDOFValues(vJointValues,checklimits); + // Reuse the SetTransform loop instead of setting link[0] directly and propagating a relative + // transform to link[i>0]. The direct set lets the floating-point error in + // baseLinkTransform * link[0].GetTransform().inverse() be the same on every call, so the + // non-root link transforms drift by that same epsilon each call and accumulate. Applying + // tapply to every link (including link[0]) lets link[0] absorb the same epsilon, so the next + // call's tapply is approximately (identity - epsilon) and the drift cancels rather than + // compounds. 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. + _SetTransformNoPostProcess(bodyTransform); + SetDOFValues(vJointValues, checklimits); } void KinBody::SetDOFValues(const std::vector& vJointValues, uint32_t checklimits, const std::vector& dofindices) From 3e180f4ec036aca5e1a319f4ef1c3ca69fc129db Mon Sep 17 00:00:00 2001 From: Puttichai Date: Mon, 15 Jun 2026 17:22:32 +0900 Subject: [PATCH 2/2] chore: improve comment --- src/libopenrave/kinbody.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 1820e59cb3..2062b5aed4 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -2193,14 +2193,13 @@ void KinBody::SetDOFValues(const std::vector& vJointValues, const Transfo if( _veclinks.size() == 0 ) { return; } - // Reuse the SetTransform loop instead of setting link[0] directly and propagating a relative - // transform to link[i>0]. The direct set lets the floating-point error in - // baseLinkTransform * link[0].GetTransform().inverse() be the same on every call, so the - // non-root link transforms drift by that same epsilon each call and accumulate. Applying - // tapply to every link (including link[0]) lets link[0] absorb the same epsilon, so the next - // call's tapply is approximately (identity - epsilon) and the drift cancels rather than - // compounds. 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. + // _SetTransformNoPostProcess applies tapply = baseLinkTransform * link[0].inverse() to every + // link, including link[0] itself. Routing link[0] through tapply (rather than assigning it + // baseLinkTransform exactly) makes link[0] absorb the same floating-point epsilon as the other + // links, so on a repeated identical call tapply is approximately (identity - epsilon) and the + // round-trip drift cancels instead of accumulating across calls. For DOF>0 bodies the + // SetDOFValues call below recomputes link[i>0] via forward kinematics, so this matters mainly + // for 0-DOF multi-link bodies. _SetTransformNoPostProcess(bodyTransform); SetDOFValues(vJointValues, checklimits); }