def __init__(self):
# self.smk = HexapodKinematicsForIK()
self.smk = hp.HexapodKinematics()
self.baseAngles = np.array(([
np.random.uniform(0, -pi / 3),
np.random.uniform(0, pi / 3),
np.random.uniform(-pi / 8, pi / 8),
np.random.uniform(-pi / 8, pi / 8),
np.random.uniform(0, pi / 3),
np.random.uniform(0, -pi / 3)
])).reshape(1, 6)
self.q0 = [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]
self.expectedPoints = np.array(
([0.25, -0.25, 0.25, -0.25, 0.25, -0.25], [0., 0., 0., 0., 0.,
0.]))
xyzrpy = np.array(([0, 0, 0.21, 0.2, 0, 0]))
self.baseFrame = self.smk.trans(xyzrpy)
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")
accelOffset = np.loadtxt("accelOffset.txt")