def quaternion_mean(self, qk, qxi):
qt = quaternion.as_quat_array(qk)
n2 = qxi.shape[1]
e_prev = np.ones((3, 1))
# weights = np.zeros(n2,np.float32)
# weights[-1] = self.lmbda/(self.n +self.lmbda)
# weights[:-1] = 1/(2*(self.n + self.lmbda))
for i in range(30):
ei = np.zeros((3, n2))
for j in range(n2):
eq = quaternion.as_quat_array(qxi[:, j]) * qt.inverse()
ei[:, j] = q2r(eq)
e = np.mean(ei, axis=1)
# e = ei * weights
# e = np.sum(e, axis=1)
qt = r2q(e) * qt
if norm(e - e_prev) < 0.01:
break
else:
e_prev = e
q_avg = quaternion.as_float_array(qt).reshape(-1, 1)
q_ei = ei
return q_avg, q_ei
def compute_angle_from_quats(quats, joint, leg):
"""Convert rotation matrix to joint angle
Input: quats (Frames, 8)
joint (Knee / Hip / Ankle)
leg (Left / Right)
Output: Joint angle (Frames, 3)
"""
quat1 = Q.as_quat_array(quats[:, :4])
quat2 = Q.as_quat_array(quats[:, 4:])
diff = quat1.conj() * quat2
rmat = Q.as_rotation_matrix(diff)
_r = R.from_matrix(rmat)
angle = _r.as_rotvec() * 180 / np.pi
angle[:, [0, 1, 2]] = angle[:, [2, 0, 1]]
flx_fct, add_fct, rot_fct = [-1, 1, 1]
if leg == 'Right':
add_fct, rot_fct = [-1, -1]
elif leg == 'Left':
add_fct, rot_fct = [1, 1]
flip = np.array([flx_fct, add_fct, rot_fct])
angle = angle * flip
return angle
def test_numpy_array_conversion(Qs):
"Check conversions between array as quaternions and array as floats"
# First, just check 1-d array
Q = Qs[Qs_nonnan][:12] # Select first 3x4=12 non-nan elements in Qs
assert Q.dtype == np.dtype(np.quaternion)
q = quaternion.as_float_array(Q) # View as array of floats
assert q.dtype == np.dtype(np.float)
assert q.shape == (12, 4) # This is the expected shape
for j in range(12):
for k in range(4): # Check each component individually
assert q[j][k] == Q[j].components[k]
assert np.array_equal(quaternion.as_quat_array(q), Q) # Check that we can go backwards
# Next, see how that works if I flatten the q array
q = q.flatten()
assert q.dtype == np.dtype(np.float)
assert q.shape == (48,)
for j in range(48):
assert q[j] == Q[j // 4].components[j % 4]
assert np.array_equal(quaternion.as_quat_array(q), Q) # Check that we can go backwards
# Finally, reshape into 2-d array, and re-check
P = Q.reshape(3, 4) # Reshape into 3x4 array of quaternions
p = quaternion.as_float_array(P) # View as array of floats
assert p.shape == (3, 4, 4) # This is the expected shape
for j in range(3):
for k in range(4):
for l in range(4): # Check each component individually
assert p[j][k][l] == Q[4 * j + k].components[l]
assert np.array_equal(quaternion.as_quat_array(p), P) # Check that we can go backwards
def from_sxs(cls, w_sxs, override_exception_from_invalidity=False):
"""Construct this object from an `sxs.WaveformModes` object
Note that the resulting object will likely contain references to the same
underlying data contained in the original object; modifying one will modify the
other. You can make a copy of the result — using code like
`WaveformModes.from_sxs(w_sxs).copy()` — to obtain separate data.
"""
import quaternion
constructor_statement = (
f"WaveformModes.from_sxs({w_sxs}, "
f"override_exception_from_invalidity={override_exception_from_invalidity})"
)
try:
frameType = [n.lower()
for n in FrameNames].index(w_sxs.frame_type.lower())
except ValueError:
frameType = 0
try:
dataType = [n.lower()
for n in DataNames].index(w_sxs.data_type.lower())
except ValueError:
dataType = 0
kwargs = dict(
t=w_sxs.t,
#frame=, # see below
data=w_sxs.data,
history=w_sxs._metadata.get("history", []),
version_hist=w_sxs._metadata.get("version_hist", []),
frameType=frameType,
dataType=dataType,
r_is_scaled_out=w_sxs._metadata.get("r_is_scaled_out", True),
m_is_scaled_out=w_sxs._metadata.get("m_is_scaled_out", True),
override_exception_from_invalidity=
override_exception_from_invalidity,
constructor_statement=constructor_statement,
ell_min=w_sxs.ell_min,
ell_max=w_sxs.ell_max,
)
frame = w_sxs.frame.ndarray
if np.array_equal(frame, [[1., 0, 0, 0]]):
pass # The default will handle itself
elif frame.shape[0] == 1:
kwargs["frame"] = quaternion.as_quat_array(frame[0, :])
elif frame.shape[0] == w_sxs.n_times:
kwargs["frame"] = quaternion.as_quat_array(frame)
else:
raise ValueError(
f"Frame size ({frame.size}) should be 1 or "
f"equal to the number of time steps ({self.n_times})")
return cls(**kwargs)
def abs_errors_orienation(y_true, y_pred):
quat_true = quaternion.as_quat_array(y_true[..., [6, 3, 4, 5]])
quat_pred = quaternion.as_quat_array(y_pred[..., [6, 3, 4, 5]])
errors = (quat_true * quat_pred.conjugate())
errors = quaternion.as_float_array(errors)
errors_vec, errors_w = errors[:, 1:], errors[:, 0]
norms = np.linalg.norm(errors_vec, axis=-1)
angles = np.arctan2(norms, np.abs(errors_w))
return np.degrees(2 * angles)
def state_addition(self, x_, Ky):
q_x_ = quaternion.as_quat_array(x_[:X_QUAT_END, 0])
q_Ky = quaternion.as_quat_array(Ky[:X_QUAT_END, 0])
q_x = q_Ky * q_x_
q_x = quaternion.as_float_array(q_x).reshape(-1, 1)
wpv_x_ = x_[X_ANG_VEL_START:, [0]]
wpv_Ky = Ky[X_ANG_VEL_START:, [0]]
wpv_x = wpv_x_ + wpv_Ky
x = np.vstack((q_x, wpv_x))
return x
def main():
import quaternion
for j in range(100):
q1 = torch.rand(7, 4)
q1_norm = normalize_quaternion(q1)
q1_inverse = inverse_quaternion(q1_norm)
q1_conjugate = conjugate_quaternion(q1_norm)
q1_norm_numpy = quaternion.as_quat_array(q1)
identity = multiply_quaternion(q1_inverse, q1_norm)
q2 = torch.rand(7, 4)
q2_norm = normalize_quaternion(q2)
q2_norm_numpy = quaternion.as_quat_array(q2)
our_mult = multiply_quaternion(q2_norm, q1_norm)
our_mult_optimized = multiply_quaternion_optimized(q2_norm, q1_norm)
for i in range(len(q1)):
q1_numpy_norm = q1_norm_numpy[i].normalized()
q1_numpy_conjugate = q1_numpy_norm.conjugate()
q1_numpy_inverse = q1_numpy_norm.inverse()
q2_numpy_norm = q2_norm_numpy[i].normalized()
mult = q2_numpy_norm * q1_numpy_norm
pdb.set_trace()
try:
assert equality_quaternion(
q1_numpy_norm, q1_norm[i]), 'Normalize does not work'
assert equality_quaternion(
q1_numpy_conjugate,
q1_conjugate[i]), 'Conjugate does not work'
assert equality_quaternion(
q1_numpy_inverse, q1_inverse[i]), 'Inverse does not work'
assert equality_quaternion(
q1_numpy_inverse * q1_numpy_norm,
identity[i]), 'Inverse does not work'
assert equality_quaternion(
mult, our_mult[i]), 'addition does not work'
assert equality_quaternion(
mult,
our_mult_optimized[i]), 'addition optimized does not work'
# assert sum(abs(quaternion.as_float_array(q2_numpy)-q2_norm[i])) < THR, 'Normalize does not work'
except Exception as e:
print(e)
pdb.set_trace()
print('Test is passed ', j)
def rotate_vect(angles, vect, axis):
'''
Rotate an array of vectors using Quaternions
params:
angles = array of angles that will be used for rotation
vect = array of vectors that will be rotated
axis = array of vectors that will be used as rotation axis
returns:
v_prime = array of rotated vectors
'''
assert vect.shape == axis.shape
#angles = np.array(angles).reshape(3,1) # for broadcast
theta = np.radians(angles) # turn to radians
#theta = angles
if len(vect.shape) == 3:
vector_zeros = np.zeros((vect.shape[0], vect.shape[1], 1))
axis_zeros = np.zeros((axis.shape[0], axis.shape[1], 1))
ax = 2
elif len(vect.shape) == 2:
vector_zeros = np.zeros((vect.shape[0], 1))
axis_zeros = np.zeros((axis.shape[0], 1))
ax = 2
else:
raise ValueError('Vector Dimensions must be rank 2 or 3. Got',
len(vect.shape))
vector = np.array(
np.concatenate([vector_zeros, vect],
range(len(
vect.shape))[-1])) # Prepare vector for quaternian
rot_axis = np.array(
np.concatenate([axis_zeros, axis],
range(len(
vect.shape))[-1])) # Prepare axis for quaternian
# Determining vector prime using quaternians
axis_angle = np.multiply(
(theta * 0.5),
np.divide(rot_axis, np.linalg.norm(rot_axis, axis=ax, keepdims=True)))
vec = quat.as_quat_array(vector)
qlog = quat.as_quat_array(axis_angle)
q = np.exp(qlog)
v_prime = q * vec * np.conjugate(q)
v_prime = quat.as_float_array(
v_prime)[:, :,
1:] # Discard the real part and return the rotated vector
return v_prime
def get_transformation(self, timestamps=None, arrays_first=True):
""" Get the transformation across all reference frames.
Parameters
----------
timestamps: array_like, shape (n_timestamps,), optional
Timestamps to which the transformation should be matched. If not
provided the matcher will call `get_timestamps` for the target
timestamps.
arrays_first: bool, default True
If True and timestamps aren't provided, the first array in the
list defines the sampling of the timestamps. Otherwise, the first
reference frame in the list defines the sampling.
Returns
-------
translation: array_like, shape (3,) or (n_timestamps, 3)
The translation across all reference frames.
rotation: array_like, shape (4,) or (n_timestamps, 4)
The rotation across all reference frames.
timestamps: array_like, shape (n_timestamps,) or None
The timestamps for which the transformation is defined.
"""
from rigid_body_motion.utils import rotate_vectors
if timestamps is None:
timestamps = self.get_timestamps(arrays_first)
translation = np.zeros(3) if timestamps is None else np.zeros((1, 3))
rotation = quaternion(1.0, 0.0, 0.0, 0.0)
for frame in self.frames:
t, r = self._transform_from_frame(frame, timestamps)
if frame.inverse:
translation = rotate_vectors(
1 / as_quat_array(r), translation - np.array(t)
)
rotation = 1 / as_quat_array(r) * rotation
else:
translation = rotate_vectors(
as_quat_array(r), translation
) + np.array(t)
rotation = as_quat_array(r) * rotation
return translation, as_float_array(rotation), timestamps
def createSequence(keyframes, dt=0.1):
timestamps = [frame.timestamp for frame in keyframes]
rotations = quaternion.as_quat_array([[frame.frame.orientation.x, frame.frame.orientation.y, frame.frame.orientation.z, frame.frame.orientation.w] for frame in keyframes])
# Coordinate frame conversion
multiplier_quat = quaternion.from_float_array([np.sqrt(2) / 2, -np.sqrt(2) / 2, 0, 0])
rotations = [rot * multiplier_quat for rot in rotations]
positions = np.array([[frame.frame.position.x, frame.frame.position.y, frame.frame.position.z] for frame in keyframes])
time_interps = list(np.arange(timestamps[0], timestamps[-1], dt)) + [timestamps[-1]]
lerp = interp1d(timestamps, positions, axis=0)
lin_interps = lerp(time_interps)
rot_interps = []
for i in range(len(keyframes) - 1):
rot_interps.extend(quaternion.as_float_array(quaternion.slerp(rotations[i], rotations[i + 1], timestamps[i], timestamps[i + 1], np.arange(timestamps[i], timestamps[i + 1], dt))))
rot_interps.append(quaternion.as_float_array(rotations[-1]))
sequence = []
for pos, ori in zip(lin_interps, rot_interps):
pose = Pose()
pose.position.x, pose.position.y, pose.position.z = pos
pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z = ori
pose_stamped = PoseStamped()
pose_stamped.pose = pose
sequence.append(pose_stamped)
g_poses[0] = sequence[-1].pose
g_poses[1] = sequence[-1].pose
return time_interps, sequence
def set_state(self):
index = 0
frame_count = 0
for frame_id in self.frame_ids:
# position 3x1
self.pred_poses[frame_id][:, [3]] = self.states[index:(index + 3)]
index += 3
# orientation as rotation matrix 3x3
q = quaternion.as_quat_array(self.states[index:(index + 4), 0])
R = quaternion.as_rotation_matrix(q)
self.pred_poses[frame_id][:, :3] = R
index += 4
# scale 1x1
# first save scale value
scale = self.states[index]
index += 1
for feature_pts in self.long_track_features:
# depth value at feature coordinates 1x1
pt_x = int(feature_pts[frame_count][0])
pt_y = int(feature_pts[frame_count][1])
depth = self.states[index]
self.pred_depths[frame_id][pt_y, pt_x] = depth
index += 1
# multiply all depth by scale
self.pred_depths[frame_id] *= scale
frame_count += 1
def as_quaternion(*args):
"""
Recombine the arguments to produce a numpy array with quaternion dtype.
Parameters
----------
*args : ndarray with dtype:float or complex
The quaternion array components: If there is 4 components, then we assume it is the four compoents of the
quaternion array: w, x, y, z. If there is only two, they are casted to complex and correspond respectively
to w + i.x and y + j.z.
Returns
-------
"""
if len(args) == 4:
# we assume here that the for components have been provided w, x, y, z
w, x, y, z = args
if len(args) == 2:
r, i = args
w, x, y, z = r.real, r.imag, i.real, i.imag
data = as_quat_array(list(zip(w.flatten(), x.flatten(), y.flatten(), z.flatten())))
return data.reshape(w.shape)
def __init__(self, q):
if q.dtype != 'quaternion':
raise ValueError('array should be of quaternion type')
self.signal = q
# compute H-extension of the signal
N = np.size(q, 0)
Q = qfft.Qfft(q)
# filter frequencies
h = np.zeros((N, 4))
h[0, 0] = 1
h[N // 2, 0] = 1
h[1:N // 2, 0] = 2
hq = quaternion.as_quat_array(h)
self.Hembedding = qfft.iQfft(Q * hq)
# Compute Euler angle form
a, theta, chi, phi = utils.quat2euler(self.Hembedding)
self.a = a
self.theta = theta
self.chi = chi
self.phi = phi
def _transform_from_frame(cls, frame, timestamps):
""" Get the transform from a frame resampled to the timestamps. """
if timestamps is None and frame.timestamps is not None:
raise ValueError("Cannot convert timestamped to static transform")
if frame.timestamps is None:
if timestamps is None:
translation = frame.translation
rotation = frame.rotation
else:
translation = np.tile(frame.translation, (len(timestamps), 1))
rotation = np.tile(frame.rotation, (len(timestamps), 1))
elif frame.discrete:
translation = np.tile(frame.translation[0], (len(timestamps), 1))
for t, ts in zip(frame.translation, frame.timestamps):
translation[timestamps >= ts, :] = t
rotation = np.tile(frame.rotation[0], (len(timestamps), 1))
for r, ts in zip(frame.rotation, frame.timestamps):
rotation[timestamps >= ts, :] = r
else:
# TODO method + optional scipy dependency?
translation = interp1d(
frame.timestamps.astype(float), frame.translation, axis=0
)(timestamps.astype(float))
rotation = as_float_array(
squad(
as_quat_array(frame.rotation),
frame.timestamps.astype(float),
timestamps.astype(float),
)
)
return translation, rotation
def _resample_array(cls, array, timestamps):
""" Resample an array to the timestamps. """
if timestamps is None and array.timestamps is not None:
raise ValueError("Cannot convert timestamped to static array")
if array.timestamps is None:
if timestamps is None:
return array.data
else:
return np.tile(array.data, (len(timestamps), 1))
else:
# TODO better way to check if quaternion
if array.data.shape[-1] == 4:
return as_float_array(
squad(
as_quat_array(array.data),
array.timestamps.astype(float),
timestamps.astype(float),
)
)
else:
# TODO method + optional scipy dependency?
return interp1d(
array.timestamps.astype(float), array.data, axis=0
)(timestamps.astype(float))
def swapdims(self, dim1, dim2, inplace=False):
"""
Swap dimension the complex array.
swapdims and swapaxes are alias.
Parameters
----------
dims : int, str or tuple of int or str, optional, default=(0,)
Dimension names or indexes along which the method should be applied.
inplace : bool, optional, default=False
Flag to say that the method return a new object (default)
or not (inplace=True)
Returns
-------
transposed
Same object or a copy depending on the ``inplace`` flag.
"""
new = super().swapdims(dim1, dim2, inplace=inplace)
# we need also to swap the quaternion
# WARNING: this work only for 2D - when swapdims is equivalent to a 2D transpose
# TODO: implement something for any n-D array (n>2)
if self.is_quaternion:
# here if it is is_quaternion
# we should interchange the imaginary component
w, x, y, z = as_float_array(new._data).T
q = as_quat_array(list(zip(w.T.flatten(), y.T.flatten(), x.T.flatten(), z.T.flatten())))
new._data = q.reshape(new.shape)
return new
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 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 pulse(self, spins):
"""
Calculate pulse evolution as applied to spin object
Arguments
---------
spins : <Spins> object
Returns
-------
array: quaternion array
evolved spins are returned as a quaternion array and are also
stored in the 'mag' attribute of the 'Spins' object argument
"""
if hasattr(self, 'amplitudes'):
q = quat.as_quat_array([1., 0., 0., 0.] * len(spins))
incr = self.increment
for rf in self.amplitudes:
beff = (spins.q_offsets + rf) * incr
q = np.exp(-0.5 * beff) * q
mag = q * spins.magnetisation * np.conjugate(q)
spins.magnetisation = mag
return mag
else:
print(
"Cannot pulse spins. Pulse calibration may be required first:")
print("Call the <calibrate> method to set pulse amplitudes.")
def get_corrected_orientation(E, target_orientation, evs):
# Correct cs orientation so that it aligns with standard defined as foot touchdown
E_quats = quaternion.from_rotation_matrix(E)
target_quats = np.repeat(
quaternion.from_rotation_matrix(target_orientation), len(evs))
# Get relative rotation between marker-cs and ideal-cs during touchdowns
rel_quats = E_quats[evs].conj() * target_quats
rel_quat_array = quaternion.as_float_array(rel_quats)
# Find the average relative orientation between the ideal cs and all touchdown cs
eigvals, eigvecs = np.linalg.eig(rel_quat_array.T @ rel_quat_array)
av_rel_quat_array = eigvecs[:, np.argmax(eigvals)]
# Check sign of the averaged quaternion for consistency
av_signs = np.sign(av_rel_quat_array)
ar_signs = np.sign(rel_quat_array)
if (av_signs != ar_signs).all():
av_rel_quat_array = -1 * av_rel_quat_array
elif (av_signs == ar_signs).all():
pass
else:
pass
av_rel_quats = np.repeat(quaternion.as_quat_array(av_rel_quat_array),
len(E_quats))
corrected_quats = E_quats * av_rel_quats
corrected_orientation = quaternion.as_rotation_matrix(corrected_quats)
return corrected_orientation
def qmul(*q, qaxis=-1):
""" Quaternion multiplication.
Parameters
----------
q: iterable of array_like
Arrays containing quaternions to multiply. Their dtype can be
quaternion, otherwise `qaxis` specifies the axis representing
the quaternions.
qaxis: int, default -1
If `q` are not quaternion dtype, axis of the quaternion arrays
representing the coordinates of the quaternions.
Returns
-------
qm: ndarray
A new array containing the multiplied quaternions.
"""
# TODO xarray support
if len(q) < 2:
raise ValueError("Please provide at least 2 quaternions to multiply")
if all(qq.dtype != quaternion for qq in q):
q = (as_quat_array(np.swapaxes(qq, qaxis, -1)) for qq in q)
qm = reduce(operator.mul, q, 1)
return np.swapaxes(as_float_array(qm), -1, qaxis)
elif all(qq.dtype == quaternion for qq in q):
return reduce(operator.mul, q, 1)
else:
raise ValueError(
"Either all or none of the provided quaternions must be "
"quaternion dtype"
)
def qinv(q, qaxis=-1):
""" Quaternion inverse.
Parameters
----------
q: array_like
Array containing quaternions whose inverse is to be computed. Its dtype
can be quaternion, otherwise `qaxis` specifies the axis representing
the quaternions.
qaxis: int, default -1
If `q` is not quaternion dtype, axis of the quaternion array
representing the coordinates of the quaternions.
Returns
-------
qi: ndarray
A new array containing the inverse values.
"""
# TODO xarray support
if q.dtype != quaternion:
q = np.swapaxes(q, qaxis, -1)
qi = as_float_array(1 / as_quat_array(q))
return np.swapaxes(qi, -1, qaxis)
else:
return 1 / q
def __init__(self, init=None):
if isinstance(init, type(self)):
self.__quat = init.__quat
elif isinstance(init, np.quaternion):
self.__quat = init
else:
self.__quat = quaternion.as_quat_array(np.array(init, copy=False))
def load_eval_data(eval_data_path, data_shape='asus'):
"""Read the data from the real world stored in jpgs"""
imgs = []
labels = []
pickle_path = os.path.join(eval_data_path, "ep_data.pkl")
with open(pickle_path, "rb") as f:
context = pickle.load(f)
files = sorted(os.listdir(eval_data_path))
obj_world_pose = context["obj_world_pose"]
obj_world_pos = obj_world_pose[:3]
obj_world_quat = quaternion.as_quat_array(obj_world_pose[3:])
zrot = quaternion.as_rotation_vector(obj_world_quat)[-1]
pose = np.zeros((3,))
pose[:2] = obj_world_pos[:2]
pose[2] = zrot
for f in files:
if not f.endswith('.png'):
continue
labels.append(pose.copy())
filename = os.path.join(eval_data_path, f)
img = plt.imread(filename)
if data_shape == 'asus':
img = preproc_image(img, dtype=np.float32)
# plt.imshow(img); plt.show()
# img = (img / 127.5) - 1.0
imgs.append(img)
return np.array(imgs, dtype=np.float32), np.array(labels, dtype=np.float32)
def angular_velocity(W, include_frame_velocity=False):
"""Angular velocity of Waveform
This function calculates the angular velocity of a Waveform object from
its modes. This was introduced in Sec. II of "Angular velocity of
gravitational radiation and the corotating frame"
<http://arxiv.org/abs/1302.2919>. Essentially, this is the angular
velocity of the rotating frame in which the time dependence of the modes
is minimized.
The vector is given in the (possibly rotating) mode frame (X,Y,Z),
which is not necessarily equal to the inertial frame (x,y,z).
"""
# Calculate the <Ldt> vector and <LL> matrix at each instant
l = W.LdtVector()
ll = W.LLMatrix()
# Solve <Ldt> = - <LL> . omega
omega = -np.linalg.solve(ll, l)
if include_frame_velocity and len(W.frame) == W.n_times:
from quaternion import derivative, as_quat_array, as_float_array
Rdot = as_quat_array(derivative(as_float_array(W.frame), W.t))
omega += as_float_array(2 * Rdot * W.frame.conjugate())[:, 1:]
return omega
def cook(self, location, interface, attrs):
points = self.client.getQuatData(0)
if not np.all(np.isnan(points[:23, 0])):
if self.skelDict is None:
print "New Skel"
self.skelDict = makeXSensSkelDict(points[:23, :])
else:
T = points[:23, :3]
R = points[:23, 3:].astype(np.float)
R = quaternion.as_quat_array(R)
self.skelDict['Gs'] = np.array(genGs(R, T), dtype=np.float32)
skelAttrs = {
'skelDict': self.skelDict,
'rootMat': np.eye(3, 4),
'originalRootMat': np.eye(3, 4),
'subjectName': self.getName(),
'boneColour': (0.3, 0.42, 0.66, 1.)
}
interface.createChild(interface.name(),
'skeleton',
atLocation=interface.parentPath(),
attrs=skelAttrs)
def arr_quat_to_rots(quats):
ret = q.as_rotation_vector(q.as_quat_array(quats))
ret = ret.reshape(-1, 3)
ret = ret[:, 1] - 3.14
ret[np.where(ret > 3.14)] -= 6.28
ret[np.where(ret < -3.14)] += 6.28
return ret
def test_as_float_quat(Qs):
qs = Qs[Qs_nonnan]
for quats in [qs, np.vstack((qs,)*3), np.vstack((qs,)*(3*5)).reshape((3, 5)+qs.shape),
np.vstack((qs,)*(3*5*6)).reshape((3, 5, 6)+qs.shape)]:
floats = quaternion.as_float_array(quats)
assert floats.shape == quats.shape+(4,)
assert allclose(quaternion.as_quat_array(floats), quats)
def _generate_goal(self):
original_obj_pos, original_obj_quat = self._p.getBasePositionAndOrientation(
self.object_bodies[self.goal_object_key])
while True:
target_xyz = self.np_random.uniform(self.robot.target_bound_lower,
self.robot.target_bound_upper)
# on the table
target_xyz[-1] = self.object_initial_pos[self.goal_object_key][2]
if np.linalg.norm(target_xyz - original_obj_pos) > 0.06:
break
target_obj_euler = quat.as_euler_angles(
quat.as_quat_array([1., 0., 0., 0.]))
target_obj_euler[-1] = self.np_random.uniform(-1.0, 1.0) * np.pi
target_obj_quat = quat.as_float_array(
quat.from_euler_angles(target_obj_euler))
target_obj_quat = np.concatenate(
[target_obj_quat[1:], [target_obj_quat[0]]], axis=-1)
self.desired_goal = np.concatenate((target_xyz, target_obj_euler))
if self.visualize_target:
self.set_object_pose(
self.object_bodies[self.goal_object_key + '_target'],
target_xyz, target_obj_quat)
def rotate_vector(self, rotation_quat, vec):
"""
Rotate a given vector vec by the quaternion rotation_quat
"""
quat_vec = qt.as_quat_array(np.insert(vec, 0, 0, axis=-1))
return qt.as_float_array(rotation_quat * quat_vec *
rotation_quat.conj())[..., 1:]
def transpose(self, *dims, inplace=False):
"""
Transpose the complex array.
Parameters
----------
dims : int, str or tuple of int or str, optional, default=(0,)
Dimension names or indexes along which the method should be applied.
inplace : bool, optional, default=False
Flag to say that the method return a new object (default)
or not (inplace=True)
Returns
-------
transposed
Same object or a copy depending on the ``inplace`` flag.
"""
new = super().transpose(*dims, inplace=inplace)
if new.is_quaternion:
# here if it is hypercomplex quaternion
# we should interchange the imaginary component
w, x, y, z = as_float_array(new._data).T
q = as_quat_array(
list(
zip(w.T.flatten(), y.T.flatten(), x.T.flatten(),
z.T.flatten())))
new._data = q.reshape(new.shape)
return new
def reset(self):
"""
Reset all spins to their initial vector specified by 'init_mag'
"""
tmp = np.tile([0] + list(self.initial_magnetisation),
len(self)).reshape(len(self), 4)
self.magnetisation = quat.as_quat_array(tmp)
def test_as_rotation_vector():
np.random.seed(1234)
n_tests = 1000
vecs = np.random.uniform(high=math.pi/math.sqrt(3), size=n_tests*3).reshape((n_tests, 3))
quats = np.zeros(vecs.shape[:-1]+(4,))
quats[..., 1:] = vecs[...]
quats = quaternion.as_quat_array(quats)
quats = np.exp(quats/2)
quat_vecs = quaternion.as_rotation_vector(quats)
assert allclose(quat_vecs, vecs)
def get_orientation(self, method='madgwick', initwindow=0.5, beta=2.86, lCa=(0.0, -0.3, -0.7), Ca=None):
if method.lower() == 'ekf':
dt = np.mean(np.diff(self.t))
if Ca is None:
Ca = np.power(10, np.array(lCa)) / dt
else:
Ca = np.array(Ca) / dt
self._get_orientation_ekf(Ca=Ca)
inertialbasis = []
for rpy in self.orient_sensor:
QT = self._getQT(rpy)
inertialbasis.append(QT.dot(np.eye(3)))
inertialbasis = np.array(inertialbasis)
self.worldbasis = np.matmul(self.chip2world_rot, inertialbasis)
self.accdyn_world = np.matmul(self.chip2world_rot, self.accdyn_sensor.T).T
self.accdyn = self.accdyn_world
elif method.lower() in ['madgwick', 'integrate_gyro']:
if method.lower() == 'integrate_gyro':
beta = 0.0
self._get_orientation_madgwick(initwindow=initwindow, beta=beta)
qorient_world = self.qchip2world.conj() * self.qorient * self.qchip2world
self.qorient_world = qorient_world
self.orient_world = np.array([quaternion.as_euler_angles(q1) for q1 in qorient_world])
self.orient = self.orient_world
# make accdyn into a quaternion with zero real part
qacc = np.zeros((self.accdyn_sensor.shape[0], 4))
qacc[:, 1:] = self.accdyn_sensor
# rotate accdyn into the world coordinate system
qaccdyn_world = self.qchip2world.conj() * quaternion.as_quat_array(qacc) * self.qchip2world
self.accdyn_world = np.array([q.components[1:] for q in qaccdyn_world])
self.accdyn = self.accdyn_world
return self.orient_world
def integrate_angular_velocity(Omega, t0, t1, R0=None, tolerance=1e-12):
"""Compute frame with given angular velocity
Parameters
==========
Omega: tuple or callable
Angular velocity from which to compute frame. Can be
1) a 2-tuple of float arrays (t, v) giving the angular velocity vector at a series of times,
2) a function of time that returns the 3-vector angular velocity, or
3) a function of time and orientation (t, R) that returns the 3-vector angular velocity
In case 1, the angular velocity will be interpolated to the required times. Note that accuracy
is poor in case 1.
t0: float
Initial time
t1: float
Final time
R0: quaternion, optional
Initial frame orientation. Defaults to 1 (the identity orientation).
tolerance: float, optional
Absolute tolerance used in integration. Defaults to 1e-12.
Returns
=======
t: float array
R: quaternion array
"""
import warnings
from scipy.integrate import ode
if R0 is None:
R0 = quaternion.one
input_is_tabulated = False
try:
t_Omega, v = Omega
from scipy.interpolate import InterpolatedUnivariateSpline
Omega_x = InterpolatedUnivariateSpline(t_Omega, v[:, 0])
Omega_y = InterpolatedUnivariateSpline(t_Omega, v[:, 1])
Omega_z = InterpolatedUnivariateSpline(t_Omega, v[:, 2])
def Omega_func(t, R):
return [Omega_x(t), Omega_y(t), Omega_z(t)]
Omega_func(t0, R0)
input_is_tabulated = True
except (TypeError, ValueError):
def Omega_func(t, R):
return Omega(t, R)
try:
Omega_func(t0, R0)
except TypeError:
def Omega_func(t, R):
return Omega(t)
Omega_func(t0, R0)
def RHS(t, y):
R = quaternion.quaternion(*y)
return (0.5 * quaternion.quaternion(0.0, *Omega_func(t, R)) * R).components
y0 = R0.components
if input_is_tabulated:
from scipy.integrate import solve_ivp
t = t_Omega
t_span = [t_Omega[0], t_Omega[-1]]
solution = solve_ivp(RHS, t_span, y0, t_eval=t_Omega, atol=tolerance, rtol=100*np.finfo(float).eps)
R = quaternion.from_float_array(solution.y.T)
else:
solver = ode(RHS)
solver.set_initial_value(y0, t0)
solver.set_integrator('dop853', nsteps=1, atol=tolerance, rtol=0.0)
solver._integrator.iwork[2] = -1 # suppress Fortran-printed warning
t = appending_array((int(t1-t0),))
t.append(solver.t)
R = appending_array((int(t1-t0), 4))
R.append(solver.y)
warnings.filterwarnings("ignore", category=UserWarning)
t_last = solver.t
while solver.t < t1:
solver.integrate(t1, step=True)
if solver.t > t_last:
t.append(solver.t)
R.append(solver.y)
t_last = solver.t
warnings.resetwarnings()
t = t.a
R = quaternion.as_quat_array(R.a)
return t, R