Prototyping a walking pattern generator

Walking pattern generation is the problem of computing an ideal locomotion trajectory, called the walking pattern. It is the first sub-problem in the traditional walking control scheme for legged robots. A walking pattern is dynamically consistent (it respects the laws of physics) and unperturbed (it assumes all controls are perfectly executed by the robot). In this post, we will see how to design a complete walking pattern generator in pymanoid using polynomial interpolation for the swing leg and model predictive control for the center of mass.

Footstep planning

The goal of our walking pattern generator will be to make the robot walk through a given sequence of contacts (in Python, a list of pymanoid.Contact objects). For walking forward, we can generate this sequence simply by placing footsteps to the left and right of a line segment:

import pymanoid

def generate_footsteps(distance, step_length, foot_spread):
    contacts = []

    def append_contact(x, y):
       contacts.append(pymanoid.Contact(
           shape=(0.11, 0.05), pos=[x, y, 0.], friction=0.7))

    append_contact(0., +foot_spread)
    append_contact(0., -foot_spread)
    x = 0.
    y = foot_spread
    while x < distance:
       if distance - x <= step_length:
           x += min(distance - x, 0.5 * step_length)
       else:  # still some way to go
           x += step_length
       y = -y
       append_contact(x, y)
    append_contact(x, -y)  # now x == distance
    return contacts

The function depends on three parameters: distance, the total distance in meters to walk, step_length, which is by definition the distance between right and left heel in double support, and the lateral distance foot_spread between left and right foot centers. The first and last steps have half step length so as to accelerate and decelerate more progressively. In what follows, the robot will walk seven steps forward with a step length of 30 cm:

footsteps = generate_footsteps(
    distance=2.1,
    step_length=0.3,
    foot_spread=0.1,
    friction=0.7)

The robot starts from a standing posture with its center of mass (COM) 85 cm above ground, which is a comfortable height for the default JVRC-1 model. We put the robot in this initial posture by solving the inverse kinematics (IK) of our desired stance (a pymanoid.Stance):

if __name__ == "__main__":
    dt = 0.03  # simulation time step, in [s]
    sim = pymanoid.Simulation(dt=dt)
    robot = pymanoid.robots.JVRC1(download_if_needed=True)
    sim.set_viewer()
    stance = pymanoid.Stance(
        com=pymanoid.PointMass([0, 0, 0.85], robot.mass),
        left_foot=footsteps[0],
        right_foot=footsteps[1])
    stance.bind(robot)
    robot.ik.solve(max_it=42)

Our robot is now ready to walk:

Humanoid model JVRC-1 is now ready to walk

Walking state machine

Walking alternates double support phases (DSP), where both feet are in contact with the ground, and single support phases (SSP), where one foot (the swing foot) is in the air while the robot supports its weight on the other one:

Single and double support phases while walking

We can represent these phases in a finite-state machine (FSM) with three states, standing, double support and single support, implemented as a pymanoid.Process:

class WalkingFSM(pymanoid.Process):

    def __init__(self, ssp_duration, dsp_duration):
        super(WalkingFSM, self).__init__()
        self.dsp_duration = dsp_duration
        self.mpc_timestep = 3 * dt  # update MPC every 90 [ms]
        self.next_footstep = 2
        self.ssp_duration = ssp_duration
        self.state = None
        #
        self.start_standing()

    def on_tick(self, sim):
        if self.state == "Standing":
            return self.run_standing()
        elif self.state == "DoubleSupport":
            return self.run_double_support()
        elif self.state == "SingleSupport":
            return self.run_single_support()
        raise Exception("Unknown state: " + self.state)

The state machine will switch between single and double support phases based on the pre-defined phase timings ssp_duration and dsp_duration.

Standing state

For each state, we define a "start" and "run" function. The standing state simply waits for the user to set the start_walking boolean attribute to True:

# class WalkingFSM(pymanoid.Process):

    def start_standing(self):
        self.start_walking = False
        self.state = "Standing"
        return self.run_standing()

    def run_standing(self):
        if self.start_walking:
            self.start_walking = False
            if self.next_footstep < len(footsteps):
                return self.start_double_support()

Double support state

For starters, the double support state simply waits for its phase duration, then switches to single support:

