Walking Gait Generation

Jacky Baltes
National Taiwan Normal University
Taipei, Taiwan
jacky.baltes@ntnu.edu.tw

19 June 2020

# These libraries provide us with the most often used features

import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
import numpy as np
import math
import random

import jb_geometry as jbg

def drawRobot( ax, q, rPos, links ):
  theta = 0.0/180.0
  prev = np.eye(4)
  T = []
  artists = []
  #print(rPos)
  #print(q)
  #print("Draw ankle", rPos[0] - links[LEFT_ANKLE]/2, rPos[0] + links[LEFT_ANKLE]/2)
  artists.append( ax.plot( [ rPos[0] - links[LEFT_ANKLE]/2 , rPos[0] + links[LEFT_ANKLE]/2 ], [0,0], '-', color='red', linewidth=5 )[0] )
  artists.append( ax.plot( [0,0], [1,1], 'g-')[0] )

  for i,angle in enumerate( q ):
    xp,yp,_,_ = prev.dot( [ rPos[0], rPos[1], 0, 1 ] )
    A = prev.dot( jbg.rotateZ( angle ) ).dot( jbg.translate( links[LEFT_CALF+i], 0, 0 ) )
    T.append(A)
    xn,yn,_,_ = A.dot( [ rPos[0], rPos[1], 0, 1 ] )
    artists.append( ax.plot( [xp, xn], [yp, yn], linestyle="-", color="#80a0a0a0", linewidth=5 )[0] )

    prev = A 
  return T, artists

LEFT_ANKLE, LEFT_CALF, LEFT_THIGH, LEFT_TORSO = range( 4 )

robotLinks = [ 0.21, 0.3, 0.25, 0.6 ]   # length of link in m (foot, calf, thigh, torso )
robotMasses = [ 0.5, 1.0, 1.5, 5 ]
robotQ = [ 0.0/180.0 * math.pi, 0.0/180.0 * math.pi, 0.0/180.0 * math.pi ]

Stability Criterion

Test or measurement for stability of robot.

Use the center of mass as a stability criterion

Suitable stability criterion if the robot is moving slowly - consider kinematics

If robot moves quickly, then the center of mass is not a good stability criterion, in that case, we use the similar center of pressure as a stability criterion.

Many dynamic walking gait algorithms today for dynamic walking are based on the zero moment point (ZMP)

def calcCenterOfMass( rPos, q, links, masses ):
  rx, ry = rPos
  prev = np.eye(4)
  # x,y,m,
  linkCoMs = [ [ rx, ry, masses[LEFT_ANKLE] ] ]
  T = []
  current = [rx, ry]
  theta = 0.0/180.0 * math.pi

  for i,angle in enumerate( q ):
    theta = theta + q[i]
    mx = current[0] + math.cos( theta ) * links[i+LEFT_CALF]/2
    my = current[1] + math.sin( theta ) * links[i+LEFT_CALF]/2
    nx = current[0] + math.cos( theta ) * links[i+LEFT_CALF]
    ny = current[1] + math.sin( theta ) * links[i+LEFT_CALF]
    linkCoMs.append( [ mx, my, masses[i+LEFT_CALF] ])
    current = nx, ny

  sumXM, sumYM, sumM = 0, 0, 0

  for p in linkCoMs:
    sumXM = sumXM + p[0] * p[2] 
    sumYM = sumYM + p[1] * p[2] 
    sumM = sumM + p[2]
  
  com = [ sumXM/sumM, sumYM/sumM, False ]
  if com[0] >= rx - links[0]/2 and com[0] <= rx + links[0]/2:
    com[2] = True
  return (com, linkCoMs)

def drawCenterOfMassInfo( ax, coms ):
  com, linkCoMs = coms
  artists = []
  for p in linkCoMs:
    px, py, pm = p
    artists.append( ax.scatter( [px], [py], pm * 15, color = "blue" ) )
  artists.append( ax.scatter( [ com[0] ], [ com[1] ], 30, color="green" ) )

  if com[2]:
    artists.append( ax.plot([ com[0], com[0] ], [com[1], 0 ], 'g--' )[0] )
  else:
    artists.append( ax.plot([ com[0], com[0] ], [com[1], 0 ], 'r--', linewidth=2 )[0] )
  return artists

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(1,3,1)
ax.set_xlim((-0.2, 1.2))
ax.set_ylim((0, 1.4))

