calling removeBody on a soft body causes OpenGL renderer to behave incorrectly

[ycheng517]

Pre-condition: a soft body and a rigid body with multiple links are loaded in the scene
Action: call removeBody on the soft body, and then reload the soft body with loadSoftBody.
Observed behaviour: the following erroneous behaviour can be observed in the GUI and by calling getCameraImage with ER_BULLET_HARDWARE_OPENGL. Calling getCameraImage without ER_BULLET_HARDWARE_OPENGL works fine.

1. after removing the soft body, the soft body remains in the scene. However one of the links on the rigid body is removed. The link number matches the id of the soft body.
2. after reloading the soft body, it spawns at an unexpected location, and it seems to interact with other objects as if those objects had shifted in the scene as well.

I've attached a script that can be used to reproduce this in the GUI, and videos rendered with and without the OpenGL renderer.

Erroneous behaviour rendered using OpenGL:
https://user-images.githubusercontent.com/9167852/103237272-1e836300-4915-11eb-9c8b-709dac3569ca.mp4

Correct behaviour rendered using not OpenGL:
https://user-images.githubusercontent.com/9167852/103237271-1deacc80-4915-11eb-84ea-fb83f3cb83eb.mp4

repro code:

import pybullet as p
import pybullet_data

def step_5_seconds():
    for i in range(240 * 5):
        p.stepSimulation()

def add_deformable_ball():
    return p.loadSoftBody(
        "ball.obj", 
        simFileName = "ball.vtk", 
        basePosition = [0,0,2],
        scale = 0.5, 
        mass = 4, 
        useNeoHookean = 1, 
        NeoHookeanMu = 400, 
        NeoHookeanLambda = 600, 
        NeoHookeanDamping = 0.001, 
        useSelfCollision = 1, 
        frictionCoeff = .5, 
        collisionMargin = 0.01
    )

physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.setGravity(0, 0, -10)
p.setTimeStep(1/240.)

planeId = p.loadURDF("plane.urdf", [0, 0, -2])
boxId = p.loadURDF("/kuka_iiwa/model.urdf", [1, 1, -2])
ballId = add_deformable_ball()
step_5_seconds()

p.removeBody(ballId)
step_5_seconds()

ballId = add_deformable_ball()
step_5_seconds()