示例#1
0
    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.)
示例#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
示例#3
0
 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.)
示例#4
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
示例#5
0
 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)
示例#6
0
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
示例#7
0
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
示例#8
0
    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.)
示例#9
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
示例#10
0
#!/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
示例#11
0
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
示例#12
0
 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.)
示例#13
0
 def calc_error(self, q):
     return quaternion.rotation_intrinsic_distance(self.curr_quat, q)
示例#14
0
    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())