Python quaternion.from_rotation_vector() Examples

def test_from_rotation_vector():
    np.random.seed(1234)
    n_tests = 1000
    vecs = np.random.uniform(high=math.pi/math.sqrt(3), size=n_tests*3).reshape((n_tests, 3))
    quats = np.zeros(vecs.shape[:-1]+(4,))
    quats[..., 1:] = vecs[...]
    quats = quaternion.as_quat_array(quats)
    quats = np.exp(quats/2)
    quat_vecs = quaternion.as_rotation_vector(quats)
    quats2 = quaternion.from_rotation_vector(quat_vecs)
    assert allclose(quats, quats2) 

def accumulate_egomotion(rots, vels):
    # Accumulate translation and rotation
    egos = []
    qa = np.quaternion(1, 0, 0, 0)
    va = np.array([0., 0., 0.])
    for rot, vel in zip(rots, vels):
        vel_rot = quaternion.rotate_vectors(qa, vel)
        va += vel_rot
        qa = qa * quaternion.from_rotation_vector(rot)
        egos.append(np.concatenate(
            (quaternion.as_rotation_vector(qa), va), axis=0))
    return egos 

def __init__(self, polar_dist_range):
        #  self.angle = np.random.uniform(-np.pi, np.pi)
        self.angle = np.random.uniform(-np.pi, np.pi)
        self.velocity = np.random.uniform(0, 1)
        self.translation = np.array([np.sin(self.angle), np.cos(self.angle), 0]) * self.velocity
        #  # self.q = rand_quat()
        #  self.q = rand_quat_planar()
        self.rel_angle = rand_angle() / 2.0
        self.q = quaternion.from_rotation_vector(np.array([0, 0, 1.]) * self.rel_angle)

        polar_angle = np.random.uniform(-np.pi, np.pi)
        polar_distance = np.random.uniform(*polar_dist_range)
        self.start_position = np.array([np.sin(polar_angle), np.cos(polar_angle), 0]) * polar_distance
        #  self.q_start = rand_quat_planar()
        self.start_angle = rand_angle()
        self.q_start = quaternion.from_rotation_vector(np.array([0, 0, 1.]) * self.rel_angle)

        self.end_position = self.start_position + self.translation
        self.end_angle = self.start_angle + self.rel_angle
        #  self.q_end = self.q_start * self.q
        self.q_end = quaternion.from_rotation_vector(np.array([0, 0, 1.]) * self.end_angle)

        #  self.transform_start = np.eye(4)
        #  self.transform_start[:3,:3] = quaternion.as_rotation_matrix(self.q_start)
        #  self.transform_start[:3,3] = self.start_position
        self.transform_start = get_mat_angle(self.start_position, self.start_angle)

        #  self.rel_transform = np.eye(4)
        #  self.rel_transform[:3,:3] = quaternion.as_rotation_matrix(self.q)
        #  self.rel_transform[:3,3] = self.translation
        self.rel_transform = get_mat_angle(self.translation, self.rel_angle)

        #  self.transform_end = np.eye(4)
        #  self.transform_end[:3,3] = self.end_position
        #  self.transform_end[:3,:3] = quaternion.as_rotation_matrix(self.q_end)
        #  self.transform_end = np.matmul(self.rel_transform, self.transform_start)
        self.transform_end = get_mat_angle(self.end_position, self.end_angle) 

def aa_to_rot_matrix(data):
    """
    Converts the orientation data to represent angle axis as rotation matrices. `data` is expected in format
    (seq_length, n*3). Returns an array of shape (seq_length, n*9).
    """
    # reshape to have sensor values explicit
    data_c = np.array(data, copy=True)
    seq_length, n = data_c.shape[0], data_c.shape[1] // 3
    data_r = np.reshape(data_c, [seq_length, n, 3])

    qs = quaternion.from_rotation_vector(data_r)
    rot = np.reshape(quaternion.as_rotation_matrix(qs), [seq_length, n, 9])

    return np.reshape(rot, [seq_length, 9*n]) 

def test_pointgoal_with_gps_compass_sensor():
    config = get_config()
    if not os.path.exists(config.SIMULATOR.SCENE):
        pytest.skip("Please download Habitat test data to data folder.")
    config.defrost()
    config.TASK.SENSORS = [
        "POINTGOAL_WITH_GPS_COMPASS_SENSOR",
        "COMPASS_SENSOR",
        "GPS_SENSOR",
        "POINTGOAL_SENSOR",
    ]
    config.TASK.POINTGOAL_WITH_GPS_COMPASS_SENSOR.DIMENSIONALITY = 3
    config.TASK.POINTGOAL_WITH_GPS_COMPASS_SENSOR.GOAL_FORMAT = "CARTESIAN"

    config.TASK.POINTGOAL_SENSOR.DIMENSIONALITY = 3
    config.TASK.POINTGOAL_SENSOR.GOAL_FORMAT = "CARTESIAN"

    config.TASK.GPS_SENSOR.DIMENSIONALITY = 3

    config.freeze()
    with habitat.Env(config=config, dataset=None) as env:
        # start position is checked for validity for the specific test scene
        valid_start_position = [-1.3731, 0.08431, 8.60692]
        expected_pointgoal = [0.1, 0.2, 0.3]
        goal_position = np.add(valid_start_position, expected_pointgoal)

        # starting quaternion is rotated 180 degree along z-axis, which
        # corresponds to simulator using z-negative as forward action
        start_rotation = [0, 0, 0, 1]

        env.episode_iterator = iter(
            [
                NavigationEpisode(
                    episode_id="0",
                    scene_id=config.SIMULATOR.SCENE,
                    start_position=valid_start_position,
                    start_rotation=start_rotation,
                    goals=[NavigationGoal(position=goal_position)],
                )
            ]
        )

        env.reset()
        for _ in range(100):
            obs = env.step(sample_non_stop_action(env.action_space))
            pointgoal = obs["pointgoal"]
            pointgoal_with_gps_compass = obs["pointgoal_with_gps_compass"]
            compass = float(obs["compass"][0])
            gps = obs["gps"]
            # check to see if taking non-stop actions will affect static point_goal
            assert np.allclose(
                pointgoal_with_gps_compass,
                quaternion_rotate_vector(
                    quaternion.from_rotation_vector(
                        compass * np.array([0, 1, 0])
                    ).inverse(),
                    pointgoal - gps,
                ),
                atol=1e-5,
            )