def gimbal_lock_app():
import redis
import numpy as np
import time
from util import redis_encode, redis_decode
import rotations
global args
KEY_PREFIX = "cs223a::gimbal_lock::"
KEY_SET = KEY_PREFIX + "set"
KEY_EULER = KEY_PREFIX + "zyx_euler_angles"
KEY_FIXED = KEY_PREFIX + "xyz_fixed_angles"
KEY_MATRIX = KEY_PREFIX + "matrix"
KEY_AXIS = KEY_PREFIX + "rotation_axis"
KEY_ANGLE = KEY_PREFIX + "rotation_angle"
KEY_EULER_DES = KEY_PREFIX + "rotation_euler"
redis_db = redis.Redis(host=args.redis_host,
port=args.redis_port,
db=args.redis_db,
decode_responses=True)
R = np.eye(3)
axis = np.array([1, 0, 0])
redis_db.mset({
KEY_SET: "",
KEY_EULER: "0 0 0",
KEY_FIXED: "0 0 0",
KEY_MATRIX: redis_encode(R),
KEY_AXIS: redis_encode(axis),
KEY_ANGLE: "0"
})
while True:
time.sleep(1)
try:
val_set = redis_db.get(KEY_SET)
if val_set == KEY_EULER:
euler_angles = redis_decode(redis_db.get(KEY_EULER))
alpha, beta, gamma = euler_angles[0], euler_angles[
1], euler_angles[2]
R = rotations.zyx_euler_angles_to_mat(alpha, beta, gamma)
elif val_set == KEY_FIXED:
fixed_angles = redis_decode(redis_db.get(KEY_FIXED))
gamma, beta, alpha = fixed_angles[0], fixed_angles[
1], fixed_angles[2]
R = rotations.xyz_fixed_angles_to_mat(gamma, beta, alpha)
elif val_set == KEY_MATRIX:
R = redis_decode(redis_db.get(KEY_MATRIX))
alpha, beta, gamma = rotations.mat_to_zyx_euler_angles(R)
elif val_set == KEY_AXIS:
axis = redis_decode(redis_db.get(KEY_AXIS))
axis /= np.linalg.norm(axis)
redis_db.mset({
KEY_SET: "",
KEY_AXIS: redis_encode(axis),
})
continue
elif val_set == KEY_ANGLE:
theta = redis_decode(redis_db.get(KEY_ANGLE))[0]
q = rotations.axis_rotation_to_quat(axis, theta)
R = rotations.quat_to_mat(q).dot(R)
R = np.maximum(-1, np.minimum(
1, R)) # Ensure R doesn't cause arccos problems
alpha, beta, gamma = rotations.mat_to_zyx_euler_angles(R)
else:
continue
redis_db.mset({
KEY_SET: "",
KEY_MATRIX: redis_encode(R),
KEY_EULER: redis_encode([alpha, beta, gamma]),
KEY_FIXED: redis_encode([gamma, beta, alpha]),
KEY_ANGLE: "0"
})
except Exception as err:
print(err)
R = np.eye(3)
axis = np.array([1, 0, 0])
redis_db.mset({
KEY_SET: "",
KEY_EULER: "0 0 0",
KEY_FIXED: "0 0 0",
KEY_MATRIX: redis_encode(R),
KEY_AXIS: redis_encode(axis),
KEY_ANGLE: "0"
})
def euler_fixed_angles_app():
import redis
import numpy as np
import time
from util import redis_encode, redis_decode
import rotations
global args
KEY_PREFIX = "cs223a::euler_fixed_angles::"
KEY_SET = KEY_PREFIX + "set"
KEY_EULER = KEY_PREFIX + "zyx_euler_angles"
KEY_FIXED = KEY_PREFIX + "xyz_fixed_angles"
KEY_MATRIX = KEY_PREFIX + "matrix"
redis_db = redis.Redis(host=args.redis_host,
port=args.redis_port,
db=args.redis_db,
decode_responses=True)
redis_db.mset({
KEY_SET: "",
KEY_EULER: "0 0 0",
KEY_FIXED: "0 0 0",
KEY_MATRIX: "1 0 0; 0 1 0; 0 0 1"
})
while True:
time.sleep(1)
try:
val_set = redis_db.get(KEY_SET)
if val_set == KEY_EULER:
euler_angles = redis_decode(redis_db.get(KEY_EULER))
alpha, beta, gamma = euler_angles[0], euler_angles[
1], euler_angles[2]
R = rotations.zyx_euler_angles_to_mat(alpha, beta, gamma)
elif val_set == KEY_FIXED:
fixed_angles = redis_decode(redis_db.get(KEY_FIXED))
gamma, beta, alpha = fixed_angles[0], fixed_angles[
1], fixed_angles[2]
R = rotations.xyz_fixed_angles_to_mat(gamma, beta, alpha)
elif val_set == KEY_MATRIX:
R = redis_decode(redis_db.get(KEY_MATRIX))
alpha, beta, gamma = rotations.mat_to_zyx_euler_angles(R)
else:
continue
redis_db.mset({
KEY_SET: "",
KEY_MATRIX: redis_encode(R),
KEY_EULER: redis_encode([alpha, beta, gamma]),
KEY_FIXED: redis_encode([gamma, beta, alpha])
})
except Exception as err:
print(err)
redis_db.mset({
KEY_SET: "",
KEY_EULER: "0 0 0",
KEY_FIXED: "0 0 0",
KEY_MATRIX: "1 0 0; 0 1 0; 0 0 1"
})