Python Rotation.from_euler方法代码示例

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_as_euler_degenerate_asymmetric_axes():
    # Since we cannot check for angle equality, we check for dcm equality
    angles = np.array([
        [45, 90, 35],
        [35, -90, 20],
        [35, 90, 25],
        [25, -90, 15]
        ])

    with pytest.warns(UserWarning, match="Gimbal lock"):
        for seq_tuple in permutations('xyz'):
            # Extrinsic rotations
            seq = ''.join(seq_tuple)
            rotation = Rotation.from_euler(seq, angles, degrees=True)
            dcm_expected = rotation.as_dcm()

            angle_estimates = rotation.as_euler(seq, degrees=True)
            dcm_estimated = Rotation.from_euler(
                seq, angle_estimates, degrees=True
                ).as_dcm()

            assert_array_almost_equal(dcm_expected, dcm_estimated)

            # Intrinsic rotations
            seq = seq.upper()
            rotation = Rotation.from_euler(seq, angles, degrees=True)
            dcm_expected = rotation.as_dcm()

            angle_estimates = rotation.as_euler(seq, degrees=True)
            dcm_estimated = Rotation.from_euler(
                seq, angle_estimates, degrees=True
                ).as_dcm()

            assert_array_almost_equal(dcm_expected, dcm_estimated)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_from_euler_rotation_order():
    # Intrinsic rotation is same as extrinsic with order reversed
    np.random.seed(0)
    a = np.random.randint(low=0, high=180, size=(6, 3))
    b = a[:, ::-1]
    x = Rotation.from_euler('xyz', a, degrees=True).as_quat()
    y = Rotation.from_euler('ZYX', b, degrees=True).as_quat()
    assert_allclose(x, y)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_as_euler_asymmetric_axes():
    np.random.seed(0)
    n = 10
    angles = np.empty((n, 3))
    angles[:, 0] = np.random.uniform(low=-np.pi, high=np.pi, size=(n,))
    angles[:, 1] = np.random.uniform(low=-np.pi / 2, high=np.pi / 2, size=(n,))
    angles[:, 2] = np.random.uniform(low=-np.pi, high=np.pi, size=(n,))

    for seq_tuple in permutations('xyz'):
        # Extrinsic rotations
        seq = ''.join(seq_tuple)
        assert_allclose(angles, Rotation.from_euler(seq, angles).as_euler(seq))
        # Intrinsic rotations
        seq = seq.upper()
        assert_allclose(angles, Rotation.from_euler(seq, angles).as_euler(seq))

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_match_vectors_noise():
    np.random.seed(0)
    n_vectors = 100
    rot = Rotation.from_euler('xyz', np.random.normal(size=3))
    vectors = np.random.normal(size=(n_vectors, 3))
    result = rot.apply(vectors)

    # The paper adds noise as indepedently distributed angular errors
    sigma = np.deg2rad(1)
    tolerance = 1.5 * sigma
    noise = Rotation.from_rotvec(
        np.random.normal(
            size=(n_vectors, 3),
            scale=sigma
        )
    )

    # Attitude errors must preserve norm. Hence apply individual random
    # rotations to each vector.
    noisy_result = noise.apply(result)

    est, cov = Rotation.match_vectors(noisy_result, vectors)

    # Use rotation compositions to find out closeness
    error_vector = (rot * est.inv()).as_rotvec()
    assert_allclose(error_vector[0], 0, atol=tolerance)
    assert_allclose(error_vector[1], 0, atol=tolerance)
    assert_allclose(error_vector[2], 0, atol=tolerance)

    # Check error bounds using covariance matrix
    cov *= sigma
    assert_allclose(cov[0, 0], 0, atol=tolerance)
    assert_allclose(cov[1, 1], 0, atol=tolerance)
    assert_allclose(cov[2, 2], 0, atol=tolerance)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_from_euler_extrinsic_rotation_313():
    angles = [
        [30, 60, 45],
        [30, 60, 30],
        [45, 30, 60]
        ]
    dcm = Rotation.from_euler('zxz', angles, degrees=True).as_dcm()

    assert_array_almost_equal(dcm[0], np.array([
        [0.43559574, -0.65973961, 0.61237244],
        [0.78914913, -0.04736717, -0.61237244],
        [0.4330127, 0.75000000, 0.500000]
    ]))

    assert_array_almost_equal(dcm[1], np.array([
        [0.62500000, -0.64951905, 0.4330127],
        [0.64951905, 0.12500000, -0.750000],
        [0.4330127, 0.75000000, 0.500000]
    ]))

    assert_array_almost_equal(dcm[2], np.array([
        [-0.1767767, -0.88388348, 0.4330127],
        [0.91855865, -0.30618622, -0.250000],
        [0.35355339, 0.35355339, 0.8660254]
    ]))

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_from_euler_intrinsic_rotation_312():
    angles = [
        [30, 60, 45],
        [30, 60, 30],
        [45, 30, 60]
        ]
    dcm = Rotation.from_euler('ZXY', angles, degrees=True).as_dcm()

    assert_array_almost_equal(dcm[0], np.array([
        [0.3061862, -0.2500000, 0.9185587],
        [0.8838835, 0.4330127, -0.1767767],
        [-0.3535534, 0.8660254, 0.3535534]
    ]))

    assert_array_almost_equal(dcm[1], np.array([
        [0.5334936, -0.2500000, 0.8080127],
        [0.8080127, 0.4330127, -0.3995191],
        [-0.2500000, 0.8660254, 0.4330127]
    ]))

    assert_array_almost_equal(dcm[2], np.array([
        [0.0473672, -0.6123725, 0.7891491],
        [0.6597396, 0.6123725, 0.4355958],
        [-0.7500000, 0.5000000, 0.4330127]
    ]))

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_spline_properties():
    times = np.array([0, 5, 15, 27])
    angles = [[-5, 10, 27], [3, 5, 38], [-12, 10, 25], [-15, 20, 11]]

    rotations = Rotation.from_euler('xyz', angles, degrees=True)
    spline = RotationSpline(times, rotations)

    assert_allclose(spline(times).as_euler('xyz', degrees=True), angles)
    assert_allclose(spline(0).as_euler('xyz', degrees=True), angles[0])

    h = 1e-8
    rv0 = spline(times).as_rotvec()
    rvm = spline(times - h).as_rotvec()
    rvp = spline(times + h).as_rotvec()
    assert_allclose(rv0, 0.5 * (rvp + rvm), rtol=1e-15)

    r0 = spline(times, 1)
    rm = spline(times - h, 1)
    rp = spline(times + h, 1)
    assert_allclose(r0, 0.5 * (rm + rp), rtol=1e-14)

    a0 = spline(times, 2)
    am = spline(times - h, 2)
    ap = spline(times + h, 2)
    assert_allclose(a0, am, rtol=1e-7)
    assert_allclose(a0, ap, rtol=1e-7)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test02():

    geom = geom_tools.from_obj_file("/home/daiver/Downloads/R3DS_Wrap_3.3.17_Linux/Models/Basemeshes/WrapHand.obj")
    # geom = geom_tools.from_obj_file("/home/daiver/tmp.obj")
    print(f"Model loaded, "
          f"n vertices: {geom.n_vertices()}, "
          f"n vts: {geom.n_texture_vertices()}, "
          f"n polygons: {geom.n_polygons()}, "
          f"n triangles: {geom.n_triangles()}")

    np.set_printoptions(threshold=np.inf, linewidth=500)
    vertices_val = geom.vertices

    adjacency = adjacency_table_from_topology(geom.triangle_vertex_indices)

    target_to_base_indices = list(range(len(vertices_val)))
    targets_val = vertices_val.copy()
    rot_mat = Rotation.from_euler('z', 90, degrees=True).as_dcm()
    print(rot_mat)
    targets_val = targets_val @ rot_mat.T

    new_vertices = deform(
        vertices_val.T, adjacency,
        target_to_base_indices, targets_val.T
    )
    print(new_vertices)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test01():

    np.set_printoptions(threshold=np.inf, linewidth=500)
    vertices_val = np.array([
        [0, 0, 1],
        [1, 0, 0],
        [0, 1, 0],
        [-1, 0, 0],
        [0, -1, 0],
    ], dtype=np.float32)

    adjacency = [
        [1, 2, 3, 4],
        [0, 2, 4],
        [0, 1, 3],
        [0, 2, 4],
        [0, 1, 3]
    ]

    target_to_base_indices = list(range(len(vertices_val)))
    targets_val = vertices_val.copy()
    rot_mat = Rotation.from_euler('z', 90, degrees=True).as_dcm()
    print(rot_mat)
    targets_val = targets_val @ rot_mat.T

    new_vertices = deform(
        vertices_val.T, adjacency,
        target_to_base_indices, targets_val.T
    )
    print(new_vertices)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_from_euler_elementary_extrinsic_rotation():
    # Simple test to check if extrinsic rotations are implemented correctly
    dcm = Rotation.from_euler('zx', [90, 90], degrees=True).as_dcm()
    expected_dcm = np.array([
        [0, -1, 0],
        [0, 0, -1],
        [1, 0, 0]
    ])
    assert_array_almost_equal(dcm, expected_dcm)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_as_euler_symmetric_axes():
    np.random.seed(0)
    n = 10
    angles = np.empty((n, 3))
    angles[:, 0] = np.random.uniform(low=-np.pi, high=np.pi, size=(n,))
    angles[:, 1] = np.random.uniform(low=0, high=np.pi, size=(n,))
    angles[:, 2] = np.random.uniform(low=-np.pi, high=np.pi, size=(n,))

    for axis1 in ['x', 'y', 'z']:
        for axis2 in ['x', 'y', 'z']:
            if axis1 == axis2:
                continue
            # Extrinsic rotations
            seq = axis1 + axis2 + axis1
            assert_allclose(
                angles, Rotation.from_euler(seq, angles).as_euler(seq))
            # Intrinsic rotations
            seq = seq.upper()
            assert_allclose(
                angles, Rotation.from_euler(seq, angles).as_euler(seq))

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_as_euler_degenerate_symmetric_axes():
    # Since we cannot check for angle equality, we check for dcm equality
    angles = np.array([
        [15, 0, 60],
        [35, 0, 75],
        [60, 180, 35],
        [15, -180, 25],
        ])

    with pytest.warns(UserWarning, match="Gimbal lock"):
        for axis1 in ['x', 'y', 'z']:
            for axis2 in ['x', 'y', 'z']:
                if axis1 == axis2:
                    continue

                # Extrinsic rotations
                seq = axis1 + axis2 + axis1
                rotation = Rotation.from_euler(seq, angles, degrees=True)
                dcm_expected = rotation.as_dcm()

                angle_estimates = rotation.as_euler(seq, degrees=True)
                dcm_estimated = Rotation.from_euler(
                    seq, angle_estimates, degrees=True
                    ).as_dcm()

                assert_array_almost_equal(dcm_expected, dcm_estimated)

                # Intrinsic rotations
                seq = seq.upper()
                rotation = Rotation.from_euler(seq, angles, degrees=True)
                dcm_expected = rotation.as_dcm()

                angle_estimates = rotation.as_euler(seq, degrees=True)
                dcm_estimated = Rotation.from_euler(
                    seq, angle_estimates, degrees=True
                    ).as_dcm()

                assert_array_almost_equal(dcm_expected, dcm_estimated)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_spline_2_rotations():
    times = [0, 10]
    rotations = Rotation.from_euler('xyz', [[0, 0, 0], [10, -20, 30]],
                                    degrees=True)
    spline = RotationSpline(times, rotations)

    rv = (rotations[0].inv() * rotations[1]).as_rotvec()
    rate = rv / (times[1] - times[0])
    times_check = np.array([-1, 5, 12])
    dt = times_check - times[0]
    rv_ref = rate * dt[:, None]

    assert_allclose(spline(times_check).as_rotvec(), rv_ref)
    assert_allclose(spline(times_check, 1), np.resize(rate, (3, 3)))
    assert_allclose(spline(times_check, 2), 0, atol=1e-16)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_single_intrinsic_extrinsic_rotation():
    extrinsic = Rotation.from_euler('z', 90, degrees=True).as_dcm()
    intrinsic = Rotation.from_euler('Z', 90, degrees=True).as_dcm()
    assert_allclose(extrinsic, intrinsic)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_euler [as 别名]
def test_from_euler_single_rotation():
    quat = Rotation.from_euler('z', 90, degrees=True).as_quat()
    expected_quat = np.array([0, 0, 1, 1]) / np.sqrt(2)
    assert_allclose(quat, expected_quat)