A method for terrain and constraint planning a step plan includes receiving, at data processing hardware of a robot, image data of an environment about the robot from at least one image sensor. The robot includes a body and legs. The method also includes generating, by the data processing hardware, a body-obstacle map, a ground height map, and a step-obstacle map based on the image data and generating, by the data processing hardware, a body path for movement of the body of the robot while maneuvering in the environment based on the body-obstacle map. The method also includes generating, by the data processing hardware, a step path for the legs of the robot while maneuvering in the environment based on the body path, the body-obstacle map, the ground height map, and the step-obstacle map.