Human Gait

Note

pygait2d and symmeplot and their dependencies must be installed first to run this example. Note that pygait2d has not been released to PyPi or Conda Forge:

conda install cython pip pydy pyyaml setuptools symmeplot sympy
python -m pip install --no-deps --no-build-isolation git+https://github.com/csu-hmc/gait2d

Objectives

This example highlights three points of interest compared to the others:

• Instance constraints are used to make the start state the same as the end state except for forward translation to solve for a cyclic trajectory, for example \(q_b(t_0) = q_e(t_f)\).

• The average speed is constrained in this variable time step solution by introducing an additional differential equation that, when integrated, gives the duration at \(\Delta_t(t)\), which can be used to calculate distance traveled with \(q_{ax}(t_f) = v_\textrm{avg} (t_f - t_0)\) and used as a constraint.

• The parallel option is enabled because the equations of motion are relatively large. This speeds up the evaluation of the constraints and its Jacobian about 1.3X.

Introduction

This example replicates a similar solution as shown in [Ackermann2010] using joint torques as inputs instead of muscle activations [1].

gait2d provides a joint torque driven 2D bipedal human dynamical model with seven body segments (trunk, thighs, shanks, feet) and foot-ground contact forces based on the description in [Ackermann2010].

The optimal control goal is to find the joint torques (hip, knee, ankle) that generate a minimal mean-torque periodic motion to ambulate at a specified average speed over half a period.

Import all necessary modules, functions, and classes:

import os
from packaging.version import parse as parse_version
from opty import Problem
from opty.utils import f_minus_ma
from pygait2d import derive, simulate
from pygait2d.segment import time_symbol, contact_force
from symmeplot.matplotlib import Scene3D
import matplotlib
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import numpy as np
import sympy as sm

Pick an average ambulation speed and the number of discretization nodes for the half period and define the time step as a variable \(h\).

speed = 3.0  # m/s
num_nodes = 40
h = sm.symbols('h', real=True, positive=True)
duration = (num_nodes - 1)*h

Derive the equations of motion using gait2d.

symbolics = derive.derive_equations_of_motion()

eom = symbolics.equations_of_motion
eom.shape

(18, 1)

The equations of motion have this many mathematical operations:

sm.count_ops(eom)

125449

\(t_f - t_0\) needs to be available to compute the average speed in the instance constraint, so add an extra differential equation that is the time derivative of the difference in time.

\[\Delta_t(t) = \int_{t_0}^{t} d\tau\]

delt = sm.Function('delt', real=True)(time_symbol)
eom = eom.col_join(sm.Matrix([delt.diff(time_symbol) - 1]))

states = symbolics.states + [delt]
num_states = len(states)

The generalized coordinates are the hip lateral position \(q_{ax}\) and veritcal position \(q_{ay}\), the trunk angle with respect to vertical \(q_a\) and the relative joint angles:

• right: hip (b), knee (c), ankle (d)

• left: hip (e), knee (f), ankle (g)

Each joint has a joint torque acting between the adjacent bodies.

qax, qay, qa, qb, qc, qd, qe, qf, qg = symbolics.coordinates
uax, uay, ua, ub, uc, ud, ue, uf, ug = symbolics.speeds
Fax, Fay, Ta, Tb, Tc, Td, Te, Tf, Tg = symbolics.specifieds

The constants are loaded from a file of realistic geometry, mass, inertia, and foot deformation properties of an adult human.

par_map = simulate.load_constants(symbolics.constants,
                                  'human-gait-constants.yml')
par_map

{g: 9.81, ma: 50.85, ia: 3.1777, xa: 0.0, ya: 0.3155, mb: 7.5, ib: 0.1522, lb: 0.441, xb: 0.0, yb: -0.191, mc: 3.4875, ic: 0.0624, lc: 0.4428, xc: 0.0, yc: -0.1917, md: 1.0875, id: 0.0184, xd: 0.0768, yd: -0.0351, hxd: -0.06, txd: 0.15, fyd: -0.07, me: 7.5, ie: 0.1522, le: 0.441, xe: 0.0, ye: -0.191, mf: 3.4875, if: 0.0624, lf: 0.4428, xf: 0.0, yf: -0.1917, mg: 1.0875, ig: 0.0184, xg: 0.0768, yg: -0.0351, hxg: -0.06, txg: 0.15, fyg: -0.07, kc: 50000000.0, cc: 0.85, mu: 1.0, vs: 0.01}

gait2d provides “hand of god” inputs to manipulate the trunk for some modeling purposes. Set these to zero.

traj_map = {
    Fax: np.zeros(num_nodes),
    Fay: np.zeros(num_nodes),
    Ta: np.zeros(num_nodes),
}

Bound all the states to human realizable ranges.

• The trunk should stay generally upright and be at a possible walking height.

• Only let the hip, knee, and ankle flex and extend to realistic limits.

• Put a maximum on the peak torque values.

