Segmentation fault due to env.Reset not invalidating some cache

[Puttichai]

The following snippet causes segmentation fault on the third iteration.

for i in range(10):
    print(i)
    # Need at least 2 kinbodies in the environment
    box0 = RaveCreateKinBody(env, ''); box0.InitFromBoxes(array([[0, 0, 0, 0.1, 0.1, 0.1]])); box0.SetName('box0'); env.Add(box0)
    box1 = RaveCreateKinBody(env, ''); box1.InitFromBoxes(array([[0, 0, 0, 0.1, 0.1, 0.1]])); box1.SetName('box1'); env.Add(box1)
    env.CheckCollision(box0)
    env.Reset()

In Environment::Reset function, we iterate through all kinbodies in the environment, set their _environmentBodyIndex to 0, Destroy them, and then call _CallBodyCallbacks. However, as Environment::_InvalidateKinBodyFromEnvBodyIndex is not called, the function RemoveKinBody of the collision checker is never called and the cache is not invalidated.

Maybe there are at least a couple of ways to fix this:

• change from manually destroying kinbodies and calling their registered callbacks to just calling this->Remove(pbody) instead.
• add a call to _InvalidateKinBodyFromEnvBodyIndex in the for-loop that iterates through _vecbodies.

cc @kanbouchou