Terrain mapping algorithm for motion planning and control in legged locomotion - View it on GitHub