Hierarchical two stage planner for little dog

We first developed a single stage footstep planner that is capable of solving local search problems for locating a goal. It is implemented with a modified A* search algorithm that utilizes a crows-fly heuristic for measuring the distance to the goal. However, pathfinding over extreme terrain with this method can take a long time: the planner becomes "stuck" if an obstacle lies along the crows-fly path. In this video, the dog struggles while searching for valid footsteps near sharp discontinuities in the terrain. In addition, the planner gives preference to solutions where the robot always faces the goal, forcing the robot to sidestep around obstacles. This can lead to unnatural footstep sequences To address the shortcomings of a single stage planner in maze-like terrain, we developed a two-stage planner that first looks at the terrain for a smooth body trajectory from the starting point to the prescribed goal location. The body trajectory is then passed to the second stage, which finds a sequence of footsteps close to that body trajectory. The first stage of our algorithm produces a terrain cost map from terrain height data that quantifies the expected difficulty of finding a path through a particular point on the terrain. The terrain cost map takes into account three main conditions. The first condition measures whether the four patches of ground for all four feet are relatively flat. This is calculated for a given body location by fitting a plane to the four terrain patches that represent locations that the dog's feet can comfortably reach. The second condition measures the amount of clearance for the robot's underbelly by comparing the terrain's highest point under the body against a preset height above the feet. The third condition measures the likelihood of all four feet finding a safe footstep away from sharp terrain discontinuities. We multiply all three scores for the given terrain location to produce a final score. To complete the first stage, we utilize this terrain cost map to search for a connected path that minimizes the average expected difficulty of crossing the terrain. The search algorithm is A* utilizing a crows-fly heuristic similar to the one employed in the original footstep planner, but the state space is much smaller. Therefore, it runs quickly, even for large, complicated terrains. In the second stage we run the footstep planner, but with a modified heuristic: the search gives preference to footstep configurations …