Fix waist initialization in pg entity

Initialize waist position from real robot state when InitState is called.

(Note that there needs to be some patches on jrl-walkgen for this to work fully, patches are coming!)