[RoboX2020]

Problem

When a body is removed after saveState, its multibody slot is freed and can be reused by a newly loaded body. restoreState validates the snapshot against the existing world using only the multibody count and per-body link counts, so a different body reusing the freed slot passes validation when its link count happens to match. The restore then silently overwrites the new body's multibody state (mass, pose, velocities) with the saved body's state, losing the new body without any error.

Reproduced in DIRECT mode:

import pybullet as p, pybullet_data
p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
cube = p.loadURDF("cube.urdf", [0, 0, 0])
state = p.saveState()
p.removeBody(cube)
sphere = p.loadURDF("sphere2.urdf", [5, 0, 0])  # reuses the freed slot
p.restoreState(state)  # succeeds silently
print(p.getBasePositionAndOrientation(sphere)[0])  # (0, 0, 0): the sphere's state is gone

Without a removal in between, a body count mismatch makes restoreState fail with an error, so the silent overwrite is inconsistent with the existing failure mode.

Fix

Record the collider shape types of each multibody slot (base collider and link colliders) in SaveStateData when a state is saved, and refuse a restore whose slots no longer match the current world, reporting CMD_RESTORE_STATE_FAILED with a warning instead of corrupting state. The check compares shape types, so it rejects reuse by a differently shaped body while still allowing a restore onto an identically shaped body.

Tests

• New regression test in saveRestoreStateTest.py: saves state, removes a body, loads a differently shaped body into the freed slot and requires restoreState to raise pybullet.error and leave the world untouched. The test fails at baseline and passes with the fix.
• Verified in DIRECT mode: the new test plus the existing saveRestoreStateTest.py, unittests.py and userDataTest.py suites all pass.

Scope

• The check compares collider shape types, not object identity: reuse by an identically shaped body is still allowed and the restore proceeds onto it.
• restoreState from a .bullet file has the same validation gap, but pybullet's own saveBullet output does not serialize multibody collider shape data to validate against, so the file path is not covered here.