Add collision spheres to srdf