From 975ecd67b8d7f4bb0bec73d6f8bb94fda056a41a Mon Sep 17 00:00:00 2001 From: Puttichai Date: Tue, 9 Jun 2026 18:26:29 +0900 Subject: [PATCH 1/6] perf(kinbody): skip redundant GetDOFValues recompute in SetDOFValues SetDOFValues opened with GetDOFValues(_vTempJoints), which reconstructs every joint value from the current link transforms (a quatMultiply chain + atan2 per DOF, i.e. the inverse of the forward kinematics it is about to perform). In the common path (full DOF vector, no NaN), every fetched value is then immediately overwritten by the input, so the recompute is pure waste. Fill _vTempJoints from the input directly in that path, and only call GetDOFValues when an input is NaN (the keep-current-value sentinel). The dofindices and NaN paths are unchanged. --- src/libopenrave/kinbody.cpp | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 65924c9b79..d2d2077852 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -2221,10 +2221,10 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim int expecteddof = dofindices.size() > 0 ? (int)dofindices.size() : GetDOF(); OPENRAVE_ASSERT_OP_FORMAT((int)dof,>=,expecteddof, "env=%s, body '%s' not enough values %d<%d", GetEnv()->GetNameId()%GetName()%dof%GetDOF(),ORE_InvalidArguments); - GetDOFValues(_vTempJoints); if( dofindices.size() > 0 ) { // user only set a certain number of indices, so have to fill the temporary array with the full set of values first // and then overwrite with the user set values + GetDOFValues(_vTempJoints); for(size_t i = 0; i < dofindices.size(); ++i) { if( !std::isnan(pJointValues[i]) ) { _vTempJoints.at(dofindices[i]) = pJointValues[i]; @@ -2232,8 +2232,29 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim } } else { - for(size_t i = 0; i < _vTempJoints.size(); ++i) { - if( !std::isnan(pJointValues[i]) ) { + // In the common full-vector path, the inputs are copied verbatim into _vTempJoints, so the + // current values fetched by GetDOFValues() would be fully overwritten. GetDOFValues() is expensive + // (it recomputes every joint angle from the link transforms - quatMultiply + atan2 per dof), so only + // call it when an input is NaN, which is the sentinel meaning "keep the current value for that dof". + const int ndof = GetDOF(); + _vTempJoints.resize(ndof); + bool bHasNaN = false; + for(int i = 0; i < ndof; ++i) { + if( std::isnan(pJointValues[i]) ) { + bHasNaN = true; + break; + } + } + if( bHasNaN ) { + GetDOFValues(_vTempJoints); + for(int i = 0; i < ndof; ++i) { + if( !std::isnan(pJointValues[i]) ) { + _vTempJoints[i] = pJointValues[i]; + } + } + } + else { + for(int i = 0; i < ndof; ++i) { _vTempJoints[i] = pJointValues[i]; } } From 5ef43a839efab4d1b9a877ce936d3b6aae6d07c5 Mon Sep 17 00:00:00 2001 From: Puttichai Date: Mon, 13 Jul 2026 11:11:22 +0900 Subject: [PATCH 2/6] chore: update comments --- src/libopenrave/kinbody.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index e8ac64f055..9bfc661bc0 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -2285,10 +2285,9 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim } } else { - // In the common full-vector path, the inputs are copied verbatim into _vTempJoints, so the - // current values fetched by GetDOFValues() would be fully overwritten. GetDOFValues() is expensive - // (it recomputes every joint angle from the link transforms - quatMultiply + atan2 per dof), so only - // call it when an input is NaN, which is the sentinel meaning "keep the current value for that dof". + // When setting values to all joints, the input pJointValues already holds every value so use it directly and + // skip the expensive GetDOFValues. Only when pJointValues contains NaN (meaning "keep the current value") do we + // fetch the current values and fill in the rest. const int ndof = GetDOF(); _vTempJoints.resize(ndof); bool bHasNaN = false; From caa1cc93fa42a1bfe61cbc58a09a4e9f72b01721 Mon Sep 17 00:00:00 2001 From: Puttichai Date: Mon, 13 Jul 2026 11:11:42 +0900 Subject: [PATCH 3/6] chore: reuse existing expecteddof --- src/libopenrave/kinbody.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 9bfc661bc0..71d250a458 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -2271,8 +2271,10 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim if( dof == 0 || _veclinks.size() == 0) { return; } - int expecteddof = dofindices.size() > 0 ? (int)dofindices.size() : GetDOF(); - OPENRAVE_ASSERT_OP_FORMAT((int)dof,>=,expecteddof, "env=%s, body '%s' not enough values %d<%d", GetEnv()->GetNameId()%GetName()%dof%GetDOF(),ORE_InvalidArguments); + const int expecteddof = dofindices.size() > 0 ? (int)dofindices.size() : GetDOF(); + OPENRAVE_ASSERT_OP_FORMAT((int)dof, >=, expecteddof, + "env=%s, body '%s' not enough values %d<%d", GetEnv()->GetNameId() % GetName() % dof % expecteddof, + ORE_InvalidArguments); if( dofindices.size() > 0 ) { // user only set a certain number of indices, so have to fill the temporary array with the full set of values first @@ -2288,10 +2290,9 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim // When setting values to all joints, the input pJointValues already holds every value so use it directly and // skip the expensive GetDOFValues. Only when pJointValues contains NaN (meaning "keep the current value") do we // fetch the current values and fill in the rest. - const int ndof = GetDOF(); - _vTempJoints.resize(ndof); + _vTempJoints.resize(expecteddof); bool bHasNaN = false; - for(int i = 0; i < ndof; ++i) { + for(int i = 0; i < expecteddof; ++i) { if( std::isnan(pJointValues[i]) ) { bHasNaN = true; break; @@ -2299,14 +2300,14 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim } if( bHasNaN ) { GetDOFValues(_vTempJoints); - for(int i = 0; i < ndof; ++i) { + for(int i = 0; i < expecteddof; ++i) { if( !std::isnan(pJointValues[i]) ) { _vTempJoints[i] = pJointValues[i]; } } } else { - for(int i = 0; i < ndof; ++i) { + for(int i = 0; i < expecteddof; ++i) { _vTempJoints[i] = pJointValues[i]; } } From 5b018d93d0db6fded0f8a8168782acdf11b76ef8 Mon Sep 17 00:00:00 2001 From: Puttichai Date: Mon, 13 Jul 2026 11:13:19 +0900 Subject: [PATCH 4/6] perf: avoid unnecessary setting of _vTempJoints in no-NaN path --- src/libopenrave/kinbody.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 71d250a458..36072b8f12 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -2285,6 +2285,7 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim _vTempJoints.at(dofindices[i]) = pJointValues[i]; } } + pJointValues = &_vTempJoints[0]; } else { // When setting values to all joints, the input pJointValues already holds every value so use it directly and @@ -2305,14 +2306,9 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim _vTempJoints[i] = pJointValues[i]; } } - } - else { - for(int i = 0; i < expecteddof; ++i) { - _vTempJoints[i] = pJointValues[i]; - } + pJointValues = &_vTempJoints[0]; } } - pJointValues = &_vTempJoints[0]; if( checklimits != CLA_Nothing ) { dReal* ptempjoints = &_vTempJoints[0]; From 3838d559bc8ef9ff6fdf9a14facb79021e6c9d00 Mon Sep 17 00:00:00 2001 From: Puttichai Date: Wed, 15 Jul 2026 09:39:18 +0900 Subject: [PATCH 5/6] chore: add clarification comment --- src/libopenrave/kinbody.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index bb0e764238..663be60e62 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -2291,7 +2291,7 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim // When setting values to all joints, the input pJointValues already holds every value so use it directly and // skip the expensive GetDOFValues. Only when pJointValues contains NaN (meaning "keep the current value") do we // fetch the current values and fill in the rest. - _vTempJoints.resize(expecteddof); + _vTempJoints.resize(expecteddof); // still need resizing because it may be used under checklimits != CLA_Nothing bool bHasNaN = false; for(int i = 0; i < expecteddof; ++i) { if( std::isnan(pJointValues[i]) ) { From 49e3d456ee5dcbfb691d2509453f3805b25c3e31 Mon Sep 17 00:00:00 2001 From: Puttichai Date: Wed, 15 Jul 2026 13:55:22 +0900 Subject: [PATCH 6/6] perf: skip GetDOFValues when the given dofindices covers all dofs --- src/libopenrave/kinbody.cpp | 37 ++++++++++++++++++++++++++++++++----- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp index 663be60e62..14906aee7c 100644 --- a/src/libopenrave/kinbody.cpp +++ b/src/libopenrave/kinbody.cpp @@ -2277,13 +2277,40 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim ORE_InvalidArguments); if( dofindices.size() > 0 ) { - // user only set a certain number of indices, so have to fill the temporary array with the full set of values first - // and then overwrite with the user set values - GetDOFValues(_vTempJoints); - for(size_t i = 0; i < dofindices.size(); ++i) { - if( !std::isnan(pJointValues[i]) ) { + // When dofindices covers every dof of the body and the input has no NaN, every element of _vTempJoints gets + // overwritten, so can skip the expensive GetDOFValues. To check the coverage, pre-fill _vTempJoints with NaN + // before writing the input values into it. If no NaN remains, all dofs were covered. + bool bNeedCurrentValues = ((int)dofindices.size() != GetDOF()); + if( !bNeedCurrentValues ) { + for(int i = 0; i < expecteddof; ++i) { + if( std::isnan(pJointValues[i]) ) { + bNeedCurrentValues = true; + break; + } + } + } + if( !bNeedCurrentValues ) { + _vTempJoints.assign(GetDOF(), std::numeric_limits::quiet_NaN()); + for(size_t i = 0; i < dofindices.size(); ++i) { _vTempJoints.at(dofindices[i]) = pJointValues[i]; } + for(const dReal dofvalue : _vTempJoints) { + if( std::isnan(dofvalue) ) { + // dofindices has duplicates, so some dof was not covered. Need current values after all. + bNeedCurrentValues = true; + break; + } + } + } + if( bNeedCurrentValues ) { + // user only set a certain number of indices, so have to fill the temporary array with the full set of values first + // and then overwrite with the user set values + GetDOFValues(_vTempJoints); + for(size_t i = 0; i < dofindices.size(); ++i) { + if( !std::isnan(pJointValues[i]) ) { + _vTempJoints.at(dofindices[i]) = pJointValues[i]; + } + } } pJointValues = &_vTempJoints[0]; }