def test_squad(Rs):
np.random.seed(1234)
squad_precision = 4.e-15
ones = [quaternion.one, quaternion.x, quaternion.y, quaternion.z, -quaternion.x, -quaternion.y, -quaternion.z]
t_in = np.linspace(0.0, 1.0, num=13, endpoint=True)
t_out = np.linspace(0.0, 1.0, num=37, endpoint=True)
t_out2 = np.array(sorted([np.random.uniform(0.0, 1.0) for i in range(59)]))
# squad interpolated onto the inputs should be the identity
for R1 in Rs:
for R2 in Rs:
R_in = np.array([quaternion.slerp(R1, R2, t) for t in t_in])
assert np.all(np.abs(quaternion.squad(R_in, t_in, t_in) - R_in) < squad_precision)
# squad should be the same as slerp for linear interpolation
for R in ones:
R_in = np.array([quaternion.slerp(quaternion.one, R, t) for t in t_in])
R_out_squad = quaternion.squad(R_in, t_in, t_out)
R_out_slerp = np.array([quaternion.slerp(quaternion.one, R, t) for t in t_out])
# print(
# R, "\n",
# np.argmax(np.abs(R_out_squad - R_out_slerp)),
# len(R_out_squad), "\n",
# np.max(np.abs(R_out_squad - R_out_slerp)), "\n",
# R_out_squad[-6:], "\n",
# R_out_slerp[-6:],
# )
assert np.all(np.abs(R_out_squad - R_out_slerp) < squad_precision), (
R,
np.argmax(np.abs(R_out_squad - R_out_slerp)),
len(R_out_squad),
R_out_squad[np.argmax(np.abs(R_out_squad - R_out_slerp))-2:np.argmax(np.abs(R_out_squad - R_out_slerp))+3],
R_out_slerp[np.argmax(np.abs(R_out_squad - R_out_slerp))-2:np.argmax(np.abs(R_out_squad - R_out_slerp))+3],
)
R_out_squad = quaternion.squad(R_in, t_in, t_out2)
R_out_slerp = np.array([quaternion.slerp(quaternion.one, R, t) for t in t_out2])
assert np.all(np.abs(R_out_squad - R_out_slerp) < squad_precision)
def get_smoothed_orientation(self, smooth=0.94):
smothness = smooth**(1 / 6)
smoothed_orientation = np.zeros(self.orientation_list.shape)
value = self.orientation_list[0, :]
for i in range(self.num_data_points):
value = quat.slerp(value, self.orientation_list[i, :],
[1 - smothness])[0]
smoothed_orientation[i] = value
# reverse pass
smoothed_orientation2 = np.zeros(self.orientation_list.shape)
value2 = smoothed_orientation[-1, :]
for i in range(self.num_data_points - 1, -1, -1):
value2 = quat.slerp(value2, smoothed_orientation[i, :],
[(1 - smothness)])[0]
smoothed_orientation2[i] = value2
# Test rotation lock (doesn't work)
#if test:
# from scipy.spatial.transform import Rotation
# for i in range(self.num_data_points):
# quat = smoothed_orientation2[i,:]
# eul = Rotation([quat[1], quat[2], quat[3], quat[0]]).as_euler("xyz")
# new_quat = Rotation.from_euler('xyz', [eul[0], eul[1], np.pi]).as_quat()
# smoothed_orientation2[i,:] = [new_quat[3], new_quat[0], new_quat[1], new_quat[2]]
return (self.time_list, smoothed_orientation2)
def get_smoothed_orientation(self, smooth=0.94):
# https://en.wikipedia.org/wiki/Exponential_smoothing
# the smooth value corresponds to the time constant
alpha = 1 - np.exp(-(1 / self.gyro_sample_rate) / smooth)
smoothed_orientation = np.zeros(self.orientation_list.shape)
value = self.orientation_list[0, :]
for i in range(self.num_data_points):
value = quat.slerp(value, self.orientation_list[i, :], [alpha])[0]
smoothed_orientation[i] = value
# reverse pass
smoothed_orientation2 = np.zeros(self.orientation_list.shape)
value2 = smoothed_orientation[-1, :]
for i in range(self.num_data_points - 1, -1, -1):
value2 = quat.slerp(value2, smoothed_orientation[i, :], [alpha])[0]
smoothed_orientation2[i] = value2
# Test rotation lock (doesn't work)
#if test:
# from scipy.spatial.transform import Rotation
# for i in range(self.num_data_points):
# quat = smoothed_orientation2[i,:]
# eul = Rotation([quat[1], quat[2], quat[3], quat[0]]).as_euler("xyz")
# new_quat = Rotation.from_euler('xyz', [eul[0], eul[1], np.pi]).as_quat()
# smoothed_orientation2[i,:] = [new_quat[3], new_quat[0], new_quat[1], new_quat[2]]
return (self.time_list, smoothed_orientation2)
def get_interpolated_stab_transform(self,
smooth,
start=0,
interval=1 / 29.97):
time_list, smoothed_orientation = self.get_stabilize_transform(smooth)
time = start
out_times = []
slerped_rotations = []
while time < 0:
slerped_rotations.append(smoothed_orientation[0])
out_times.append(time)
time += interval
while time_list[0] >= time:
slerped_rotations.append(smoothed_orientation[0])
out_times.append(time)
time += interval
for i in range(len(time_list) - 1):
if time_list[i] <= time < time_list[i + 1]:
# interpolate between two quaternions
weight = (time - time_list[i]) / (time_list[i + 1] -
time_list[i])
slerped_rotations.append(
quat.slerp(smoothed_orientation[i],
smoothed_orientation[i + 1], [weight]))
out_times.append(time)
time += interval
return (out_times, slerped_rotations)
def intermediate_view_linear(view_init, view_last, qstart, qend,
number_of_frames, frameno):
# we don't want slerp - the l there means "linear"
# (quat start, quat end, time start, time end, time evaluated)
# of example (qstart, qend, 0, number_of_frames, frameno)
# (qstart, qend, 0, 1, frameno/number_of_frames) is npt working, even though these
# numbers ar esupposed to be floats - some numpy shit I would guess
# easing: (qstart, qend, 0, number_of_frames, number_of_frames*(1-cos(pi*frameno/number_of_frames))/2
qcur = quaternion.slerp(
qstart, qend, 0, number_of_frames,
number_of_frames * (1 - cos(pi * frameno / number_of_frames)) / 2)
# some funny results can be obtained with tiny numbers (2D proteins and such) not sure where that comes from
intermediate_view = [
x if fabs(x) > 0.001 else 0
for x in list(quaternion.as_rotation_matrix(qcur).flatten())
]
for i in range(9, len(view_init)):
# pymol is sitll on python2; in python2 3/5 = 0; in python3 it is 0.6
update = view_init[i] + (view_last[i] - view_init[i]) * (
float(frameno) / number_of_frames)
if fabs(update) < 0.001: update = 0
intermediate_view.append(update)
return intermediate_view
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 interpolate_pose(pose1, pose2, t1, t2, t_out):
tau = (t_out - t1) / (t2 - t1)
trans = (1 - tau) * pose1[0] + tau * pose2[0]
quat = quaternion.slerp(
np.quaternion(pose1[1][3], pose1[1][0], pose1[1][1], pose1[1][2]),
np.quaternion(pose2[1][3], pose2[1][0], pose2[1][1], pose2[1][2]), t1,
t2, t_out)
return (trans, np.array([quat.x, quat.y, quat.z, quat.w]))
def bislerp(Qa, Qb, t):
r"""
Linear interpolation of two orthogonal matrices.
Assiume that :math:`Q_a` and :math:`Q_b` refers to
timepoint :math:`0` and :math:`1` respectively.
Using spherical linear interpolation (slerp) find the
orthogonal matrix at timepoint :math:`t`.
"""
if Qa is None and Qb is None:
return None
if Qa is None:
return Qb
if Qb is None:
return Qa
tol = 1e-12
qa = quaternion.from_rotation_matrix(Qa)
qb = quaternion.from_rotation_matrix(Qb)
quat_i = quaternion.quaternion(0, 1, 0, 0)
quat_j = quaternion.quaternion(0, 0, 1, 0)
quat_k = quaternion.quaternion(0, 0, 0, 1)
quat_array = [
qa,
-qa,
qa * quat_i,
-qa * quat_i,
qa * quat_j,
-qa * quat_j,
qa * quat_k,
-qa * quat_k,
]
def dot(qi, qj):
return np.sum(
[getattr(qi, s) * getattr(qj, s) for s in ["x", "y", "z", "w"]])
dot_arr = [abs(dot(qi, qb)) for qi in quat_array]
max_idx = np.argmax(dot_arr)
max_dot = dot_arr[max_idx]
qm = quat_array[max_idx]
if max_dot > 1 - tol:
return Qb
else:
qm_slerp = quaternion.slerp(qm, qb, 0, 1, t)
qm_norm = qm_slerp.normalized()
Qab = quaternion.as_rotation_matrix(qm_norm)
return Qab
def bislerp(
Qa: np.ndarray,
Qb: np.ndarray,
t: float,
) -> np.ndarray:
r"""
Linear interpolation of two orthogonal matrices.
Assiume that :math:`Q_a` and :math:`Q_b` refers to
timepoint :math:`0` and :math:`1` respectively.
Using spherical linear interpolation (slerp) find the
orthogonal matrix at timepoint :math:`t`.
"""
if ~Qa.any() and ~Qb.any():
return np.zeros((3, 3))
if ~Qa.any():
return Qb
if ~Qb.any():
return Qa
tol = 1e-12
qa = quaternion.from_rotation_matrix(Qa)
qb = quaternion.from_rotation_matrix(Qb)
quat_i = quaternion.quaternion(0, 1, 0, 0)
quat_j = quaternion.quaternion(0, 0, 1, 0)
quat_k = quaternion.quaternion(0, 0, 0, 1)
quat_array = [
qa,
-qa,
qa * quat_i,
-qa * quat_i,
qa * quat_j,
-qa * quat_j,
qa * quat_k,
-qa * quat_k,
]
dot_arr = [abs((qi.components * qb.components).sum()) for qi in quat_array]
max_idx = int(np.argmax(dot_arr))
max_dot = dot_arr[max_idx]
qm = quat_array[max_idx]
if max_dot > 1 - tol:
return Qb
qm_slerp = quaternion.slerp(qm, qb, 0, 1, t)
qm_norm = qm_slerp.normalized()
Qab = quaternion.as_rotation_matrix(qm_norm)
return Qab
def intermediate_tfm(source_tfm, target_tfm, number_of_frames, frameno):
tfm = [0] * 16
qstart = tfm2quat(source_tfm)
qend = tfm2quat(target_tfm)
qcur = quaternion.slerp(
qstart, qend, 0, number_of_frames,
number_of_frames * (1 - cos(pi * frameno / number_of_frames)) / 2)
intermediate_rot = list(quaternion.as_rotation_matrix(qcur).flatten())
tfm[0:3] = intermediate_rot[:3]
tfm[4:7] = intermediate_rot[3:6]
tfm[8:11] = intermediate_rot[6:9]
for index in [3, 7, 11]:
tfm[index] += (target_tfm[index] -
source_tfm[index]) * float(frameno) / number_of_frames
return tfm
def get_smoothed_orientation(self, smooth=0.94):
smothness = smooth**(1 / 6)
smoothed_orientation = np.zeros(self.orientation_list.shape)
value = self.orientation_list[0, :]
for i in range(self.num_data_points):
value = quart.slerp(value, self.orientation_list[i, :],
[1 - smothness])[0]
smoothed_orientation[i] = value
# reverse pass
smoothed_orientation2 = np.zeros(self.orientation_list.shape)
value2 = smoothed_orientation[-1, :]
for i in range(self.num_data_points - 1, -1, -1):
value2 = quart.slerp(value2, self.orientation_list[i, :],
[1 - smothness])[0]
smoothed_orientation2[i] = value2
return (self.time_list, smoothed_orientation2)
def get_interpolation_data(poses, dist_func=None, len_traj=100, use_extra_info=False):
"""This provids a 7d (pos+quat) trajectory """
## assert len(poses)<len_traj, "interpolation will not work with the short target length"
ndim = len(poses[0])
assert ndim>=7, "Need to convert rpy to quaternion"
if type(poses) is list: poses = np.array(poses)
# A list of distance
L = []
for i in range(1, len(poses)):
if dist_func is None:
dist = np.linalg.norm(poses[i][:3]-poses[i-1][:3])
else:
dist = dist_func(poses[i-1], poses[i])
L.append(dist)
# Scaling the list wrt desired length
L_traj = []
for i in range(len(L)):
L_traj.append( max(int(len_traj*float(L[i])/float(sum(L))), 1) )
# get linear trajectory
traj = [poses[0]]
for i in range(1, len(poses)):
dx = (poses[i][:3]-poses[i-1][:3])/float(L_traj[i-1])
if use_extra_info:
dg = (poses[i][7:]-poses[i-1][7:])/float(L_traj[i-1])
for j in range(L_traj[i-1]):
point = copy.deepcopy(poses[i-1])
point[:3] += dx*float(j+1)
q = qt.slerp(poses[i-1][3:7], poses[i][3:7], float(j+1)/float(L_traj[i-1]))
point[3] = q[0]
point[4] = q[1]
point[5] = q[2]
point[6] = q[3]
if use_extra_info:
# gripper info only #TEMP
point[7:] = poses[i-1][7:] + dg*float(j+1)
traj.append(point)
return traj
def correct_gyro_drifting(rv,
magnet,
gravity,
alpha=0.98,
min_cos=0.0,
global_orientation=None):
"""
This function implements the complementary filter that reduces the drifting error
of angular rates from the gyroscope with magnetometer data. In indoor environment
the magnetic field is highly unstable, thus this function is unused.
"""
assert rv.shape[0] == magnet.shape[0]
assert rv.shape[0] == gravity.shape[0]
rv_quats = []
for r in rv:
rv_quats.append(quaternion.quaternion(*r))
if global_orientation is None:
global_orientation = rv_quats[0]
# fake the angular velocity by differentiating the rotation vector
rv_dif = [
quaternion.quaternion(1.0, 0.0, 0.0, 0.0) for _ in range(rv.shape[0])
]
for i in range(1, rv.shape[0]):
rv_dif[i] = rv_quats[i] * rv_quats[i - 1].inverse()
# complementary filter
rv_filtered = [global_orientation for _ in range(rv.shape[0])]
rv_mag_init_trans = global_orientation * orientation_from_gravity_and_magnet(
magnet[0], gravity[0]).inverse()
fused = [False for _ in range(rv.shape[0])]
for i in range(1, rv.shape[0]):
# from gyroscop
rv_filtered[i] = rv_dif[i] * rv_filtered[i - 1]
# from magnetometer
rv_mag = rv_mag_init_trans * orientation_from_gravity_and_magnet(
magnet[i], gravity[i])
diff_angle = rv_filtered[i].inverse() * rv_mag
# only fuse when the magnetometer is "reasonable"
if diff_angle.w >= min_cos:
fused[i] = True
rv_filtered[i] = quaternion.slerp(rv_filtered[i], rv_mag, 0.0, 1.0,
1.0 - alpha)
return quaternion.as_float_array(rv_filtered), fused
def plot_arc(center, start_pose, end_pose, n):
u = start_pose[0]
v = end_pose[0]
start_wxyz = start_pose[1]
end_wxyz = end_pose[1]
a = vector_angle(u, v)
poses = []
for k in range(n + 1):
theta = k * a / n
offset = np.sin(a - theta) * u + np.sin(theta) * v * np.sin(a)
wxyz = quaternion.slerp(start_wxyz, end_wxyz, 0, 1, (1 / float(n)) * k)
xyzw = [wxyz.x, wxyz.y, wxyz.z, wxyz.w]
poses.append((center + offset, xyzw))
return poses
def compute_intermediate_pose(timestamp: int,
low_ts: int, low_p: PoseTransform,
up_ts: int, up_p: PoseTransform) -> PoseTransform:
"""
Compute an intermediate pose between two poses.
It does not come from the recorded data, but is purely computed by interpolation based on given poses.
We suppose that we move at a regular speed.
:param timestamp: the timestamp at which time to compute the pose
:param low_ts: the first timestamp
:param low_p: the first pose
:param up_ts: the second timestamp
:param up_p: the second pose
:return: the computed pose
"""
rotation = quaternion.slerp(low_p.r, up_p.r, low_ts, up_ts, timestamp)
# translation = t0 + (ts-ts0)/(ts1-ts0) * (t1 - t0)
translation = [low_p.t[0] + (timestamp - low_ts) / (up_ts - low_ts) * (up_p.t[0] - low_p.t[0]),
low_p.t[1] + (timestamp - low_ts) / (up_ts - low_ts) * (up_p.t[1] - low_p.t[1]),
low_p.t[2] + (timestamp - low_ts) / (up_ts - low_ts) * (up_p.t[2] - low_p.t[2])]
return PoseTransform(rotation, translation)
def interp_traj_slerp(x_orig: np.ndarray, quat_orig: np.ndarray,
x: np.ndarray) -> np.ndarray:
"""Use SLERP to interpolate quaternion trajectory returning NaNs outside of region specfied in x_orig."""
# find at which indices the desired x would have occurred
idx_interp = np.interp(x,
x_orig,
np.arange(x_orig.size),
left=np.nan,
right=np.nan)
# now perform quaternion interpolation
quat_interp = np.empty(x.size, dtype=np.quaternion)
for n, x_idx in enumerate(idx_interp):
if np.isnan(x_idx):
quat_interp[n] = q.from_float_array(
[np.nan, np.nan, np.nan, np.nan])
else:
t1 = int(np.floor(x_idx))
t2 = int(np.ceil(x_idx))
r1 = quat_orig[t1]
r2 = quat_orig[t2]
quat_interp[n] = q.slerp(r1, r2, t1, t2, x_idx)
return quat_interp
def slerp(q0, qf, t, t0=0., tf=1.):
"""
Interpolate between two quaternions using Spherical Linear intERPolation (SLERP).
Args:
q0 (np.array[float[4]], quaternion.quaternion): initial quaternion.
qf (np.array[float[4]], quaternion.quaternion): final quaternion.
t (float, list[float], np.array[float]): the times to which the quaternions should be interpolated.
t0 (float): initial time corresponding to the initial quaternion.
tf (float): final time corresponding to the final quaternion.
Returns:
np.array, quaternion, np.array[quaternion]: one or multiple interpolated quaternions
References:
- [1] "Understanding Quaternions", https://www.3dgep.com/understanding-quaternions
- [2] Documentation of numpy-quaternion
"""
# convert if necessary
is_input_quaternion = True
if not isinstance(q0, quaternion.quaternion):
q0 = quaternion.quaternion(q0[3], q0[0], q0[1], q0[2])
is_input_quaternion = False
if not isinstance(qf, quaternion.quaternion):
qf = quaternion.quaternion(qf[3], qf[0], qf[1], qf[2])
t = np.asarray(t)
# interpolate using quaternion library
qs = quaternion.slerp(q0, qf, t0, tf, t)
# if we need to convert back to np.array
if not is_input_quaternion:
if isinstance(qs, np.ndarray):
qs = quaternion.as_float_array(qs)
qs = np.hstack((qs[:, 1:], qs[:, 0, np.newaxis]))
return qs
return np.array([qs.x, qs.y, qs.z, qs.w])
return qs
def get_rotation(
model_name, plate_id, time, verbose=False, depth=0, rowset=None, safe=True,
):
"""Core function to rotate a plate to a time by accumulating quaternions"""
time = float(time)
cache_args = (model_name, plate_id, Decimal(time))
if cache_args in cache:
return cache[cache_args]
if safe:
check_model_id(model_name)
__cache = lambda q: build_cache(q, cache_args)
prefix = " " * depth
if verbose:
secho(prefix + str(plate_id))
if plate_id is None or plate_id == 0:
return __cache(N.quaternion(1, 0, 0, 0))
params = dict(plate_id=plate_id, model_name=model_name, time=time)
row = None
pairs = []
if rowset:
for ix, p in enumerate(rowset):
if p.plate_id == plate_id and p.r1_step <= time:
pairs = [p]
# Remove this row from the rowset (prevents weird recursion errors)
rowset = rowset[:ix] + rowset[ix + 1 :]
break
else:
# Fall back to fetching the data ourselves
pairs = db.execute(__sql, **params).fetchall()
if len(pairs) == 0:
return __cache(None)
if verbose:
for i, pair in enumerate(pairs):
color = "green" if i == 0 else "white"
secho(prefix + f"{pair.plate_id} → {pair.ref_plate_id}", fg=color)
row = pairs[0]
q1 = euler_to_quaternion(row.r1_rotation)
if not row.interpolated:
base = get_rotation(
model_name,
row.ref_plate_id,
row.r1_step,
verbose=verbose,
depth=depth + 1,
rowset=rowset,
safe=False,
)
if base is None:
return __cache(None)
# We have an exact match!
# Just a precautionary guard, this should be assured by our SQL
return __cache(base * q1)
# Interpolated rotations
base = get_rotation(
model_name,
row.ref_plate_id,
time,
verbose=verbose,
depth=depth + 1,
rowset=rowset,
safe=False,
)
if base is None:
return __cache(None)
q2 = euler_to_quaternion(row.r2_rotation)
# Calculate interpolated rotation between the two timesteps
res = Q.slerp(q1, q2, float(row.r1_step), float(row.r2_step), time)
return __cache(base * res)
def smooth_orientations_internal(self, times, orientation_list):
# To be overloaded
# https://en.wikipedia.org/wiki/Exponential_smoothing
# the smooth value corresponds to the time constant
alpha = 1
high_alpha = 1
smooth = self.get_user_option_value("smoothness")
if smooth > 0:
alpha = 1 - np.exp(-(1 / self.gyro_sample_rate) / smooth)
high_alpha = 1 - np.exp(-(1 / self.gyro_sample_rate) /
(smooth * 0.1))
# forward pass
smoothed_orientation = np.zeros(orientation_list.shape)
value = orientation_list[0, :]
for i in range(self.num_data_points):
value = quat.slerp(value, orientation_list[i, :], [alpha])[0]
smoothed_orientation[i] = value
# reverse pass
smoothed_orientation2 = np.zeros(orientation_list.shape)
value2 = smoothed_orientation[-1, :]
for i in range(self.num_data_points - 1, -1, -1):
value2 = quat.slerp(value2, smoothed_orientation[i, :], [alpha])[0]
smoothed_orientation2[i] = value2
high_smooth = smoothed_orientation2
# forward pass
smoothed_orientation = np.zeros(orientation_list.shape)
value = orientation_list[0, :]
for i in range(self.num_data_points):
value = quat.slerp(value, orientation_list[i, :], [high_alpha])[0]
smoothed_orientation[i] = value
# reverse pass
smoothed_orientation2 = np.zeros(orientation_list.shape)
value2 = smoothed_orientation[-1, :]
for i in range(self.num_data_points - 1, -1, -1):
value2 = quat.slerp(value2, smoothed_orientation[i, :],
[high_alpha])[0]
smoothed_orientation2[i] = value2
low_smooth = smoothed_orientation2
# calculate distance between high_smooth and low_smooth
distance = np.zeros(self.num_data_points)
for i in range(self.num_data_points):
distance[i] = quat.angle_between(high_smooth[i], low_smooth[i])
# get rot limit
rot_limit = self.get_user_option_value("rotlimit") * np.pi / 180
# divided by 2 so the limit is closer to the inputed value
rot_limit /= 2
# limit rotation
interp_factor = 1 - (rot_limit / (distance + rot_limit / 2))
interp_factor = np.maximum(interp_factor, 0)
interp_factor *= interp_factor
final_orientation = np.zeros(orientation_list.shape)
for i in range(self.num_data_points):
final_orientation[i] = quat.slerp(high_smooth[i], low_smooth[i],
[interp_factor[i]])[0]
return times, final_orientation
def test_slerp(Rs):
slerp_precision = 4.e-15
ones = [quaternion.one, quaternion.x, quaternion.y, quaternion.z, -quaternion.x, -quaternion.y, -quaternion.z]
# Check extremes
for Q1 in ones:
assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, Q1, 0.0), Q1) < slerp_precision
assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, Q1, 1.0), Q1) < slerp_precision
assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, -Q1, 0.0), Q1) < slerp_precision
assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, -Q1, 1.0), Q1) < slerp_precision
for Q2 in ones:
assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, Q2, 0.0), Q1) < slerp_precision
assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, Q2, 1.0), Q2) < slerp_precision
assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, -Q2, 0.0), Q1) < slerp_precision
assert quaternion.rotation_chordal_distance(quaternion.slerp(Q1, -Q2, 1.0), -Q2) < slerp_precision
assert quaternion.rotation_chordal_distance(quaternion.slerp(Q2, Q1, 0.0), Q2) < slerp_precision
assert quaternion.rotation_chordal_distance(quaternion.slerp(Q2, Q1, 1.0), Q1) < slerp_precision
# Test simple increases in each dimension
for Q2 in ones[1:]:
for t in np.linspace(0.0, 1.0, num=100, endpoint=True):
assert quaternion.rotation_chordal_distance(quaternion.slerp(quaternion.one, Q2, t),
(np.cos(np.pi * t / 2) * quaternion.one + np.sin(
np.pi * t / 2) * Q2)) < slerp_precision
# Test that (slerp of rotated rotors) is (rotated slerp of rotors)
for R in Rs:
for Q2 in ones[1:]:
for t in np.linspace(0.0, 1.0, num=100, endpoint=True):
assert quaternion.rotation_chordal_distance(R * quaternion.slerp(quaternion.one, Q2, t),
quaternion.slerp(R * quaternion.one, R * Q2,
t)) < slerp_precision
def ApplyComplementaryFilter(time, acc, gyro, mag, alpha):
"""
@brief: Sensor fusion algorithm to translate IMUs and Smartphone data to Earth's reference frame
:param time:
:param acc: array, shape(N, 3)
:param gyro: array, shape(N, 3)
:param mag: array, shape(N, 3)
:param alpha: filter gain
:return: quaternion describing rotations of reference frame
"""
numData = len(acc)
quatFinal = []
for ii in range(numData):
accelVec = acc[ii, :]
magVec = mag[ii, :]
angvelVec = gyro[ii, :]
# normalized acc vector
accelVec_n = accelVec / (np.linalg.norm(accelVec))
#normalized mag vector
magVec_n = magVec / (np.linalg.norm(magVec))
# earth coordinates: ENU (East, North, Up) configuration
east = np.cross(magVec_n, accelVec_n)
east /= np.linalg.norm(east)
north = np.cross(accelVec_n, east)
north /= np.linalg.norm(north)
# Rotation Matrix
basisVectors = np.vstack((east, north, accelVec_n))
# Reference quaternion: acc + mag information
quatRef = quaternion.quaternion(
quaternion.from_rotation_matrix(basisVectors))
if ii == 0:
quatFinal.append(quaternion.as_float_array(quatRef))
else:
if np.isnan(angvelVec / np.linalg.norm(angvelVec)).any():
gyroVec_n = angvelVec
else:
gyroVec_n = angvelVec / np.linalg.norm(angvelVec)
dt = (time[ii] - time[ii - 1])
#gyroscope quaternion
theta = (np.linalg.norm(angvelVec) * dt) / 2
quatUpdate = np.array([
np.cos(theta), gyroVec_n[0] * np.sin(theta),
gyroVec_n[1] * np.sin(theta), gyroVec_n[2] * np.sin(theta)
])
#Final quaternion: acc+mag+gyro information
quatFinal.append(
quaternion.as_float_array(
quaternion.slerp(
quatRef,
quaternion.quaternion(*quatFinal[ii - 1]) *
quaternion.quaternion(*quatUpdate), 0, 1, alpha)))
return np.array(quatFinal)
def talker():
pub = rospy.Publisher('ee_data', end_effector, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(100) # 100hz
#Quaternion for example orientation
q0 = quaternion.from_spherical_coords(0, 0)
q1 = quaternion.from_spherical_coords(pi, 0)
q2 = quaternion.from_spherical_coords(pi / 2, 0)
while not rospy.is_shutdown():
data = end_effector()
time = rospy.get_time()
data.time = time
dt = 0.1
# #Modify next value will change end-effector position and velocity
# data.position = [0,0,0,0,0,0.7]
# data.velocity = [1,1,1,1,1,1]
# pub.publish(data)
# rospy.loginfo(data)
# rate.sleep()
#Some test for trajectory execution
if time < 10:
q_sl = quaternion.slerp(q0, q1, 0, 10, time)
q_sl_old = quaternion.slerp(q0, q1, 0, 10, time - 1)
q_sl_d = (q_sl - q_sl_old) / dt
w_sl = -2 * q_sl.conjugate() * q_sl_d
w_sl = quaternion.as_float_array(w_sl)
q_sl = quaternion.as_euler_angles(q_sl)
data.position = [0, 0, 0, 0.5, 0.5, 0.5]
data.velocity = [10, 10, 10, 10, 10, 10]
pub.publish(data)
rate.sleep()
elif time >= 10 and time < 20:
q_sl = quaternion.slerp(q1, q2, 10, 20, time)
q_sl_old = quaternion.slerp(q1, q2, 10, 20, time - 1)
q_sl_d = (q_sl - q_sl_old) / dt
w_sl = -2 * q_sl.conjugate() * q_sl_d
w_sl = quaternion.as_float_array(w_sl)
q_sl = quaternion.as_euler_angles(q_sl)
data.position = [0, 0, 0, 0, 0, 1]
data.velocity = [10, 10, 10, 10, 10, 10]
pub.publish(data)
rate.sleep()
else:
q_sl = quaternion.as_euler_angles(q0)
data.position = [0, 0, 0, 0, 0, 1.2]
data.velocity = [0, 0, 0, 0, 0, 0]
pub.publish(data)
rate.sleep()