# class WalkingFSM(pymanoid.Process):

    def start_double_support(self):
        if self.next_footstep % 2 == 1:  # left foot swings
            self.stance_foot = stance.right_foot
        else:  # right foot swings
            self.stance_foot = stance.left_foot
        dsp_duration = self.dsp_duration
        if self.next_footstep == 2 or self.next_footstep == len(footsteps) - 1:
            # double support is a bit longer for the first and last steps
            dsp_duration = 4 * self.dsp_duration
        self.swing_target = footsteps[self.next_footstep]
        self.rem_time = dsp_duration
        self.state = "DoubleSupport"
        self.start_com_mpc_dsp()
        return self.run_double_support()

    def run_double_support(self):
        if self.rem_time <= 0.:
            return self.start_single_support()
        self.run_com_mpc()
        self.rem_time -= dt

We use the start function of the state to save two important contact locations, namely the stance foot and swing target of the next SSP. The reason for saving them here rather than in the start function of the SSP itself will become clear later on, when we implement the start_com_mpc_dsp() and run_com_mpc() functions. For now, let us complete them with a dummy linear interpolation to debug the development of our ongoing FSM:

# class WalkingFSM(pymanoid.Process):

    def start_com_mpc_dsp(self):
        pass  # to be implemented later

    def run_com_mpc(self):
        stance.com.set_x(0.5 * (self.swing_foot.x + self.stance_foot.x))

Single support state

The single support phase interpolates both COM and swing foot trajectories, and updates stance targets accordingly:

# class WalkingFSM(pymanoid.Process):

    def start_single_support(self):
        if self.next_footstep % 2 == 1:  # left foot swings
            self.swing_foot = stance.free_contact('left_foot')
        else:  # right foot swings
            self.swing_foot = stance.free_contact('right_foot')
        self.next_footstep += 1
        self.rem_time = self.ssp_duration
        self.state = "SingleSupport"
        self.start_swing_foot()
        self.start_com_mpc_ssp()
        self.run_single_support()

    def run_single_support(self):
        if self.rem_time <= 0.:
            stance.set_contact(self.swing_foot)
            if self.next_footstep < len(footsteps):
                return self.start_double_support()
            else:  # footstep sequence is over
                return self.start_standing()
        self.run_swing_foot()
        self.run_com_mpc()
        self.rem_time -= dt

For now, we apply dummy linear interpolations for both the COM and swing foot trajectories:

# class WalkingFSM(pymanoid.Process):

    def start_swing_foot(self):
        self.swing_start = self.swing_foot.pose

    def start_com_mpc_ssp(self):
        pass  # to be implemented later

    def run_swing_foot(self):
        from pymanoid.interp import interpolate_pose_linear
        progress = min(1., max(0., 1. - self.rem_time / self.ssp_duration))
        new_pose = interpolate_pose_linear(self.swing_start, self.swing_target.pose, progress)
        self.swing_foot.set_pose(new_pose)

When the phase time reaches the desired SSP duration, the state machine switches back to double support and the process repeats until the robot has traversed all footsteps in the sequence, finally switching back to the standing state.

Scheduling simulation processes

Although we haven't implemented neither swing foot nor COM trajectories yet, we can already validate this code in simulation. Let us create an FSM process with phase durations roughly 0.7/0.1 seconds for single and double support respectively:

ssp_duration = 24 * dt  # 720 [ms]
dsp_duration = 3 * dt  # 90 [ms]
fsm = WalkingFSM(ssp_duration, dsp_duration)

As the FSM updates targets, we schedule it before inverse kinematics in the simulation:

sim.schedule(fsm)
sim.schedule(robot.ik)
sim.start()

Let us throw in some extra drawer processes to keep track of the COM and swing foot position targets while the robot moves:

com_traj_drawer = TrajectoryDrawer(robot.stance.com, 'b-')
lf_traj_drawer = TrajectoryDrawer(robot.left_foot, 'g-')
rf_traj_drawer = TrajectoryDrawer(robot.right_foot, 'r-')
sim.schedule_extra(com_traj_drawer)
sim.schedule_extra(lf_traj_drawer)
sim.schedule_extra(rf_traj_drawer)

Now, the only thing left to do to start walking is:

In [1]: fsm.start_walking = True
This simple finite-state machine makes the legged robot shuffle its feet

The robot shuffles its feet in a motion that is kinematically but not dynamically consistent. We can see that by scheduling an extra wrench drawer to check the dynamics of the simulation:

