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)
# embed()
# shoulders = names[::3]
# imu = HebiLookup.getGroupFromNames(shoulders)
#snakeMonster = HebiLookup.getGroupFromNames(names)
# while imu.getNumModules() != 6:
# print('Found {} modules in shoulder group, {} in robot.'.format(imu.getNumModules(), snakeMonster.getNumModules()), end=' \r')
# imu = HebiLookup.getGroupFromNames(shoulders)
# print('Found {} modules in shoulder group, {} in robot.'.format(imu.getNumModules(), snakeMonster.getNumModules()))
# snakeData = setup.setupSnakeMonsterShoulderData()
# smk = hp.HexapodKinematics()
# fbk = feedbackStructure(imu)
# gyroOffset, accelOffset = SMCF.calibrateOffsets(fbk)
# #setup.setupSnakeMonster()
cmd = tools.CommandStruct()
# CF = SMCF.SMComplementaryFilter(accelOffset=accelOffset, gyroOffset=gyroOffset)
# fbk.getNextFeedback()
# CF.update(fbk)
# time.sleep(0.02)
# pose = []
# while pose is None or not list(pose):
# fbk.getNextFeedback()
# CF.update(fbk)
# pose = copy(CF.R)
print('Setup complete!')
## Initialize Variables
'planeLog': []
}
cpg['zHistory'] = cpg['zHistory'] * cpg['bodyHeight']
## Walk the Snake Monster
print('Finding initial stance...')
#joy = Controller()
cpgJoy = True
shoulders2 = list(range(2, 18, 3))
elbows = list(range(3, 18, 3))
sampleIter = 600
cnt = 0
controller = joystickController(cpg)
controller.cmd = tools.CommandStruct()
controller.dt = dt
controller.nIter = nIter
controller.cntr = cntr
controller.t = 0
#rospy.init_node('cmd_vel_listener', anonymous=False)
rospy.Subscriber("/cmd_vel", Twist, controller.callback, queue_size=10)
#rospy.Rate(1/dt)
rospy.spin()