def vectorPairsToEuler(v1, v2, v3, v4): assert(v1.shape[0] == 3) assert(v2.shape[0] == 3) assert(v3.shape[0] == 3) assert(v4.shape[0] == 3) v1 = v1/LA.norm(v1) v2 = v2/LA.norm(v2) v3 = v3/LA.norm(v3) v4 = v4/LA.norm(v4) assert(np.abs(np.dot(v1, v3) - np.dot(v2, v4)) < 1e-6) # for plane 1 p1 = np.array([0,0,0]) p2 = 0.5*(v1 + v2) p3 = np.cross(v1, v2) A1, B1, C1, D1 = getPlane(p1,p2,p3) # for plane 2 p1 = np.array([0,0,0]) p2 = 0.5*(v3 + v4) p3 = np.cross(v3, v4) A2, B2, C2, D2 = getPlane(p1,p2,p3) # compute axis M = [[B1, C1], [B2, C2]] b = [-D1-A1, -D2-A2] parameters = np.matmul(LA.inv(M), b) axis = np.array([1, parameters[0], parameters[1]]) axis = axis/LA.norm(axis) # compute theta t1 = v1 - np.dot(v1, axis)*axis t2 = v2 - np.dot(v2, axis)*axis theta = np.arccos(np.dot(t1, t2)/LA.norm(t1)/LA.norm(t2)) # check direction euler = trans.axangle2euler(axis, theta) rotMat = trans.euler2mat(euler[0], euler[1], euler[2]) v2New = np.matmul(rotMat, v1) if (LA.norm(v2 - v2New) < 1e-6): return axis, theta, euler euler = trans.axangle2euler(axis, -theta) return axis, -theta, euler
def get_yaw(self): o = self.client.getOrientation() vec, theta = quaternions.quat2axangle( [o.w_val, o.x_val, o.y_val, o.z_val]) new_vec = self.units.vel3d_from_as(vec) roll, pitch, yaw = euler.axangle2euler(new_vec, theta) return yaw
def test_euler_axes(): # Test there and back with all axis specs aba_perms = [(v[0], v[1], v[0]) for v in permutations('xyz', 2)] axis_perms = list(permutations('xyz', 3)) + aba_perms for (a, b, c), mat in zip(euler_tuples, euler_mats): for rs, axes in product('rs', axis_perms): ax_spec = rs + ''.join(axes) conventioned = [EulerFuncs(ax_spec)] if ax_spec in euler.__dict__: conventioned.append(euler.__dict__[ax_spec]) mat = euler2mat(a, b, c, ax_spec) a1, b1, c1 = mat2euler(mat, ax_spec) mat_again = euler2mat(a1, b1, c1, ax_spec) assert_almost_equal(mat, mat_again) quat = euler2quat(a, b, c, ax_spec) a1, b1, c1 = quat2euler(quat, ax_spec) mat_again = euler2mat(a1, b1, c1, ax_spec) assert_almost_equal(mat, mat_again) ax, angle = euler2axangle(a, b, c, ax_spec) a1, b1, c1 = axangle2euler(ax, angle, ax_spec) mat_again = euler2mat(a1, b1, c1, ax_spec) assert_almost_equal(mat, mat_again) for obj in conventioned: mat = obj.euler2mat(a, b, c) a1, b1, c1 = obj.mat2euler(mat) mat_again = obj.euler2mat(a1, b1, c1) assert_almost_equal(mat, mat_again) quat = obj.euler2quat(a, b, c) a1, b1, c1 = obj.quat2euler(quat) mat_again = obj.euler2mat(a1, b1, c1) assert_almost_equal(mat, mat_again) ax, angle = obj.euler2axangle(a, b, c) a1, b1, c1 = obj.axangle2euler(ax, angle) mat_again = obj.euler2mat(a1, b1, c1) assert_almost_equal(mat, mat_again)
def update_hkl(_=None): h = int(txt_h.text) k = int(txt_k.text) l = int(txt_l.text) structure = structures[current_structure]['structure'] rotation_angle = angle_between_directions(structure, (0, 0, 1), (h, k, l)) direction = direction_to_cartesian(structure, (h, k, l)) direction /= np.linalg.norm(direction) axis = np.cross(np.array([0.0, 0.0, 1.0]), direction) if np.count_nonzero(axis) == 0: # Guard against parallel directions axis = np.array([0, 0, 1]) axis /= np.linalg.norm(axis) # The rotation should describe the "camera" rotation, so we need to invert it rotation_angle = -rotation_angle phi, theta, psi = np.rad2deg( axangle2euler(axis, rotation_angle, axes='rzxz')) if phi < 0: phi += 360 if theta < 0: theta += 360 if psi < 0: psi += 360 slider_phi.set_val(phi) slider_theta.set_val(theta) slider_psi.set_val(psi) update_pattern()
def _get_rotation_to_beam_direction(beam_direction): """ A helper function for getting rotations around a beam direction, the returns the first two angles (szxz) needed to place the viewer looking down the given zone axis. Parameters ---------- beam_direction : [vx,vy,vz] Returns ------- alpha,beta : angles in degrees See Also -------- generators.get_grid_around_beam_direction """ from transforms3d.euler import axangle2euler beam_direction = np.divide(beam_direction, np.linalg.norm(beam_direction)) axis = np.cross( beam_direction, [0, 0, 1]) # [0,0,1] is the starting direction for diffsims angle = np.arcsin(np.linalg.norm(axis)) alpha, beta, gamma = axangle2euler(axis, angle, 'szxz') return np.rad2deg(alpha), np.rad2deg(beta)
def test_euler_axes(): # Test there and back with all axis specs aba_perms = [(v[0], v[1], v[0]) for v in permutations('xyz', 2)] axis_perms = list(permutations('xyz', 3)) + aba_perms for (a, b, c), mat in zip(euler_tuples, euler_mats): for rs, axes in product('rs', axis_perms): ax_spec = rs + ''.join(axes) conventioned = [EulerFuncs(ax_spec)] if ax_spec in euler.__dict__: conventioned.append(euler.__dict__[ax_spec]) mat = euler2mat(a, b, c, ax_spec) a1, b1, c1 = mat2euler(mat, ax_spec) mat_again = euler2mat(a1, b1, c1, ax_spec) assert_almost_equal(mat, mat_again) quat = euler2quat(a, b, c, ax_spec) a1, b1, c1 = quat2euler(quat, ax_spec) mat_again = euler2mat(a1, b1, c1, ax_spec) assert_almost_equal(mat, mat_again) ax, angle = euler2axangle(a, b, c, ax_spec) a1, b1, c1 = axangle2euler(ax, angle, ax_spec) mat_again = euler2mat(a1, b1, c1, ax_spec) assert_almost_equal(mat, mat_again) for obj in conventioned: mat = obj.euler2mat(a, b, c) a1, b1, c1 = obj.mat2euler(mat) mat_again = obj.euler2mat(a1, b1, c1) assert_almost_equal(mat, mat_again) quat = obj.euler2quat(a, b, c) a1, b1, c1 = obj.quat2euler(quat) mat_again = obj.euler2mat(a1, b1, c1) assert_almost_equal(mat, mat_again) ax, angle = obj.euler2axangle(a, b, c) a1, b1, c1 = obj.axangle2euler(ax, angle) mat_again = obj.euler2mat(a1, b1, c1) assert_almost_equal(mat, mat_again)
def createJoint(self, solid, parent): jParam = parent.find('.//jointParameters') motor = parent.find('.//RotationalMotor') parent_link = parent anchor = jParam.attrib.get('anchor') if anchor is not None: origin = np.array(list(map(float, anchor.split()))) else: origin = np.array([0, 0, 0]) while parent_link.tag != 'Solid': parent_link = self.parent_map[parent_link] if parent_link != self.root: parent_joint = self.parent_map[parent_link].find('.//jointParameters') if parent_joint is not None: if parent_joint.attrib.get('anchor') is not None: origin2 = np.array(list(map(float, parent_joint.attrib.get('anchor').split()))) else: origin2 = np.array([0, 0, 0]) if parent_link.attrib.get('translation') is not None: origin3 = np.array(list(map(float, parent_link.attrib.get('translation').split()))) else: origin3 = np.array([0, 0, 0]) origin = origin - origin2 + origin3 origin = ' '.join(map(str, np.round(origin,6))) self.elemList.append(ET.SubElement(self.urdfRoot, 'joint')) self.elemList[-1].set('type', 'revolute') originElem = ET.SubElement(self.elemList[-1], 'origin', attrib={'xyz': origin}) try: linkname = '_'.join(parent_link.get('name').split()) except: linkname = 'link_' + str(self.chain.index(parent_link)) ET.SubElement(self.elemList[-1], 'parent', attrib={'link': linkname }) try: childname = '_'.join(solid.attrib.get('name').split()) except: childname = 'link_' + str(self.chain.index(solid)) ET.SubElement(self.elemList[-1], 'child', attrib={'link': linkname}) try: name = '_'.join(motor.attrib.get('name').split()) except: name ='joint_' + linkname + '_' + childname self.elemList[-1].set('name', name) axis = jParam.attrib.get('axis') ET.SubElement(self.elemList[-1], 'axis', attrib={'xyz': axis if axis is not None else '1 0 0'}) try: limit = ET.SubElement(self.elemList[-1], 'limit') limitList = [motor.attrib.get('maxTorque'), motor.attrib.get('minPosition'), motor.attrib.get('maxPosition'), motor.attrib.get('maxVelocity')] limitTypes = ['effort', 'lower', 'upper', 'velocity'] for i in range(4): if limitList[i] is not None: limit.set(limitTypes[i], limitList[i]) except: print('no limits') rotation = solid.attrib.get('rotation') if rotation is not None: rotation = np.array(list(map(float, rotation.split()))) rpy = euler.axangle2euler(rotation[:3],rotation[3]) originElem.set('rpy', ' '.join(map(str, rpy)))
def vectors2Euler(vec0, vec1): # rotate from vec0 to vec1 assert(vec0.shape[0] == 3) assert(vec1.shape[0] == 3) axis = np.cross(vec0, vec1); angle = np.arccos(np.dot(vec0, vec1)/(LA.norm(vec0) * LA.norm(vec1))) euler = trans.axangle2euler(axis, angle) return (180/np.pi)*np.array(euler)
def test_vectorised_axangle2euler(random_axangles, axis_convention): fast = vectorised_axangle2euler(random_axangles, axis_convention) stored_euler = np.ones((random_axangles.shape[0], 3)) for i, row in enumerate(random_axangles): a_array = axangle2euler(row[:3], row[3], axis_convention) for j in [0, 1, 2]: stored_euler[i, j] = a_array[j] assert np.allclose(fast, stored_euler)
def rot2szxz(rot_vector, rot_angle): ''' Compiles the input rotation as ZXZ rotations. Inputs: rot_vector (float): direction for the rotation rot_angle (float): angle for the rotation Output: Angle Z,Angle X,Angle Z ''' return euler.axangle2euler(rot_vector, rot_angle, axes='szxz')
def em2euler(em): ''' :param em: rotation in expo-map (3,) :return: rotation in euler angles (3,) ''' from transforms3d.euler import axangle2euler theta = np.sqrt((em**2).sum()) axis = em / theta return np.array(axangle2euler(axis, theta))
def get_grid_around_beam_direction(beam_rotation, resolution, angular_range=(0, 360)): """ Creates a rotation list of rotations for which the rotation is about given beam direction Parameters ---------- beam_rotation : tuple A desired beam direction as a rotation (rzxz eulers), usually found via get_rotation_from_z_to_direction resolution : float The resolution of the grid (degrees) angular_range : tuple The minimum (included) and maximum (excluded) rotation around the beam direction to be included Returns ------- rotation_list : list of tuples Example ------- >>> from diffsims.generators.zap_map_generator import get_rotation_from_z_to_direction >>> beam_rotation = get_rotation_from_z_to_direction(structure,[1,1,1]) >>> grid = get_grid_around_beam_direction(beam_rotation,1) """ beam_rotation = np.deg2rad(beam_rotation) axangle = euler2axangle(beam_rotation[0], beam_rotation[1], beam_rotation[2], 'rzxz') euler_szxz = axangle2euler(axangle[0], axangle[1], 'szxz') # convert to szxz rotation_alpha, rotation_beta = np.rad2deg(euler_szxz[0]), np.rad2deg( euler_szxz[1]) # see _create_advanced_linearly_spaced_array_in_rzxz for details steps_gamma = int( np.ceil((angular_range[1] - angular_range[0]) / resolution)) alpha = np.asarray([rotation_alpha]) beta = np.asarray([rotation_beta]) gamma = np.linspace(angular_range[0], angular_range[1], num=steps_gamma, endpoint=False) z = np.asarray(list(product(alpha, beta, gamma))) raw_grid = Euler( z, axis_convention='szxz' ) # we make use of an uncommon euler angle set here for speed grid_rzxz = raw_grid.to_AxAngle().to_Euler(axis_convention='rzxz') rotation_list = grid_rzxz.to_rotation_list(round_to=2) return rotation_list
def update(self, dt): # forces axis, theta = _euler.euler2axangle(self.pqr.x, self.pqr.y, self.pqr.z) axis = _vp.vector(axis[0], axis[1], axis[2]) up = _vp.rotate(_vp.vector(0,1,0), theta, axis) a = _vp.vector(0, -_gravity, 0) a = a + (self.thrust1+self.thrust2+self.thrust3+self.thrust4)/self.mass * up + self.wind/self.mass a = a - (_lin_drag_coef * _vp.mag(self.xyz_dot)**2)/self.mass * self.xyz_dot self.xyz_dot = self.xyz_dot + a * dt # torques (ignoring propeller torques) cg = self.cgpos * up tpos1 = _vp.rotate(_vp.vector(1.3*_size,0,0), theta, axis) tpos2 = _vp.rotate(_vp.vector(0,0,1.3*_size), theta, axis) tpos3 = _vp.rotate(_vp.vector(-1.3*_size,0,0), theta, axis) tpos4 = _vp.rotate(_vp.vector(0,0,-1.3*_size), theta, axis) torque = _vp.cross(cg, _vp.vector(0, -_gravity, 0)) torque = torque + _vp.cross(tpos1, self.thrust1 * up) torque = torque + _vp.cross(tpos2, self.thrust2 * up) torque = torque + _vp.cross(tpos3, self.thrust3 * up) torque = torque + _vp.cross(tpos4, self.thrust4 * up) torque = torque - _rot_drag_coef * self.pqr_dot aa = torque/self.inertia if _vp.mag(aa) > 0: aai, aaj, aak = _euler.axangle2euler((aa.x, aa.y, aa.z), _vp.mag(aa)) aa = _vp.vector(aai, aaj, aak) self.pqr_dot = self.pqr_dot + aa * dt else: self.pqr_dot = _vp.vector(0,0,0) # ground interaction if self.xyz.y <= 0: self.xyz.y = 0 if self.xyz_dot.y <= 0: self.xyz_dot.x = self.xyz_dot.x * _ground_friction self.xyz_dot.y = 0 self.xyz_dot.z = self.xyz_dot.z * _ground_friction self.pqr_dot = self.pqr_dot * _ground_friction # energy update self.energy += _power_coef * (self.thrust1**1.5 + self.thrust2**1.5 + self.thrust3**1.5 + self.thrust4**1.5) * dt # time update self.xyz += self.xyz_dot * dt self.pqr += self.pqr_dot * dt # callback if self.updated is not None: self.updated(self) self.draw()
def get_rotation_from_z_to_direction(structure, direction): """ Finds the rotation that takes [001] to a given zone axis. Parameters ---------- structure : diffpy.structure The structure for which a rotation needs to be found direction : array like [UVW] direction that the 'z' axis should end up point down. Returns ------- euler_angles : tuple 'rzxz' in degrees See Also -------- generate_zap_map get_grid_around_beam_direction Notes ----- This implementation works with an axis arrangement that has +x as left to right, +y as bottom to top and +z as out of the plane of a page. Rotations are counter clockwise as you look from the tip of the axis towards the origin """ # Case where we don't need a rotation, As axis is [0,0,z] or [0,0,0] if np.dot(direction, [0, 0, 1]) == np.linalg.norm(direction): return (0, 0, 0) # Normalize our directions cartesian_direction = structure.lattice.cartesian(direction) cartesian_direction = cartesian_direction / np.linalg.norm( cartesian_direction) # Find the rotation using cartesian vector geometry rotation_axis = np.cross([0, 0, 1], cartesian_direction) rotation_angle = np.arccos(np.dot([0, 0, 1], cartesian_direction)) euler = axangle2euler(rotation_axis, rotation_angle, axes="rzxz") return np.rad2deg(euler)
def update_uvw(_=None): u = int(txt_u.text) v = int(txt_v.text) w = int(txt_w.text) lattice = structures[current_structure]['structure'].lattice uvw = np.array((u, v, w)) up = np.array( (0.0, 0.0, 1.0) ) # Following diffpy, the z-axis is aligned in the crystal and lab frame. rotation_angle = np.deg2rad(lattice.angle( up, uvw)) # Because lattice.angle returns degrees... rotation_axis = np.cross(lattice.cartesian(up), lattice.cartesian(uvw)) if np.count_nonzero(rotation_axis) == 0: # Guard against parallel directions rotation_axis = up rotation_axis /= np.linalg.norm(rotation_axis) phi, theta, psi = np.rad2deg( axangle2euler(rotation_axis, rotation_angle, axes='rzxz')) if phi < 0: phi += 360 if theta < 0: theta += 360 if psi < 0: psi += 360 slider_phi.eventson = False # Prevent set_val from running update_pattern multiple times slider_theta.eventson = False # Prevent set_val from running update_pattern multiple times slider_psi.eventson = False # Prevent set_val from running update_pattern multiple times slider_phi.set_val(phi) slider_theta.set_val(theta) slider_psi.set_val(psi) slider_phi.eventson = True slider_theta.eventson = True slider_psi.eventson = True global current_rotation_list current_rotation_list = None update_pattern()
def update_hkl(_=None): h = int(txt_h.text) k = int(txt_k.text) l = int(txt_l.text) rotation_angle = angle_between_directions(structure, (0, 0, 1), (h, k, l)) if structure == structure_wz: # k is along second axis, rotated 30 degrees past the y axis, 120 degrees from the x axis # -> [math.cos(np.deg2rad(120)), math.sin(np.deg2rad(120)), 0] = [sqrt(3)/2, -1/2, 0] direction_axis = np.array([h, 0, l]) + k * np.array( [math.cos(np.deg2rad(120)), math.sin(np.deg2rad(120)), 0]) # [-0.5, math.sqrt(3)/2, 0]) else: direction_axis = np.array([h, k, l]) axis = np.cross(np.array([0.0, 0.0, 1.0]), direction_axis) if np.count_nonzero(axis) == 0: # Guard against parallel directions axis = np.array([0, 0, 1]) axis /= np.linalg.norm(axis) # The rotation should describe the "camera" rotation, so we need to invert it rotation_angle = -rotation_angle phi, theta, psi = np.rad2deg( axangle2euler(axis, rotation_angle, axes='rzxz')) if phi < 0: phi += 360 if theta < 0: theta += 360 if psi < 0: psi += 360 slider_phi.set_val(phi) slider_theta.set_val(theta) slider_psi.set_val(psi) update_pattern()
def euler_from_as(self, roll, pitch, yaw): rot_vec_as, theta = euler.euler2axangle(roll, pitch, yaw) rot_vec = self.vel3d_from_as(rot_vec_as) r, p, y = euler.axangle2euler(rot_vec, theta) return r, p, y