wrench_drawer = PointMassWrenchDrawer(stance.com, stance)
sim.schedule_extra(wrench_drawer)

This process draws feasible contact forces when it finds some, otherwise it colors the background in red to show that the motion is not dynamically feasible. This is what happens most of the time with our controller at this point.

Swing foot interpolation

Our fixed footstep sequence provides initial and target configurations of the swing foot for each SSP. For this first prototype, let us implement a "flat foot" walk where the swing foot stays parallel to the ground. This strategy is easier to implement, but on real robots it has a tendency to yield early contacts and larger impacts than the heel-strike toe-off walk commonly applied by humans.

The orientation of the swing foot being given, the main parameter to tune is the vertical clearance of the trajectory. The default pymanoid.SwingFoot interpolates a cubic Hermite spline parameterized by a takeoff clearance height \(a\) and a landing clearance height \(b\):

Swing foot trajectory interpolation

For us the ground contact normal \(\bfn\) coincides with the world vertical \(\bfe_z\) in the figure above. The swing foot interpolator computes a polynomial \(P(t)\) such that:

  • \(P(0)\) is the initial foot position,
  • \(P(T_\textit{SSP})\) is the target foot position, with \(T_\textit{SSP}\) the duration of the single support phase,
  • \(P(\frac{1}{4} T_\textit{SSP})\) is at height greater than \(a\) from the initial contact, and
  • \(P(\frac{3}{4} T_\textit{SSP})\) is at height greater than \(b\) from the target contact.

We can choose \(a = b = 5~\text{cm}\) so that the foot detaches neatly from the ground without causing too much vertical height variations of the COM. First, let us instantiate the interpolator by:

# class WalkingFSM(pymanoid.Process):

    def start_swing_foot(self):
        self.swing_start = self.swing_foot.pose
        self.swing_interp = SwingFoot(
            self.swing_foot, self.swing_target, ssp_duration,
            takeoff_clearance=0.05, landing_clearance=0.05)

Second, we replace the dummy linear interpolation by the output from the swing foot interpolator:

# class WalkingFSM(pymanoid.Process):

    def run_swing_foot(self):
        new_pose = self.swing_interp.integrate(dt)
        self.swing_foot.set_pose(new_pose)
Now the legged robot swings its feet properly between footsteps

Now the robot properly lifts its feet in a motion that is kinematically but still not dynamically consistent.

Center of mass trajectory

The core problem in walking pattern generation is to make the motion dynamically consistent, that is, to make sure that the equations of motion

\begin{equation*} \bfM(\bfq) \qdd + \qd^\top \bfC(\bfq) \qd + \bfg(\bfq) = \bfS^\top \bftau + \sum_{i=1}^N \calJ_{C_i}^{\top} \bfw_{C_i} \end{equation*}

have physically feasible joint torques \(\bftau\) and contact wrenches \(\bfw_{C_i} = [\bftau_{C_i}\ \bff_i]\).

Linear inverted pendulum mode

Assuming that the robot is powerful enough to execute the walking motion, we can omit the lines that contain \(\bftau\) and focus on the Newton-Euler equations that govern the floating-base dynamics of the robot:

\begin{equation*} \begin{array}{rcl} m \bfpdd_G & = & m \bfg + \sum_i \bff_i \\ \dot{\bfL}_G & = & \sum_i (\bfp_{C_i} - \bfp_G) \times \bff_i + \bftau_{C_i} \end{array} \end{equation*}

For this first walking pattern generator, we won't use rotations of the upper-body and focus on producing the linear horizontal motion of the center of mass. This is done by using the linear inverted pendulum mode, keeping the COM at a constant height \(h\) as well as a constant angular momentum \(\bfL_G = \boldsymbol{0}\). As a consequence, the Euler equation reduces to having the resultant of contact forces going from the zero-tilting moment point (ZMP) to the COM, and the equation of motion becomes:

\begin{equation*} \bfpdd_G = \frac{g}{h} (\bfp_G - \bfp_Z) \end{equation*}

where \(Z\) denotes the ZMP.

Feasibility of contact forces

