Task Planning and Environment Recognition by Sensor Fusion for a Humanoid Robot