Energy optimization for a robust and flexible interaction control