def test_rotation_nd(self):
for rand1 in np.random.uniform(-np.pi, np.pi, 100):
# Test 2D case
is_close(rotation_from_angle(rand1),
rotation_from_angle_and_plane(rand1, (1, 0), (0, 1)))
# Test 3D case
is_close(
rotation_around_z(rand1),
rotation_from_angle_and_plane(rand1, (1, 0, 0), (0, 1, 0)))
# Test normalisation of first parameter
is_close(
rotation_around_z(rand1),
rotation_from_angle_and_plane(rand1, (2, 0, 0), (0, 1, 0)))
# Test normalisation of second parameter
is_close(
rotation_around_z(rand1),
rotation_from_angle_and_plane(rand1, (1, 0, 0), (0, 2, 0)))
# Test normalisation of both parameters
is_close(
rotation_around_z(rand1),
rotation_from_angle_and_plane(rand1, (2, 0, 0), (0, 2, 0)))
is_close(
rotation_around_x(rand1),
rotation_from_angle_and_plane(rand1, (0, 1, 0), (0, 0, 1)))
is_close(
rotation_around_y(rand1),
rotation_from_angle_and_plane(rand1, (0, 0, 1), (1, 0, 0)))
# Test non orthogonal vectors
is_close(
rotation_around_x(rand1),
rotation_from_angle_and_plane(rand1, (0, 1, 1), (0, 1, 2)))
# Test generic properties of higher dimensional rotations
# create two perpendicular random vectors
dimension = random.randint(4, 100)
v1 = np.random.uniform(-1., 1., dimension)
v1 = v1 / np.linalg.norm(v1)
v2 = np.random.uniform(-1., 1., dimension)
v2 = v2 / np.linalg.norm(v2)
v2 = v2 - v2.dot(v1) * v1
m = rotation_from_angle_and_plane(rand1, v1, v2)
m_inv = rotation_from_angle_and_plane(-rand1, v1, v2)
is_close(m.dot(m.T), np.eye(dimension))
self.assertAlmostEqual(np.linalg.det(m), 1.0)
is_close(m, m_inv.T)
with self.assertRaises(ValueError):
rotation_from_angle_and_plane(0., (1, 0, 0), (0, 1, 0, 0))
with self.assertRaises(ValueError):
rotation_from_angle_and_plane(0., (1, 0, 0, 0), (0, 1, 0))
with self.assertRaises(ValueError):
rotation_from_angle_and_plane(0., (1, 0, 0), (1, 0, 0))
with self.assertRaises(ValueError):
rotation_from_angle_and_plane(0., (0, 0), (1, 0))
with self.assertRaises(ValueError):
rotation_from_angle_and_plane(0., (1, 0), (0, 0))
def test_rotation_around_axis(self):
for random in (np.random.rand(10000) - 0.5) * 2 * np.pi:
is_close(rotation_around_axis([1, 0, 0], random), rotation_around_x(random))
is_close(rotation_around_axis([0, 1, 0], random), rotation_around_y(random))
is_close(rotation_around_axis([0, 0, 1], random), rotation_around_z(random))
# Test if non-normalised axis works, too
is_close(rotation_around_axis([2, 0, 0], random), rotation_around_x(random))
is_close(rotation_around_axis([1, 1, 1], 2*np.pi/3), rotation_from_angles([0, np.pi/2, np.pi/2], 'XYZ'))
def test_rotation_y(self):
is_close(rotation_around_y(0), np.eye(3))
is_close(rotation_around_y(np.pi/2), [[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
is_close(rotation_around_y(np.pi), np.diag([-1, 1, -1]))
def update(self, dt, controls):
# Read controls into intermediate axes
phase_table_switch = controls.get_switch(0)
control_mode_switch = controls.get_switch(1)
if (control_mode_switch == 0):
# Movement
self.axis_forward = Tools.toward(self.axis_forward,
controls.get_axis(0), 4.0 * dt)
self.axis_side = Tools.toward(self.axis_side, controls.get_axis(1),
4.0 * dt)
self.axis_speed = Tools.toward(
self.axis_speed,
Tools.ramp(controls.get_axis(2), -1.0, 0.0, 1.0, 1.0),
2.0 * dt)
self.axis_turn = Tools.toward(self.axis_turn, controls.get_axis(3),
4.0 * dt)
self.axis_leg_lift = Tools.toward(
self.axis_leg_lift,
Tools.ramp(controls.get_axis(4), -1.0, 0.5, 0.0, 1.0),
0.5 * dt)
self.axis_body_lift = Tools.toward(
self.axis_body_lift,
Tools.ramp(controls.get_axis(4), 0.0, 0.0, 1.0, 1.0), 0.5 * dt)
# Keep attitude, no twist
self.axis_tilt_forward = 0.0
self.axis_tilt_side = 0.0
self.twist = 0.0
elif (control_mode_switch == 1):
# Reset movement axes
self.axis_forward = Tools.toward(self.axis_forward, 0.0, 4.0 * dt)
self.axis_side = Tools.toward(self.axis_side, 0.0, 4.0 * dt)
self.axis_speed = Tools.toward(self.axis_speed, 0.0, 4.0 * dt)
self.axis_turn = Tools.toward(self.axis_turn, 0.0, 4.0 * dt)
self.axis_leg_lift = Tools.toward(self.axis_leg_lift, 0.0,
4.0 * dt)
self.axis_body_lift = Tools.toward(self.axis_body_lift, 0.0,
4.0 * dt)
# Incremental attitude control, absolute twist
self.axis_tilt_forward = Tools.toward(self.axis_tilt_forward,
-1.0 * controls.get_axis(0),
4.0 * dt)
self.axis_tilt_side = Tools.toward(self.axis_tilt_side,
controls.get_axis(1), 4.0 * dt)
self.twist = controls.get_axis(3) * TWIST_LIMIT
else:
# Reset movement inputs
self.axis_forward = Tools.toward(self.axis_forward, 0.0, 4.0 * dt)
self.axis_side = Tools.toward(self.axis_side, 0.0, 4.0 * dt)
self.axis_speed = Tools.toward(self.axis_speed, 0.0, 4.0 * dt)
self.axis_turn = Tools.toward(self.axis_turn, 0.0, 4.0 * dt)
self.axis_leg_lift = Tools.toward(self.axis_leg_lift, 0.0,
4.0 * dt)
self.axis_body_lift = Tools.toward(self.axis_body_lift, 0.0,
4.0 * dt)
# Absolute attitude control
self.axis_tilt_forward = 0.0
self.axis_tilt_side = 0.0
self.tilt_forward = controls.get_axis(0) * TILT_LIMIT_FORWARD
self.tilt_side = controls.get_axis(1) * TILT_LIMIT_SIDE
self.twist = controls.get_axis(3) * TWIST_LIMIT
# Calculate movement parameters
minimum_movement = max(abs(self.axis_forward), abs(self.axis_side),
abs(self.axis_turn))
leg_lift_enabler = Tools.ramp(minimum_movement, 0.0, 0.0, 0.1, 1.0)
leg_lift = LEG_LIFT_HEIGHT * self.axis_leg_lift * leg_lift_enabler
body_lift = BODY_LIFT_HEIGHT * Tools.ramp(self.axis_body_lift, 0.0,
0.0, 1.0, -1.0)
stride_x = HALF_STRIDE_X * self.axis_forward
stride_y = HALF_STRIDE_Y * self.axis_side
rotation = HALF_ROTATION * self.axis_turn
if (phase_table_switch == 1):
phase_table = PHASE_TABLE_BUG
else:
phase_table = PHASE_TABLE_THREEPOINT
gait_period = fmod(self.time * 1.0, 2.0)
# Calculate attitude parameters
self.tilt_forward += (self.axis_tilt_forward * TILT_SPEED * dt)
self.tilt_side = Tools.saturate(self.tilt_side, -TILT_LIMIT_SIDE,
TILT_LIMIT_SIDE)
self.tilt_side += (self.axis_tilt_side * TILT_SPEED * dt)
self.tilt_forward = Tools.saturate(self.tilt_forward,
-TILT_LIMIT_FORWARD,
TILT_LIMIT_FORWARD)
attitude_transform = numpy.matmul(
mgen.rotation_around_z(self.twist),
numpy.matmul(mgen.rotation_around_y(self.tilt_forward),
mgen.rotation_around_x(self.tilt_side)))
# Work out each leg
for i in range(6):
# Calculate stride time with phase offset
self.leg_phase[i] = Tools.toward(self.leg_phase[i], phase_table[i],
0.5 * dt)
leg_period = fmod(gait_period + self.leg_phase[i], 2.0)
if (leg_period < 1.0):
stride_progress = Tools.ramp(leg_period, 0.0, -1.0, 1.0, 1.0)
else:
stride_progress = Tools.ramp(leg_period, 1.0, 1.0, 2.0, -1.0)
# Calculate translation
offset_x = stride_x * stride_progress
offset_y = stride_y * stride_progress
offset_z = body_lift + (leg_lift * max(0.0, sin(leg_period * pi)))
offset = [offset_x, offset_y, offset_z]
# Calculate rotation
turn_transform = mgen.rotation_around_z(rotation * stride_progress)
# Calculate final tip location with attitude transform
tip_location_translated = numpy.add(
HexapodConstants.TIP_LOCATION_DEFAULT[i], offset)
tip_location = attitude_transform.dot(
turn_transform.dot(tip_location_translated))
# Solve for joint angles using leg model
angles, can_calculate_angles = self.legs[i].solve_joint_angles(
tip_location)
# Only update angles if solution was found
if (can_calculate_angles):
self.joint_angles[3 * i] = angles[0]
self.joint_angles[3 * i + 1] = angles[1]
self.joint_angles[3 * i + 2] = angles[2]
# Increment time
time_multiplier = Tools.ramp(self.axis_speed, 0.0, 0.5, 1.0, 3.0)
self.time += dt * time_multiplier
def run_stratcal_native(logfile=None, **options):
"""Run stratcal for native phasing (mode 6)"""
# Programmers note:
#Plain names are in lab coordinate system,
# suffux _cr are in orthobnormal crystal system.
print('@~@~ run_stratcal_native', sorted(tt for tt in options.items()))
fp_in = options.get('input')
input_data_exch = f90nml.read(fp_in)
input_data_org = stratcal_exch2org(input_data_exch)
# NB assumes only one crystal
crystal_data = input_data_exch['crystal_list']
sg_name = crystal_data['sg_name'].strip()
laue_group = get_laue_group(sg_name)
crystal_matrix = [
crystal_data['cell_a_axis'],
crystal_data['cell_b_axis'],
crystal_data['cell_c_axis'],
]
# coordinates of reciprocal-space crystal matrix
# in real-space coordinate system
# Could also be done with metric tensor
mm = [
np.cross(crystal_matrix[1], crystal_matrix[2]),
np.cross(crystal_matrix[2], crystal_matrix[0]),
np.cross(crystal_matrix[0], crystal_matrix[1]),
]
# Now make orthonormal coordinate system
if laue_group == '-1':
mm[2] = unit_vector(np.cross(mm[0], mm[1]))
mm[1] = unit_vector(np.cross(mm[2], mm[0]))
mm[0] = unit_vector(mm[0])
elif laue_group == '2/m':
mm[2] = unit_vector(np.cross(mm[0], mm[1]))
mm[1] = unit_vector(mm[1])
mm[0] = unit_vector(mm[0])
else:
mm[2] = unit_vector(mm[2])
mm[1] = unit_vector(np.cross(mm[2], mm[0]))
mm[0] = unit_vector(mm[0])
crystal_unit_np = np.array(mm)
# NB for all except triclinic crystals, the Y and Z axes of
# crystal_unit_np match the Y and Z axes of the real-space lattice
# Only the X axis does not
# scan axis in real-space coordinate system. NB assumes omega scan axis
scan_axis = unit_vector(
input_data_exch['stratcal_instrument_list']['omega_axis'])
# orthog_component is a unit vector orthogonal to the symmetry axis
# and in the same plane as the symmetry and rotation axes.
# omega_projection_angle is the angle from the orthonormal X axis to the
# projection of the omega axis
scan_axis_cr = unit_vector(crystal_unit_np.dot(scan_axis))
if laue_group == '2/m':
# Index of symmetry axis
symm_axis_index = 1
symm_axis_cr = np.array((0, 1, 0))
xx = scan_axis_cr[0]
zz = scan_axis_cr[2]
omega_projection_angle = -math.atan2(zz, xx)
if zz:
orthog_component_cr = unit_vector((xx, 00, zz))
else:
orthog_component_cr = np.array((1, 0, 0))
else:
# For triclinic there is no symmetry axis but we might as well use c*
symm_axis_index = 2
symm_axis_cr = np.array((0, 0, 1))
xx = scan_axis_cr[0]
yy = scan_axis_cr[1]
omega_projection_angle = math.atan2(yy, xx)
if yy:
orthog_component_cr = unit_vector((xx, yy, 0))
else:
orthog_component_cr = np.array((1, 0, 0))
cr_symm_axis = crystal_unit_np[symm_axis_index]
symm_projection = symm_axis_cr.dot(scan_axis_cr)
print('@~@~ scan_axis_cr', scan_axis_cr)
# if abs(symm_projection) == 1:
# orthog_component = crystal_unit_np[0]
# omega_projection_angle = 0
# else:
# orthog_component = unit_vector(
# scan_axis - cr_symm_axis.dot(scan_axis) * cr_symm_axis
# )
# # Angle between the projection of the omega axis in the crystal
# # orthogonal plane and the crystal symmetry axis
# omega_projection_angle = angle_between(crystal_unit_np[0],
# orthog_component)
print('@~@~ crystal', sg_name, laue_group, '\n', crystal_unit_np)
print('@~@~ axis', scan_axis, scan_axis_cr, symm_projection,
orthog_component_cr)
print('@~@~ cell lengths',
[np.linalg.norm(crystal_matrix[ii]) for ii in range(3)])
print('@~@~ +/- angle bc',
angle_between(crystal_matrix[1], crystal_matrix[2], deg=True),
angle_between(crystal_unit_np[1], crystal_unit_np[2], deg=True))
print('@~@~ +/- angle ac',
angle_between(crystal_matrix[0], crystal_matrix[2], deg=True),
angle_between(crystal_unit_np[0], crystal_unit_np[2], deg=True))
print('@~@~ +/- angle ab',
angle_between(crystal_matrix[0], crystal_matrix[1], deg=True),
angle_between(crystal_unit_np[0], crystal_unit_np[1], deg=True))
print('@~@~ +/- angle a-omega',
angle_between(crystal_matrix[0], scan_axis, deg=True),
angle_between(crystal_unit_np[0], scan_axis_cr, deg=True))
print('@~@~ +/- angle b-omega',
angle_between(crystal_matrix[1], scan_axis, deg=True),
angle_between(crystal_unit_np[1], scan_axis_cr, deg=True))
print('@~@~ +/- angle c-omega',
angle_between(crystal_matrix[2], scan_axis, deg=True),
angle_between(crystal_unit_np[2], scan_axis_cr, deg=True))
print('@~@~ omega_projection_angle', math.degrees(omega_projection_angle))
if laue_group == '-1':
# Triclinic, nothing doing. Pass on to default stratcal
running_process = run_stratcal(**options)
running_process.wait()
return running_process.returncode
elif laue_group == '2/m':
# Monoclinic
# Two directions, theta=54.7deg; phi +/-45 deg away from symmetry axis
zval = math.tan(math.radians(35.3))
vv = orthog_component_cr + symm_axis_cr * zval
for ii in (1, -1):
rot_matrix = mgen.rotation_around_y(ii * math.pi / 4)
# We are using post-multiplication (of row vectors)
# so we must transpose the rotation matrix.
# See mgen docs and links therein
rot_matrix.transpose()
orientation = tuple(vv.dot(rot_matrix))
orthog_vector = tuple(np.cross(orientation, symm_axis_cr))
add_crystal_orientation(input_data_org, orientation, orthog_vector)
elif laue_group == 'mmm':
# Orthorhombic
# Along a body diagonal - selecting diagonal closest to the rotation axis
orientation = tuple(math.copysign(1, x) for x in scan_axis_cr)
orthog_vector = tuple(np.cross(orientation, symm_axis_cr))
add_crystal_orientation(input_data_org, orientation, orthog_vector)
elif laue_group == '4/m':
# 'C4'
# One directions, theta=54.7deg
zval = math.tan(math.radians(35.3))
orientation = tuple(orthog_component_cr + zval * symm_axis_cr)
orthog_vector = tuple(np.cross(orientation, symm_axis_cr))
add_crystal_orientation(input_data_org, orientation, orthog_vector)
elif laue_group == '4/mmm':
# 422
# theta=67.5, 22.5 deg from the plane of a twofold axis
# As close as possible to omega axis
mult, remainder = nearest_modulo(omega_projection_angle, math.pi / 2)
aa = mult * math.pi / 2 + math.copysign(math.pi / 8, remainder)
orientation = (math.cos(aa), math.sin(aa), math.tan(math.pi / 8))
orthog_vector = tuple(np.cross(orientation, symm_axis_cr))
add_crystal_orientation(input_data_org, orientation, orthog_vector)
elif laue_group == '-3':
# 'C3'
# Two directions ca. 30deg and 40 deg above the plane, 60deg apart
for ii in (1, -1):
zval = math.tan(math.radians(35.3 + ii * 5))
vv = orthog_component_cr + zval * symm_axis_cr
rot_matrix = mgen.rotation_around_z(ii * math.pi / 6)
# We are using post-multiplication (of row vectors)
# so we must transpose the rotation matrix.
# See mgen docs and links therein
rot_matrix.transpose()
orientation = tuple(vv.dot(rot_matrix))
orthog_vector = tuple(np.cross(orientation, symm_axis_cr))
add_crystal_orientation(input_data_org, orientation, orthog_vector)
elif laue_group == '-3m':
# '32'
# One direction, theta=60deg, phi 30 deg from a twofold axis
# NB X* is 30 deg from a twofold axis
# As close as possible to omega axis
mult, remainder = nearest_modulo(omega_projection_angle, math.pi / 3)
# aa = mult*math.pi/3 + math.copysign(math.pi/6, remainder)
aa = mult * math.pi / 3
orientation = (math.cos(aa), math.sin(aa), math.tan(math.pi / 6))
orthog_vector = tuple(np.cross(orientation, symm_axis_cr))
add_crystal_orientation(input_data_org, orientation, orthog_vector)
elif laue_group == '6/m':
# 'C6'
# One directions, theta=60deg
zval = math.tan(math.pi / 6)
orientation = tuple(orthog_component_cr + zval * symm_axis_cr)
orthog_vector = tuple(np.cross(orientation, symm_axis_cr))
add_crystal_orientation(input_data_org, orientation, orthog_vector)
elif laue_group == '6/mmm':
# '622'
# theta=70, 15 deg from the plane of a twofold axis
# NB X* is on a twofold axis
# As close as possible to omega axis
mult, remainder = nearest_modulo(omega_projection_angle, math.pi / 3)
aa = mult * math.pi / 3 + math.copysign(math.pi / 12, remainder)
orientation = (math.cos(aa), math.sin(aa), math.tan(math.pi / 9))
orthog_vector = tuple(np.cross(orientation, cr_symm_axis))
add_crystal_orientation(input_data_org, orientation, orthog_vector)
elif laue_group == 'm-3':
# '23'
# CA. 1, 0.3, 0.3 (on diagonal cross of a face)
orientation = (1, 0.3, 0.3)
# Put in same quadrant as rotation axis
orientation = tuple(
math.copysign(orientation[ii], scan_axis_cr[ii])
for ii in range(3))
orthog_vector = tuple(np.cross(orientation, cr_symm_axis))
add_crystal_orientation(input_data_org, orientation, orthog_vector)
elif laue_group == 'm-3m':
# '432'
# Ca, 1.0, 0.2, 0.4
orientation = (1, 0.2, 0.4)
# Put in same quadrant as rotation axis
orientation = tuple(
math.copysign(orientation[ii], scan_axis_cr[ii])
for ii in range(3))
orthog_vector = tuple(np.cross(orientation, cr_symm_axis))
add_crystal_orientation(input_data_org, orientation, orthog_vector)
else:
# sg_name must be empty string
raise ValueError("Illegal value (empty string) for SG_NAME")
# Add orient list count
orient_list = input_data_org['orient_list']
n_orients = len(orient_list)
input_data_org['setup_list']['n_orients'] = n_orients
# Move aside old input and save new input in input_data_org['orient_list'])ts place
tt = os.path.splitext(fp_in)
fp_in_backup = '_wrap'.join(tt)
os.rename(fp_in, fp_in_backup)
f90nml.write(input_data_org, fp_in)
# Run stratcal
running_process = run_stratcal(logfile=logfile, **options)
running_process.wait()
# NBNB TODO rewrite .out file to get correct ids etc.
outfile = options['output']
stem, junk = os.path.splitext(outfile)
for ext in ('.out.def', '.out2.def'):
fp_out = stem + ext
if os.path.exists(fp_out):
fp_bak = '_raw'.join((stem, ext))
# out_data = f90nml.read(fp_out)
# Temporary, while reading .out instead of .out.def
out_data = f90nml.read(fp_out[:-4])
out_data = stratcal_merge_output_2(input_data_exch, out_data)
# out_data = f90nml.read(fp_out)
# out_data = stratcal_merge_output(input_data_exch, out_data)
os.rename(fp_out, fp_bak)
f90nml.write(out_data, fp_out)
return running_process.returncode