def test_inv_single_rotation(): np.random.seed(0) p = Rotation.from_quat(np.random.normal(size=(4,))) q = p.inv() p_dcm = p.as_dcm() q_dcm = q.as_dcm() res1 = np.dot(p_dcm, q_dcm) res2 = np.dot(q_dcm, p_dcm) eye = np.eye(3) assert_array_almost_equal(res1, eye) assert_array_almost_equal(res2, eye) x = Rotation.from_quat(np.random.normal(size=(1, 4))) y = x.inv() x_dcm = x.as_dcm() y_dcm = y.as_dcm() result1 = np.einsum('...ij,...jk->...ik', x_dcm, y_dcm) result2 = np.einsum('...ij,...jk->...ik', y_dcm, x_dcm) eye3d = np.empty((1, 3, 3)) eye3d[:] = np.eye(3) assert_array_almost_equal(result1, eye3d) assert_array_almost_equal(result2, eye3d)
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)
def test_apply_single_rotation_single_point(): dcm = np.array([ [0, -1, 0], [1, 0, 0], [0, 0, 1] ]) r_1d = Rotation.from_dcm(dcm) r_2d = Rotation.from_dcm(np.expand_dims(dcm, axis=0)) v_1d = np.array([1, 2, 3]) v_2d = np.expand_dims(v_1d, axis=0) v1d_rotated = np.array([-2, 1, 3]) v2d_rotated = np.expand_dims(v1d_rotated, axis=0) assert_allclose(r_1d.apply(v_1d), v1d_rotated) assert_allclose(r_1d.apply(v_2d), v2d_rotated) assert_allclose(r_2d.apply(v_1d), v2d_rotated) assert_allclose(r_2d.apply(v_2d), v2d_rotated) v1d_inverse = np.array([2, -1, 3]) v2d_inverse = np.expand_dims(v1d_inverse, axis=0) assert_allclose(r_1d.apply(v_1d, inverse=True), v1d_inverse) assert_allclose(r_1d.apply(v_2d, inverse=True), v2d_inverse) assert_allclose(r_2d.apply(v_1d, inverse=True), v2d_inverse) assert_allclose(r_2d.apply(v_2d, inverse=True), v2d_inverse)
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)
def test_zero_norms_from_quat(): x = np.array([ [3, 4, 0, 0], [0, 0, 0, 0], [5, 0, 12, 0] ]) with pytest.raises(ValueError): Rotation.from_quat(x)
def test_match_vectors_no_noise(): np.random.seed(0) c = Rotation.from_quat(np.random.normal(size=4)) b = np.random.normal(size=(5, 3)) a = c.apply(b) est, cov = Rotation.match_vectors(a, b) assert_allclose(c.as_quat(), est.as_quat())
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)
def test_from_dcm_calculation(): expected_quat = np.array([1, 1, 6, 1]) / np.sqrt(39) dcm = np.array([ [-0.8974359, -0.2564103, 0.3589744], [0.3589744, -0.8974359, 0.2564103], [0.2564103, 0.3589744, 0.8974359] ]) assert_array_almost_equal( Rotation.from_dcm(dcm).as_quat(), expected_quat) assert_array_almost_equal( Rotation.from_dcm(dcm.reshape((1, 3, 3))).as_quat(), expected_quat.reshape((1, 4)))
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))
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] ]))
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] ]))
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)
def test_apply_multiple_rotations_single_point(): dcm = np.empty((2, 3, 3)) dcm[0] = np.array([ [0, -1, 0], [1, 0, 0], [0, 0, 1] ]) dcm[1] = np.array([ [1, 0, 0], [0, 0, -1], [0, 1, 0] ]) r = Rotation.from_dcm(dcm) v1 = np.array([1, 2, 3]) v2 = np.expand_dims(v1, axis=0) v_rotated = np.array([[-2, 1, 3], [1, -3, 2]]) assert_allclose(r.apply(v1), v_rotated) assert_allclose(r.apply(v2), v_rotated) v_inverse = np.array([[2, -1, 3], [1, 3, -2]]) assert_allclose(r.apply(v1, inverse=True), v_inverse) assert_allclose(r.apply(v2, inverse=True), v_inverse)
def test_as_dcm_from_square_input(): quats = [ [0, 0, 1, 1], [0, 1, 0, 1], [0, 0, 0, 1], [0, 0, 0, -1] ] mat = Rotation.from_quat(quats).as_dcm() assert_equal(mat.shape, (4, 3, 3)) expected0 = np.array([ [0, -1, 0], [1, 0, 0], [0, 0, 1] ]) assert_array_almost_equal(mat[0], expected0) expected1 = np.array([ [0, 0, 1], [0, 1, 0], [-1, 0, 0] ]) assert_array_almost_equal(mat[1], expected1) assert_array_almost_equal(mat[2], np.eye(3)) assert_array_almost_equal(mat[3], np.eye(3))
def test_slerp_num_rotations_mismatch(): with pytest.raises(ValueError, match="number of rotations to be equal to " "number of timestamps"): np.random.seed(0) r = Rotation.from_quat(np.random.uniform(size=(5, 4))) t = np.arange(7) Slerp(t, r)
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)
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)
def test_as_dcm_from_generic_input(): quats = [ [0, 0, 1, 1], [0, 1, 0, 1], [1, 2, 3, 4] ] mat = Rotation.from_quat(quats).as_dcm() assert_equal(mat.shape, (3, 3, 3)) expected0 = np.array([ [0, -1, 0], [1, 0, 0], [0, 0, 1] ]) assert_array_almost_equal(mat[0], expected0) expected1 = np.array([ [0, 0, 1], [0, 1, 0], [-1, 0, 0] ]) assert_array_almost_equal(mat[1], expected1) expected2 = np.array([ [0.4, -2, 2.2], [2.8, 1, 0.4], [-1, 2, 2] ]) / 3 assert_array_almost_equal(mat[2], expected2)
def test_slerp_time_dim_mismatch(): with pytest.raises(ValueError, match="times to be specified in a 1 dimensional array"): np.random.seed(0) r = Rotation.from_quat(np.random.uniform(size=(2, 4))) t = np.array([[1], [2]]) Slerp(t, r)
def test_as_rotvec_single_2d_input(): quat = np.array([[1, 2, -3, 2]]) expected_rotvec = np.array([[0.5772381, 1.1544763, -1.7317144]]) actual_rotvec = Rotation.from_quat(quat).as_rotvec() assert_equal(actual_rotvec.shape, (1, 3)) assert_allclose(actual_rotvec, expected_rotvec)
def test_rotvec_calc_pipeline(): # Include small angles rotvec = np.array([ [0, 0, 0], [1, -1, 2], [-3e-4, 3.5e-4, 7.5e-5] ]) assert_allclose(Rotation.from_rotvec(rotvec).as_rotvec(), rotvec)
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)
def test_slerp_call_time_out_of_range(): np.random.seed(0) r = Rotation.from_quat(np.random.uniform(size=(5, 4))) t = np.arange(5) + 1 s = Slerp(t, r) with pytest.raises(ValueError, match="times must be within the range"): s([0, 1, 2]) with pytest.raises(ValueError, match="times must be within the range"): s([1, 2, 6])
def test_apply_single_rotation_multiple_points(): dcm = np.array([ [0, -1, 0], [1, 0, 0], [0, 0, 1] ]) r1 = Rotation.from_dcm(dcm) r2 = Rotation.from_dcm(np.expand_dims(dcm, axis=0)) v = np.array([[1, 2, 3], [4, 5, 6]]) v_rotated = np.array([[-2, 1, 3], [-5, 4, 6]]) assert_allclose(r1.apply(v), v_rotated) assert_allclose(r2.apply(v), v_rotated) v_inverse = np.array([[2, -1, 3], [5, -4, 6]]) assert_allclose(r1.apply(v, inverse=True), v_inverse) assert_allclose(r2.apply(v, inverse=True), v_inverse)
def test_from_single_2d_dcm(): dcm = [ [0, 0, 1], [1, 0, 0], [0, 1, 0] ] expected_quat = [0.5, 0.5, 0.5, 0.5] assert_array_almost_equal( Rotation.from_dcm(dcm).as_quat(), expected_quat)
def test_as_dcm_single_2d_quaternion(): quat = [[0, 0, 1, 1]] mat = Rotation.from_quat(quat).as_dcm() assert_equal(mat.shape, (1, 3, 3)) expected_mat = np.array([ [0, -1, 0], [1, 0, 0], [0, 0, 1] ]) assert_array_almost_equal(mat[0], expected_mat)
def test_from_single_3d_dcm(): dcm = np.array([ [0, 0, 1], [1, 0, 0], [0, 1, 0] ]).reshape((1, 3, 3)) expected_quat = np.array([0.5, 0.5, 0.5, 0.5]).reshape((1, 4)) assert_array_almost_equal( Rotation.from_dcm(dcm).as_quat(), expected_quat)
def test_from_square_quat_matrix(): # Ensure proper norm array broadcasting x = np.array([ [3, 0, 0, 4], [5, 0, 12, 0], [0, 0, 0, 1], [0, 0, 0, -1] ]) r = Rotation.from_quat(x) expected_quat = x / np.array([[5], [13], [1], [1]]) assert_array_almost_equal(r.as_quat(), expected_quat)
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))
def test_slerp_call_time_dim_mismatch(): np.random.seed(0) r = Rotation.from_quat(np.random.uniform(size=(5, 4))) t = np.arange(5) s = Slerp(t, r) with pytest.raises(ValueError, match="times to be specified in a 1 dimensional array"): interp_times = np.array([[3.5], [4.2]]) s(interp_times)
def test_from_single_2d_matrix(): mat = [[0, 0, 1], [1, 0, 0], [0, 1, 0]] expected_quat = [0.5, 0.5, 0.5, 0.5] assert_array_almost_equal( Rotation.from_matrix(mat).as_quat(), expected_quat)
def test_slerp_single_rot(): with pytest.raises(ValueError, match="must be a sequence of rotations"): r = Rotation.from_quat([1, 2, 3, 4]) Slerp([1], r)
def test_malformed_1d_from_rotvec(): with pytest.raises(ValueError, match='Expected `rot_vec` to have shape'): Rotation.from_rotvec([1, 2])
def rotate_axis(inp): hacky_trans_matrix = R.from_euler('xyz', [1.57, -1.57, 0]).as_dcm() hacky_trans_matrix = np.concatenate((hacky_trans_matrix, np.zeros(3)[:, np.newaxis]), axis=1) hacky_trans_matrix = np.concatenate((hacky_trans_matrix, np.array([[0, 0, 0, 1]])), axis=0) return np.dot(hacky_trans_matrix, inp.transpose())
def test_malformed_1d_from_mrp(): with pytest.raises(ValueError, match='Expected `mrp` to have shape'): Rotation.from_mrp([1, 2])
def rotation_ply(filename): with open('point_cloud/' + filename,'r') as input_file: header_cnt = 8 cnt = 0 all_lines = input_file.readlines() #Get header and data header = all_lines[:header_cnt] split_line = header[3].strip().split(' ') vertex_cnt = int(split_line[2]) data = all_lines[header_cnt:(vertex_cnt + header_cnt)] point_cloud = [] # Random sampling replace = True if vertex_cnt < 20000 else False sampled_int = np.random.choice(vertex_cnt, 20000, replace=replace) if vertex_cnt < 20000: vertex_cnt = vertex_cnt else: vertex_cnt = 20000 header[3] = ' '.join(split_line[:2] + [str(vertex_cnt) + '\n']) #We will randomly choose 20000 points, so change the point cloud info #Get point cloud if vertex_cnt >= 20000: idx = -1 for line in data: idx += 1 if idx not in sampled_int: continue line_split = line.strip().split(' ') new_line = [] for item in line_split: new_line.append(float(item)) point_cloud.append(new_line) else: for line in data: line_split = line.strip().split(' ') new_line = [] for item in line_split: new_line.append(float(item)) point_cloud.append(new_line) #Apply rotation by 90 degree along x-axis axis = [1,0,0] axis = axis / norm(axis) theta = math.pi/2 rot = Rotation.from_rotvec(theta * axis) new_point_cloud = rot.apply(point_cloud) # Saving point cloud out_filename = 'rotated_pc.ply' with open('point_cloud/' + out_filename,'w') as output_file: output_file.writelines(header) for line in new_point_cloud.tolist(): print_line = '' for item in line: print_line += "{:.5f}".format(item) + ' ' print_line += '\n' output_file.write(print_line) print('Rotation completed') return out_filename
def test_from_single_2d_quaternion(): x = np.array([[3, 4, 0, 0]]) r = Rotation.from_quat(x) expected_quat = x / 5 assert_array_almost_equal(r.as_quat(), expected_quat)
def convert_quat_to_rpy(self, w, x, y, z): r = Rotation.from_quat([x, y, z, w]) return r.as_euler('xyz', degrees=True)
def test_from_2d_single_mrp(): mrp = [[0, 0, 1.0]] expected_quat = np.array([[0, 0, 1, 0]]) result = Rotation.from_mrp(mrp) assert_array_almost_equal(result.as_quat(), expected_quat)
def test_concatenate_wrong_type(): with pytest.raises(TypeError, match='Rotation objects only'): Rotation.concatenate([Rotation.identity(), 1, None])
def test_matrix_calculation_pipeline(): mat = special_ortho_group.rvs(3, size=10, random_state=0) assert_array_almost_equal(Rotation.from_matrix(mat).as_matrix(), mat)
def test_generic_quat_matrix(): x = np.array([[3, 4, 0, 0], [5, 12, 0, 0]]) r = Rotation.from_quat(x) expected_quat = x / np.array([[5], [13]]) assert_array_almost_equal(r.as_quat(), expected_quat)
def make_arrow(name, midpoint, length=1, radius=0.2, rotation=(0,0,0), radius_ratio=3): """ Make an arrowbased on the centre position and rotation Parameters ---------- name : str Name of arrow midpoint : array (3,) Position in 3D space of arrow length : float Length of arrow radius : float Radius of arrow cylinder rotation : array (3,) Rotation of arrow in Euler XYZ rotation radius_ratio : float Ratio of the arrow cylinder to arrow head widths """ strut = bpy.ops.mesh.primitive_cylinder_add( vertices=16, radius=radius, depth=length, location=(float(midpoint[0]),float(midpoint[1]),float(midpoint[2])), rotation=np.radians(rotation) ) obj1 = bpy.context.object m1 = obj1.data obj1.name = f"strut_{name}" # Make rotator and get the position of the cone rotator = R.from_euler("xyz", rotation, degrees=True) cone_pos = midpoint + length/2 * unit_vector(rotator.apply([0,0,1])) cone = bpy.ops.mesh.primitive_cone_add( radius1=radius * radius_ratio, radius2=0, depth=length / radius_ratio, enter_editmode=False, align='WORLD', location=cone_pos, rotation=np.radians(rotation), scale=(1, 1, 1) ) obj2 = bpy.context.object m2 = obj2.data obj2.name = f"cone_{name}" # Do blender magic to select both objects and join them bpy.ops.object.select_all(action='DESELECT') obj2.select_set(True) obj1.select_set(True) bpy.context.view_layer.objects.active = obj2 bpy.context.view_layer.objects.active = obj1 bpy.ops.object.join() obj = bpy.context.object obj.name = f"{name}" mesh = obj.data mesh.name = f"mesh_{name}" return obj, mesh
def quat_transform(self, qua): # alpha beta gamma R1 = Rotation.from_quat(qua).as_matrix() return R1
def test_malformed_2d_from_rotvec(): with pytest.raises(ValueError, match='Expected `rot_vec` to have shape'): Rotation.from_rotvec([[1, 2, 3, 4], [5, 6, 7, 8]])
def update(self, t, state, flat_output): """ This function receives the current time, true state, and desired flat outputs. It returns the command inputs. Inputs: t, present time in seconds state, a dict describing the present state with keys # State x, position, m v, linear velocity, m/s q, quaternion [i,j,k,w] w, angular velocity, rad/s flat_output, a dict describing the present desired flat outputs with keys # Desired x, position, m x_dot, velocity, m/s x_ddot, acceleration, m/s**2 x_dddot, jerk, m/s**3 x_ddddot, snap, m/s**4 yaw, yaw angle, rad yaw_dot, yaw rate, rad/s Outputs: control_input, a dict describing the present computed control inputs with keys # Output cmd_motor_speeds, rad/s cmd_thrust, N (for debugging and laboratory; not used by simulator) cmd_moment, N*m (for debugging; not used by simulator) cmd_q, quaternion [i,j,k,w] (for laboratory; not used by simulator) """ cmd_motor_speeds = np.zeros((4, )) cmd_thrust = 0 cmd_moment = np.zeros((3, )) cmd_q = np.zeros((4, )) # STUDENT CODE HERE -------------------------------------------------------------------------------------------- # define error error_pos = state.get('x') - flat_output.get('x') error_vel = state.get('v') - flat_output.get('x_dot') error_vel = np.array(error_vel).reshape(3, 1) error_pos = np.array(error_pos).reshape(3, 1) # Equation 26 rdd_des = np.array(flat_output.get('x_ddot')).reshape( 3, 1) - np.matmul(self.Kd, error_vel) - np.matmul( self.Kp, error_pos) # Equation 28 F_des = (self.mass * rdd_des) + np.array([0, 0, self.mass * self.g ]).reshape(3, 1) # (3 * 1) # Find Rotation matrix R = Rotation.as_matrix(Rotation.from_quat( state.get('q'))) # Quaternions to Rotation Matrix # print(R.shape) # Equation 29, Find u1 b3 = R[0:3, 2:3] # print(b3) u1 = np.matmul(b3.T, F_des) # u1[0,0] to access value # print(np.transpose(b3)) # ----------------------- the following is to find u2 --------------------------------------------------------- # Equation 30 b3_des = F_des / np.linalg.norm(F_des) # 3 * 1 a_Psi = np.array([ np.cos(flat_output.get('yaw')), np.sin(flat_output.get('yaw')), 0 ]).reshape(3, 1) # 3 * 1 b2_des = np.cross(b3_des, a_Psi, axis=0) / np.linalg.norm( np.cross(b3_des, a_Psi, axis=0)) b1_des = np.cross(b2_des, b3_des, axis=0) # Equation 33 R_des = np.hstack((b1_des, b2_des, b3_des)) # print(R_des) # Equation 34 # R_temp = 0.5 * (np.matmul(np.transpose(R_des), R) - np.matmul(np.transpose(R), R_des)) temp = R_des.T @ R - R.T @ R_des R_temp = 0.5 * temp # orientation error vector e_R = 0.5 * np.array([-R_temp[1, 2], R_temp[0, 2], -R_temp[0, 1] ]).reshape(3, 1) # Equation 35 u2 = self.inertia @ ( -self.K_R @ e_R - self.K_w @ (np.array(state.get('w')).reshape(3, 1))) gama = self.k_drag / self.k_thrust Len = self.arm_length cof_temp = np.array([ 1, 1, 1, 1, 0, Len, 0, -Len, -Len, 0, Len, 0, gama, -gama, gama, -gama ]).reshape(4, 4) u = np.vstack((u1, u2)) F_i = np.matmul(np.linalg.inv(cof_temp), u) for i in range(4): if F_i[i, 0] < 0: F_i[i, 0] = 0 cmd_motor_speeds[i] = self.rotor_speed_max cmd_motor_speeds[i] = np.sqrt(F_i[i, 0] / self.k_thrust) if cmd_motor_speeds[i] > self.rotor_speed_max: cmd_motor_speeds[i] = self.rotor_speed_max cmd_thrust = u1[0, 0] cmd_moment[0] = u2[0, 0] cmd_moment[1] = u2[1, 0] cmd_moment[2] = u2[2, 0] cmd_q = Rotation.as_quat(Rotation.from_matrix(R_des)) control_input = { 'cmd_motor_speeds': cmd_motor_speeds, 'cmd_thrust': cmd_thrust, 'cmd_moment': cmd_moment, 'cmd_q': cmd_q } return control_input
symbol = "P43212" N_MOS_DOMAINS = 100 MOS_SPREAD = 1 ANISO_MOS_SPREAD = 0.5, 0.75, 1 eta_diffBragg_id = 19 miller_array_GT = fcalc_from_pdb(resolution=2, wavelength=1, algorithm='fft', ucell=ucell, symbol=symbol) Ncells_gt = 15, 15, 15 np.random.seed(3142019) # generate a random rotation rotation = Rotation.random(num=1, random_state=100)[0] Q = rec(rotation.as_quat(), n=(4, 1)) rot_ang, rot_axis = Q.unit_quaternion_as_axis_and_angle() a_real, b_real, c_real = sqr( uctbx.unit_cell( ucell).orthogonalization_matrix()).transpose().as_list_of_lists() x = col((-1, 0, 0)) y = col((0, -1, 0)) z = col((0, 0, -1)) rx, ry, rz = np.random.uniform(-180, 180, 3) RX = x.axis_and_angle_as_r3_rotation_matrix(rx, deg=True) RY = y.axis_and_angle_as_r3_rotation_matrix(ry, deg=True) RZ = z.axis_and_angle_as_r3_rotation_matrix(rz, deg=True) M = RX * RY * RZ a_real = M * col(a_real)
def test_from_generic_mrp(): mrp = np.array([[1, 2, 2], [1, -1, 0.5], [0, 0, 0]]) expected_quat = np.array( [[0.2, 0.4, 0.4, -0.8], [0.61538462, -0.61538462, 0.30769231, -0.38461538], [0, 0, 0, 1]]) assert_array_almost_equal(Rotation.from_mrp(mrp).as_quat(), expected_quat)
from cv2 import * # H = K2 * R2^T * [Id - (C1-C2) * (-n^T) / (n^T * (t-C1))] * R1 * K1^-1 # file: # folderName = 'Built_stereo_image' + '/Normal_case_1' # if not os.path.exists(folderName): # os.makedirs(folderName) text_name = 'ground_truth.txt' text = open(text_name, 'w+') # planes # blender use extrinsic rotation parameter to rotate the object. n = R.from_euler('xyz', [[89.7747, -6.769, 90], [48.4647, 75.4723, -32.5045], [-12.878, -54.781, 166.181]], degrees=True).apply([0.0, 0.0, 1.0]) t = array([[-1.58333, -0.444686, 0.412037], [1.18149, 0.41549, 0.285719], [1.40202, -0.012275, 0.578975]]) # cameras C = array([[8.0, -7.0, 5.0], [8.4642, -6.35394, 5]]) # 1 is left camera, H transform left to right r = R.from_euler('xyz', [[65.0, 0.0, 50.0], [64.6, 0, 54.7]], degrees=True) # camera internals f = focal_length * k = focal_length * 100 f = 1500 p = array([600, 400])
def test_deepcopy(): r = Rotation.from_quat([0, 0, np.sin(np.pi / 4), np.cos(np.pi / 4)]) r1 = copy.deepcopy(r) assert_allclose(r.as_matrix(), r1.as_matrix(), atol=1e-15)
dataset = sys.argv[1].replace("_local.txt", "") dataset = dataset.replace("_global.txt", "") folder = dataset # folder='p2at_met' rot_magnitude = [] with open(sys.argv[1]) as csvfile: file_reader = csv.DictReader(csvfile, delimiter=' ') for row in file_reader: source_file = f"{folder}/{row['source']}" target_file = f"{folder}/{row['target']}" trans = [float(row[f"t{index}"]) for index in range(1, 13)] # trans[3] = 0 # trans[7] = 0 # trans[11] = 0 trans = trans + [0, 0, 0, 1] trans = numpy.asarray(trans).reshape(4, 4) rot = Rotation.from_dcm(trans[:3, :3]) rot_magnitude.append(degrees(numpy.linalg.norm(rot.as_rotvec()))) print(f"{source_file} - {target_file} - {row['overlap']}") source = o3d.io.read_point_cloud(source_file) target = o3d.io.read_point_cloud(target_file) moved_source = copy.deepcopy(source) moved_source.transform(trans) source.paint_uniform_color([0, 1, 0]) target.paint_uniform_color([1, 0, 0]) moved_source.paint_uniform_color([0, 0, 1]) o3d.visualization.draw_geometries([source, target, moved_source]) # plt.plot(rot_magnitude,'*r') # plt.show()
def test_pickling(): r = Rotation.from_quat([0, 0, np.sin(np.pi / 4), np.cos(np.pi / 4)]) pkl = pickle.dumps(r) unpickled = pickle.loads(pkl) assert_allclose(r.as_matrix(), unpickled.as_matrix(), atol=1e-15)
def transform_and_save(path, df): ImgIds = [] if os.path.isdir('train_inputs'): shutil.rmtree('train_inputs', ignore_errors=True) if os.path.isdir('train_hms'): shutil.rmtree('train_hms', ignore_errors=True) if os.path.isdir('train_regs'): shutil.rmtree('train_regs', ignore_errors=True) os.mkdir('train_inputs') os.mkdir('train_hms') os.mkdir('train_regs') print('created directories') for n in range(len(df)): img_file = path + '_images/%s.jpg' % df.iloc[n].ImageId mask_file = path + '_masks/%s.jpg' % df.iloc[n].ImageId img = cv2.imread(img_file) mask = cv2.imread(mask_file) parameters = get_parameters() if mask is not None: img = img * (mask < 128) img = img[1355:, :, ::-1] #parameters = get_parameters() string = df.iloc[n].PredictionString roti, trxi = str_to_arrays(string) #print(rot) resized_img = cv2.resize(img, (iimg_w, iimg_h)) #print(s) rot = np.array(roti) trx = np.array(trxi) alpha, beta, gamma = parameters['train_rot'] M = parameters['pers'] RotP, RotM = RotateImage(alpha, beta, gamma, dx=img_w / 2) perspected_img = cv2.warpPerspective(resized_img, M, (ip_w, ip_h)) perspected_img_e = get_enhanced(perspected_img) rotated_img = cv2.warpPerspective(img, RotP, (img_w, img_h)) r_rotated_img = cv2.resize(rotated_img, (iimg_w, iimg_h)) p_rotated_img = cv2.warpPerspective(r_rotated_img, M, (ip_w, ip_h)) p_rotated_img_e = get_enhanced(p_rotated_img) r1 = R.from_euler('yxz', [-beta, alpha, gamma], degrees=False) r2 = R.from_euler('yxz', rot, degrees=False) r = r1 * r2 rot = r.as_euler('yxz') trx = np.dot(RotM[:3, :3], trx.T).T pred_str = '' for dof_id in range(len(rot)): dof_str = ' 1 %f %f %f %f %f %f' % ( rot[dof_id, 1], rot[dof_id, 0], rot[dof_id, 2], trx[dof_id, 0], trx[dof_id, 1], trx[dof_id, 2]) pred_str += dof_str perspected_img = perspected_img.astype(np.uint8) perspected_img_e = perspected_img_e.astype(np.uint8) p_rotated_img = p_rotated_img.astype(np.uint8) p_rotated_img_e = p_rotated_img_e.astype(np.uint8) np.save('train_inputs/%s_%d_n.npy' % (df.iloc[n].ImageId, 0), perspected_img) np.save('train_inputs/%s_%d_e.npy' % (df.iloc[n].ImageId, 0), perspected_img_e) np.save('train_inputs/%s_%d_n.npy' % (df.iloc[n].ImageId, 1), p_rotated_img) np.save('train_inputs/%s_%d_e.npy' % (df.iloc[n].ImageId, 1), p_rotated_img_e) ImgIds.append('%s_%d' % (df.iloc[n].ImageId, 0)) ImgIds.append('%s_%d' % (df.iloc[n].ImageId, 1)) coords = str_to_coords(string) hm, reg = pose(coords, iimg_h, iimg_w) perspected_hm = cv2.warpPerspective(hm, M, (ip_w, ip_h)) perspected_reg = cv2.warpPerspective(reg, M, (ip_w, ip_h), flags=cv2.INTER_NEAREST) perspected_hm_tf = tf.reshape(perspected_hm, [1, ip_h, ip_w, 1]) op_hm = tf.nn.max_pool2d(perspected_hm_tf, 4, 4, padding='VALID') op_reg_t = cv2.resize(perspected_reg, (op_w, op_h), interpolation=cv2.INTER_NEAREST) op_hm = np.squeeze(op_hm.numpy()) op_hm[(op_hm * (op_hm == maximum_filter(op_hm, footprint=np.ones( (3, 3)))) > 0.1)] = 1 op_reg = np.zeros_like(op_reg_t) y, x = np.where(op_hm == 1) op_reg[y, x, :] = op_reg_t[y, x, :] op_hm = np.reshape(op_hm, op_hm.shape + (1, )) np.save('train_hms/%s_%d.npy' % (df.iloc[n].ImageId, 0), op_hm) np.save('train_regs/%s_%d.npy' % (df.iloc[n].ImageId, 0), op_reg) coords = str_to_coords(pred_str) hm, reg = pose(coords, iimg_h, iimg_w) perspected_hm = cv2.warpPerspective(hm, M, (ip_w, ip_h)) perspected_reg = cv2.warpPerspective(reg, M, (ip_w, ip_h), flags=cv2.INTER_NEAREST) perspected_hm_tf = tf.reshape(perspected_hm, [1, ip_h, ip_w, 1]) op_hm = tf.nn.max_pool2d(perspected_hm_tf, 4, 4, padding='VALID') reg = cv2.resize(perspected_reg, (op_w, op_h), interpolation=cv2.INTER_NEAREST) op_hm = np.squeeze(op_hm.numpy()) op_hm[(op_hm * (op_hm == maximum_filter(op_hm, footprint=np.ones( (3, 3)))) > 0.1)] = 1 op_reg = np.zeros_like(reg) y, x = np.where(op_hm == 1) op_reg[y, x, :] = reg[y, x, :] op_hm = np.reshape(op_hm, op_hm.shape + (1, )) np.save('train_hms/%s_%d.npy' % (df.iloc[n].ImageId, 1), op_hm) np.save('train_regs/%s_%d.npy' % (df.iloc[n].ImageId, 1), op_reg) if n % 200 == 0: print('completed %d images' % n) np.save('image_names.npy', np.array(ImgIds))
def test_multiplication_stability(): qs = Rotation.random(50, random_state=0) rs = Rotation.random(1000, random_state=1) for q in qs: rs *= q * rs assert_allclose(np.linalg.norm(rs.as_quat(), axis=1), 1)
pfName = file.split(')')[0].split('(')[1] try: hkls.append(tuple([int(c) for c in pfName])) files.append(os.path.join(datadir, file)) except: #not hkls continue sortby = [sum([c**2 for c in h]) for h in hkls] hkls = [x for _, x in sorted(zip(sortby, hkls), key=lambda pair: pair[0])] files = [ x for _, x in sorted(zip(sortby, files), key=lambda pair: pair[0]) ] rot = R.from_euler('XZY', (13, -88, 90), degrees=True).as_dcm() pf = poleFigure(files, hkls, crystalSym, 'nd') """ rotate """ pf.rotate(rot) od = bunge(cellSize, crystalSym, sampleSym) hkls = np.array(hkls) phi = np.linspace(0, 2 * pi, 73) """ symmetry after """ fibre_e = {} fibre_q = {} weights = {} refls = symmetrise(crystalSym, hkls)
def test_from_single_3d_matrix(): mat = np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]).reshape((1, 3, 3)) expected_quat = np.array([0.5, 0.5, 0.5, 0.5]).reshape((1, 4)) assert_array_almost_equal( Rotation.from_matrix(mat).as_quat(), expected_quat)
def mrp_to_rotation_matrix(sigma): sigma_squared = np.inner(sigma, sigma) q0 = (1 - sigma_squared) / (1 + sigma_squared) q = [2 * sigma_i / (1 + sigma_squared) for sigma_i in sigma] q.extend([q0]) return Rotation.from_quat(q).as_matrix().T
def test_slerp_decreasing_times(): with pytest.raises(ValueError, match="strictly increasing order"): rnd = np.random.RandomState(0) r = Rotation.from_quat(rnd.uniform(size=(5, 4))) t = [0, 1, 3, 2, 4] Slerp(t, r)
def take_snapshot_rotation(queue): pipeline = rs.pipeline() pc = rs.pointcloud() config = rs.config() config.enable_stream(stream_type=rs.stream.depth, width=640, height=480, format=rs.format.z16, framerate=30) config.enable_stream(stream_type=rs.stream.color, width=640, height=480, format=rs.format.bgr8, framerate=30) pipeline.start(config) cnt = 0 #try: while True: frames = pipeline.wait_for_frames() cnt += 1 if cnt > 5: depth_frame = frames.get_depth_frame() color_frame = frames.get_color_frame() if not depth_frame or not color_frame: continue print("Get depth frame") color_image = np.asanyarray(color_frame.get_data()) color_image = color_image[..., ::-1] #Get intrinsic depth_intrinsics = rs.video_stream_profile( depth_frame.profile).get_intrinsics() w, h = depth_intrinsics.width, depth_intrinsics.height Rtilt = np.reshape(np.eye(3), -1) K = np.array([ depth_intrinsics.fx, 0, 0, 0, depth_intrinsics.fy, 0, depth_intrinsics.ppx, depth_intrinsics.ppy, 1 ]) print(Rtilt, K) if save_file: filename = 'snapshot_' + str(currentTime) + '.ply' if not os.path.exists('point_cloud'): os.mkdir('point_cloud') ply = rs.save_to_ply('point_cloud/' + filename) ply.set_option(rs.save_to_ply.option_ply_binary, False) ply.set_option(rs.save_to_ply.option_ignore_color, True) ply.set_option(rs.save_to_ply.option_ply_normals, False) ply.set_option(rs.save_to_ply.option_ply_mesh, False) print("Saving point cloud...") ply.process(depth_frame) print("Point cloud is created as {}".format(filename)) rgb_filename = filename[:-3] + 'jpg' if not os.path.exists('rgb_image'): os.mkdir('rgb_image') imageio.imwrite('rgb_image/' + rgb_filename, color_image) print("RGB Image is created as {}".format(rgb_filename)) else: decimate = rs.decimation_filter() decimate.set_option(rs.option.filter_magnitude, 2**1) depth_frame = decimate.process(depth_frame) points = pc.calculate(depth_frame) pc.map_to(depth_frame) pipeline.stop() break if save_file: with open('point_cloud/' + filename, 'r') as input_file: header_cnt = 8 cnt = 0 all_lines = input_file.readlines() #Get header and data header = all_lines[:header_cnt] split_line = header[3].strip().split(' ') vertex_cnt = int(split_line[2]) data = all_lines[header_cnt:(vertex_cnt + header_cnt)] point_cloud = [] # Random sampling replace = True if vertex_cnt < 20000 else False sampled_int = np.random.choice(vertex_cnt, 20000, replace=replace) if vertex_cnt < 20000: vertex_cnt = vertex_cnt else: vertex_cnt = 20000 header[3] = ' '.join( split_line[:2] + [str(vertex_cnt) + '\n'] ) #We will randomly choose 20000 points, so change the point cloud info #Get point cloud if vertex_cnt >= 20000: idx = -1 for line in data: idx += 1 if idx not in sampled_int: continue line_split = line.strip().split(' ') new_line = [] for item in line_split: new_line.append(float(item)) point_cloud.append(new_line) else: for line in data: line_split = line.strip().split(' ') new_line = [] for item in line_split: new_line.append(float(item)) point_cloud.append(new_line) #Apply rotation by 90 degree along x-axis axis = [1, 0, 0] axis = axis / norm(axis) theta = math.pi / 2 rot = Rotation.from_rotvec(theta * axis) new_point_cloud = rot.apply(point_cloud) # Saving point cloud out_filename = filename[:-4] + '_r.ply' with open('point_cloud/' + out_filename, 'w') as output_file: output_file.writelines(header) for line in new_point_cloud.tolist(): print_line = '' for item in line: print_line += "{:.5f}".format(item) + ' ' print_line += '\n' output_file.write(print_line) print('Rotation completed') #queue.put(out_filename) else: v = points.get_vertices() verts = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz verts = verts[verts[:, 2] < 5] # clip where z is farther than 10 verts = verts[verts[:, 2] > 0.1] # clip where z is closer than 0.1 data = verts vertex_cnt = data.shape[0] # Random sampling replace = True if vertex_cnt < 20000 else False sampled_int = np.random.choice(vertex_cnt, 20000, replace=replace) point_cloud = verts[sampled_int] if vertex_cnt < 20000: vertex_cnt = vertex_cnt else: vertex_cnt = 20000 #Apply rotation by 90 degree along x-axis axis = [1, 0, 0] axis = axis / norm(axis) theta = -math.pi / 2 rot = Rotation.from_rotvec(theta * axis) new_point_cloud = rot.apply(point_cloud) print('Rotation completed') #print("Point cloud shape", new_point_cloud.shape) # (20000, 3) pc_path = os.path.join(BASE_DIR, 'point_cloud', 'pc.npy') np.save(pc_path, new_point_cloud) img_path = os.path.join(BASE_DIR, 'rgb_image', 'rgb.npy') #print("Image shape", color_image.shape) #(480, 640, 3) np.save(img_path, color_image) queue.put(pc_path) queue.put(img_path) queue.put((Rtilt, K)) print("Sending queue finished") """ Using shared array for multiprocessing... replaced
def test_from_2d_single_rotvec(): rotvec = [[1, 0, 0]] expected_quat = np.array([[0.4794255, 0, 0, 0.8775826]]) result = Rotation.from_rotvec(rotvec) assert_array_almost_equal(result.as_quat(), expected_quat)