Intermediate waypoint generator

1. A computer-implemented method when executed by data processing hardware of a robot causes the data processing hardware to perform operations comprising:

receiving image data of an environment of the robot from an image sensor;

receiving a body obstacle map comprising a map of obstacles that a body of the robot cannot traverse;

generating a sparse graph based on the body obstacle map, the sparse graph comprising a set of nodes and edges that begin at a starting location and end at a destination, the nodes and edges representative of paths the robot may travel in the environment;

planning a coarse path from the starting location to the destination;

determining a point along the coarse path between a first node of the set of nodes and a second node of the set of nodes where line of sight by the image sensor is lost;

generating an intermediate waypoint at the point along the coarse path where line of sight by the image sensor is lost; and

navigating the robot from the starting location along the coarse path and the intermediate waypoint toward the destination.

2. The method of claim 1 , wherein planning the coarse path comprises determining a route of valid interconnections between respective nodes and edges of the set of nodes and edges.

3. The method of claim 1 , wherein generating the sparse graph comprises:

generating a full configuration space map based on the body obstacle map, the full configuration space map comprising a two-dimensional grid of elements, each element of the grid representing a space of the environment, and each element of the full configuration space map comprising a respective set of yaw configurations; and

generating the sparse graph from the full configuration space map.

4. The method of claim 3 , wherein:

each yaw configuration is classified as valid or invalid;

a valid yaw configuration represents a yaw configuration of the robot that is safe from contacting obstacles at the space associated with the respective element; and

an invalid yaw configuration represents a yaw configuration of the robot that is not safe from contacting obstacles at the space associated with the respective element.

5. The method of claim 3 , wherein generating the sparse graph comprises:

generating, from the full configuration space map, a compressed configuration space map comprising a second two-dimensional grid of elements, each element of the second grid representing a space of the environment, and each element of the second grid categorized as one of a:

yaw collision zone;

a yaw free zone, or

a yaw constrained zone; and

generating the sparse graph from the compressed configuration space map.

6. The method of claim 5 , wherein planning the coarse path from the starting location to the destination comprises:

generating a dense graph from the compressed configuration space map, the dense graph comprising elements categorized as yaw free zone; and

linking edges from the sparse graph with elements from the dense graph.

7. The method of claim 6 , wherein linking the edges with elements from the dense graph comprises:

combining the sparse graph and the dense graph to generate a final graph; and

executing an A* search algorithm on the final graph.

8. The method of claim 1 , wherein determining the point along the coarse path where line of sight by the image sensor is lost comprises:

determining a minimum allowable yaw and a maximum allowable yaw at each element along the planned coarse path;

determining a smallest envelope based on the minimum allowable yaw and the maximum allowable yaw; and

determining that a required yaw at a point on the coarse path is outside the smallest envelope.

9. The method of claim 1 , wherein the destination represents a location in the environment of the robot.

10. The method of claim 1 , wherein the destination comprises a respective node of the set of nodes of the sparse graph.

11. A robot comprising:

a body;

legs coupled to the body and configured to maneuver the robot about an environment;

data processing hardware in communication with the legs; and

memory hardware in communication with the data processing hardware, the memory hardware storing instructions that when executed on the data processing hardware cause the data processing hardware to perform operations comprising:

receiving image data of an environment of the robot from an image sensor;

receiving a body obstacle map comprising a map of obstacles that the body of the robot cannot traverse;

generating a sparse graph based on the body obstacle map, the sparse graph comprising a set of nodes and edges that begin at a starting location and end at a destination, the nodes and edges representative of paths the robot may travel in the environment;

planning a coarse path from the starting location to the destination;

determining a point along the coarse path between a first node of the set of nodes and a second node of the set of nodes where line of sight by the image sensor is lost;

generating an intermediate waypoint at the point along the coarse path where line of sight by the image sensor is lost; and

navigating the robot from the starting location along the coarse path and the intermediate waypoint toward the destination.

12. The robot of claim 11 , wherein planning the coarse path comprises determining a route of valid interconnections between respective nodes and edges of the set of nodes and edges.

13. The robot of claim 11 , wherein generating the sparse graph comprises:

generating a full configuration space map based on the body obstacle map, the full configuration space map comprising a two-dimensional grid of elements, each element of the grid representing a space of the environment, and each element of the full configuration space map comprising a respective set of yaw configurations; and

generating the sparse graph from the full configuration space map.

14. The robot of claim 13 , wherein:

each yaw configuration is classified as valid or invalid;

a valid yaw configuration represents a yaw configuration of the robot that is safe from contacting obstacles at the space associated with the respective element; and

an invalid yaw configuration represents a yaw configuration of the robot that is not safe from contacting obstacles at the space associated with the respective element.

15. The robot of claim 13 , wherein generating the sparse graph comprises:

generating, from the full configuration space map, a compressed configuration space map comprising a second two-dimensional grid of elements, each element of the second grid representing a space of the environment, and each element of the second grid categorized as one of a:

yaw collision zone;

a yaw free zone, or

a yaw constrained zone; and

generating the sparse graph from the compressed configuration space map.

16. The robot of claim 15 , wherein planning the coarse path from the starting location to the destination comprises:

generating a dense graph from the compressed configuration space map, the dense graph comprising elements categorized as yaw free zone; and

linking edges from the sparse graph with elements from the dense graph.

17. The robot of claim 16 , wherein linking the edges with elements from the dense graph comprises:

combining the sparse graph and the dense graph to generate a final graph; and

executing an A* search algorithm on the final graph.

18. The robot of claim 11 , wherein determining the point along the coarse path where line of sight by the image sensor is lost comprises:

determining a minimum allowable yaw and a maximum allowable yaw at each element along the planned coarse path;

determining a smallest envelope based on the minimum allowable yaw and the maximum allowable yaw; and

determining that a required yaw at a point on the coarse path is outside the smallest envelope.

19. The robot of claim 11 , wherein the destination represents a location in the environment of the robot.

20. The robot of claim 11 , wherein the destination comprises a respective node of the set of nodes of the sparse graph.