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