def quater2ang_v(quater_1, quater_2, dt):
delta_q = qmult(quater_2, qconjugate(quater_1))
delta_q_len = np.linalg.norm(delta_q[1:])
delta_q_angle = 2 * np.arctan2(delta_q_len, delta_q[0])
w = delta_q[1:] * delta_q_angle * 1 / dt
return w
def _update_coordinate_system(self,
origin_device_id: Optional[DeviceId],
device_positions: Dict[DeviceId, np.ndarray],
device_rotations: Dict[DeviceId, np.ndarray],
num_samples: int = 10):
"""Updates the coordinate system origin."""
if origin_device_id:
# Collect samples for the devices.
pos_samples = []
quat_samples = []
for _ in range(num_samples):
self._refresh_poses()
state = self._get_tracker_state(origin_device_id,
ignore_device_transform=True)
pos_samples.append(state.pos)
quat_samples.append(state.rot_quat)
global_translation = -np.mean(pos_samples, axis=0)
global_rotation = qconjugate(average_quaternions(quat_samples))
origin_rx, origin_ry, origin_rz = quat2euler(global_rotation,
axes='rxyz')
logging.info('Have origin rotation: %1.2f %1.2f %1.2f', origin_rx,
origin_ry, origin_rz)
self._coord_system.set_global_transform(global_translation,
euler2mat(0, 0, origin_rz))
for device_id, position in device_positions.items():
self._coord_system.set_local_transform(device_id,
translation=position)
for device_id, rotation in device_rotations.items():
self._coord_system.set_local_transform(device_id,
rotation=rotation)
def test_qnorm():
qi = tq.qeye()
assert tq.qnorm(qi) == 1
assert tq.qisunit(qi)
qi[1] = 0.2
assert not tq.qisunit(qi)
# Test norm is sqrt of scalar for multiplication with conjugate.
# https://en.wikipedia.org/wiki/Quaternion#Conjugation,_the_norm,_and_reciprocal
for q in quats:
q_c = tq.qconjugate(q)
exp_norm = np.sqrt(tq.qmult(q, q_c)[0])
assert np.allclose(tq.qnorm(q), exp_norm)
def test_qnorm():
qi = tq.qeye()
assert tq.qnorm(qi) == 1
assert tq.qisunit(qi)
qi[1] = 0.2
assert not tq.qisunit(qi)
# Test norm is sqrt of scalar for multiplication with conjugate.
# https://en.wikipedia.org/wiki/Quaternion#Conjugation,_the_norm,_and_reciprocal
for q in quats:
q_c = tq.qconjugate(q)
exp_norm = np.sqrt(tq.qmult(q, q_c)[0])
assert np.allclose(tq.qnorm(q), exp_norm)
def difference_quat(q1, q2):
"""
Calculates the difference between two quaternions
Args:
q1 (np.array): quaternion
q2 (np.array): quaternion
Returns:
float: difference
"""
q2_conj = quat.qconjugate(q2)
return quat.qmult(q1, q2_conj)
def quar_axis_error(q_sp, q_state):
"""Compute the error in quaternions from the setpoints and robot state
Args:
q_sp (np.array): Reference signal Set point quaternion
q_state (np.array): Sensor Feedback quaternion
Returns:
exponential angle (np.array)
"""
# Quaternion multiplication q_set * (q_state)' target - state
q_state_conj = quaternions.qconjugate(q_state)
q_error = quaternions.qmult(q_sp, q_state_conj)
# Nearest rotation
if (q_error[0] < 0):
q_error = -1. * q_error
axis_error = quaternions.quat2axangle(q_error)
return axis_error[0] * axis_error[1]
def test_qconjugate():
# Takes sequence
cq = tq.qconjugate((1, 0, 0, 0))
# Returns float type
assert cq.dtype.kind == 'f'
def quat_sub(q1, q2):
"""
Returns q1 * cong(q2)
"""
return qops.qmult(q2, qops.qconjugate(q1))
def quat_frame_transform_inv(axes):
return qops.qconjugate(quat_frame_transform(axes))
def kinematic_tree(poses, j2d, cam): #, proc_param):
joints = {
'Pelvis': 0,
'Neck': 12,
'Chest': 3,
'L_Shoulder': 16,
'L_Elbow': 18,
'R_Shoulder': 17,
'R_Elbow': 19,
'L_Hip': 1,
'L_Knee': 4,
'L_Ankle': 7,
'R_Hip': 2,
'R_Knee': 5,
'R_Ankle': 8
}
# LR reverse for deepmimic
target_joints = {
'Pelvis': [4, 5, 6, 7],
'Neck': [12, 13, 14, 15],
'Chest': [8, 9, 10, 11],
'L_Shoulder': [39, 40, 41, 42],
'L_Elbow': [43],
'R_Shoulder': [25, 26, 27, 28],
'R_Elbow': [29],
'L_Hip': [30, 31, 32, 33],
'L_Knee': [34],
'L_Ankle': [35, 36, 37, 38],
'R_Hip': [16, 17, 18, 19],
'R_Knee': [20],
'R_Ankle': [21, 22, 23, 24],
}
"""
# motions[:, 0] = 0.0625
# motions[:, 1:4] # root position
# motions[:, 4:8] # root rotation
# motions[:, 8:12] # chest rotation
# motions[:, 12:16] # neck rotation
# motions[:, 16:20] # right hip rot
# motions[:, 20] # right knee
# motions[:, 21:25] # right ankle rot
# motions[:, 25:29] # right shoulder rotation
# motions[:, 30] # right elbow
# motions[:, 30:34] # left hip rot
# motions[:, 34] # left knee
# motions[:, 35:39] # left ankle
# motions[:, 39:43] # left shoulder rot
# motions[:, 43] # left elbow rot """
poses = poses.reshape((-1, 3))
motions = np.zeros(44)
r = [0.7071, 0, 0.7071,
0] # # Quaternion that represents 90 degrees around Y
# qconjugate(r) = [ 0.7071, 0, -0.7071, 0]
motions[1:4] = cam #root_position(j2d, cam, proc_param)
for joi, num in joints.items():
x = poses[num]
# change of basis: SMPL to DeepMimic
if joi in ['R_Knee', 'L_Knee']:
q = -vec_to_angle(x)
elif joi in ['L_Elbow', 'R_Elbow']:
q = vec_to_angle(x)
elif joi in ['Pelvis']:
q = vec_to_quaternion(x)
q = qq.qmult(r, q)
q = qq.qmult(q, qq.qconjugate(r))
q = qq.qmult([0, 1, 0, 0], q)
elif joi in ['L_Shoulder']:
q = vec_to_quaternion(x)
q = qq.qmult(
q, [0.7071, 0, 0, 0.7071]
) # [0.7071, 0, 0, 0.7071] Quaternion that represents 90 degrees around Z
q = qq.qmult(r, q)
q = qq.qmult(q, qq.qconjugate(r))
elif joi in ['R_Shoulder']:
q = vec_to_quaternion(x)
q = qq.qmult(
q, [0.7071, 0, 0, -0.7071]
) #[0.7071, 0, 0, -0.7071] Quaternion that represents 90 degrees around -Z
q = qq.qmult(r, q)
q = qq.qmult(q, qq.qconjugate(r))
else:
q = vec_to_quaternion(x)
q = qq.qmult(r, q)
q = qq.qmult(q, qq.qconjugate(r))
motions[target_joints[joi]] = q
return motions
def rotate(q, pt):
qmult1 = tfq.qmult(q, (np.append(0, pt).transpose()))
qmult2 = tfq.qmult(qmult1, tfq.qconjugate(q))
return (np.asarray(qmult2))[1:]
def build_kinematic_tree(theta, j2d, cam, proc_param):
"""
z: 3D joint coordinates 14x3
v: vectors
"""
# 0 r foot
# 1 r knee
# 2 r hip
# 3 l hip
# 4 l knee
# 5 l foot
# 6 r hand
# 7 r elbow
# 8 r shoulder
# 9 l shoulder
# 10 l elbow
# 11 l hand
# 12 thorax
# 13 head
# GYM joints
# motions[:, 0] = 0.0625
# motions[:, 4:8] = [1, 0,0,0] # root rotation
# motions[:, 8:12] = [1, 0,0,0] # chest rotation
# motions[:, 12:16] = [1, 0, 0, 0] # neck rotation
# motions[:, 16:20] = [1, 0, 0, 0] # right hip rot
# motions[:, 20] = [1, 0, 0, 0] # right knee
# motions[:, 21:25] = [1, 0, 0, 0] # right ankle rot
# motions[:, 25:29] = [1, 0, 0, 0] # right shoulder rotation
# motions[:, 30] = [1, 0, 0, 0] # right elbow
# motions[:, 30:34] = [1, 0, 0, 0] # left hip rot
# motions[:, 34] = [1, 0, 0, 0] # left knee
# motions[:, 35:39] = [1, 0, 0, 0] # left ankle
# motions[:, 39:43] = [1, 0, 0, 0] # left shoulder rot
# motions[:, 43] = [1, 0, 0, 0] # left elbow rot
# theta = theta[self.num_cam:(self.num_cam + self.num_theta)]
theta = theta.reshape((-1, 3))
z = np.zeros(44)
r = [0.7071, 0, 0.7071, 0] # SMPL to DeepMimic
z[1:4] = calcRootTranslation(j2d, cam, proc_param)
for joi, num in joints.items():
x = theta[num]
# change of basis: SMPL to DeepMimic
if joi in ['R_Knee', 'L_Knee']:
q = -vec_to_angle(x)
elif joi in ['L_Elbow', 'R_Elbow']:
q = vec_to_angle(x)
elif joi in ['Pelvis']:
q = vec_to_quaternion(x)
q = qq.qmult(r, q)
q = qq.qmult(q, qq.qconjugate(r))
q = qq.qmult([0, 1, 0, 0], q)
elif joi in ['L_Shoulder']:
q = vec_to_quaternion(x)
q = qq.qmult(q, [0.7071, 0, 0, 0.7071])
q = qq.qmult(r, q)
q = qq.qmult(q, qq.qconjugate(r))
elif joi in ['R_Shoulder']:
q = vec_to_quaternion(x)
q = qq.qmult(q, [0.7071, 0, 0, -0.7071])
q = qq.qmult(r, q)
q = qq.qmult(q, qq.qconjugate(r))
else:
q = vec_to_quaternion(x)
q = qq.qmult(r, q)
q = qq.qmult(q, qq.qconjugate(r))
z[target_joints[joi]] = q
return z
def test_qconjugate():
# Takes sequence
cq = tq.qconjugate((1, 0, 0, 0))
# Returns float type
assert cq.dtype.kind == 'f'
