Fix waist initialization in pg entity

Initialize waist position from real robot state when InitState is called.