Dirty hack to restart Herdt's PG
Put the CoM at the center of the feet when stopping
Set running value properly for Herdt's PG