# Improved low speed step selection

In my original series on balancing while walking, (part 1, part 2, part 3), I described some heuristics I used to handle changing directions. That was minimally sufficient in 1D, however in 2D it still leaves something to be desired, as there are more possible degenerate cases. The biggest is when spinning in place. There, the center of mass doesn’t really move at all relative to the balance line, but we still need to take steps!

## Existing state

In that first pass, I relied on limiting the maximum time spent in the stance phase. However, even with that, it was possible to get into situations where the robot would eventually tip over. Here’s a quick clip showing that happen with the maximum stance time artificially increased:

Clearly, something better could be done.

## New approach

While I expect to more seriously revamp the step selection method soon to enable gaits with a flight phase, I figured that I could make incremental progress here with a technique that would also be useful going forward. The basic concept is to keep track of how long until any given foot will reach an infeasible position in its configuration space. If that looks to be soon enough, then that leg (well, virtual leg), will be forced to take a step regardless of balance considerations.

The first part of that is determining when a leg position will become infeasible. I chose to make several approximations. When starting up, for each leg, a sampling approach is used to mark out the feasible region in the joint reference frame for the standard height, and for the leg-lifted height. This results in two 2D polygons:

Then, I intersect those two polygons and simplify to find an XY polygon that gives a valid region for the leg hopefully across the entire used Z volume.

At runtime, I use the calculation I made earlier for balancing segment wise, to determine which segment the leg will exit first and at what time. The “virtual leg’s” exit time is just the minimum of its two component legs.

Finally, I extended the logic to determine when a leg should be picked up to incorporate this “invalid time” in addition to the other constraints (it is getting a little unwieldy at this point).

## Result

This approach is definitely is much more robust, especially at low speed rotation. Of course, as always, there is still room for improvement, even with this approach, the legs can get into a configuration where the support polygon is very small, and in some cases very large steps are taken. Addressing those will likely require either incorporating knowledge of the support polygon directly or indirectly, or more forward planning than the “step at a time” approach I’ve got now.

Here’s a video where I stress tested the step selection algorithm by aggressively changing requested walking speed and rotation rates, forcing the machine to make sub-optimal steps and recover from them over and over.