class revolute(object):
qd_zero = matrix.zeros(n=1)
qdd_zero = matrix.zeros(n=1)
def __init__(O, qE):
O.qE = qE
O.q_size = len(qE)
#
c, s = math.cos(qE[0]), math.sin(qE[0])
O.E = matrix.sqr((c, s, 0, -s, c, 0, 0, 0, 1)) # RBDA Tab. 2.2
O.r = matrix.col((0, 0, 0))
#
O.Tps = matrix.rt((O.E, (0, 0, 0)))
O.Tsp = matrix.rt((O.E.transpose(), (0, 0, 0)))
O.Xj = T_as_X(O.Tps)
O.S = matrix.col((0, 0, 1, 0, 0, 0))
def Xj_S(O, q):
return O.Xj, O.S
def time_step_position(O, qd, delta_t):
new_qE = O.qE + qd * delta_t
return revolute(qE=new_qE)
def time_step_velocity(O, qd, qdd, delta_t):
return qd + qdd * delta_t
def get_q(O):
return O.qE.elems
def new_q(O, q):
return revolute(qE=matrix.col(q))
class spherical(object):
qd_zero = matrix.zeros(n=3)
qdd_zero = matrix.zeros(n=3)
def __init__(O, type, qE):
assert type in ["euler_params", "euler_angles_xyz"]
if (type == "euler_params"):
if (len(qE.elems) == 3):
qE = euler_angles_xyz_qE_as_euler_params_qE(qE=qE)
else:
if (len(qE.elems) == 4):
qE = euler_params_qE_as_euler_angles_xyz_qE(qE=qE)
O.type = type
O.qE = qE
O.q_size = len(qE)
#
if (type == "euler_params"):
O.unit_quaternion = qE.normalize() # RBDA, bottom of p. 86
O.E = RBDA_Eq_4_12(q=O.unit_quaternion)
else:
O.E = RBDA_Eq_4_7(q=qE)
#
O.Tps = matrix.rt((O.E, (0, 0, 0)))
O.Tsp = matrix.rt((O.E.transpose(), (0, 0, 0)))
O.Xj = T_as_X(O.Tps)
O.S = matrix.rec(
(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0), n=(6, 3))
def Xj_S(O, q):
return O.Xj, O.S
def time_step_position(O, qd, delta_t):
w_body_frame = qd
if (O.type == "euler_params"):
qEd = RBDA_Eq_4_13(q=O.unit_quaternion) * w_body_frame
new_qE = (O.qE + qEd * delta_t).normalize()
else:
qEd = RBDA_Eq_4_8_inv(q=O.qE.elems) * w_body_frame
new_qE = O.qE + qEd * delta_t
return spherical(type=O.type, qE=new_qE)
def time_step_velocity(O, qd, qdd, delta_t):
return qd + qdd * delta_t
def tau_as_d_pot_d_q(O, tau):
if (O.type == "euler_params"):
d = d_unit_quaternion_d_qE_matrix(q=O.qE)
c = d * 4 * RBDA_Eq_4_13(q=O.unit_quaternion)
else:
c = RBDA_Eq_4_8(q=O.qE).transpose()
n = tau
return c * n
def get_q(O):
return O.qE.elems
def new_q(O, q):
return spherical(type=O.type, qE=matrix.col(q))
def d_pot_d_q(O): model = featherstone_system_model(I=O.I_spatial, A=O.A, J=O.J) q = [None] # already stored in joint as qE and qr qd = [matrix.zeros(n=6)] qdd = [matrix.zeros(n=6)] grav_accn = [0,0,0] tau = featherstone.ID(model, q, qd, qdd, [O.f_ext_bf], grav_accn)[0] assert approx_equal(tau, -O.f_ext_bf) return O.J.tau_as_d_pot_d_q(tau=tau)
def d_pot_d_q(O): model = featherstone_system_model(I=O.I_spatial, A=O.A, J=O.J) q = [None] # already stored in joint as qE and qr qd = [matrix.zeros(n=6)] qdd = [matrix.zeros(n=6)] grav_accn = [0,0,0] tau = featherstone.ID(model, q, qd, qdd, [O.f_ext_bf], grav_accn)[0] assert approx_equal(tau, -O.f_ext_bf) return O.J.tau_as_d_pot_d_q(tau=tau)
def centring_translation_peak_sites(self):
f = self.f_in_p1
cc_sf = f * f.conjugate().data() / f.sum_sq()
cc_map = cc_sf.fft_map(symmetry_flags=maptbx.use_space_group_symmetry,
resolution_factor=self.grid_resolution_factor)
heights = flex.double()
sites = []
cc_map_peaks = cc_map.peak_search(self.search_parameters)
zero = mat.zeros(3)
for cc_peak_info in itertools.islice(cc_map_peaks, 5):
if abs(mat.col(cc_peak_info.site) - zero) < 0.01: continue
heights.append(cc_peak_info.height)
sites.append(cc_peak_info.site)
if not heights: return ()
clusters = clustering.two_means(heights)
if clusters.highest_stat < self.cross_correlation_cutoff_for_centring:
return ()
return sites[:clusters.cut]
def centring_translation_peak_sites(self):
f = self.f_in_p1
cc_sf = f * f.conjugate().data() / f.sum_sq()
cc_map = cc_sf.fft_map(
symmetry_flags=maptbx.use_space_group_symmetry,
resolution_factor=self.grid_resolution_factor)
heights = flex.double()
sites = []
cc_map_peaks = cc_map.peak_search(self.search_parameters)
zero = mat.zeros(3)
for cc_peak_info in itertools.islice(cc_map_peaks, 5):
if abs(mat.col(cc_peak_info.site) - zero) < 0.01: continue
heights.append(cc_peak_info.height)
sites.append(cc_peak_info.site)
if not heights: return ()
clusters = clustering.two_means(heights)
if clusters.highest_stat < self.cross_correlation_cutoff_for_centring:
return ()
return sites[:clusters.cut]
def _average_transformation(matrices, keys):
"""The _average_transformation() function determines the average
rotation and translation from the transformation matrices in @p
matrices with keys matching @p keys. The function returns a
two-tuple of the average rotation in quaternion representation and
the average translation.
XXX Alternative: use average of normals, weighted by element size,
to determine average orientation.
"""
from scitbx.array_family import flex
from tntbx import svd
# Sum all rotation matrices and translation vectors.
sum_R = flex.double(flex.grid(3, 3))
sum_t = flex.double(flex.grid(3, 1))
nmemb = 0
for key in keys:
T = matrices[key][1]
sum_R += flex.double((T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1),
T(1, 2), T(2, 0), T(2, 1), T(2, 2)))
sum_t += flex.double((T(0, 3), T(1, 3), T(2, 3)))
nmemb += 1
if nmemb == 0:
# Return zero-rotation and zero-translation.
return (matrix.col((1, 0, 0, 0)), matrix.zeros((3, 1)))
# Calculate average rotation matrix as U * V^T where sum_R = U * S *
# V^T and S diagonal (Curtis et al. (1993) 377-385 XXX proper
# citation, repeat search), and convert to quaternion.
svd = svd(sum_R)
R_avg = matrix.sqr(list(svd.u().matrix_multiply_transpose(svd.v())))
o_avg = R_avg.r3_rotation_matrix_as_unit_quaternion()
t_avg = matrix.col(list(sum_t / nmemb))
return (o_avg, t_avg)
def _average_transformation(matrices, keys):
"""The _average_transformation() function determines the average
rotation and translation from the transformation matrices in @p
matrices with keys matching @p keys. The function returns a
two-tuple of the average rotation in quaternion representation and
the average translation.
XXX Alternative: use average of normals, weighted by element size,
to determine average orientation.
"""
from scitbx.array_family import flex
from tntbx import svd
# Sum all rotation matrices and translation vectors.
sum_R = flex.double(flex.grid(3, 3))
sum_t = flex.double(flex.grid(3, 1))
nmemb = 0
for key in keys:
T = matrices[key][1]
sum_R += flex.double((T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1), T(2, 2)))
sum_t += flex.double((T(0, 3), T(1, 3), T(2, 3)))
nmemb += 1
if nmemb == 0:
# Return zero-rotation and zero-translation.
return (matrix.col((1, 0, 0, 0)), matrix.zeros((3, 1)))
# Calculate average rotation matrix as U * V^T where sum_R = U * S *
# V^T and S diagonal (Curtis et al. (1993) 377-385 XXX proper
# citation, repeat search), and convert to quaternion.
svd = svd(sum_R)
R_avg = matrix.sqr(list(svd.u().matrix_multiply_transpose(svd.v())))
o_avg = R_avg.r3_rotation_matrix_as_unit_quaternion()
t_avg = matrix.col(list(sum_t / nmemb))
return (o_avg, t_avg)
class six_dof(object):
qd_zero = matrix.zeros(n=6)
qdd_zero = matrix.zeros(n=6)
def __init__(O, type, qE, qr, r_is_qr=False):
assert type in ["euler_params", "euler_angles_xyz"]
if (type == "euler_params"):
if (len(qE.elems) == 3):
qE = euler_angles_xyz_qE_as_euler_params_qE(qE=qE)
else:
if (len(qE.elems) == 4):
qE = euler_params_qE_as_euler_angles_xyz_qE(qE=qE)
O.type = type
O.qE = qE
O.qr = qr
O.r_is_qr = r_is_qr
O.q_size = len(qE) + len(qr)
#
if (type == "euler_params"):
O.unit_quaternion = qE.normalize() # RBDA, bottom of p. 86
O.E = RBDA_Eq_4_12(q=O.unit_quaternion)
else:
O.E = RBDA_Eq_4_7(q=qE)
if (r_is_qr):
O.r = qr
else:
O.r = O.E.transpose() * qr # RBDA Tab. 4.1
#
O.Tps = matrix.rt((O.E, -O.E * O.r)) # RBDA Eq. 2.28
O.Tsp = matrix.rt((O.E.transpose(), O.r))
O.Xj = T_as_X(O.Tps)
O.S = None
def Xj_S(O, q):
return O.Xj, O.S
def time_step_position(O, qd, delta_t):
w_body_frame, v_body_frame = matrix.col_list(
[qd.elems[:3], qd.elems[3:]])
if (O.type == "euler_params"):
qEd = RBDA_Eq_4_13(q=O.unit_quaternion) * w_body_frame
new_qE = (O.qE + qEd * delta_t).normalize()
else:
qEd = RBDA_Eq_4_8_inv(q=O.qE) * w_body_frame
new_qE = O.qE + qEd * delta_t
if (O.r_is_qr):
qrd = O.E.transpose() * v_body_frame
else:
qrd = v_body_frame - w_body_frame.cross(
O.qr) # RBDA Eq. 2.38 p. 27
new_qr = O.qr + qrd * delta_t
return six_dof(type=O.type, qE=new_qE, qr=new_qr, r_is_qr=O.r_is_qr)
def time_step_velocity(O, qd, qdd, delta_t):
return qd + qdd * delta_t
def tau_as_d_pot_d_q(O, tau):
if (O.type == "euler_params"):
d = d_unit_quaternion_d_qE_matrix(q=O.qE)
c = d * 4 * RBDA_Eq_4_13(q=O.unit_quaternion)
else:
c = RBDA_Eq_4_8(q=O.qE).transpose()
n, f = matrix.col_list([tau.elems[:3], tau.elems[3:]])
if (O.r_is_qr): result = (c * n, O.E.transpose() * f)
else: result = (c * (n + O.qr.cross(f)), f)
return matrix.col(result).resolve_partitions()
def get_q(O):
return O.qE.elems + O.qr.elems
def new_q(O, q):
i = len(O.qE.elems)
new_qE, new_qr = matrix.col_list((q[:i], q[i:]))
return six_dof(type=O.type, qE=new_qE, qr=new_qr, r_is_qr=O.r_is_qr)