We want to make sure that, at every time instant, there exists feasible contact wrenches \(\bfw_{C_i} = [\bftau_{C_i}\ \bff_i]\) that lie in their wrench friction cone. In the linear inverted pendulum mode, this is equivalent to having the ZMP inside its ZMP support area. This area is a convex polytope projection in general, fortunately when walking on a flat floor with large friction it reduces to the convex hull \(\cal C\) of ground contact points:

\begin{equation*} \bfp_Z = \bfp_G - \frac{h}{g} \bfpdd_G \in {\cal C} \end{equation*}

Linear model predictive control

Walking involves falling forward (during single support phases), which is instantaneously unstable and only becomes "stable" when considering the future of the system (for instance, the next double support phase where the biped may slow down to a stop if needed). As such, walking pattern generation is commonly solved by model predictive control (MPC) where the COM trajectory is computed over a preview horizon of one or two steps.

Let us focus on lateral COM motions (sagittal motions can be derived similarly), and define the state of our system as \(\bfx = [y_G\ \dot{y}_G\ \ddot{y}_G]\). The MPC problem is then to generate a trajectory \(y_G(t)\) such that:

  • at the current time \(t_0\), \(\bfx(t_0)\) is equal to the current state of the COM \([y_G(t_0)\ \dot{y}_G(t_0)\ \ddot{y}_G(t_0)]\);
  • at all times, the ZMP \(y_\textit{min}(t) \leq y_Z(\bfx(t)) \leq y_\textit{max}(t)\) where \(y_\textit{min}(t)\) and \(y_\textit{max}(t)\) define the edges of the support area at time \(t\);
  • at the end of the preview horizon \(t_f\), \(\bfx(t_f)\) is equal to a desired terminal state, for instance \([y_f\ 0\ 0]\) where the robot has stopped walking over the last footstep.

We define control input as the COM jerk \(u = \dddot{y}_G\), and discretize our system as:

\begin{equation*} \bfx_{k+1} = \bfA \bfx_k + \bfB u_k \end{equation*}

where the state matrix \(\bfA\) and control matrix \(\bfB\) are defined by:

\begin{equation*} \bfA = \begin{bmatrix} 1 & T & \frac{T^2}{2} \\ 0 & 1 & T \\ 0 & 0 & 1 \end{bmatrix} \quad \bfB = \begin{bmatrix} \frac{T^3}{6} \\ \frac{T^2}{2} \\ T \end{bmatrix} \end{equation*}

The ZMP is obtained from the state as:

\begin{equation*} y_Z(x_k) = \begin{bmatrix} 1 & 0 & -\frac{h}{g} \end{bmatrix} \bfx_k \end{equation*}

At every discretized time \(t_k\), we want the ZMP \(y_Z\) to lie between \(y_{\textit{min},k}\) and \(y_{\textit{max},k}\). This can be written as a linear matrix inequality on the state:

\begin{equation*} \bfC \bfx_k \leq \bfe_k \end{equation*}

where:

\begin{equation*} \bfC = \begin{bmatrix} +1 & 0 & -\frac{h}{g} \\ -1 & 0 & +\frac{h}{g} \end{bmatrix} \quad \bfe_k = \begin{bmatrix} +y_{\textit{max},k} \\ -y_{\textit{min},k} \end{bmatrix} \end{equation*}

We now have all the ingredients to build a linear model predictive control problem. The discretized time step of our problem is \(T = 90\) ms, and our preview window will have 16 such steps:

# class WalkingFSM(pymanoid.Process):

    def update_mpc(self, dsp_duration, ssp_duration):
        nb_preview_steps = 16
        T = self.mpc_timestep
        nb_init_dsp_steps = int(round(dsp_duration / T))
        nb_init_ssp_steps = int(round(ssp_duration / T))
        nb_dsp_steps = int(round(self.dsp_duration / T))

Our preview consists of an initial DSP of duration dsp_duration (can be zero), followed by an SSP of duration ssp_duration, followed by a DSP of regular duration self.dsp_duration, followed by a second SSP lasting until the preview horizon. We build the matrices of the problem following the derivation above:

