Central Pattern Generators
Jacky Baltes
National Taiwan Normal University
Taipei, Taiwan
jacky.baltes@ntnu.edu.tw
31 May 2021
LEFT_ANKLE, RIGHT_ANKLE, LEFT_CALF, RIGHT_CALF, LEFT_THIGH, RIGHT_THIGH, TORSO = range( 7 )
robotLinks = [ 0.21, 0.21, 0.3, 0.3, 0.25, 0.25, 0.6 ] # length of link in m (foot, calf, thigh, torso )
robotMasses = [ 0.5, 0.5, 1.0, 1.0, 1.5, 1.5, 5.0 ]
robotQ = [ 0.0/180.0 * math.pi, 0.0/180.0 * math.pi, 0.0/180.0 * math.pi, 0.0/180.0 * math.pi,
0.0/180.0 * math.pi, 0.0/180.0 * math.pi ]
LEFT_SUPPORT_OFFSET, RIGHT_SUPPORT_OFFSET = range(2)
LEFT_LEG_SUPPORT, RIGHT_LEG_SUPPORT = LEFT_SUPPORT_OFFSET, RIGHT_SUPPORT_OFFSET
legColors = [ "#ff8080", "#8080ff" ]
ORIGIN = [0, 0, 0, 1]
def drawRobot( ax, support, q, rPos, links ):
# Draw support leg Ankle, Calf, Thigh
theta = 0.0/180.0
prev = np.eye(4).dot( jbg.translate( rPos[0], rPos[1], 0) )
T = []
artists = []
pos = []
#print(rPos)
#print(q)
#print("Draw ankle", rPos[0] - links[LEFT_ANKLE]/2, rPos[0] + links[LEFT_ANKLE]/2)
artists.append( ax.plot( [ prev.dot( jbg.translate( -links[LEFT_ANKLE+support]/2, 0, 0 ) )[0],
prev.dot( jbg.translate( links[LEFT_ANKLE+support]/2, 0, 0) ) [0] ],
[ prev.dot(jbg.translate( -links[LEFT_ANKLE+support]/2, 0, 0 ) )[1],
prev.dot( jbg.translate( links[LEFT_ANKLE+support]/2, 0, 0) ) [1] ],
'-', color=legColors[support], linewidth=5 )[0] )
xp,yp,_,_ = prev.dot( ORIGIN )
pos.append([xp,yp])
#artists.append( ax.plot( [0,0], [1,1], 'g-')[0] )
for i in range( LEFT_ANKLE+support, LEFT_ANKLE+4+support, 2 ):
#print("Plotting support leg", i)
angle = q[i]
xp,yp,_,_ = prev.dot( ORIGIN )
A = prev.dot( jbg.rotateZ( angle ) ).dot( jbg.translate( links[i+2], 0, 0 ) )
T.append(A)
xn,yn,_,_ = A.dot( ORIGIN )
pos.append([xn,yn])
#print( f"[{xp},{yp}]->[{xn},{yn}]")
artists.append( ax.plot( [xp, xn], [yp, yn], linestyle="-", color=legColors[support], linewidth=7 )[0] )
prev = A
# torso
hip = A
xt, yt, _, _ = hip.dot( ORIGIN )
angle = q[LEFT_THIGH + support ]
AHead = hip.dot( jbg.rotateZ( angle) ).dot( jbg.translate( links[TORSO], 0, 0 ) )
prev = AHead.dot( jbg.translate( -links[TORSO], 0, 0 ) )
xh,yh,_,_ = AHead.dot( ORIGIN )
pos.append([xh,yh])
artists.append( ax.plot( [ xt, xh], [yt, yh], linestyle="-", color="#808010", linewidth=12)[0] )
xta, yta, _, _ = AHead.dot( np.array( [ 1, 0, 0, 1 ] ) )
torsoAngle = math.atan2( yta - yh, xta - xh )
angle = torsoAngle
#print("torsoAngle", torsoAngle/math.pi*180.0)
# Draw the swing leg
swing = 1 - support
for i in range( LEFT_ANKLE+4+swing, LEFT_ANKLE+swing, -2 ):
#print("Plotting support leg", i)
#print('q', i, q[i]/math.pi * 180.0)
angle = angle - q[i]
#print('angle', angle/math.pi * 180.0)
xp,yp,_,_ = prev.dot( ORIGIN )
A = prev.dot( jbg.rotateZ( -q[i] ) ).dot( jbg.translate( -links[i-2], 0, 0 ) )
T.append(A)
xn,yn,_,_ = A.dot( ORIGIN )
pos.append([xn,yn])
#print( f"[{xp},{yp}]->[{xn},{yn}]")
artists.append( ax.plot( [xp, xn], [yp, yn], linestyle="-", color=legColors[swing], linewidth=5 )[0] )
prev = A
# artists.append( ax.plot( [ prev.dot(jbg.rotateZ(angle).dot(jbg.translate( -links[LEFT_ANKLE+swing]/2, 0, 0 ) ) )[0],
# prev.dot( jbg.rotateZ(angle).dot(jbg.translate( links[LEFT_ANKLE+swing]/2, 0, 0) ) ) [0] ],
# [ prev.dot(jbg.rotateZ(angle).dot( jbg.translate( -links[LEFT_ANKLE+swing]/2, 0, 0 ) ) )[1],
# prev.dot( jbg.rotateZ(angle).dot( jbg.translate( links[LEFT_ANKLE+swing]/2, 0, 0) ) )[1] ],
# '-', color=legColors[swing], linewidth=5 )[0] )
return artists, pos
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))
ax.set_aspect("equal")
robotQ = [ 90.0/180.0 * math.pi, 80.0/180.0 * math.pi,
20.0/180.0 * math.pi, 20.0/180.0 * math.pi,
-40.0/180.0 * math.pi, -30.0/180.0 * math.pi ]
frame = drawRobot( ax, LEFT_LEG_SUPPORT, robotQ, [0.5,0], robotLinks )
Central Pattern Generators
Clusters of cells discovered by neuroscientists that produce a cyclical control signal without direct control from the brain or other cyclical input.
Control tightly coupled motions such as swimming, flying, running
class CPG:
def __init__(self, omega, amplitude, phase = 0 ):
self.omega = omega
self.amplitude = amplitude
self.phase = phase
def control(self, t):
return math.sin( t * self.omega + self.phase ) * self.amplitude
def __mul__( self, factor ):
n = CPG( self.omega, self.amplitude * factor, self.phase )
return n
def __add__( self, ad ):
n = CPG( self.omega, self.amplitude, self.phase + ad )
return n
class CPG:
def __init__(self, omega, amplitude, phase = 0 ):
self.omega = omega
self.amplitude = amplitude
self.phase = phase
def control(self, t):
return self.amplitude * math.sin( self.phase + self.omega * t )
def __mul__( self, factor ):
n = CPG( self.omega, self.amplitude * factor, self.phase )
return n
def __add__( self, ad ):
n = CPG( self.omega, self.amplitude, self.phase + ad )
return n
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
for i,c in enumerate( cpgs ):
ys[i] = c.control(t)
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
# for i,c in enumerate( cpgs ):
# for j,tt in enumerate( t ):
# ys[i,j] = c.control(tt)
for i,c in enumerate( cpgs ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
ys[i.:] = fv( t )
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
# for i,c in enumerate( cpgs ):
# for j,tt in enumerate( t ):
# ys[i,j] = c.control(tt)
for i,c in enumerate( cpgs ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
ys[i,:] = fv( t )
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
# for i,c in enumerate( cpgs ):
# for j,tt in enumerate( t ):
# ys[i,j] = c.control(tt)
for i,c in enumerate( cpgs ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
ys[i,:] = fv( t )
print(ys)
[[ 0. 0.10083842 0.20064886 0.2984138 0.39313661 0.48385164 0.56963411 0.64960951 0.72296256 0.78894546 0.84688556 0.8961922 0.93636273 0.96698762 0.98775469 0.99845223 0.99897117 0.98930624 0.96955595 0.93992165 0.90070545 0.85230712 0.79522006 0.73002623 0.65739025 0.57805259 0.49282204 0.40256749 0.30820902 0.21070855 0.11106004 0.01027934 -0.09060615 -0.19056796 -0.28858706 -0.38366419 -0.47483011 -0.56115544 -0.64176014 -0.7158225 -0.7825875 -0.84137452 -0.89158426 -0.93270486 -0.96431712 -0.98609877 -0.99782778 -0.99938456 -0.99075324 -0.97202182 -0.94338126 -0.90512352 -0.85763861 -0.80141062 -0.73701276 -0.66510151 -0.58640998 -0.50174037 -0.41195583 -0.31797166 -0.22074597 -0.12126992 -0.0205576 0.0803643 0.18046693 0.27872982 0.37415123 0.46575841 0.55261747 0.63384295 0.7086068 0.77614685 0.83577457 0.8868821 0.92894843 0.96154471 0.98433866 0.99709789 0.99969234 0.99209556 0.97438499 0.94674118 0.90944594 0.86287948 0.8075165 0.74392141 0.6727425 0.59470541 0.51060568 0.42130064 0.32770071 0.23076008 0.13146699 0.03083368 -0.07011396 -0.17034683 -0.26884313 -0.36459873 -0.45663749 -0.54402111] [ 0. 0.20064886 0.39313661 0.56963411 0.72296256 0.84688556 0.93636273 0.98775469 0.99897117 0.96955595 0.90070545 0.79522006 0.65739025 0.49282204 0.30820902 0.11106004 -0.09060615 -0.28858706 -0.47483011 -0.64176014 -0.7825875 -0.89158426 -0.96431712 -0.99782778 -0.99075324 -0.94338126 -0.85763861 -0.73701276 -0.58640998 -0.41195583 -0.22074597 -0.0205576 0.18046693 0.37415123 0.55261747 0.7086068 0.83577457 0.92894843 0.98433866 0.99969234 0.97438499 0.90944594 0.8075165 0.6727425 0.51060568 0.32770071 0.13146699 -0.07011396 -0.26884313 -0.45663749 -0.62585878 -0.76962418 -0.88208623 -0.95867071 -0.99626264 -0.99333304 -0.95000106 -0.86802917 -0.75075145 -0.60293801 -0.43060093 -0.24074979 -0.0411065 0.16020873 0.35500771 0.53536727 0.69395153 0.82431033 0.9211415 0.98050658 0.99999098 0.9788022 0.91780205 0.81947165 0.68781042 0.5281735 0.34705389 0.15181837 -0.04959214 -0.24898556 -0.43825186 -0.6096929 -0.75633557 -0.87221538 -0.95261911 -0.99427643 -0.995493 -0.95621934 -0.87805285 -0.76417283 -0.61921119 -0.44906404 -0.26065185 -0.06163804 0.13988282 0.33571414 0.51789078 0.67900297 0.81249769 0.91294525] [ 0. 0.20167684 0.40129771 0.59682761 0.78627322 0.96770328 1.13926821 1.29921903 1.44592512 1.57789093 1.69377113 1.7923844 1.87272545 1.93397525 1.97550938 1.99690445 1.99794234 1.97861247 1.9391119 1.8798433 1.80141089 1.70461424 1.59044011 1.46005246 1.31478049 1.15610517 0.98564409 0.80513498 0.61641803 0.4214171 0.22212008 0.02055868 -0.18121229 -0.38113593 -0.57717412 -0.76732838 -0.94966022 -1.12231087 -1.28352028 -1.431645 -1.56517501 -1.68274904 -1.78316851 -1.86540971 -1.92863423 -1.97219755 -1.99565556 -1.99876912 -1.98150649 -1.94404365 -1.88676252 -1.81024703 -1.71527722 -1.60282124 -1.47402552 -1.33020303 -1.17281996 -1.00348074 -0.82391166 -0.63594333 -0.44149195 -0.24253984 -0.04111519 0.1607286 0.36093386 0.55745964 0.74830246 0.93151681 1.10523494 1.2676859 1.4172136 1.5522937 1.67154914 1.7737642 1.85789686 1.92308943 1.96867732 1.99419578 1.99938468 1.98419112 1.94876998 1.89348236 1.81889189 1.72575896 1.61503301 1.48784282 1.34548501 1.18941083 1.02121136 0.84260128 0.65540142 0.46152015 0.26293398 0.06166736 -0.14022792 -0.34069366 -0.53768625 -0.72919747 -0.91327498 -1.08804222] [ 0.47942554 0.56547586 0.64576151 0.71946403 0.78583206 0.84418904 0.89394004 0.93457789 0.96568831 0.98695414 0.9981586 0.99918747 0.99003025 0.9707803 0.94163385 0.90288804 0.85493786 0.79827213 0.73346852 0.66118766 0.5821664 0.49721034 0.40718552 0.31300971 0.21564296 0.11607786 0.01532943 -0.08557528 -0.1856076 -0.28374777 -0.37899532 -0.47037924 -0.55696794 -0.63787871 -0.71228671 -0.77943339 -0.83863424 -0.88928575 -0.93087155 -0.9629677 -0.98524701 -0.99748234 -0.99954898 -0.99142584 -0.97319574 -0.94504452 -0.90725918 -0.8602249 -0.80442117 -0.74041687 -0.6688645 -0.59049347 -0.50610273 -0.4165526 -0.32275597 -0.22566905 -0.12628158 -0.02560675 0.07532913 0.17549707 0.27387593 0.3694628 0.46128323 0.54840117 0.62992851 0.70503412 0.77295236 0.83299083 0.88453749 0.92706685 0.96014534 0.98343576 0.99670068 0.99980486 0.99271666 0.97550835 0.94835534 0.91153445 0.86542104 0.81048521 0.74728699 0.67647066 0.59875813 0.51494165 0.42587566 0.33246813 0.23567131 0.13647196 0.03588137 -0.06507501 -0.16536799 -0.26397515 -0.35989125 -0.45213849 -0.53977646 -0.62191174 -0.69770704 -0.76638965 -0.8272594 -0.87969576]]
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
timeit.timeit( """
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
""")
timeit.timeit( """
for i,c in enumerate( cpgs ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
ys[i,:] = fv( t )
""")
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
timeit.timeit( "loop1()")
timeit.timeit( "loop2()")
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
timeit.timeit( "loop1()")
timeit.timeit( "loop2()")
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
timeit.timeit( loop1)
timeit.timeit( loop2 )
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
#timeit.timeit( loop1)
loop1()
#timeit.timeit( loop2 )
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
timeit.timeit( loop1)
#loop1()
#timeit.timeit( loop2 )
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
timeit.timeit( loop1, number=1)
#loop1()
#timeit.timeit( loop2 )
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
print( "time loop1: ", timeit.timeit( loop1, number=1) )
#loop1()
#timeit.timeit( loop2 )
time loop1: 0.0005228680001891917
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
print( "time loop1: ", timeit.timeit( loop1, number=100) )
#loop1()
#timeit.timeit( loop2 )
time loop1: 0.05343027700018865
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
print( "time loop1:", timeit.timeit( loop1, number=100) )
#loop1()
print( "time loop2:", timeit.timeit( loop2, number = 100 )
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
print( "time loop1:", timeit.timeit( loop1, number=100) )
#loop1()
print( "time loop2:", timeit.timeit( loop2, number = 100 ) )
time loop1: 0.05019361700033187 time loop2: 0.026424144000884553
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100000)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
t1 = timeit.timeit( loop1, number=100)
print( "time loop1:", t1 )
#loop1()
timeit.timeit( loop2, number = 100 )
print( "time loop2:", t2 )
print( f"Speedup {t1/t2:.2f}" )
time loop1: 50.15082331800022
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100000)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
t1 = timeit.timeit( loop1, number=50)
print( "time loop1:", t1 )
#loop1()
t2 = timeit.timeit( loop2, number = 50 )
print( "time loop2:", t2 )
print( f"Speedup {t1/t2:.2f}" )
time loop1: 24.845633763000478 time loop2: 11.112092035000387 Speedup 2.24
Central Pattern Generator Design
Support Leg
- Fully extended. As tall as possible. Knee is locked to 0 degrees.
- Leg is rotating around the foot
t = np.linspace(0, 10, 100000)
params = [
[ 1, 1, 0 ],
[ 2, 1, 0 ],
[ 1, 2, 0 ],
[ 1, 1, 0.5 ]
]
cpgs = [ CPG(a, o, p ) for a, o, p in params ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
for i, param in enumerate( params ):
a, o, p = param
ax.plot(t, ys, label= f"Ampl={a}, Omega={o}, Phase={p}" )
ax.legend()
class CPG:
def __init__(self, amplitude, omega, phase = 0 ):
self.omega = omega
self.amplitude = amplitude
self.phase = phase
def control(self, t):
return self.amplitude * math.sin( self.phase + self.omega * t )
def __mul__( self, factor ):
n = CPG( self.omega, self.amplitude * factor, self.phase )
return n
def __add__( self, ad ):
n = CPG( self.omega, self.amplitude, self.phase + ad )
return n
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100000)
cpgs = [CPG( 1, 1, 0 ), CPG( 2, 1, 0 ), CPG( 1, 2, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
t1 = timeit.timeit( loop1, number=50)
print( "time loop1:", t1 )
#loop1()
t2 = timeit.timeit( loop2, number = 50 )
print( "time loop2:", t2 )
print( f"Speedup {t1/t2:.2f}" )
time loop1: 25.301924719000453 time loop2: 11.204088593000051 Speedup 2.26
t = np.linspace(0, 4*math.pi, 100000)
params = [
[ 1, 1, 0 ],
[ 2, 1, 0 ],
[ 1, 2, 0 ],
[ 1, 1, 0.5 ]
]
cpgs = [ CPG(a, o, p ) for a, o, p in params ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
for i, param in enumerate( params ):
a, o, p = param
ax.plot(t, ys[i], label= f"Ampl={a}, Omega={o}, Phase={p}" )
ax.legend()
g1 = addJBFigure("g1", 0, 0, fig )
plt.close()
CPG Examples
Amplitude changes height of the curve
Omega changes frequency
Phase shifts the entire curve along the time axis
Central Pattern Generator Design
Support Leg
- Fully extended. As tall as possible. Knee is locked to 0 degrees.
- Leg is rotating around the foot
Walking Animation
Walking Animation
class CPG:
def __init__(self, omega, amplitude, phase = 0 ):
self.omega = omega
self.amplitude = amplitude
self.phase = phase
def control(self, t):
return self.amplitude * math.sin( self.phase + self.omega * t )
def __mul__( self, factor ):
n = CPG( self.omega, self.amplitude * factor, self.phase )
return n
def __add__( self, ad ):
n = CPG( self.omega, self.amplitude, self.phase + ad )
return n
import timeit
fig = plt.figure()
t = np.linspace(0, 10, 100000)
cpgs = [CPG( 1, 1, 0 ), CPG( 1, 12, 0 ), CPG( 2, 1, 0 ), CPG( 1, 1, 0.5 ) ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def loop1():
for i,c in enumerate( cpgs ):
for j,tt in enumerate( t ):
ys[i,j] = c.control(tt)
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
def loop2():
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
t1 = timeit.timeit( loop1, number=50)
print( "time loop1:", t1 )
#loop1()
t2 = timeit.timeit( loop2, number = 50 )
print( "time loop2:", t2 )
print( f"Speedup {t1/t2:.2f}" )
time loop1: 25.453426924999803 time loop2: 11.17831539100007 Speedup 2.28
t = np.linspace(0, 4*math.pi, 100000)
params = [
[ 1, 1, 0 ],
[ 1, 2, 0 ],
[ 2, 1, 0 ],
[ 1, 1, 0.5 ]
]
cpgs = [ CPG(a, o, p ) for a, o, p in params ]
ys = np.zeros( ( len(cpgs), len(t) ) )
def make_fv( c ):
def fs( t ):
return c.control(t)
fv = np.vectorize( fs )
return fv
fvs = [ make_fv( c ) for c in cpgs ]
for i,fv in enumerate( fvs ):
ys[i,:] = fv( t )
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
for i, param in enumerate( params ):
a, o, p = param
ax.plot(t, ys[i], label= f"Ampl={a}, Omega={o}, Phase={p}" )
ax.legend()
g1 = addJBFigure("g1", 0, 0, fig )
plt.close()
CPG Examples
Amplitude changes height of the curve
Omega changes frequency
Phase shifts the entire curve along the time axis
Central Pattern Generator Design
Support Leg
- Fully extended. As tall as possible. Knee is locked to 0 degrees.
- Leg is rotating around the foot
Walking Animation
CPG-based Walking Gait