Real-time replanning using 3D environment for humanoid robot