bounds = {
    h: (0.001, 0.1),
    delt: (0.0, 10.0),
    qax: (0.0, 10.0),
    qay: (0.5, 1.5),
    qa: np.deg2rad((-60.0, 60.0)),
    uax: (0.0, 10.0),
    uay: (-10.0, 10.0),
}
# hip
bounds.update({k: (-np.deg2rad(40.0), np.deg2rad(40.0))
               for k in [qb, qe]})
# knee
bounds.update({k: (-np.deg2rad(60.0), 0.0)
               for k in [qc, qf]})
# foot
bounds.update({k: (-np.deg2rad(30.0), np.deg2rad(30.0))
               for k in [qd, qg]})
# all rotational speeds
bounds.update({k: (-np.deg2rad(400.0), np.deg2rad(400.0))
               for k in [ua, ub, uc, ud, ue, uf, ug]})
# all joint torques
bounds.update({k: (-100.0, 100.0)
               for k in [Tb, Tc, Td, Te, Tf, Tg]})

The average speed can be fixed by constraining the total distance traveled. To enforce a half period, set the right leg’s angles at the initial time to be equal to the left leg’s angles at the final time and vice versa. The same goes for the joint angular rates.

instance_constraints = (
    delt.func(0*h) - 0.0,
    qax.func(0*h) - 0.0,
    qax.func(duration) - speed*delt.func(duration),
    qay.func(0*h) - qay.func(duration),
    qa.func(0*h) - qa.func(duration),
    qb.func(0*h) - qe.func(duration),
    qc.func(0*h) - qf.func(duration),
    qd.func(0*h) - qg.func(duration),
    qe.func(0*h) - qb.func(duration),
    qf.func(0*h) - qc.func(duration),
    qg.func(0*h) - qd.func(duration),
    uax.func(0*h) - uax.func(duration),
    uay.func(0*h) - uay.func(duration),
    ua.func(0*h) - ua.func(duration),
    ub.func(0*h) - ue.func(duration),
    uc.func(0*h) - uf.func(duration),
    ud.func(0*h) - ug.func(duration),
    ue.func(0*h) - ub.func(duration),
    uf.func(0*h) - uc.func(duration),
    ug.func(0*h) - ud.func(duration),
)

The objective is to minimize the mean of all joint torques.

def obj(free):
    """Minimize the sum of the squares of the control torques."""
    T, h = free[num_states*num_nodes:-1], free[-1]
    return h*np.sum(T**2)


def obj_grad(free):
    T, h = free[num_states*num_nodes:-1], free[-1]
    grad = np.zeros_like(free)
    grad[num_states*num_nodes:-1] = 2.0*h*T
    grad[-1] = np.sum(T**2)
    return grad

Create an optimization problem and solve it.

prob = Problem(
    obj,
    obj_grad,
    eom,
    states,
    num_nodes,
    h,
    known_parameter_map=par_map,
    known_trajectory_map=traj_map,
    instance_constraints=instance_constraints,
    bounds=bounds,
    time_symbol=time_symbol,
    parallel=True,
)

Find the optimal solution and save it if it converges.

This loads a precomputed solution to save computation time. Delete the file to try one of the suggested initial guesses.

fname = f'human_gait_{num_nodes}_nodes_solution.csv'
if os.path.exists(fname):
    solution = np.loadtxt(fname)
else:
    # choose one initial guess, comment others
    initial_guess = prob.lower_bound + (
        prob.upper_bound -
        prob.lower_bound)*np.random.random_sample(prob.num_free)
    initial_guess = 0.01*np.ones(prob.num_free)
    initial_guess = np.zeros(prob.num_free)
    solution, info = prob.solve(initial_guess)
    if info['status'] in (0, 1):
        np.savetxt(f'human_gait_{num_nodes}_nodes_solution.csv', solution,
                   fmt='%.2f')

Use symmeplot to make an animation of the motion.

xs, rs, _, h_val = prob.parse_free(solution)
times = prob.time_vector(solution=solution)