robotQ = [ 60.0/180.0 * math.pi, 80.0/180.0 * math.pi, -70.0/180.0 * math.pi ]
rPos = [0,0]
drawRobot( ax, robotQ, rPos, robotLinks )
com = calcCenterOfMass( rPos, robotQ, robotLinks, robotMasses )
drawCenterOfMassInfo( ax, com )

ax2 = fig.add_subplot(1,3,2)
ax2.set_xlim((-0.2, 1.2))
ax2.set_ylim((0, 1.4))
robotQ = [90.0/180.0*math.pi, 0/180.0 * math.pi, 0.0/180.0 * math.pi]
drawRobot( ax2, robotQ, [0,0], robotLinks )
com = calcCenterOfMass( rPos, robotQ, robotLinks, robotMasses )
drawCenterOfMassInfo( ax2, com )

ax3 = fig.add_subplot(1,3,3)
ax3.set_xlim((-0.2, 1.2))
ax3.set_ylim((0, 1.4))

robotQ = [40.0/180.0*math.pi, 0/180.0 * math.pi, 0.0/180.0 * math.pi]
drawRobot( ax3, robotQ, [0,0], robotLinks )
com = calcCenterOfMass( rPos, robotQ, robotLinks, robotMasses )
drawCenterOfMassInfo( ax3, com )

figpose2 = addJBFigure("figpose2", 0, 0, fig )
plt.close()

Sample View of Center of Mass

from matplotlib.animation import ArtistAnimation

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(1,1,1)
ax.set_xlim((-1.2, 1.2))
ax.set_ylim((0, 1.4))

frames = []

robotQ = np.array( [ 80.0/180.0 * math.pi, 110.0/180.0 * math.pi, -80.0/180.0 * math.pi ] )
robotQV = np.array( [ 0.5/180.0*math.pi, 3.0/180.0*math.pi, 2/180.0*math.pi ] )

rPos = [0,0]

artists = drawRobot( ax, robotQ, rPos, robotLinks )[1]
#print("artists in init", artists )

com = calcCenterOfMass( rPos, robotQ, robotLinks, robotMasses )
#drawCenterOfMassInfo( ax, com )

frames.append( artists )

for frame in range( 100 ):
  #print("frame", frame )

  robotQ = robotQ + robotQV
  if frame % 25 == 0:
    robotQV = -1.25 * robotQV

  artists = []

  a = drawRobot( ax, robotQ, rPos, robotLinks )[1]
  artists.extend( a )
  #print("artists in loop", artists )

  com = calcCenterOfMass( rPos, robotQ, robotLinks, robotMasses )
  artists.extend( drawCenterOfMassInfo( ax, com ) )

  frames.append( artists )

#print("frames", frames[0:5])
ani = ArtistAnimation(fig, frames, interval=50, blit=True, repeat_delay=1000)

from jblecture import addJBAnimation
ani1 = addJBAnimation( "ani1", 0, 0, ani )

plt.close()

Walking Animation

Center of Mass

The Center of Mass is the point at which I can lift a structure by applying force and it won't rotate.

The Center of Mass is the point where all moments sum up to 0.

\[ X_{CoM} := \sum_p ( X_{CoM} - X_p ) * M_p = 0 \\ \]

Derive a formula to calculate the CoM:

\[ \sum_p ( X_{CoM} - X_p ) * M_p = 0\\ \sum_p X_{CoM} * M_p - \sum_p X_p * M_p = 0 \\ X_{CoM} * \sum_p M_p = \sum_p X_p * M_p \\ X_{CoM} = \frac{\sum_p X_p * M_p}{\sum_p M_p} \\ \]

Terminology

Bipedal walking is a cyclical, symmetric motion

Single support phase: Time during the walk where only one foot is on the ground
Double support phase: Time during the walk where both feet are on the ground
Support leg: The leg during single support phase, which is on the ground
Swing leg: The leg during single support phase, which is in the air and swings from back to front