def test_euler(self):
ang = [0.1, 0.2, 0.3]
for seq in ['XYZ', 'YXZ', 'xyz', 'yxz', 'xyx', 'XYX']:
expq = npq.from_float_array(
Rotation.from_euler(seq, ang).as_quat()[[3, 0, 1, 2]])
self.assertAlmostEqual(
npq.rotation_intrinsic_distance(quat_from_euler(seq, ang),
expq), 0.)
self.assertAlmostEqual(
npq.rotation_intrinsic_distance(quat_from_euler('Z', [1]),
quat_from_euler('z', [1])), 0.)
self.assertAlmostEqual(
npq.rotation_intrinsic_distance(quat_from_euler('Z', 2),
quat_from_euler('z', 2)), 0.)
def test_from_rotation_matrix(Rs):
rot_mat_eps = 10*eps
try:
from scipy import linalg
except ImportError:
rot_mat_eps = 400*eps
for i, R1 in enumerate(Rs):
R2 = quaternion.from_rotation_matrix(quaternion.as_rotation_matrix(R1))
d = quaternion.rotation_intrinsic_distance(R1, R2)
assert d < rot_mat_eps, (i, R1, R2, d) # Can't use allclose here; we don't care about rotor sign
Rs2 = quaternion.from_rotation_matrix(quaternion.as_rotation_matrix(Rs))
for R1, R2 in zip(Rs, Rs2):
d = quaternion.rotation_intrinsic_distance(R1, R2)
assert d < rot_mat_eps, (R1, R2, d) # Can't use allclose here; we don't care about rotor sign
def assert_pose(self, current_pose, desired_pose):
""" Assert pose based on the distances. """
current_pose = cast_transformation(current_pose)
desired_pose = cast_transformation(desired_pose)
self.assertAlmostEqual(
np.linalg.norm(current_pose[0] - desired_pose[0]), 0.)
self.assertAlmostEqual(
npq.rotation_intrinsic_distance(current_pose[1], desired_pose[1]),
0.)
def test_as_euler_angles():
np.random.seed(1843)
random_angles = [[np.random.uniform(-np.pi, np.pi),
np.random.uniform(-np.pi, np.pi),
np.random.uniform(-np.pi, np.pi)]
for i in range(5000)]
for alpha, beta, gamma in random_angles:
R1 = quaternion.from_euler_angles(alpha, beta, gamma)
R2 = quaternion.from_euler_angles(*list(quaternion.as_euler_angles(R1)))
d = quaternion.rotation_intrinsic_distance(R1, R2)
assert d < 4e3*eps, ((alpha, beta, gamma), R1, R2, d) # Can't use allclose here; we don't care about rotor sign
def test_joint(self):
j0 = Joint(name='j0')
self.assertEqual(j0.name, 'j0')
p, q = j0.joint_transformation()
self.assertAlmostEqual(np.linalg.norm(p), 0.)
self.assertAlmostEqual(npq.rotation_intrinsic_distance(q, npq.one), 0.)
p, q = j0.joint_transformation(5.)
self.assertAlmostEqual(np.linalg.norm(p), 0.)
self.assertAlmostEqual(npq.rotation_intrinsic_distance(q, npq.one), 0.)
self.assertTrue(j0.is_fixed)
self.assertFalse(j0.is_prismatic)
self.assertFalse(j0.is_revolute)
j2 = Joint(name='j2', joint_type='prismatic', null_value=0.1)
p, q = j2.joint_transformation(0)
self.assertAlmostEqual(np.linalg.norm(p), 0.)
self.assertAlmostEqual(npq.rotation_intrinsic_distance(q, npq.one), 0.)
p, q = j2.joint_transformation()
self.assertAlmostEqual(np.linalg.norm(p - [0.1, 0., 0.]), 0.)
self.assertAlmostEqual(npq.rotation_intrinsic_distance(q, npq.one), 0.)
self.assertFalse(j2.is_fixed)
self.assertTrue(j2.is_prismatic)
self.assertFalse(j2.is_revolute)
j3 = Joint(name='j3', joint_type='revolute')
p, q = j3.joint_transformation()
self.assertAlmostEqual(np.linalg.norm(p), 0.)
self.assertAlmostEqual(npq.rotation_intrinsic_distance(q, npq.one), 0.)
p, q = j3.joint_transformation(0.1)
self.assertAlmostEqual(np.linalg.norm(p), 0.)
self.assertAlmostEqual(
npq.rotation_intrinsic_distance(q, quat_from_euler('x', [0.1])),
0.)
self.assertFalse(j3.is_fixed)
self.assertFalse(j3.is_prismatic)
self.assertTrue(j3.is_revolute)
def get_rotations_diff(imgs_query, imgs_map):
rad_to_deg = 180.0 / math.pi
rotations_query = np.empty((len(imgs_query), 1), dtype=np.quaternion)
for i, (_, pose) in enumerate(imgs_query):
rotations_query[i, 0] = pose.r
rotations_query_tile = np.tile(rotations_query, (1, len(imgs_map)))
rotations_map = np.empty((1, len(imgs_map)), dtype=np.quaternion)
for i, (_, pose) in enumerate(imgs_map):
rotations_map[0, i] = pose.r
rotations_map_tile = np.tile(rotations_map, (len(imgs_query), 1))
return quaternion.rotation_intrinsic_distance(
rotations_query_tile, rotations_map_tile) * rad_to_deg
def pose_transform_distance(pose_a: kapture.PoseTransform, pose_b: kapture.PoseTransform) -> Tuple[float, float]:
"""
get translation and rotation distance between two PoseTransform
:return: (position_distance, rotation_distance in rad), can be nan is case of invalid comparison
"""
# handle NoneType with try expect blocks
try:
translation_distance = np.linalg.norm(pose_a.t - pose_b.t)
except TypeError:
translation_distance = math.nan
try:
rotation_distance = quaternion.rotation_intrinsic_distance(pose_a.r, pose_b.r)
except TypeError:
rotation_distance = math.nan
return translation_distance, rotation_distance
def test_quat_between_vectors(self):
v = np.array([1, 0, 0])
u = np.array([0, 0, 1])
q = quat_between_two_vectors(v, u)
u1 = npq.rotate_vectors(q, v)
self.assertAlmostEqual(np.linalg.norm(u - u1), 0.)
v = np.array([0.3, 0.56, .12])
u = np.array([0.16, 0.985, 1.65])
v = v / np.linalg.norm(v)
u = u / np.linalg.norm(u)
q = quat_between_two_vectors(v, u)
u1 = npq.rotate_vectors(q, v)
self.assertAlmostEqual(np.linalg.norm(u - u1), 0.)
self.assertAlmostEqual(
npq.rotation_intrinsic_distance(npq.one,
quat_between_two_vectors(v, v)),
0.)
def getError(cur_pose, prev_pose, cur_gt, prev_gt):
"""
Computes the error of the transformation between 2 poses
"""
error_t = np.linalg.norm((prev_pose[:3, -1] - cur_pose[:3, -1]) -
(cur_gt[:3, -1] - prev_gt[:3, -1]))
gt_prev_qt = quaternion.from_rotation_matrix(prev_gt[:3, :3])
gt_qt = quaternion.from_rotation_matrix(cur_gt[:3, :3])
gt_tform = gt_prev_qt * gt_qt.inverse()
cur_qt = quaternion.from_rotation_matrix(prev_pose[:3, :3])
prev_qt = quaternion.from_rotation_matrix(cur_pose[:3, :3])
qt_tform = prev_qt * cur_qt.inverse()
error_r = np.sum(
np.rad2deg(quaternion.rotation_intrinsic_distance(gt_tform, qt_tform)))
return error_r, error_t
#!/usr/bin/env python from __future__ import print_function, division, absolute_import import sys import numpy as np import quaternion from numpy import * eps = np.finfo(float).eps rot_mat_eps = 200*eps R1 = np.array([quaternion.quaternion(0, -math.sqrt(0.5), 0, -math.sqrt(0.5))]) print(R1) print(quaternion.as_rotation_matrix(R1)) R2 = quaternion.from_rotation_matrix(quaternion.as_rotation_matrix(R1)) print(R2) d = quaternion.rotation_intrinsic_distance(R1[0], R2[0]) print(d) print() sys.stdout.flush() sys.stderr.flush() assert d < rot_mat_eps, (R1, R2, d) # Can't use allclose here; we don't care about rotor sign
def test_metrics(Rs):
metric_precision = 4.e-15
# Check non-negativity
for R1 in Rs:
for R2 in Rs:
assert quaternion.rotor_chordal_distance(R1, R2) >= 0.
assert quaternion.rotor_intrinsic_distance(R1, R2) >= 0.
assert quaternion.rotation_chordal_distance(R1, R2) >= 0.
assert quaternion.rotation_intrinsic_distance(R1, R2) >= 0.
# Check discernibility
for R1 in Rs:
for R2 in Rs:
assert bool(quaternion.rotor_chordal_distance(R1, R2) > 0.) != bool(R1 == R2)
assert bool(quaternion.rotor_intrinsic_distance(R1, R2) > 0.) != bool(R1 == R2)
assert bool(quaternion.rotation_chordal_distance(R1, R2) > 0.) != bool(R1 == R2 or R1 == -R2)
assert bool(quaternion.rotation_intrinsic_distance(R1, R2) > 0.) != bool(R1 == R2 or R1 == -R2)
# Check symmetry
for R1 in Rs:
for R2 in Rs:
assert abs(quaternion.rotor_chordal_distance(R1, R2)
- quaternion.rotor_chordal_distance(R2, R1)) < metric_precision
assert abs(quaternion.rotor_intrinsic_distance(R1, R2)
- quaternion.rotor_intrinsic_distance(R2, R1)) < metric_precision
assert abs(quaternion.rotation_chordal_distance(R1, R2)
- quaternion.rotation_chordal_distance(R2, R1)) < metric_precision
assert abs(quaternion.rotation_intrinsic_distance(R1, R2)
- quaternion.rotation_intrinsic_distance(R2, R1)) < metric_precision
# Check triangle inequality
for R1 in Rs:
for R2 in Rs:
for R3 in Rs:
assert quaternion.rotor_chordal_distance(R1, R3) - metric_precision \
<= (quaternion.rotor_chordal_distance(R1, R2) + quaternion.rotor_chordal_distance(R2, R3))
assert quaternion.rotor_chordal_distance(R1, R3) - metric_precision \
<= (quaternion.rotor_chordal_distance(R1, R2) + quaternion.rotor_chordal_distance(R2, R3))
assert quaternion.rotation_intrinsic_distance(R1, R3) - metric_precision \
<= (
quaternion.rotation_intrinsic_distance(R1, R2) + quaternion.rotation_intrinsic_distance(R2, R3))
assert quaternion.rotation_intrinsic_distance(R1, R3) - metric_precision \
<= (
quaternion.rotation_intrinsic_distance(R1, R2) + quaternion.rotation_intrinsic_distance(R2, R3))
# Check distances from self or -self
for R in Rs:
# All distances from self should be 0.0
assert quaternion.rotor_chordal_distance(R, R) == 0.0
assert quaternion.rotor_intrinsic_distance(R, R) == 0.0
assert quaternion.rotation_chordal_distance(R, R) == 0.0
assert quaternion.rotation_intrinsic_distance(R, R) == 0.0
# Chordal rotor distance from -self should be 2
assert abs(quaternion.rotor_chordal_distance(R, -R) - 2.0) < metric_precision
# Intrinsic rotor distance from -self should be 2pi
assert abs(quaternion.rotor_intrinsic_distance(R, -R) - 2.0 * np.pi) < metric_precision
# Rotation distances from -self should be 0
assert quaternion.rotation_chordal_distance(R, -R) == 0.0
assert quaternion.rotation_intrinsic_distance(R, -R) == 0.0
# We expect the chordal distance to be smaller than the intrinsic distance (or equal, if the distance is zero)
for R in Rs:
assert (
quaternion.rotor_chordal_distance(quaternion.one, R) < quaternion.rotor_intrinsic_distance(quaternion.one,
R)
or R == quaternion.one)
# Check invariance under overall rotations: d(R1, R2) = d(R3*R1, R3*R2) = d(R1*R3, R2*R3)
for R1 in Rs:
for R2 in Rs:
for R3 in Rs:
assert abs(quaternion.rotor_chordal_distance(R1, R2)
- quaternion.rotor_chordal_distance(R3 * R1, R3 * R2)) < metric_precision
assert abs(quaternion.rotor_chordal_distance(R1, R2)
- quaternion.rotor_chordal_distance(R1 * R3, R2 * R3)) < metric_precision
assert abs(quaternion.rotation_intrinsic_distance(R1, R2)
- quaternion.rotation_intrinsic_distance(R3 * R1, R3 * R2)) < metric_precision
assert abs(quaternion.rotation_intrinsic_distance(R1, R2)
- quaternion.rotation_intrinsic_distance(R1 * R3, R2 * R3)) < metric_precision
def test_unit_pose(self):
pose = unit_pose()
self.assertAlmostEqual(np.linalg.norm(pose[0]), 0.)
self.assertAlmostEqual(
npq.rotation_intrinsic_distance(pose[1], npq.one), 0.)
def calc_error(self, q):
return quaternion.rotation_intrinsic_distance(self.curr_quat, q)
def step(self, action):
self.iter += 1
""" Step in pyphysx. """
dt = self.control_frequency.period() / self.num_sub_steps
dpose = (0.3 * np.tanh(action[:3]) * dt,
quat_from_euler('xyz',
np.pi * np.tanh(action[3:6]) * dt))
if self.open_gripper_on_leave and self.scene.simulation_time > self.action_end_time:
self.set_grip(self.FINGER_OPEN_POS)
elif self.close_gripper_on_leave and self.scene.simulation_time > self.action_end_time:
self.set_grip(0.)
else:
grip = np.tanh(action[
6]) * self.FINGER_OPEN_POS / 2 + self.FINGER_OPEN_POS / 2
# grip = 0.04 if grip < 0.05 else self.FINGER_OPEN_POS
self.set_grip(grip)
pose = self.hand_actors[0].get_global_pose()
for _ in range(self.num_sub_steps):
pose = multiply_transformations(dpose, pose)
self.hand_actors[0].set_kinematic_target(pose)
self.scene.simulate(dt)
if self.render:
if self.iter == 1:
self.renderer.clear_physx_scenes()
self.renderer.add_physx_scene(self.scene)
self.renderer.update()
# self.renderer.render_scene(self.scene, recompute_actors=self.iter == 1)
if self.realtime:
self.control_frequency.sleep()
""" Compute rewards """
hp, hq = self.hand_actors[0].get_global_pose()
op, oq = self.obj.get_global_pose()
grip = self.get_grip()
hrot = azimuth_elevation_from_quat(hq)
hp[2] -= 0.5 * self.obj_height
op[2] -= 0.5 * self.obj_height
traj_rewards = np.zeros(self.demo_hpos.shape[1])
b, s = self.reward_params['demo_hand_pos']['b'], self.reward_params[
'demo_hand_pos']['scale']
traj_rewards += s * np.exp(-0.5 * np.sum(
(self.demo_hpos[self.iter] - hp)**2, axis=-1) * b)
b, s = self.reward_params['demo_obj_pos']['b'], self.reward_params[
'demo_obj_pos']['scale']
traj_rewards += s * np.exp(-0.5 * np.sum(
(self.demo_opos[self.iter] - op)**2, axis=-1) * b)
b, s = self.reward_params['demo_hand_azi_ele'][
'b'], self.reward_params['demo_hand_azi_ele']['scale']
traj_rewards += s * np.exp(-0.5 * np.sum(
(self.demo_hrot[self.iter] - hrot)**2, axis=-1) * b)
b, s = self.reward_params['demo_obj_rot']['b'], self.reward_params[
'demo_obj_rot']['scale']
traj_rewards += s * np.exp(-0.5 * npq.rotation_intrinsic_distance(
self.demo_orot[self.iter], oq) * b)
b, s = self.reward_params['demo_touch']['b'], self.reward_params[
'demo_touch']['scale']
traj_rewards += s * self.demo_grip_weak_closed[self.iter] * np.exp(
-0.5 * b * (grip - (self.obj_radius - 0.005))**2)
if self.two_objects:
op2, oq2 = self.obj2.get_global_pose()
op2[2] -= 0.5 * self.obj2_height
b, s = self.reward_params['demo_obj_pos']['b'], self.reward_params[
'demo_obj_pos']['scale']
traj_rewards += s * np.exp(-0.5 * np.sum(
(self.demo_opos2[self.iter] - op2)**2, axis=-1) * b)
b, s = self.reward_params['demo_obj_rot']['b'], self.reward_params[
'demo_obj_rot']['scale']
traj_rewards += s * np.exp(-0.5 * npq.rotation_intrinsic_distance(
self.demo_orot2[self.iter], oq2) * b)
b, s = self.reward_params['demo_touch']['b'], self.reward_params[
'demo_touch']['scale']
traj_rewards += s * self.demo_grip_weak_closed[self.iter] * np.exp(
-0.5 * b * (grip - (self.obj2_radius - 0.005))**2)
if traj_rewards.size == 0:
reward = 0.
else:
reward = np.max(traj_rewards) if self.use_max_reward else np.mean(
traj_rewards)
""" Resolve singularities and collisions that we don't want. """
if self.hand_plane_hit():
if self.reset_on_plane_hit:
return EnvStep(self.get_obs(), 0., True, EnvInfo())
reward = 0.
if np.abs(hrot[1]) > np.deg2rad(
85): # do not allow motion close to singularity
if self.reset_on_singularity:
return EnvStep(self.get_obs(), 0., True, EnvInfo())
reward = 0.
return EnvStep(self.get_obs(), reward / self.horizon,
self.iter == self.horizon, EnvInfo())
