Fix issue when pose optimization is used and no legs are grounded.

[pfankhauser]

[remod]

Crashes, if no robot state is received.

[FGiraldez]

@remod did #39 fix it?

[remod]

Is this a visualization issue?

[pfankhauser]

If I remember correctly this can be a visualization or real issue on the robot. It's about making things robust and not letting e.g. the pose optimizer crash in no-contact situations, but to handle the situation better.