perf(kinbody): skip redundant GetDOFValues recompute in SetDOFValues#1564
perf(kinbody): skip redundant GetDOFValues recompute in SetDOFValues#1564Puttichai wants to merge 11 commits into
Conversation
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.
…values_bookkeeping_2_20260609
…values_bookkeeping_2_20260609
…values_bookkeeping_2_20260609
| // 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); |
There was a problem hiding this comment.
perhaps only resize if bHasNAN?
There was a problem hiding this comment.
I thought about it too but _vTempJoints may also be used under if( checklimits != CLA_Nothing ) below. I added a comment here to clarify.
| ORE_InvalidArguments); | ||
|
|
||
| GetDOFValues(_vTempJoints); | ||
| if( dofindices.size() > 0 ) { |
There was a problem hiding this comment.
@Puttichai I wonder if there could be another fast path where dofindices.size() == expecteddof, and dofinidces contains all the indices of the robot (even if they are in a different order).
There was a problem hiding this comment.
I see. Thank you for the suggestion. I made a change to skip GetDOFValues when dofindices already covers all dofs. Also updated test cases.
| // 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) { |
There was a problem hiding this comment.
@Puttichai we should also becareful that dofindices doesn't have repeated indices. at the very least we have to assert if you don't think this is a valid input
2 participants
Description
KinBody::SetDOFValuesopened withGetDOFValues(_vTempJoints), which reconstructs every joint angle from the current link transforms (aquatMultiplychain +atan2per DOF — the inverse of the forward kinematics it is about to redo). In the common full-vector path every fetched value is then immediately overwritten by the input, so the recompute is pure waste.This fills
_vTempJointsfrom the input directly, and only callsGetDOFValueswhen an input isNaN(the "keep the current value" sentinel). Thedofindicesand NaN paths are unchanged.SetDOFValues~36% faster on average (median 38%, up to 53%)Unit test: https://tiny.mujin.co.jp/7f4na