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!

[rschlaikjer]

Therefore, this line should check Prop_LinkEnable as well.

The Prop_Links mask matches all of the Prop_Link* subfields, so this will match for transform or enable.

I think the only thing that is not handled by the current save option is the velocities, but looking at it now I'm not sure why those are being saved at all? It doesn't look like calculating the self-colliding links cares about velocity? If it doesn't, then we can just cut that instead of plumbing invalidation for it.

[snozawa]

1. as for Prop_LinkEanble, you are right. it's included in Prop_Links.
2. Prop_LinkTransforms seems not included in Prop_Links (could you double-check this?)
3. As for LinkVelocities, i agree with you. I don't know why it's saved to saver, and thus, Grabbed needed to differentiate the behavior.

[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)

[rschlaikjer]

@rschlaikjer What is the computation time with production branch and this feature branch?

This feature branch results in a ~7x speedup to KinBody::ResetGrabbed for the scene that we are attempting to optimize, which works out to maybe 30ms on average as part of the joint log module update.

Also, do you know how much of computation time in production branch is coming from memory (de)allocation?

About half of the benefit is allocation, the other half is the cost of copying all of the transforms/velocities.

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.

Since we already have the Prop* based notification mechanism, it makes sense to me to take advantage of it - there's already a single point that events pass through.

[snozawa]

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.

Since we already have the Prop* based notification mechanism, it makes sense to me to take advantage of it - there's already a single point that events pass through.

I'm not sure Prop works perfectly.
For example, Prop_LinkVelocities does not looks working.
Need more time to investigate other Prop* flags.

This PR's approach is making a cache.
If we can come up with the cache-less optimization method, it would be simpler and safer for me.

#1480 is just an example of it.
If the saver in Grabbed is theoretically unnecessary, we don't need to compute it.
so, i'm interested in the nature of the target scene file and how correctly the target scene file is modeled.
(mentioned in the internal issue ticket on this topic)