Now that I finally have tplot2 working sufficiently to diagnose problems in 3D, it is time to start actually fixing those problems. The first obvious thing I noticed when watching data replay was that the legs scooted around a lot after making contact with the ground. Absent 3D visualization, I knew something was wrong, but couldn’t easily tell what.

Diagnosing the first problem

Once I was able to plot the commanded position and velocity trajectory, I could clearly see a number of problems. For one, the trajectory was not terribly achievable. The velocity jumped in a discontinuous manner between different phases of the swing cycle, which resulted in large tracking errors when moving the physical legs:

Also, there are those odd periods near the downturn where the commanded Z velocity goes to exactly zero for a while, then resumes its downward trend in a non-physical manner.

When I first wrote the simple walk cycle, I didn’t spend a whole lot (well almost zero) time debugging it, as I didn’t have appropriate debugging tools. Clearly it wasn’t working and something better needed to be done.

Updated swing trajectory

While not the entirety of the problem by any stretch, I figured fixing the swing trajectory was a fine first step that would be mostly independent of any other resolutions. I wanted the swing phase of the leg movement to have a few properties:

• Continuous velocity profile (I don’t care about jerk)
• When lifting off and touching down, maintain the ground velocity for a brief period of time
• For now, I’m not doing whole body control, so the trajectory can be scripted, and it is acceptable to lock in the target position at foot liftoff time

I decided to tackle the problem independently in the Z axis and in the XY plane. In both cases, the approach is based on piecewise cubic bezier curves. In one dimension, these curves have a continuous first and second derivative, but only the position and first derivative are controllable.

For the equation:

The position, velocity, and acceleration are as follows:

Z axis

To generate the Z trajectory, we’ll just stick two of these back to back in a mirrored fashion, so the Z height raises to a peak at the halfway point, then lowers back to the original value with a continuous velocity reaching exactly 0 velocity at the touch down point. That makes the overall Z trajectory look like:

X-Y Plane

In the X-Y plane, I broke up the swing into 3 piecewise sections. The first is a constant deceleration profile from the initial velocity to 0, and the last section is a constant acceleration profile from 0 to the target velocity. The middle section is just a single cubic bezier curve independently applied in the X and Y axes. A sample trajectory (with velocities shown as vectors), might look like:

Then to put the Z and X/Y pieces together, here’s a plot in the XZ plane of a similar system:

So yes, it seems to be doing what we want in that the velocity is continuous in all 3 axes — we lift off gradually, perform our swing, then set back down gradually.

Testing on the robot

Well, I actually tested it first in simulation, but where’s the excitement in that! Here’s what the tplot2 video looks like with the new leg trajectory in a slightly stuttery GIF:

The green and blue feet in the 3D view show that the legs track the control points well, and that 2D plots shows that yes, the Z position and velocity are smooth and continuous as we desired.