def getDistanceNeighbouringLeg(indAnt, indLeg):
"""Compute the distance between two neighbouring legs
"""
legMatch = [(0, 1), (1, 2), None, (3, 4), (4, 5), None]
if indLeg in (0, 1, 3, 4):
posA = up.getLegPos(indAnt, legMatch[indLeg][0])
posB = up.getLegPos(indAnt, legMatch[indLeg][1])
dist = np.linalg.norm(posA - posB)
return dist
else:
return 100
def closeEnough(indAnt, indLeg, targetPos):
"""Check if the foot position is close enough to the target position
check the position of the swing leg, if close enough, return true, so we can attach it to the target position
"""
curFootPos = up.getLegPos(indAnt, indLeg)
if (np.linalg.norm(curFootPos - targetPos) < 1):
return True
else:
return False
def determineFeetPos(indAnt, legTable, speed):
"""Determine the foot positions, given the table which contains the information for all six legs
"""
feetPos = []
for indLeg in range(6):
legInfo = legTable[indLeg]
if legInfo[0] == True:
# stance leg
feetPos.append(legInfo[3])
else:
# swing leg
if speed == 0:
feetPos.append(legInfo[2])
else:
T = computePeriod(speed)
t = legInfo[1]
lastPos = legInfo[2]
targetPos = legInfo[3]
dpos = swingNet2_position(T, t, lastPos, targetPos)
legPos = up.getLegPos(indAnt, indLeg)
feetPos.append(legPos + dpos/24)
return feetPos
