def rotate_ligand(self,
qrotor=None,
geometrical_center='heavy',
centered=False):
if geometrical_center == 'heavy':
tmp_list = self.molcomplex.ligand._heavy_atoms_indices
elif geometrical_center == 'CA':
tmp_list = self.molcomplex.ligand._ca_atoms_indices
elif geometrical_center == 'All':
tmp_list = np.arange(self.molcomplex.ligand.n_atoms)
if type(qrotor) != quaternion.quaternion:
qrotor = quaternion.as_quat_array(qrotor)
tmp_positions = self.get_ligand_positions()
tmp_unit = tmp_positions.unit
if centered:
tmp_positions = quaternion.rotate_vectors(
qrotor, tmp_positions._value) * tmp_unit
else:
geometrical_center_positions = utils.geometrical_center(
tmp_positions, tmp_list)
tmp_positions = tmp_positions - geometrical_center_positions
tmp_positions = quaternion.rotate_vectors(
qrotor, tmp_positions._value) * tmp_unit
tmp_positions = tmp_positions + geometrical_center_positions
del (geometrical_center_positions)
self.set_ligand_positions(tmp_positions)
del (tmp_positions, tmp_unit, tmp_list)
def __init__(self,
coordinates=(0, 0, 0),
euler=(0, 0, 0),
position=0,
quaternion=None):
# TODO rename coordinates into relative position
self.base_coordinates = np.asanyarray(coordinates)
self.euler = np.asanyarray(euler)
self.position = position
self.quaternion = quaternion
if quaternion == None:
self.quaternion = from_euler_angles(self.euler)
self.angular_speed = 0
# Finding orientation vectors
self.orientation_vector_x = np.asanyarray(
rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_FRONT))
self.orientation_vector_y = np.asanyarray(
rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_SIDE))
self.orientation_vector_z = np.asanyarray(
rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_TOP))
self.joined_servo_rotation_quaternion = as_quat_array(
np.insert((self.orientation_vector_y * math.sin(math.pi / 4)), 0,
-math.sin(math.pi / 4)))
# Calculate coordinates of a joint
self.update_joint_coordinates()
def gravity_removal(acc, gyro, init_q, data_freq, update_freq, vis):
"""Remove gravity for one hand."""
# Initialize
madgwick = MadgwickFusion(init_q, data_freq)
# Initialize visualization
pv = None
if vis:
pv = PygameViewer(640, 480, init_q, data_freq)
# Process
acc_0 = []
i = 0
for acc_t, gyro_t in zip(acc, gyro):
# Sensor fusion update
madgwick.update(acc_t, gyro_t)
if vis:
pv.set_quaternion(madgwick.q)
# Remove gravity from acceleration
acc_t0 = quaternion.rotate_vectors(madgwick.q, np.array(acc_t))
acc_t0 -= np.array([0, 0, 1])
acc_t0 = quaternion.rotate_vectors(madgwick.q.inverse(), acc_t0)
acc_0.append(acc_t0.tolist())
# Update screen according to update rate
if vis:
if i % (data_freq // update_freq) == 0:
if not pv.update():
break
i += 1
return acc_0
def set_registers(self):
gravity = q.rotate_vectors(self.quat, [0, 0, -1])
mag = q.rotate_vectors(self.quat, [0, 0.6, -0.8])
gravity *= 0x4000
mag *= 800
vals = np.hstack([gravity, mag]).astype("int")
struct.pack_into(">3h8x3h", self.registers, 0x3B, vals[1], -vals[0],
-vals[2], vals[3], -vals[4], -vals[5])
def Tumble(diff_angle, spin_angle, vec_int):
diff_vec = Normalise(np.cross(vec_int, vec_z))
diff_quat = MakeRotationQuaternion(diff_angle, diff_vec)
vec_mid = quaternion.rotate_vectors(diff_quat, vec_int)
spin_vec = Normalise(vec_int)
spin_quat = MakeRotationQuaternion(spin_angle, spin_vec)
vec_final = quaternion.rotate_vectors(spin_quat, vec_mid)
return vec_final
def set_quaternion(self, quaternion):
self.quaternion = quaternion
# Update all the orientation vectors
self.orientation_vector_x = np.asanyarray(
rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_FRONT))
self.orientation_vector_y = np.asanyarray(
rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_SIDE))
self.orientation_vector_z = np.asanyarray(
rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_TOP))
def inertia_tensor(self, coord_sys):
m = MASS(self.movement_time)
rc = self.center_of_mass('SSK')
jx = JX(self.movement_time)
jy = jz = JYZ(self.movement_time)
J = np.diagflat([jx, jy, jz])
J = J + m * (np.eye(3) * np.inner(rc, rc) - np.outer(rc, rc))
if coord_sys == 'ISK':
J = qn.rotate_vectors(self.Λ, J.T)
J = qn.rotate_vectors(self.Λ, J.T)
rc = qn.rotate_vectors(self.Λ, rc)
s = as_matrix(m * rc)
m = np.diagflat([m] * 3)
return np.block([[m, -s], [s, J]])
def to_frame(self, new_frame):
"""Returns a new Point in reference to the new coordinate frame.
Input point is not mutated.
"""
if self._frame is new_frame:
return self
world_coords = quaternion.rotate_vectors(self._frame.orientation,
self.vector)
x = quaternion.rotate_vectors(new_frame.orientation, world_coords)
x = np.around(x, decimals=12)
new_vel = Vector3(new_frame.origin.coords + x)
return Velocity(vel=new_vel, frame=new_frame,
dimension_unit=self.units[0])
def trust(self, coord_sys):
if self.event['launch_engine'] is None:
return np.zeros(6)
else:
p = [TRUST(self.engine_time), 0, 0]
rp = self.trust_points('SSK')
force = []
for q in self.Λp:
force.append(qn.rotate_vectors(q, p))
moment = np.cross(rp, force)
force = np.sum(np.array(force), axis=0)
moment = np.sum(np.array(moment), axis=0)
if coord_sys == 'ISK':
force, moment = qn.rotate_vectors(self.Λ, (force, moment))
return np.append(force, moment)
def _update(self):
if self.kT == 0:
q_B = np.zeros(6) # no brownian motion at 0 temperature
else:
# calc q^B in particle frame with appropriate scaling
q_B = self._q_random() * np.sqrt(2 * self.timestep)
# calculate generalized force in lab frame
force = self.f_ext(
self.particle._pos,
quaternion.as_rotation_matrix(self.particle._orient))
# find vector d from COM to COD, known in particle frame,
# in lab frame.
# quaternion package does this by converting quaternion
# to rotation matrix, but could be done via conjugation
d = quaternion.rotate_vectors(self.particle._orient, self.particle.cod)
# correct torque (last 3 elts of generalized force) to be about cod
# minus sign because d points TO the COD
force[3:] = force[3:] - np.cross(d, force[0:3])
# convert generalized force from lab to particle frame
# need inverse of orientation quaternion
force_pf = np.ravel(
quaternion.rotate_vectors(self.particle._orient.inverse(),
force.reshape((2, -1))))
# Calculate q^D in particle frame
# particle.Ddim has no kT in it
q_D = np.matmul(self.particle.Ddim / self.viscosity,
force_pf) * self.timestep
# find q_total
q_total = q_B + q_D # still in particle frame
# convert spatial part of generalized displacement to lab frame
# following Garcia de la Torre, use non-updated orientation
delta_xyzlab = quaternion.rotate_vectors(self.particle._orient,
q_total[0:3])
# Update particle COM position
self.particle._pos = self.particle._pos + delta_xyzlab
# Update orientation quaternion
infntsml_rotmat = unbiased_rotation(*q_total[3:6])
infntsml_quat = quaternion.from_rotation_matrix(infntsml_rotmat)
# see BROWNRIG paper eq. 19, use quaternion composition
self.particle._orient = self.particle._orient * infntsml_quat
def move_target(self, d):
target = self._target
eye = self._pose[:3, 3].flatten()
_, a, _ = self.cartesian_to_spherical(eye, target)
v = npq.rotate_vectors(quat_from_euler('z', a), d)
self._n_target += v
self._n_pose[:3, 3] += v
def projOnDetector(xyz_exit, alpha, xy_PC, L):
'''
calculate the projection on the detector of a given escape direction
xyz_array = list of escape direction unit vectors in sampe frame
alpha = angle in degrees that brings the detector frame coincidental with the sample frame
by a CW rotation around x-axis
returns list of x,y projection on the detector
'''
# let t be the translation vector from detector frame origin to the sample frame origin
t = np.array([xy_PC[0], xy_PC[1], L])
# let q_SD be the rotation that brings in coincidence the detector frame with the sample frame
# q_SD = (x, alpha)
c_hAlpha = cos(np.radians(alpha) / 2.)
s_hAlpha = (1. - c_hAlpha**2)**0.5
q_SD = np.quaternion(c_hAlpha, s_hAlpha, 0., 0.)
# the plane equaiton of the detector in the detector frame is z=0
# so the x,y components of the projections are just the two components of the
# translated dir vector, obviously
return [(quaternion.rotate_vectors(q_SD.conj(), one_dir) - t)[0:2]
for one_dir in xyz_exit]
def solve_kinematic_motion_equation(
time_span: Tuple[int, int],
state: KinematicState,
acceleration: np.ndarray,
gravity_acceleration: np.ndarray,
angular_velocity: np.ndarray) -> KinematicState:
"""
Solve kinematic differential equation of motion.
Describe motion in phase space that represent spatial position and angular position of body.
'Body' is a rigid body (physical model).
Phase space is:
- position vector (cartesian coordinates),
- velocity vector (cartesian coordinates),
- quaternion that represent angular position according to inertial frame.
NB: All vectors related to uniform acceleration motion must be in the same measurements units, e.g. x is [km, km/sec],
acceleration and gravity_acceleration are [km/sec^2] .
NB: angular_velocity must be in [rad, sec].
:param time_span: time span.
:param state: value of state space vector of body in phase space.
:param acceleration: acceleration of body that caused by all non-gravitational forces in body frame.
:param gravity_acceleration: acceleration of body that caused by gravitational force.
:param angular_velocity: angular velocity of rotation of body frame respect to inertial frame, [rad, sec].
:return: State space vector at current time, i.e.
- position vector (cartesian coordinates),
- velocity vector (cartesian coordinates),
- quaternion that represent angular position according to inertial frame.
"""
acc = npq.rotate_vectors(state.quaternion, acceleration) + gravity_acceleration
x0 = np.hstack((state.position, state.velocity))
x_new = solve_ivp(lambda t, x: uniform_acceleration_motion_equation(x, acc), time_span, x0, method="RK45")
q_new = solve_ivp(lambda t, q: uniform_angular_motion_equation(q, angular_velocity), time_span, state.quaternion.components, method="RK45")
return KinematicState(x_new.y[:3, -1], x_new.y[3:, -1], npq.from_float_array(q_new.y[:, -1]).normalized())
def move_fixed_distance(angle, longitude, latitude, direction):
"""
:param angle: radians
:param longitude: degrees
:param latitude: degrees
:param direction:
:return: new point
"""
if direction in ["east", "west"]:
# Along parallel
axis_longitude = longitude
axis_latitude = latitude - 90
if axis_latitude < -90:
axis_latitude = -180 - axis_latitude
axis_longitude += 180
else:
# Along meridian
axis_longitude = longitude + 90
axis_latitude = 0
start_point = to_cartesian(longitude, latitude)
axis = normalize(to_cartesian(axis_longitude, axis_latitude))
if direction in ["south", "east"]:
angle *= -1
q = quaternion.from_rotation_vector(angle * axis)
return quaternion.rotate_vectors(q, start_point)
def rotate_vector(self, v):
s = self[0]
r = np.array([self[1], self[2], self[3]])
m = np.inner(self, self).real
vr = v + np.cross(2.0 * r, s * v + np.cross(r, v)) / m
vr2 = quaternion.rotate_vectors([self], v)[0, :]
#assert(np.array_equal(vr, vr2)) #TODO: Assertion raised. Check equation
return vr2
def aerodynamic_focus(self, coord_sys):
rd = self.bottom_center('SSK')
alpha = self.attack_angle()
focus = np.array([CD(alpha) * LENGTH, 0, 0])
res = rd + focus
if coord_sys == 'ISK':
res = qn.rotate_vectors(self.Λ, res)
return res
def rotate_point(q, point):
if isinstance(point[0], Iterable):
raise TypeError("Function is not vectorized")
return point
v0 = sph2cart(*point)
q = N.quaternion(*q)
v1 = Q.rotate_vectors(q, v0)
return cart2sph(v1)
def __rmul__(self: _T, other: quaternion) -> _T:
if isinstance(other, quaternion):
return self._create(
Vector3D(*rotate_vectors(other, self._translation)),
other * self._rotation,
)
return NotImplemented
def as_inverse(self):
# There is probably a faster way to do this, but I can't find it now.
# q_inverse = quaternion.from_rotation_matrix(np.linalg.inv(quaternion.as_rotation_matrix(self.rotation)))
q_inverse = self.rotation.conj()
t_inverse = quaternion.rotate_vectors(q_inverse,
-self.translation,
axis=0)
return Transformation(rotation=q_inverse, translation=t_inverse)
def move_along_parallel(angle, longitude, latitude, direction):
start_point = to_cartesian(longitude, latitude)
axis = np.array([0, 0, -1])
if direction == "east":
angle *= -1
q = quaternion.from_rotation_vector(angle * axis)
return quaternion.rotate_vectors(q, start_point)
def rotated(self, rot: "Orientation", inverse: bool = False) -> "Position":
q = rot.as_quaternion()
if inverse:
q = q.inverse()
rotated_vector = quaternion.rotate_vectors([q], [list(self)])[0][0]
return Position(rotated_vector[0], rotated_vector[1], rotated_vector[2])
def test_quat_between_vectors(self):
v = np.array([1, 0, 0])
u = np.array([0, 0, 1])
q = quat_between_two_vectors(v, u)
u1 = npq.rotate_vectors(q, v)
self.assertAlmostEqual(np.linalg.norm(u - u1), 0.)
v = np.array([0.3, 0.56, .12])
u = np.array([0.16, 0.985, 1.65])
v = v / np.linalg.norm(v)
u = u / np.linalg.norm(u)
q = quat_between_two_vectors(v, u)
u1 = npq.rotate_vectors(q, v)
self.assertAlmostEqual(np.linalg.norm(u - u1), 0.)
self.assertAlmostEqual(
npq.rotation_intrinsic_distance(npq.one,
quat_between_two_vectors(v, v)),
0.)
def filter_pose(ukf, z, R, dt, model):
ukf = UnscentedKalmanFilter()
ukf.predict(dt=dt)
predicted_state = ukf.x
positions_relative_to_drone = model.features_positions - np.tile(
predicted_state[7:10], (model.number_of_features, 1))
positions_in_drone_space = quaternion.rotate_vectors(
predicted_state[0:4], positions_relative_to_drone)
def test_transform_point2():
tqs = Quaternion(np.random.random((10, 4))).norm()
np.testing.assert_array_almost_equal(
tqs.transform_point(Point(1, 1, 1)).data,
quaternion.rotate_vectors(np.array(
[np.quaternion(*q.data[0]) for q in tqs]),
Point(1, 1, 1).tile(1).data,
axis=1).reshape(10, 3))
def make_plane_to_rotation_fixtures(samples):
for i in range(samples):
orientation = np.quaternion(1, 0, 0, 0)
orientation *= np.exp(quaternion.x * np.random.uniform(-0.1, 0.1))
orientation *= np.exp(quaternion.z * np.random.uniform(-0.1, 0.1))
z = quaternion.rotate_vectors(orientation, [0, 1, 0])
if random.choice([True, False]):
z *= -1
print bracketiser(z), ','
def get_steering(self, car_pos, car_rot, target_pos, target_rot):
relative_pos = target_pos - car_pos
distance = np.linalg.norm(relative_pos)
no_rot_relative_pos = quaternion.rotate_vectors(
1 / car_rot, relative_pos) / distance
x_difference = no_rot_relative_pos[0]
steering = np.clip(x_difference * self.kp, -1.0, 1.0)
return steering
def inertia_force(self, coord_sys):
ω = self.angular_velocity('SSK')
tensor = self.inertia_tensor('SSK')
J = tensor[3:6, 3:6]
s = np.array([tensor[1, 5], -tensor[0, 5], tensor[0, 4]])
force = -np.cross(ω, np.cross(ω, s))
moment = -np.cross(ω, J @ ω)
if coord_sys == 'ISK':
force, moment = qn.rotate_vectors(self.Λ, (force, moment))
return np.append(force, moment)
def inverse(self: _T) -> _T:
"""Return the inverse of the transform.
Returns:
A :class:`Transform3D` object representing the inverse of this :class:`Transform3D`.
"""
rotation = self._rotation.inverse()
translation = Vector3D(*rotate_vectors(rotation, -self._translation))
return self._create(translation, rotation)
def rotated(self, ori: Orientation, inverse: bool = False) -> Position:
q = ori.as_quaternion()
if inverse:
q = q.inverse()
rotated_vector = quaternion.rotate_vectors([q], [list(self)])[0][0]
return Position(rotated_vector[0], rotated_vector[1], rotated_vector[2])
def support_force(self, coord_sys):
if self.event['lift_off'] is not None:
return np.zeros(6)
else:
alt = np.clip(self.flight_altitude(), *SUPPORT.domain)
force = np.array([0, SUPPORT(alt), 0])
if coord_sys == 'SSK':
force = qn.rotate_vectors(self.Λ.conj(), force)
rs = self.support_point(coord_sys)
moment = np.cross(rs, force)
return np.append(force, moment)
