Topic/fix pg initialization

This pull request fix some issues with PG initialization.

In particular, on the current version, if you do:
robot.pg.parseCmd(":stepseq 0 0.095 0 0 -0.19 20 0 0.19 20 0 -0.19 20 0 0.19 20 0 -0.19 0 0 0.19 0");
wait for the robot to complete the movement then repeat the command, you get a discontinuity that results in erratic behaviour. Similarly, trying to start Herdt's PG after this command will fail.

This pull request fixes that on the condition that you call:
robot.pg.setInitByRealState(True);
robot.pg.initState();