Add reference for SPD controller
Fix inertia of cubes