Actually remove collision objects when clearing the collision environment

When clearing the collision environment, all objects actually need to be deleted from the cache. Since the object pose doesn't get updated when readding objects after they were removed but not deleted, the collision environment map cannot be updated. This should be fixed now.