Planning and Control Strategies for Motion and Interaction of the Humanoid Robot COMAN+