def initGait(legAngs, legPairs, delta, joyS):
for pair in range(0,2):
for i in range(0,3):
leg = legPairs[pair, i]
#swing and raise one leg at a time to initial position
if pair == 0:
delta[0,0] = joyS[0,0]/2.00
delta[1,0] = joyS[1,0]/2.00
delta[2,0] = -60
delta[3,0] = joyS[3,0]/2.00
delta[4,0] = joyS[4,0]/2.00
delta[5,0] = joyS[5,0]/2.00
else:
delta[0,0] = -joyS[0,0]/2.00
delta[1,0] = -joyS[1,0]/2.00
delta[2,0] = -60
delta[3,0] = -joyS[3,0]/2.00
delta[4,0] = -joyS[4,0]/2.00
delta[5,0] = -joyS[5,0]/2.00
legAngs[:,leg] = conv.diffMotion(delta, legAngs, leg)
sc.tx(legAngs, 250, 0)
#place leg back on the ground after it has reached its final position
delta[0,0] = 0
delta[1,0] = 0
delta[2,0] = 60
delta[3,0] = 0
delta[4,0] = 0
delta[5,0] = 0
legAngs[:,leg] = conv.diffMotion(delta, legAngs, leg)
sc.tx(legAngs, 250, 0)
return legAngs
def setTo90():
T = np.matrix(
[[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[np.pi / 2, np.pi / 2, np.pi / 2, np.pi / 2, np.pi / 2, np.pi / 2]])
sc.tx(T, 250, 0)
time.sleep(0.25)
return T
import serComms as sc
import initLegs as il
d = np.matrix([[0], [0], [40], [0], [0], [0]])
legAngs = il.setTo90()
##curTrans = np.matrix([[0, 0, 0, 0],
## [0, 0, 0, 0],
## [0, 0, 1, 0],
## [0, 0, 0, 1]])
Tcur = c.curFrame(legAngs[:, 0], 0)
deltaM = np.matrix([[0, -d[5], d[4], d[0]], [d[5], 0, -d[3], d[1]],
[-d[4], d[3], 0, d[2]], [0, 0, 0, 0]])
dT = deltaM * Tcur
legAngs[:, 0] = ik.ik(dT[0:3, 3], legAngs[0, 0], legAngs[1, 0], legAngs[2, 0],
0)
Tcur = c.curFrame(legAngs[:, 3], 3)
deltaM = np.matrix([[0, -d[5], d[4], d[0]], [d[5], 0, -d[3], d[1]],
[-d[4], d[3], 0, d[2]], [0, 0, 0, 0]])
dT = deltaM * Tcur
legAngs[:, 3] = ik.ik(dT[0:3, 3], legAngs[0, 3], legAngs[1, 3], legAngs[2, 3],
3)
sc.tx(legAngs, 250, 0)
#newAng = ik.ik(dU, a1, a2, a3)
def RPY(legAngs):
delta = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
s = 250
motion = 0
while motion != -1:
#get direction
input_char = msvcrt.getch()
if input_char.upper() == 'Q':
delta[0] = 5
delta[1] = 0
delta[2] = 0
delta[3] = 0
delta[4] = 0
delta[5] = 0
motion = 1
elif input_char.upper() == 'W':
delta[0] = 0
delta[1] = 5
delta[2] = 0
delta[3] = 0
delta[4] = 0
delta[5] = 0
motion = 1
elif input_char.upper() == 'E':
delta[0] = 0
delta[1] = 0
delta[2] = 5
delta[3] = 0
delta[4] = 0
delta[5] = 0
motion = 1
elif input_char.upper() == 'R':
delta[0] = 0
delta[1] = 0
delta[2] = 0
delta[3] = np.pi / 20
delta[4] = 0
delta[5] = 0
motion = 1
elif input_char.upper() == 'T':
delta[0] = 0
delta[1] = 0
delta[2] = 0
delta[3] = 0
delta[4] = np.pi / 20
delta[5] = 0
motion = 1
elif input_char.upper() == 'Y':
delta[0] = 0
delta[1] = 0
delta[2] = 0
delta[3] = 0
delta[4] = 0
delta[5] = np.pi / 20
motion = 1
elif input_char.upper() == 'A':
delta[0] = -5
delta[1] = 0
delta[2] = 0
delta[3] = 0
delta[4] = 0
delta[5] = 0
motion = 1
elif input_char.upper() == 'S':
delta[0] = 0
delta[1] = -5
delta[2] = 0
delta[3] = 0
delta[4] = 0
delta[5] = 0
motion = 1
elif input_char.upper() == 'D':
delta[0] = 0
delta[1] = 0
delta[2] = -5
delta[3] = 0
delta[4] = 0
delta[5] = 0
motion = 1
elif input_char.upper() == 'F':
delta[0] = 0
delta[1] = 0
delta[2] = 0
delta[3] = -np.pi / 20
delta[4] = 0
delta[5] = 0
motion = 1
elif input_char.upper() == 'G':
delta[0] = 0
delta[1] = 0
delta[2] = 0
delta[3] = 0
delta[4] = -np.pi / 20
delta[5] = 0
motion = 1
elif input_char.upper() == 'H':
delta[0] = 0
delta[1] = 0
delta[2] = 0
delta[3] = 0
delta[4] = 0
delta[5] = -np.pi / 20
motion = 1
elif input_char.upper() == 'X':
motion = -1
else:
motion = 0
if motion > 0:
for i in range(0, 6):
legAngs[:, i] = C.diffMotion(delta, legAngs, i)
sc.tx(legAngs, s, 0)
return legAngs
def gait(legAngs, JS):
#init gait params
s = 200
k = 0
kmax = 4.00
steps = 50
legPairs = np.matrix([[0, 2, 4],[1, 3, 5]])
delta = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
liftPair = 0
motion = 0
init = 0
stride = 30
lift = 60
turn = np.pi/6
Q = '+' #query move status
joyS = JS
seqComp = False
while True:
#print motion
while seqComp == False:
for pair in range(0,2):
for i in range(0,3):
leg = legPairs[pair, i]
if joyS[0,0]!=0 or joyS[1,0]!=0 or joyS[5,0]!=0:
if k < 1 or k >= (kmax):
if pair == liftPair:
delta[0,0] = 0
delta[1,0] = 0
delta[2,0] = -1*joyS[2,0]*(((0.500-(k/kmax))))
delta[3,0] = 0
delta[4,0] = 0
delta[5,0] = 0
else:
delta[0,0] = 0
delta[1,0] = 0
delta[2,0] = 0
delta[3,0] = 0
delta[4,0] = 0
delta[5,0] = 0
elif k >= 1 and k < (kmax):
if pair == liftPair:
delta[0,0] = -1*joyS[0,0]/(kmax-2.00)
delta[1,0] = -1*joyS[1,0]/(kmax-2.00)
delta[2,0] = -1*joyS[2,0]*(((0.500-(k/kmax))))
delta[3,0] = -1*joyS[3,0]/(kmax-2.00)
delta[4,0] = -1*joyS[4,0]/(kmax-2.00)
delta[5,0] = -1*joyS[5,0]/(kmax-2.00)
else:
delta[0,0] = joyS[0,0]/(kmax-2.00)
delta[1,0] = joyS[1,0]/(kmax-2.00)
delta[2,0] = 0
delta[3,0] = joyS[3,0]/(kmax-2.00)
delta[4,0] = joyS[4,0]/(kmax-2.00)
delta[5,0] = joyS[5,0]/(kmax-2.00)
## else:
## #stop leg from moving
## delta[0,0] = 0
## delta[1,0] = 0
## delta[2,0] = 0
## delta[3,0] = 0
## delta[4,0] = 0
## delta[5,0] = 0
legAngs[:,leg] = conv.diffMotion(delta, legAngs, leg)
Q = sc.tx(legAngs, s, 1)
while Q is '+':
Q = sc.tx(legAngs, s, 1)
sc.tx(legAngs, s, 0)
if joyS[0,0]!=0 or joyS[1,0]!=0 or joyS[5,0]!=0 or joyS[2,0]!=0:
k += 1
if k > kmax:
#seqComp = True
k = 0
if liftPair == 1:
liftPair = 0
else:
liftPair += 1
else:
seqComp = False
#Put legs back down here upon exiting while loop
#legAngs = il.setTo90()
return legAngs
def gait(legAngs):
#init gait params
s = 200
k = 0
kmax = 4.00
steps = 50
legPairs = np.matrix([[0, 2, 4], [1, 3, 5]])
delta = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
joyS = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
liftPair = 0
motion = 0
init = 0
stride = 30
lift = 60
turn = np.pi / 6
Q = '+' #query move status
## #stand robot up
## legAngs = il.setTo90()
while motion != -1:
#get direction
input_char = msvcrt.getch()
if input_char.upper() == 'W':
joyS[0, 0] = stride
joyS[1, 0] = 0.00
joyS[2, 0] = lift
joyS[5, 0] = 0.00
motion = 1
seqComp = False
if init == 0:
init = 1
elif input_char.upper() == 'S':
joyS[0, 0] = -stride
joyS[1, 0] = 0.00
joyS[2, 0] = lift
joyS[5, 0] = 0.00
motion = 2
seqComp = False
if init == 0:
init = 1
elif input_char.upper() == 'A':
joyS[0, 0] = 0.00
joyS[1, 0] = -stride
joyS[2, 0] = lift
joyS[5, 0] = 0.00
motion = 3
seqComp = False
if init == 0:
init = 1
elif input_char.upper() == 'D':
joyS[0, 0] = 0.00
joyS[1, 0] = stride
joyS[2, 0] = lift
joyS[5, 0] = 0.00
motion = 4
seqComp = False
if init == 0:
init = 1
elif input_char.upper() == 'E':
joyS[0, 0] = 0.00
joyS[1, 0] = 0.00
joyS[2, 0] = lift
joyS[5, 0] = turn
motion = 5
seqComp = False
if init == 0:
init = 1
elif input_char.upper() == 'R':
joyS[0, 0] = 0.00
joyS[1, 0] = 0.00
joyS[2, 0] = lift
joyS[5, 0] = -turn
motion = 6
seqComp = False
if init == 0:
init = 1
elif input_char.upper() == 'Q':
motion = -1
else:
motion = 0
if init == 1:
#init leg positions
legAngs = initGait(legAngs, legPairs, delta, joyS)
init = 2
if motion > 0:
#print motion
while seqComp == False:
for pair in range(0, 2):
for i in range(0, 3):
leg = legPairs[pair, i]
if joyS[0, 0] != 0 or joyS[1, 0] != 0 or joyS[5,
0] != 0:
if k < 1 or k >= (kmax):
if pair == liftPair:
delta[0, 0] = 0
delta[1, 0] = 0
delta[2, 0] = -1 * joyS[2, 0] * ((
(0.500 - (k / kmax))))
delta[3, 0] = 0
delta[4, 0] = 0
delta[5, 0] = 0
else:
delta[0, 0] = 0
delta[1, 0] = 0
delta[2, 0] = 0
delta[3, 0] = 0
delta[4, 0] = 0
delta[5, 0] = 0
elif k >= 1 and k < (kmax):
if pair == liftPair:
delta[0,
0] = -1 * joyS[0, 0] / (kmax - 2.00)
delta[1,
0] = -1 * joyS[1, 0] / (kmax - 2.00)
delta[2, 0] = -1 * joyS[2, 0] * ((
(0.500 - (k / kmax))))
delta[3,
0] = -1 * joyS[3, 0] / (kmax - 2.00)
delta[4,
0] = -1 * joyS[4, 0] / (kmax - 2.00)
delta[5,
0] = -1 * joyS[5, 0] / (kmax - 2.00)
else:
delta[0, 0] = joyS[0, 0] / (kmax - 2.00)
delta[1, 0] = joyS[1, 0] / (kmax - 2.00)
delta[2, 0] = 0
delta[3, 0] = joyS[3, 0] / (kmax - 2.00)
delta[4, 0] = joyS[4, 0] / (kmax - 2.00)
delta[5, 0] = joyS[5, 0] / (kmax - 2.00)
## else:
## #stop leg from moving
## delta[0,0] = 0
## delta[1,0] = 0
## delta[2,0] = 0
## delta[3,0] = 0
## delta[4,0] = 0
## delta[5,0] = 0
legAngs[:, leg] = conv.diffMotion(delta, legAngs, leg)
Q = sc.tx(legAngs, s, 1)
while Q is '+':
Q = sc.tx(legAngs, s, 1)
sc.tx(legAngs, s, 0)
if joyS[0, 0] != 0 or joyS[1, 0] != 0 or joyS[
5, 0] != 0 or joyS[2, 0] != 0:
k += 1
if k > kmax:
seqComp = True
k = 0
if liftPair == 1:
liftPair = 0
else:
liftPair += 1
else:
seqComp = False
#Put legs back down here upon exiting while loop
#legAngs = il.setTo90()
return legAngs
def freeGait(legAngs, legMode, onGround, drctn, LRU):
#drctn: x -> 0
# y -> 1
# rz -> 2
#set variables
diffMot = np.matrix([
[5, 0, 0], #rows are dx dy dz
[0, 5, 0],
[0, 0, np.pi / 20]
])
swingDiff = np.matrix([
[5, 0, 0], #rows are dx dy dz
[0, 5, 0],
[0, 0, np.pi / 20]
])
dF = [60, 60, 5 * (np.pi / 20)] #[xF,yF,rzF]
#dF = [60,80,5*(np.pi/20)]
w = [np.pi / dF[0], np.pi / dF[1], np.pi / dF[2]]
## dx = 5
## dy = 5
## rotz = np.pi/20
## swingDx = 5
## swingDy = 5
## swingRz = np.pi/20
delF = -60 #max lift amplitude
## yF = 60
## xF = 50
## rzF = (np.pi/20)*5
## w = np.pi/xF
maxRestrct = 0.8
delta = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
curStatus = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
curDM = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
#curDy = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
#curRz = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
desZ = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
prevZ = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
prevLegMode = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
prevLegModeP1 = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
prevRestrct = np.matrix([[0.00], [0.00], [0.00], [0.00], [0.00], [0.00]])
feetPos = np.matrix([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]])
data = np.matrix([[0.00, 0.00, 0.00, 0.00, 0.00],
[0.00, 0.00, 0.00, 0.00, 0.00],
[0.00, 0.00, 0.00, 0.00, 0.00],
[0.00, 0.00, 0.00, 0.00, 0.00],
[0.00, 0.00, 0.00, 0.00, 0.00],
[0.00, 0.00, 0.00, 0.00, 0.00]])
Q = '+' #query move status
s = 200
x = 0
g = 0
#while x<250:
while True:
legMode, onGround, restrct, rstDiff, prevRestrct = checkLegMode(
legMode, legAngs, onGround, prevRestrct, drctn, LRU)
for k in range(0, 6):
if legMode[k, 0] == 1 and onGround[
k, 0] == 0: #if leg is in swing mode do sine trajectory
if curDM[k, 0] <= dF[drctn]:
desZ[k, 0] = delF * np.sin(w[drctn] * curDM[k, 0])
curDM[k, 0] = curDM[k, 0] + swingDiff[drctn, drctn]
dz = desZ[k, 0] - prevZ[k, 0]
prevZ[k, 0] = desZ[k, 0]
if abs(dz) > 0:
delta[0, 0] = -swingDiff[0, drctn]
delta[1, 0] = -swingDiff[1, drctn]
delta[2, 0] = dz
delta[3, 0] = 0
delta[4, 0] = 0
delta[5, 0] = -swingDiff[2, drctn]
g = 1
else:
delta[0, 0] = 0
delta[1, 0] = 0
delta[2, 0] = 0
delta[3, 0] = 0
delta[4, 0] = 0
delta[5, 0] = 0
else:
onGround[k, 0] = 1
prevZ[k, 0] = 0
curDM[k, 0] = 0
elif legMode[k, 0] == 0 and (restrct[k, 0] < maxRestrct
or prevLegMode[k, 0] == 1
or rstDiff[k, 0] < 0):
delta[0, 0] = diffMot[0, drctn]
delta[1, 0] = diffMot[1, drctn]
delta[2, 0] = 0
delta[3, 0] = 0
delta[4, 0] = 0
delta[5, 0] = diffMot[2, drctn]
g = 2
else:
delta[0, 0] = 0
delta[1, 0] = 0
delta[2, 0] = 0
delta[3, 0] = 0
delta[4, 0] = 0
delta[5, 0] = 0
g = 3
legAngs[:, k] = conv.diffMotion(delta, legAngs, k)
prevLegMode[k, 0] = legMode[k, 0]
prevLegModeP1[k, 0] = prevLegMode[k, 0]
data[:, 0] = legMode
data[:, 1] = onGround
data[:, 2] = restrct
data[:, 3] = rstDiff
#data[:,4] = prevLegMode
#print curStatus
#print legMode
#print onGround
#print restrct
#print data
#print '----------------------'
#time.sleep(0.5)
Q = sc.tx(legAngs, s, 1)
while Q is '+':
Q = sc.tx(legAngs, s, 1)
sc.tx(legAngs, s, 0)
x += 1
print x