Fix waist initialization in pg entity

Initialize waist position from real robot state when InitState is called.
Merge pull request #7 from gergondet/topic/fixWaistInitialization

Fix waist initialization in pg entity