demo locomotion

[mfocchi]

• add possibility to toggle ground truth and state estimator on the keyboard
• in ground truth mode I would add the map of the environment
• add a button to reset the simulation if the robot falls from stairs or better implement a self righting scheme if it is falling on back ( I can give you the code we used with "sandrone" https://www.dropbox.com/s/foouybfzp1ljde2/self-righting.avi) otherwise go to fold and standup again. For self righting I would do that with a state machine in joint space. The easiest is to give a lateral HAA push from one side legs with the legs on the other side completely folded, enough to roll over, as they did for ANYMAL. Otherwise we can do some more fancy stuff insired by anymals... https://www.youtube.com/watch?v=n3xUmW_8pSA&t=11s