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")