Reuse grabber state saver if grabber hasn't changed

[rschlaikjer]

In cases where we do a lot of grabbing with the same body that doesn't change and has a lot of links (e.g a mobile robot map), constantly creating new grabber state savers causes a lot of allocation churn / copying for the link transforms/velocities.

Instead, when grabbing, cache the grabber saver for reuse and invalidate on _PostprocessChangedParameters if any of the affected fields are altered. This noticeably improves environment sync / grab updates for large mobile robot scenes.

[rdiankov]

@snozawa @kanbouchou please review since people are starting to use this on site and getting in trouble. thanks

[rdiankov]

@snozawa @kanbouchou can you review this? Thanks

[kanbouchou]

@snozawa @kanbouchou can you review this? Thanks

Got it.

[snozawa]

@snozawa @kanbouchou can you review this? Thanks

got it~

[ziyan]

Ping @snozawa @kanbouchou

[snozawa]

@rschlaikjer

Thanks for great and interesting idea!

In the Grabbed class, for example, the followings saverOption is used:

    _CreateSaverForGrabbedAndGrabber(_pGrabberSaver,
                                     pGrabber,
                                     KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_JointLimits|KinBody::Save_LinkVelocities, // Need to save link velocities of the grabber since will be used for computing link velocities of the grabbed bodies.
                                     bDisableRestoreOnDestructor);

In the state saver, for example of linkEnable, the following KinBody API is used:

    if( _options & Save_LinkEnable ) {
        _vEnabledLinks.resize(_pbody->GetLinks().size());
        for(size_t i = 0; i < _vEnabledLinks.size(); ++i) {
            _vEnabledLinks[i] = _pbody->GetLinks().at(i)->IsEnabled();
        }
    }

In the KinBody::Enable to mutate such status, it using the folloring Prop

    if( bchanged ) {
        _PostprocessChangedParameters(Prop_LinkEnable);
    }

Therefore, this line should check Prop_LinkEnable as well.

Based on the similar observation, several other saver options are not well checked, and several other Prop options are not checked as well.

In addition, if I think about Save_LinkVelocities, I'm not sure SetDOFVelocities or SetLinkVelocities which is the setter function of relevant members are well maintained via Prop.

[snozawa]

Therefore, let's figure out what happens if we use saver options like KinBody::Save_LinkTransformation|KinBody::Save_LinkEnable|KinBody::Save_JointLimits|KinBody::Save_LinkVelocities.

• First, could you figure out which KinBody API and internal member variables are referred from KinBodyStateSaver by them?
• Next, could you figure out which Prop options can be used for them?
• Next, could you check if the necessary member variables are all managed by the given Prop option? for example, i'm bit questionable that LinkVelocities is managed by Prop.
• Finally, could you do the enough testing and write the enough test cases to internal repos?

Thanks a lot!

[kanbouchou]

In cases where we do a lot of grabbing with the same body that doesn't change and has a lot of links (e.g a mobile robot map), constantly creating new grabber state savers causes a lot of allocation churn / copying for the link transforms/velocities. Instead, when grabbing, cache the grabber saver for reuse and invalidate on _PostprocessChangedParameters if any of the affected fields are altered.

@rschlaikjer What is the computation time with production branch and this feature branch? Also, do you know how much of computation time in production branch is coming from memory (de)allocation? If memory resource is the dominant portion of the computation time, can we have a memory pool of grabbed state to avoid memory (de)allocation? Basically, request a memory chunk from pool in the constructor of Grabbed, and release it back to the pool in the destructor. This simplifies cache invalidation problem currently handled in _PostprocessChangedParameters. We would still perform computation of link transforms etc, so that part of computation cannot be saved.

With the current approach, I'm a bit concerned about "invalidate if any of the affected fields are altered." part, we can easily miss to check a small condition.

[snozawa]

I and @kanbouchou are not fully sure that what was the exact bottleneck and what kind of situation in that scene file.

but, i'm guessing now the situation and here is experimental alternative : #1480

(not tested nor verified yet)