diff --git a/sbpl_collision_checking/src/attached_bodies_collision_state.cpp b/sbpl_collision_checking/src/attached_bodies_collision_state.cpp
index 1b512326..7f311161 100644
--- a/sbpl_collision_checking/src/attached_bodies_collision_state.cpp
+++ b/sbpl_collision_checking/src/attached_bodies_collision_state.cpp
@@ -75,6 +75,10 @@ bool AttachedBodiesCollisionState::updateVoxelsState(int vsidx)
     }
 
     state.voxels = std::move(new_voxels);
+    
+    if (m_voxels_state_versions.size()<=vsidx)
+        m_voxels_state_versions.resize(vsidx+1);
+
     m_voxels_state_versions[vsidx] = attachedBodyTransformVersion(bidx);
     return true;
 }