Real-time Replanning Using 3D Environment for Humanoid Robot