def setupWrapper():
# Wrapper sets up the snake monster for use in the CPG code.
print('Setting up snake monster...')
with open('names.dat', 'rb') as f:
names = pickle.load(f)
shoulders = names[1::3]
imu = tools.HebiLookup.newGroupFromNames(shoulders)
snakeData = setup.setupSnakeMonsterShoulderData()
smk = sm.SnakeMonsterKinematics()
with open('offsets.dat', 'rb') as f:
offsets = pickle.load(f)
setup.setupSnakeMonster()
cmd = tools.CommandStruct()
CF = SMCF.SMComplementaryFilter(snakeData, 'accelOffsets', accelOffsets,
'gyroOffsets', gyroOffsets)
CF.update(imu.getNextFeedback())
print('Setup complete!')
return (imu, smk, cmd, CF)
names = SMCF.NAMES
HebiLookup = tools.HebiLookup
shoulders = names[::3]
imu = HebiLookup.getGroupFromNames(shoulders)
snakeMonster = HebiLookup.getGroupFromNames(names)
while imu.getNumModules() != 6 or snakeMonster.getNumModules() != 18:
print('Found {} modules in shoulder group, {} in robot.'.format(
imu.getNumModules(), snakeMonster.getNumModules()),
end=' \n')
imu = HebiLookup.getGroupFromNames(shoulders)
snakeMonster = HebiLookup.getGroupFromNames(names)
print('Found {} modules in shoulder group, {} in robot.'.format(
imu.getNumModules(), snakeMonster.getNumModules()))
snakeData = setupSnakeMonsterShoulderData()
smk = hp.HexapodKinematics()
fbk_imu = feedbackStructure(imu)
while not fbk_imu.initialized:
fbk_imu = feedbackStructure(imu)
print('fbk_imu structure created')
fbk_sm = feedbackStructure(snakeMonster)
while not fbk_sm.initialized:
fbk_sm = feedbackStructure(imu)
print('fbk_sm structure created')
# Reading calibration files (or calibrating)
try:
gyroOffset = np.loadtxt("gyroOffset.txt")