def test_vectors2quat(self):
""" Test constructing quaternion from two given vectors """
vectors = [
np.array([1, 0, 0]),
np.array([0, 1, 0]),
np.array([0, 0, 1]),
-np.array([1, 0, 0]),
-np.array([0, 1, 0]),
-np.array([0, 0, 1]),
random_unit_length_vec(),
random_unit_length_vec(),
random_unit_length_vec(),
]
for v1, v2 in it.product(vectors, vectors):
quat = vectors2quat(v1, v2)
mat = quat2mat(quat)
maybe_v2 = mat @ v1
# Test that quat is normalized
assert np.abs(np.linalg.norm(quat) - 1.0) < 1e-8
# Make sure that given quaterion is the shortest path rotation
# np.clip is necessary due to potential minor numerical instabilities
assert quat_magnitude(quat) <= np.arccos(
np.clip(v1 @ v2, -1.0, 1.0)) + 1e-8
# Test that quaternion rotates input vector to output vector
assert np.linalg.norm(maybe_v2 - v2) < 1e-8
def test_distance_quat_from_being_up():
""" Test function 'distance_quat_from_being_up' """
initial_configuration = np.array([1.0, 0.0, 0.0, 0.0])
assert (np.linalg.norm(
cube_utils.distance_quat_from_being_up(initial_configuration, 2, 1) -
initial_configuration) < 1e-8)
assert (np.abs(
rotation.quat_magnitude(
cube_utils.distance_quat_from_being_up(initial_configuration, 2,
-1)) - np.pi) < 1e-8)
assert (np.abs(
rotation.quat_magnitude(
cube_utils.distance_quat_from_being_up(initial_configuration, 0,
1)) - np.pi / 2) < 1e-8)
assert (np.abs(
rotation.quat_magnitude(
cube_utils.distance_quat_from_being_up(initial_configuration, 0,
-1)) - np.pi / 2) < 1e-8)
assert (np.abs(
rotation.quat_magnitude(
cube_utils.distance_quat_from_being_up(initial_configuration, 1,
1)) - np.pi / 2) < 1e-8)
assert (np.abs(
rotation.quat_magnitude(
cube_utils.distance_quat_from_being_up(initial_configuration, 1,
-1)) - np.pi / 2) < 1e-8)
# Rotate along each axis but only slightly
transformations = np.eye(3) * 0.4
for i in range(3):
quat = rotation.euler2quat(transformations[i])
distance_quat = cube_utils.distance_quat_from_being_up(quat, 2, 1)
if i in [0, 1]:
result = rotation.quat_mul(quat, distance_quat)
assert np.linalg.norm(result - initial_configuration) < 1e-8
else:
assert np.linalg.norm(distance_quat - initial_configuration) < 1e-8
transformations = np.eye(3) * (np.pi / 2 - 0.3)
for i in range(3):
quat = rotation.euler2quat(transformations[i])
if i == 0:
distance_quat = cube_utils.distance_quat_from_being_up(quat, 1, 1)
assert np.abs(rotation.quat_magnitude(distance_quat) - 0.3) < 1e-8
elif i == 1:
distance_quat = cube_utils.distance_quat_from_being_up(quat, 0, -1)
assert np.abs(rotation.quat_magnitude(distance_quat) - 0.3) < 1e-8
else:
distance_quat = cube_utils.distance_quat_from_being_up(quat, 2, 1)
assert np.linalg.norm(distance_quat - initial_configuration) < 1e-8
def goal_distance(self, goal_state: dict, current_state: dict) -> dict:
""" Distance from the current goal to the target state. """
relative_goal = self.relative_goal(goal_state, current_state)
goal_distance = {
"cube_quat": rotation.quat_magnitude(relative_goal["cube_quat"]),
}
return goal_distance
def goal_distance(self, goal_state, current_state):
""" Distance from the current goal to the target state. """
relative_goal = self.relative_goal(goal_state, current_state)
goal_distance = {
"cube_pos": 0.0,
"cube_quat": rotation.quat_magnitude(relative_goal["cube_quat"]),
"cube_face_angle":
np.linalg.norm(relative_goal["cube_face_angle"]),
}
return goal_distance
def euler_angle_difference_single_pair(
q1: np.ndarray, q2: np.ndarray,
parallel_quats: List[np.ndarray]) -> np.ndarray:
assert q1.shape == q2.shape == (4, )
if np.allclose(q1, q2):
return np.zeros(3)
diffs = np.array([
rotation.quat_difference(rotation.quat_mul(q1, parallel_quat), q2)
for parallel_quat in parallel_quats
])
dists = rotation.quat_magnitude(diffs)
indices = np.argmin(dists, axis=0)
return rotation.quat2euler(diffs[indices])
def goal_distance(self, goal_state: dict, current_state: dict) -> dict:
relative_goal = self.relative_goal(goal_state, current_state)
pos_distances = np.linalg.norm(relative_goal["obj_pos"], axis=-1)
rot_distances = rotation.quat_magnitude(
rotation.quat_normalize(
rotation.euler2quat(relative_goal["obj_rot"])))
return {
"relative_goal":
relative_goal,
"obj_pos":
np.maximum(pos_distances + self.mujoco_simulation.goal_pos_offset,
0), # L2 dist
"obj_rot":
self.mujoco_simulation.goal_rot_weight *
rot_distances, # quat magnitude
}
def goal_distance(self, goal_state, current_state):
""" Distance from the current goal to the target state. """
relative_goal = self.relative_goal(goal_state, current_state)
goal_distance = {
"cube_pos":
0.0,
"cube_quat":
rotation.quat_magnitude(relative_goal["cube_quat"]),
"cube_face_angle":
np.linalg.norm(relative_goal["cube_face_angle"]),
"steps_to_solve":
len(self.goal_sequence) -
(self.goal_step % len(self.goal_sequence)),
"goal_step":
self.goal_step,
}
return goal_distance
def test_randomize_obs_wrapper():
state = np.random.get_state()
try:
np.random.seed(1)
quat_noise_factor = QUAT_NOISE_CORRECTION
# test that randomization of Euler angles and quaternions has same distance
n = 10000
a_bias = 0.1
additive_bias = a_bias * np.random.standard_normal(size=(n, 3))
# multiplicative bias does not make sense for random angles
angle = np.random.uniform(-np.pi, np.pi, size=(n, 3))
new_angle = angle + additive_bias
angle_dist = np.linalg.norm(rotation.subtract_euler(new_angle, angle),
axis=-1)
angle = np.random.uniform(-np.pi, np.pi, size=(n, 1))
axis = np.random.uniform(-1.0, 1.0, size=(n, 3))
quat = rotation.quat_from_angle_and_axis(angle, axis)
# double the additive bias to roughly equal the angular distance
noise_angle = a_bias * quat_noise_factor * np.random.standard_normal(
size=(n, ))
noise_axis = np.random.uniform(-1.0, 1.0, size=(n, 3))
noise_quat = rotation.quat_from_angle_and_axis(noise_angle, noise_axis)
new_quat = rotation.quat_mul(quat, noise_quat)
quat_diff = rotation.quat_difference(quat, new_quat)
quat_dist = rotation.quat_magnitude(quat_diff)
mean_angle_dist = np.mean(angle_dist)
mean_quat_dist = np.mean(quat_dist)
assert ((mean_angle_dist - mean_quat_dist) / mean_angle_dist) < 0.01
finally:
np.random.set_state(state)