# class WalkingFSM(pymanoid.Process):

    # def update_mpc(self, dsp_duration, ssp_duration):

        A = array([[1., T, T ** 2 / 2.], [0., 1., T], [0., 0., 1.]])
        B = array([T ** 3 / 6., T ** 2 / 2., T]).reshape((3, 1))
        h = stance.com.z
        g = -sim.gravity[2]
        zmp_from_state = array([1., 0., -h / g])
        C = array([+zmp_from_state, -zmp_from_state])
        D = None
        e = [[], []]
        cur_vertices = self.stance_foot.get_scaled_contact_area(0.8)
        next_vertices = self.swing_target.get_scaled_contact_area(0.8)
        for coord in [0, 1]:
            cur_max = max(v[coord] for v in cur_vertices)
            cur_min = min(v[coord] for v in cur_vertices)
            next_max = max(v[coord] for v in next_vertices)
            next_min = min(v[coord] for v in next_vertices)
            e[coord] = [
                array([+1000., +1000.]) if i < nb_init_dsp_steps else
                array([+cur_max, -cur_min]) if i - nb_init_dsp_steps <= nb_init_ssp_steps else
                array([+1000., +1000.]) if i - nb_init_dsp_steps - nb_init_ssp_steps < nb_dsp_steps else
                array([+next_max, -next_min])
                for i in range(nb_preview_steps)]

Here, we construct two lists of vectors \(\bfe = [\bfe_0\ \bfe_1\ \ldots]\) in parallel: e[0] for the sagittal direction and e[1] for the lateral direction. We put constraints on the ZMP during single support phases based on the vertices of the support area (the area is scaled by 0.9 to make sure that the ZMP stays well inside). Finally, we build and solve MPC problems for both directions using the pymanoid.LinearPredictiveControl class:

# class WalkingFSM(pymanoid.Process):

    # def update_mpc(self, dsp_duration, ssp_duration):

        self.x_mpc = LinearPredictiveControl(
            A, B, C, D, e[0],
            x_init=array([stance.com.x, stance.com.xd, stance.com.xdd]),
            x_goal=array([self.swing_target.x, 0., 0.]),
            nb_steps=nb_preview_steps,
            wxt=1., wu=0.01)
        self.y_mpc = LinearPredictiveControl(
            A, B, C, D, e[1],
            x_init=array([stance.com.y, stance.com.yd, stance.com.ydd]),
            x_goal=array([self.swing_target.y, 0., 0.]),
            nb_steps=nb_preview_steps,
            wxt=1., wu=0.01)
        self.x_mpc.solve()
        self.y_mpc.solve()
        self.preview_time = 0.

We use this update_mpc() function to initialize these problems at the beginning of double and single support phases:

# class WalkingFSM(pymanoid.Process):

    def start_com_mpc_dsp(self):
        self.update_mpc(self.rem_time, self.ssp_duration)

    def start_com_mpc_ssp(self):
        self.update_mpc(0., self.rem_time)

Finally, in the run_com_mpc() function we update the COM by integrating jerk outputs from model predictive control:

# class WalkingFSM(pymanoid.Process):

    def run_com_mpc(self):
        if self.preview_time >= self.mpc_timestep:
            if self.state == "DoubleSupport":
                self.update_mpc(self.rem_time, self.ssp_duration)
            else:  # self.state == "SingleSupport":
                self.update_mpc(0., self.rem_time)
        com_jerk = array([self.x_mpc.U[0][0], self.y_mpc.U[0][0], 0.])
        stance.com.integrate_constant_jerk(com_jerk, dt)
        self.preview_time += dt

The preview_time variable is used to compute new predictive solutions after we are done integrating their first timestep. This is one of the key concepts in MPC: we only execute the first control of the output trajectory, and recompute that trajectory when this is done.

Dynamic constraints on the ZMP naturally produce a center of mass motion that sways laterally:

Humanoid model JVRC-1 walks dynamically

Now our robot model is properly walking with a motion that is both kinematically and dynamically consistent. You can check out the complete walking controller in the horizontal_walking.py example in pymanoid.

To go further

The concepts we have seen in this introduction are used in a full-fledged walking controller developed for walking and stair climbing with the HRP-4 humanoid.

Our model predictive control problem is reduced to the strict minimum: dynamic constraints on the ZMP and a terminal condition on the COM state. You can find a more advanced implementation in the ModelPredictiveControl.cpp source of the controller: it adds velocity and jerk terms to the cost function, as well as a (strict) terminal condition on the divergent component of motion (DCM). Those are implemented conveniently using the Copra predictive control library.

Although we didn't cover it in this introduction, the DCM is a major concept that has been applied to walking and running pattern generation.

Pages of this website are under the CC-BY 4.0 license.