In [1]:
%matplotlib inline
In [2]:
from __future__ import print_function
from __future__ import division
import matplotlib.pyplot as plt
import numpy as np
from numpy.linalg import norm
from math import sin, cos, pi, sqrt
from math import radians as d2r
Example diagram from wikipedia:
$\phi$ is the phase of each leg and $z$ is the foot height. The black bars indicate when a foot is in contact with the ground.
Common gaits for quadrupeds (spider/crab configuration):
Ripple Gait: when one leg is in the air while the other three are holding up the body. This tends to be a stable gait when you keep the center of mass (CM) inside the triangle formed by the three legs in contact with the ground. Becuase of the stability, you could interrupt this gait at any time (i.e., stop it) and the robot will not fall over.
Trott Gait: A fast gait where two legs are in the air moving while the other two legs are in contact with the ground. This is an unstable gait and the robot could fall over if the gait is not executed quickly enough.
Terminology:
Duty Factor: Duty factor is simply the percent of the total cycle which a given foot is on the ground. Duty factors over 50% are considered a "walk", while those less than 50% are considered a run.
Forelimb-hindlimb Phase (or just Phase): is the temporal relationship between the limb pairs.
In [3]:
# def printFoot(i, newpos):
# if i == 0:
# print('New [{}](x,y,z): {:.2f}\t{:.2f}\t{:.2f}'.format(i, newpos[0], newpos[1], newpos[2]))
def rot_z(t, c):
"""
t - theta [radians]
c - [x,y,z] or [x,y] ... the function detects 2D or 3D vector
"""
if len(c) == 3:
ans = np.array([
c[0]*cos(t)-c[1]*sin(t),
c[0]*sin(t)+c[1]*cos(t),
c[2]
])
else:
ans = np.array([
c[0]*cos(t)-c[1]*sin(t),
c[0]*sin(t)+c[1]*cos(t)
])
return ans
In [4]:
print('{:.2f} {:.2f} {:.2f}'.format(*rot_z(pi/4, np.array([84,0,-65]))))
In [4]:
class Gait(object):
def __init__(self):
self.legOffset = [0, 6, 3, 9]
# self.body = np.array([72.12, 0, 0])
self.rest = None
def calcRotatedOffset(self, cmd, frame_angle):
"""
calculate the foot offsets for each leg and delta linear/rotational
in - cmd(x,y,z_rotation)
out - array(leg0, leg1, ...)
where leg0 = {'linear': [x,y], 'rotational': [x,y], 'angle': zrotation(rads)}
"""
# I could do the same here as I do below for rotation
# rotate the command into the leg frame
rc = rot_z(frame_angle, cmd)
# get rotation distance: dist = rot_z(angle, rest) - rest
# this just reduces the function calls and math
zrot = d2r(float(cmd[2])) # should I assume this is always radians? save conversion
# fromcenter = self.rest + self.body
# value of this?
# rot = rot_z(zrot/2, fromcenter) - rot_z(-zrot/2, fromcenter)
# ans = {'linear': rc, 'rotational': rot, 'angle': zrot}
ans = {'linear': rc, 'angle': zrot}
return ans
def command(self, cmd, func, steps=12):
# handle no movement command ... do else where?
if sqrt(cmd[0]**2 + cmd[1]**2 + cmd[2]**2) < 0.001:
for leg in range(0, 4):
func(leg, self.rest) # move to resting position
return
cmd = [100.0, 0.0, 0.0]
# frame rotations for each leg
frame = [-pi/4, pi/4, 3*pi/4, -3*pi/4]
for i in range(0, steps): # iteration, there are 12 steps in gait cycle
for legNum in [0, 2, 1, 3]: # order them diagonally
rcmd = self.calcRotatedOffset(cmd, frame[legNum])
pos = self.eachLeg(i, rcmd) # move each leg appropriately
func(legNum, pos)
In [5]:
class DiscreteRippleGait(Gait):
def __init__(self, h, r):
Gait.__init__(self)
self.phi = [9/9, 6/9, 3/9, 0/9, 1/9, 2/9, 3/9, 4/9, 5/9, 6/9, 7/9, 8/9] # foot pos in gait sequence
maxl = h #
minl = maxl/2
self.z = [minl, maxl, maxl, minl, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # leg height
self.rest = r # idle leg position
def eachLeg(self, index, cmd):
"""
interpolates the foot position of each leg
cmd:
linear (mm)
angle (rads)
"""
rest = self.rest
i = index % 12
phi = self.phi[i]
# rotational commands -----------------------------------------------
angle = cmd['angle']/2-cmd['angle']*phi
rest_rot = rot_z(-angle, rest)
# linear commands ----------------------------------------------------
linear = cmd['linear']
xx = linear[0]
yy = linear[1]
# create new move command
move = np.array([
xx/2 - phi*xx,
yy/2 - phi*yy,
self.z[i]
])
# new foot position: newpos = rest + move ----------------------------
newpos = move + rest_rot
return newpos
In [78]:
# class ContinousRippleGait(Gait):
# alpha = 0.5
# def __init__(self, h, r):
# Gait.__init__(self)
# self.height = h
# self.rest = r
# def phi(self, x):
# """
# The phase
# """
# phi = 0.0
# if x <= 3.0:
# phi = 1/3*(3.0-x)
# else:
# phi = 1/9*(x-3)
# return phi
# def z(self, x):
# """
# Leg height
# duty cycle:
# 0-3: leg lifted
# 3-12: leg on ground
# duty = (12-3)/12 = 0.75 = 75% a walking gait
# """
# height = self.height
# z = 0.0
# if x <= 1:
# z = height/1.0*x
# elif x <= 2.0:
# z = height
# elif x <= 3.0:
# z = -height/1.0*(x-2.0)+height
# return z
# def eachLeg(self, index, cmd):
# """
# interpolates the foot position of each leg
# """
# rest = self.rest
# i = (index*self.alpha) % 12
# phi = self.phi(i)
# z = self.z(i)
# # rotational commands -----------------------------------------------
# angle = cmd['angle']/2-cmd['angle']*phi
# rest_rot = rot_z(-angle, rest)
# # linear commands ----------------------------------------------------
# linear = cmd['linear']
# xx = linear[0]
# yy = linear[1]
# # create new move command
# move = np.array([
# xx/2 - phi*xx,
# yy/2 - phi*yy,
# z
# ])
# # new foot position: newpos = rest + move ----------------------------
# newpos = move + rest_rot
# return newpos
In [6]:
cmd = {'linear': [0,0], 'angle': pi/4}
leg = np.array([84,0.0,-65]) # idle leg position
height = 25
# gait = ContinousRippleGait(height, leg)
gait = DiscreteRippleGait(height, leg)
alpha = 1
pos = []
for i in range(0,12):
p = gait.eachLeg(i*alpha,cmd)
pos.append(p)
print(p)
Now the code below runs the for all 4 legs and only prints out the position for leg 0. You can modify this above in the printFoot function. In reality, you would pass a function to move the leg.
In [8]:
# Run the entire class
# remember!! command does a rotation of the leg coord system, so it
# will output different numbers than above.
cmd = [0,0,pi/4]
leg = np.array([84,0.0,-65]) # idle leg position
height = 25
# gait = ContinousRippleGait(height, leg)
# gait.alpha = 0.5
gait = DiscreteRippleGait(height, leg)
gait.command(cmd, print, steps=12) # doesn't return anything
In [9]:
px = []
py = []
pz = []
for p in pos:
px.append(p[0])
py.append(p[1])
pz.append(p[2])
plt.subplot(2,2,1);
plt.plot(px)
plt.ylabel('x')
plt.subplot(2,2,2)
plt.plot(py)
plt.ylabel('y')
plt.subplot(2,2,3);
plt.plot(pz)
plt.ylabel('z')
plt.subplot(2,2,4);
plt.plot(px,py)
plt.ylabel('y')
plt.xlabel('x');
In [93]:
for p in pos:
print(p)
In [ ]:
In [ ]:
In [ ]:
In [ ]:
This work is licensed under a Creative Commons Attribution-ShareAlike 4.0 International License.