def animate():

    origin = symbolics.origin
    ground = symbolics.inertial_frame
    trunk, rthigh, rshank, rfoot, lthigh, lshank, lfoot = symbolics.segments

    fig = plt.figure(figsize=(10.0, 4.0))

    ax3d = fig.add_subplot(1, 2, 1, projection='3d')
    ax2d = fig.add_subplot(1, 2, 2)

    hip_proj = origin.locatenew('m', qax*ground.x)
    scene = Scene3D(ground, hip_proj, ax=ax3d)

    # creates the stick person
    scene.add_line([
        rshank.joint,
        rfoot.toe,
        rfoot.heel,
        rshank.joint,
        rthigh.joint,
        trunk.joint,
        trunk.mass_center,
        trunk.joint,
        lthigh.joint,
        lshank.joint,
        lfoot.heel,
        lfoot.toe,
        lshank.joint,
    ], color="k")

    # creates a moving ground (many points to deal with matplotlib limitation)
    if parse_version(matplotlib.__version__) >= parse_version('3.10'):
        scene.add_line([origin.locatenew('gl', s*ground.x) for s in
                        np.linspace(-2.0, 2.0)], linestyle='--',
                       color='tab:green', axlim_clip=True)
    else:
        scene.add_line([origin.locatenew('gl', s*ground.x) for s in
                        np.linspace(-2.0, 2.0)], linestyle='--',
                       color='tab:green')

    # adds CoM and unit vectors for each body segment
    for seg in symbolics.segments:
        scene.add_body(seg.rigid_body)

    # show ground reaction force vectors at the heels and toes, scaled to
    # visually reasonable length
    scene.add_vector(contact_force(rfoot.toe, ground, origin)/600.0,
                     rfoot.toe, color="tab:blue")
    scene.add_vector(contact_force(rfoot.heel, ground, origin)/600.0,
                     rfoot.heel, color="tab:blue")
    scene.add_vector(contact_force(lfoot.toe, ground, origin)/600.0,
                     lfoot.toe, color="tab:blue")
    scene.add_vector(contact_force(lfoot.heel, ground, origin)/600.0,
                     lfoot.heel, color="tab:blue")

    scene.lambdify_system(states + symbolics.specifieds + symbolics.constants)
    gait_cycle = np.vstack((
        xs,  # q, u shape(2n, N)
        np.zeros((3, len(times))),  # Fax, Fay, Ta (hand of god), shape(3, N)
        rs,  # r, shape(q, N)
        np.repeat(np.atleast_2d(np.array(list(par_map.values()))).T,
                  len(times), axis=1),  # p, shape(r, N)
    ))
    scene.evaluate_system(*gait_cycle[:, 0])

    scene.axes.set_proj_type("ortho")
    scene.axes.view_init(90, -90, 0)
    scene.plot(prettify=False)

    ax3d.set_xlim((-0.8, 0.8))
    ax3d.set_ylim((-0.2, 1.4))
    ax3d.set_aspect('equal')
    for axis in (ax3d.xaxis, ax3d.yaxis, ax3d.zaxis):
        axis.set_ticklabels([])
        axis.set_ticks_position("none")

    eval_rforce = sm.lambdify(
        states + symbolics.specifieds + symbolics.constants,
        (contact_force(rfoot.toe, ground, origin) +
         contact_force(rfoot.heel, ground, origin)).to_matrix(ground),
        cse=True)

    eval_lforce = sm.lambdify(
        states + symbolics.specifieds + symbolics.constants,
        (contact_force(lfoot.toe, ground, origin) +
         contact_force(lfoot.heel, ground, origin)).to_matrix(ground),
        cse=True)

    rforces = np.array([eval_rforce(*gci).squeeze() for gci in gait_cycle.T])
    lforces = np.array([eval_lforce(*gci).squeeze() for gci in gait_cycle.T])

    ax2d.plot(times, rforces[:, :2], times, lforces[:, :2])
    ax2d.grid()
    ax2d.set_ylabel('Force [N]')
    ax2d.set_xlabel('Time [s]')
    ax2d.legend(['Horizontal GRF (r)', 'Vertical GRF (r)',
                 'Horizontal GRF (l)', 'Vertical GRF (l)'], loc='upper right')
    ax2d.set_title('Foot Ground Reaction Force Components')
    vline = ax2d.axvline(times[0], color='black')

    def update(i):
        scene.evaluate_system(*gait_cycle[:, i])
        scene.update()
        vline.set_xdata([times[i], times[i]])
        return scene.artists + (vline,)

    ani = FuncAnimation(
        fig,
        update,
        frames=range(len(times)),
        interval=h_val*1000,
    )

    return ani


animation = animate()

Once Loop Reflect

Now see what the solution looks like in the Moon’s gravitational field.

g = symbolics.constants[0]
prob.collocator.known_parameter_map[g] = 1.625  # m/s**2
prob.collocator.known_parameter_map

{g: 1.625, ma: 50.85, ia: 3.1777, xa: 0.0, ya: 0.3155, mb: 7.5, ib: 0.1522, lb: 0.441, xb: 0.0, yb: -0.191, mc: 3.4875, ic: 0.0624, lc: 0.4428, xc: 0.0, yc: -0.1917, md: 1.0875, id: 0.0184, xd: 0.0768, yd: -0.0351, hxd: -0.06, txd: 0.15, fyd: -0.07, me: 7.5, ie: 0.1522, le: 0.441, xe: 0.0, ye: -0.191, mf: 3.4875, if: 0.0624, lf: 0.4428, xf: 0.0, yf: -0.1917, mg: 1.0875, ig: 0.0184, xg: 0.0768, yg: -0.0351, hxg: -0.06, txg: 0.15, fyg: -0.07, kc: 50000000.0, cc: 0.85, mu: 1.0, vs: 0.01}

Use earth’s solution as initial guess.

solution, info = prob.solve(solution)

Animate the second solution.

xs, rs, _, h_val = prob.parse_free(solution)

animation = animate()

plt.show()

Once Loop Reflect

Footnotes

[1]

The 2010 Ackermann and van den Bogert solution was the original target problem opty was written to solve, with an aim to extend it to parameter identification of closed loop control walking. For various reasons, this example was not added until 2025, 10 years after the example was first proposed.