A method for terrain and constraint planning a step plan includes receiving, at data processing hardware (36) of a robot (10), image data (17) of an environment (8) about the robot from at least one image sensor (31). The robot includes a body (11) and legs (12). The method also includes generating, by the data processing hardware, a body-obstacle map (112), a ground height map (116), and a step-obstacle map (114) based on the image data and generating, by the data processing hardware, a body path (510) 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 (350) 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.
展开▼
