def rotate_vector(vector, quat_array):
'''Given an array of quaternions, rotate every vector in vector.
Parameters
----------
vector : array_like
Nx3 array
quat_array : array of quaternions as defined by np.quaternion
Returns
-------
vector : array_like
TODO
----------
Replace this by v' = v + 2 * r x (s * v + r x v) / m
where x represents the cross product, s and r are the scalar and
vector parts of the quaternion, respectively, and m is the sum of
the squares of the components of the quaternion. Implemented with numba.
'''
if type(quat_array[0]) != quaternion.quaternion:
quat_array = quaternion.from_float_array(quat_array)
N = vector.shape[0]
V = quaternion.from_float_array(np.hstack((np.zeros((N, 1)), vector)))
return quaternion.as_float_array(
quat_array * V * np.invert(quat_array)
)[:, 1:] # norm is the square of the II-norm in np.quaternion
def compute_quat(self, predictions, targets, reduce_fn="mean"):
"""
Compute the chosen metrics. Predictions and targets are assumed to be quaternions.
Args:
predictions: An np array of shape (n, seq_length, n_joints*4)
targets: An np array of the same shape as `predictions`
reduce_fn: Which reduce function to apply to the joint dimension, if applicable. Choices are [mean, sum].
Returns:
A dictionary {metric_name -> values} where the values are given per batch entry and frame as an np array
of shape (n, seq_length). `reduce_fn` is only applied to metrics where it makes sense, i.e. not to PCK
and euler angle differences.
"""
assert predictions.shape[-1] % 4 == 0, "predictions are not quaternions"
assert targets.shape[-1] % 4 == 0, "targets are not quaternions"
assert reduce_fn in ["mean", "sum"]
assert not self._should_call_reset, "you should reset the state of this class after calling `finalize`"
dof = 4
batch_size = predictions.shape[0]
seq_length = predictions.shape[1]
# for simplicity we just convert quaternions to rotation matrices
pred_q = quaternion.from_float_array(np.reshape(predictions, [batch_size, seq_length, -1, dof]))
targ_q = quaternion.from_float_array(np.reshape(targets, [batch_size, seq_length, -1, dof]))
pred_rots = quaternion.as_rotation_matrix(pred_q)
targ_rots = quaternion.as_rotation_matrix(targ_q)
preds = np.reshape(pred_rots, [batch_size, seq_length, -1])
targs = np.reshape(targ_rots, [batch_size, seq_length, -1])
return self.compute_rotmat(preds, targs, reduce_fn)
def angular_velocity(R, t):
from scipy.interpolate import CubicSpline
R = quaternion.as_float_array(R)
Rdot = CubicSpline(t, R).derivative()(t)
R = quaternion.from_float_array(R)
Rdot = quaternion.from_float_array(Rdot)
return quaternion.as_float_array(2*Rdot/R)[:, 1:]
def angular_velocity(R, t):
from scipy.interpolate import InterpolatedUnivariateSpline as spline
R = quaternion.as_float_array(R)
Rdot = np.empty_like(R)
for i in range(4):
Rdot[:, i] = spline(t, R[:, i]).derivative()(t)
R = quaternion.from_float_array(R)
Rdot = quaternion.from_float_array(Rdot)
return np.array([omega.vec for omega in (2*Rdot/R)])
def angular_velocity(R, t):
from scipy.interpolate import InterpolatedUnivariateSpline as spline
R = quaternion.as_float_array(R)
Rdot = np.empty_like(R)
for i in range(4):
Rdot[:, i] = spline(t, R[:, i]).derivative()(t)
R = quaternion.from_float_array(R)
Rdot = quaternion.from_float_array(Rdot)
return quaternion.as_float_array(2*Rdot/R)[:, 1:]
def load_dataset_6d_rvec(imu_data_filename, gt_data_filename, window_size=200, stride=10):
#imu_data = np.genfromtxt(imu_data_filename, delimiter=',')
#gt_data = np.genfromtxt(gt_data_filename, delimiter=',')
imu_data = pd.read_csv(imu_data_filename).values
gt_data = pd.read_csv(gt_data_filename).values
#imu_data = imu_data[1200:-300]
#gt_data = gt_data[1200:-300]
gyro_acc_data = np.concatenate([imu_data[:, 4:7], imu_data[:, 10:13]], axis=1)
pos_data = gt_data[:, 2:5]
ori_data = np.concatenate([gt_data[:, 8:9], gt_data[:, 5:8]], axis=1)
init_q = quaternion.from_float_array(ori_data[window_size//2 - stride//2, :])
init_rvec = np.empty((3, 1))
cv2.Rodrigues(quaternion.as_rotation_matrix(init_q), init_rvec)
init_tvec = pos_data[window_size//2 - stride//2, :]
x = []
y_delta_rvec = []
y_delta_tvec = []
for idx in range(0, gyro_acc_data.shape[0] - window_size - 1, stride):
x.append(gyro_acc_data[idx + 1 : idx + 1 + window_size, :])
tvec_a = pos_data[idx + window_size//2 - stride//2, :]
tvec_b = pos_data[idx + window_size//2 + stride//2, :]
q_a = quaternion.from_float_array(ori_data[idx + window_size//2 - stride//2, :])
q_b = quaternion.from_float_array(ori_data[idx + window_size//2 + stride//2, :])
rmat_a = quaternion.as_rotation_matrix(q_a)
rmat_b = quaternion.as_rotation_matrix(q_b)
delta_rmat = np.matmul(rmat_b, rmat_a.T)
delta_rvec = np.empty((3, 1))
cv2.Rodrigues(delta_rmat, delta_rvec)
delta_tvec = tvec_b - np.matmul(delta_rmat, tvec_a.T).T
y_delta_rvec.append(delta_rvec)
y_delta_tvec.append(delta_tvec)
x = np.reshape(x, (len(x), x[0].shape[0], x[0].shape[1]))
y_delta_rvec = np.reshape(y_delta_rvec, (len(y_delta_rvec), y_delta_rvec[0].shape[0]))
y_delta_tvec = np.reshape(y_delta_tvec, (len(y_delta_tvec), y_delta_tvec[0].shape[0]))
return x, [y_delta_rvec, y_delta_tvec], init_rvec, init_tvec
def quatUnion(q_parent, q_child):
q_parent_conj = np.array([q_parent[0], -q_parent[1], -q_parent[2], -q_parent[3]])
q_parent = quaternion.from_float_array(q_parent)
q_child = quaternion.from_float_array(q_child)
q_parent_conj = quaternion.from_float_array(q_parent_conj)
qU = q_parent_conj * q_child
return quaternion.as_float_array(qU).astype(np.float32)
def load(self, data_path):
if data_path[-1] == '/':
data_path = data_path[:-1]
with open(osp.join(data_path, 'info.json')) as f:
self.info = json.load(f)
# {'tango_reference_time': 3697949488366.0, 'date': '01/21/19', 'imu_init_gyro_bias': [0.005569000000000001, 0.009201, -0.00018299999999999914], 'imu_acce_scale': [0.9987918889533104, 0.997062129866083, 0.9932574091553678], 'grv_ori_error': 8.353779492444412, 'align_tango_to_body': [-0.4841758535176575, -0.4938374978693588, -0.5424326634072199, 0.47693298715598254], 'start_frame': 5896, 'imu_end_gyro_bias': [0.005432, 0.008774, -4.6e-05], 'type': 'annotated', 'imu_reference_time': 3610942440968.0, 'start_calibration': [0.0, 0.99995545, 0.009439, 0.0], 'ekf_ori_error': 3.6239102945197676, 'imu_acce_bias': [-0.15661902624553775, -0.026333329541761968, 0.05681364453654479], 'gyro_integration_error': 8.481689436777705, 'device': 'asus4', 'length': 323.7550000070669, 'imu_time_offset': -0.07619643889847794, 'end_calibration': [0.0, 0.99998599, 0.0052938, 0.0]}
self.info['path'] = osp.split(data_path)[-1]
self.info['ori_source'], ori, self.info['source_ori_error'] = select_orientation_source( # ori is hdf5.synced.game_rv i. e. Android Sensor.TYPE_GAME_ROTATION_VECTOR
data_path, self.max_ori_error, self.grv_only)
#('game_rv', array([[ 0.02301384, -0.734161 , 0.00956859, 0.67851714], [ 0.02296023, -0.73417201, 0.00956628, 0.67850771], ..., [ 0.05427992, 0.70881762, 0.07637797, -0.6991363 ]]]), 8.353779492444412)
with h5py.File(osp.join(data_path, 'data.hdf5')) as f:
gyro_uncalib = f['synced/gyro_uncalib'] # <HDF5 dataset "gyro_uncalib": shape (64752, 3), type "<f8">
acce_uncalib = f['synced/acce'] # <HDF5 dataset "acce": shape (64752, 3), type "<f8">
gyro = gyro_uncalib - np.array(self.info['imu_init_gyro_bias']) # array([[-0.02230625, 0.01376489, 0.01876768], [-0.01673078, 0.00785385, 0.01808999], ..., [ 0.00657855, -0.02807542, 0.00804713]])
acce = np.array(self.info['imu_acce_scale']) * (acce_uncalib - np.array(self.info['imu_acce_bias'])) # array([[-9.76895341, -0.19332236, -0.85234999], [-9.76140265, -0.2099069 , -0.81018915], ..., [-9.82066284, -0.32593967, -0.28265888]])
ts = np.copy(f['synced/time']) # array([3641.39639807, 3641.40139807, 3641.40639807, ..., 3965.14139807, 3965.14639807, 3965.15139807])
tango_pos = np.copy(f['pose/tango_pos']) # array([[ 0.00747055, 0.0794231 , 0.04721916], [ 0.00743938, 0.07954534, 0.04721213], ..., [ 0.04869788, -0.01891041, -0.03532039]])
init_tango_ori = quaternion.quaternion(*f['pose/tango_ori'][0]) # quaternion(0.500218919180744, 0.498520940104168, 0.458115146317795, -0.539803994906776)
# Compute the IMU orientation in the Tango coordinate frame.
ori_q = quaternion.from_float_array(ori) # array([quaternion(0.0230138445473811, -0.734161004581412, 0.00956858773770847, 0.678517142961637), quaternion(0.0229602307881793, -0.734172007659053, 0.00956628356173319, 0.678507709011024), ..., quaternion(0.0542799190079345, 0.708817623072373, 0.0763779734707989, -0.6991362963311)], dtype=quaternion)
# hdf5.synced.game_rv i. e. Android Sensor.TYPE_GAME_ROTATION_VECTOR
rot_imu_to_tango = quaternion.quaternion(*self.info['start_calibration']) # quaternion(0, 0.99995545, 0.009439, 0)
init_rotor = init_tango_ori * rot_imu_to_tango * ori_q[0].conj() # quaternion(-0.695289552060529, 0.00118374652078425, 0.00248606386725569, 0.718723783950829)
ori_q = init_rotor * ori_q # array([quaternion(-0.5028224217168, 0.50529138394065, -0.535057892743795, -0.453388785030156), quaternion(-0.502778345472314, 0.505300603413145, -0.535064320967734, -0.453420734559951), ..., quaternion(0.463716682908265, -0.54940199756135, 0.457301820727505, 0.523442677366401)], dtype=quaternion)
# ori_q = f['pose/tango_ori'][0] * self.info['start_calibration'] * conj(f['synced/game_rv'][0]) * f['synced/game_rv']
dt = (ts[self.w:] - ts[:-self.w])[:, None] # array([[1.], [1.], [1.], ..., [1.],[1.], [1.]])
glob_v = (tango_pos[self.w:] - tango_pos[:-self.w]) / dt # array([[-0.00533056, 0.01667982, -0.00509732], [-0.00531125, 0.016594 , -0.00511179], ..., [-0.0023489 , 0.00633583, -0.00057166]])
# these two below are position vector arrays
gyro_q = quaternion.from_float_array(np.concatenate([np.zeros([gyro.shape[0], 1]), gyro], axis=1)) # array([quaternion(0, -0.0223062507369463, 0.0137648946088728, 0.0187676819234896), quaternion(0, -0.0167307794589504, 0.00785385399152264, 0.018089989505323), ..., quaternion(0, 0.00657855020053501, -0.0280754170707831, 0.0080471337151681)], dtype=quaternion)
acce_q = quaternion.from_float_array(np.concatenate([np.zeros([acce.shape[0], 1]), acce], axis=1)) # array([quaternion(0, -9.76895340810378, -0.193322357928738, -0.852349993053583), quaternion(0, -9.76140265037272, -0.209906902110648, -0.810189151712629), ..., quaternion(0, -9.8206628384554, -0.325939671417927, -0.282658875290474)], dtype=quaternion)
# each element vector rotated by the corresponding ori_q rotation quaternion
# At test time, we use the coordinate frame defined by system device orientations from Android or iOS, whose Z axis is aligned with gravity.
# The whole is transformed into the global (Tango) coordinate frame.
glob_gyro = quaternion.as_float_array(ori_q * gyro_q * ori_q.conj())[:, 1:] # array([[-0.01258328, 0.02161022, 0.02034508], [-0.00665554, 0.02000185, 0.0149826 ], ..., [ 0.02674353, 0.01209917, -0.0058859 ]])
glob_acce = quaternion.as_float_array(ori_q * acce_q * ori_q.conj())[:, 1:] # array([[-3.46650073e-02, -3.36474233e-02, 9.80783499e+00], [-1.38849800e-02, 6.54256497e-03, 9.79719413e+00], ..., [ 3.31727211e-02, -6.26925428e-02, 9.82980926e+00]])
start_frame = self.info.get('start_frame', 0) # 5896
self.ts = ts[start_frame:] # array([3670.87639807, 3670.88139807, 3670.88639807, ..., 3965.14139807, 3965.14639807, 3965.15139807])
self.features = np.concatenate([glob_gyro, glob_acce], axis=1)[start_frame:] # array([[-5.94662071e-03, -9.38751552e-03, -5.15188486e-03, -3.08588928e-02, 6.39869105e-02, 9.86019268e+00], [-5.27530580e-03, -7.75847573e-03, -1.59778536e-02, -3.54599110e-02, 5.16253587e-02, 9.82394159e+00], ..., [ 2.67435308e-02, 1.20991655e-02, -5.88589595e-03, 3.31727211e-02, -6.26925428e-02, 9.82980926e+00]])
self.targets = glob_v[start_frame:, :2] # array([[-0.02427537, 0.02117807], [-0.02406481, 0.02145767], ..., [-0.0023489 , 0.00633583]]) targets are averaged for the window w
self.orientations = quaternion.as_float_array(ori_q)[start_frame:] # array([[-0.51946022, 0.24279321, -0.60182678, 0.55589147], [-0.51947897, 0.24272502, -0.6018242 , 0.55590699], ..., [ 0.46371668, -0.549402 , 0.45730182, 0.52344268]])
self.gt_pos = tango_pos[start_frame:] # array([[ 0.17387274, -0.14344794, -0.0743621 ], [ 0.17362087, -0.14350179, -0.07425673], ..., [ 0.04869788, -0.01891041, -0.03532039]])
def initPosTransfer(bodypose, q):
q_conj = np.array([q[0], -q[1], -q[2], -q[3]])
q = quaternion.from_float_array(q)
q_conj = quaternion.from_float_array(q_conj)
for i in range(len(bodypose)):
v = np.array([0, bodypose[i][0],bodypose[i][1], bodypose[i][2]])
v = quaternion.from_float_array(v)
v_new = q_conj*v*q
bodypose[i] = np.array([v_new.x, v_new.y, v_new.z])
return bodypose
def generate_trajectory_6d_quat(init_p, init_q, y_delta_p, y_delta_q):
cur_p = np.array(init_p)
cur_q = quaternion.from_float_array(init_q)
pred_p = []
pred_p.append(np.array(cur_p))
for [delta_p, delta_q] in zip(y_delta_p, y_delta_q):
cur_p = cur_p + np.matmul(quaternion.as_rotation_matrix(cur_q),
delta_p.T).T
cur_q = cur_q * quaternion.from_float_array(delta_q).normalized()
pred_p.append(np.array(cur_p))
return np.reshape(pred_p, (len(pred_p), 3))
def load(self, data_path):
if data_path[-1] == '/':
data_path = data_path[:-1]
with open(osp.join(data_path, 'info.json')) as f:
self.info = json.load(f)
self.info['path'] = osp.split(data_path)[-1]
if self.with_orientation:
self.info['ori_source'], ori, self.info['source_ori_error'] = select_orientation_source(
data_path, self.max_ori_error, self.grv_only)
with h5py.File(osp.join(data_path, 'data.hdf5')) as f:
gyro_uncalib = f['synced/gyro_uncalib']
acce_uncalib = f['synced/acce']
gyro = gyro_uncalib - np.array(self.info['imu_init_gyro_bias'])
# acce = np.array(self.info['imu_acce_scale']) * (acce_uncalib - np.array(self.info['imu_acce_bias']))
acce = acce_uncalib
ts = np.copy(f['synced/time'])
# tango_pos = np.copy(f['pose/tango_pos'])
# init_tango_ori = quaternion.quaternion(*f['pose/tango_ori'][0])
if self.with_orientation:
gyro = np.copy(gyro)
acce = np.copy(acce)
else:
glob_gyro = np.copy(gyro)
glob_acce = np.copy(acce)
# Compute the IMU orientation in the Tango coordinate frame.
if self.with_orientation:
ori_q = quaternion.from_float_array(ori)
# rot_imu_to_tango = quaternion.quaternion(*self.info['start_calibration'])
# init_rotor = init_tango_ori * rot_imu_to_tango * ori_q[0].conj()
# ori_q = init_rotor * ori_q
gyro_q = quaternion.from_float_array(np.concatenate([np.zeros([gyro.shape[0], 1]), gyro], axis=1))
acce_q = quaternion.from_float_array(np.concatenate([np.zeros([acce.shape[0], 1]), acce], axis=1))
glob_gyro = quaternion.as_float_array(ori_q * gyro_q * ori_q.conj())[:, 1:]
glob_acce = quaternion.as_float_array(ori_q * acce_q * ori_q.conj())[:, 1:]
# dt = (ts[self.w:] - ts[:-self.w])[:, None]
# glob_v = (tango_pos[self.w:] - tango_pos[:-self.w]) / dt
start_frame = self.info.get('start_frame', 0)
self.ts = ts[start_frame:]
self.features = np.concatenate([glob_gyro, glob_acce], axis=1)[start_frame:]
def pow(self, exponent):
"""self^exponent
:param exponent: single float
"""
exponent = float(exponent)
q_r_float = quaternion.as_float_array(self.q_r)
q_d_float = quaternion.as_float_array(self.q_d)
if len(self)==1:
q_r_w, q_r_vec = self.q_r.w, self.q_r.vec
q_d_w, q_d_vec = self.q_d.w, self.q_d.vec
q_r_w, q_r_vec = np.array([q_r_w])[:,None], np.array([q_r_vec])[:,None]
q_d_w, q_d_vec = np.array([q_d_w])[:, None], np.array([q_d_vec])[:, None]
else:
q_r_float = quaternion.as_float_array(self.q_r)
q_d_float = quaternion.as_float_array(self.q_d)
q_r_w = q_r_float[:,0]
q_r_vec = q_r_float[:,1:]
q_d_w = q_d_float[:, 0]
q_d_vec = q_d_float[:,1:]
theta = 2 * np.arccos(q_r_w)
if np.all(np.isclose(theta, 0)):
return DualQuaternion.from_translation_vector(exponent * np.array(self.translation()))
else:
s0 = q_r_vec / np.sin(theta[:,None] / 2)
d = -2. * q_d_w / np.sin(theta / 2)
se = (q_d_vec - s0 * d[:,None] / 2 * np.cos(theta[:,None] / 2)) / np.sin(theta[:,None] / 2)
q_r_float = np.zeros((len(self),4))
q_r_float[:,0] = np.cos(exponent * theta / 2)
q_r_float[:,1:] = np.sin(exponent * theta[:,None] / 2) * s0
q_r = quaternion.from_float_array(q_r_float)
q_d_float = np.zeros((len(self), 4))
q_d_float[:,0] = -exponent * d / 2 * np.sin(exponent * theta / 2)
#q_d_float[:,1:] = exponent * d / 2 * np.cos(exponent * theta / 2) * s0 + np.sin(exponent * theta / 2) * se
p3 = (exponent * d /2 *np.cos(exponent * theta / 2))[:,None] * s0 + np.sin(exponent * theta[:,None] / 2) * se
q_d_float[:,1:] = p3
q_d = quaternion.from_float_array(q_d_float)
return DualQuaternion(q_r, q_d)
def world_coordinates_from_rot_trans(init_t, init_q, relative_t, relative_q):
t = np.array(init_t)
q = quaternion.from_float_array(init_q)
pred_t = []
pred_t.append(t)
for [delta_t, delta_q] in zip(relative_t, relative_q):
t = t + np.matmul(quaternion.as_rotation_matrix(q), delta_t.T).T
q = q * quaternion.from_float_array(delta_q).normalized()
pred_t.append(np.array(t))
coord = np.reshape(pred_t, (len(pred_t), 3))
x = coord[:, 0]
y = coord[:, 1]
z = coord[:, 2]
return (x, y, z)
def calibrateRawIMU(ori):
import quaternion
import itertools
import myUtil
######### **********the order of IMUs are: [left_lower_wrist, right_lower_wrist, left_lower_leg, right_loewr_leg, head, back]
SMPL_SENSOR = [
'L_Shoulder', 'R_Shoulder', 'L_Knee', 'R_Knee', 'Head', 'Pelvis'
]
############### calculation in Rotation Matrix #########################
seq_len = len(ori)
# head sensor for the first frame
head_quat = quaternion.from_float_array(ori[100, 4, :])
Q = np.quaternion(0.5, 0.5, 0.5, 0.5)
# calib: R(T_I) which is constant over the frames
calib = np.linalg.inv(quaternion.as_rotation_matrix(np.dot(head_quat, Q)))
# bone2sensor: R(B_S) calculated once for each sensor and remain constant over the frames as used further
bone2sensor = {}
for i in range(len(SMPL_SENSOR)):
sensorid = SMPL_SENSOR[i]
qbone = quaternion.as_rotation_matrix(
quaternion.from_rotation_vector(myUtil.getGlobalBoneOri(sensorid)))
qsensor = np.dot(
calib,
quaternion.as_rotation_matrix(
quaternion.from_float_array(ori[100, i, :])))
boneQuat = np.dot(np.linalg.inv(qsensor), qbone)
bone2sensor[i] = boneQuat
# calibrated_ori: R(T_B) calculated as calib * sensor data(changes every frame) * bone2sensor(corresponding sensor)
calibrated_ori = np.asarray([
np.linalg.multi_dot([
calib,
quaternion.as_rotation_matrix(
quaternion.from_float_array(ori[k, j, :])), bone2sensor[j]
]) for k, j in itertools.product(range(seq_len), range(6))
])
calibrated_ori = calibrated_ori.reshape(-1, 6, 3, 3)
root_inv = np.linalg.inv(calibrated_ori[:, 5, :, :])
# root_inv = np.transpose(calibrated_ori[:, 5], [0, 2, 1])
norm_ori = np.asarray([
np.matmul(root_inv[k], calibrated_ori[k, j, :, :])
for k, j in itertools.product(range(seq_len), range(6))
])
norm_ori = norm_ori.reshape(-1, 6, 3, 3)
return norm_ori[:, 0:5, :, :]
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 test_avg(self):
# distribute a bunch of rotation around I,
# such that the average should be I:
# at random axis but constant angle,
nb_rotations = 1000
angles = np.linspace(0.0, np.deg2rad(45), 100)
averages_q = []
np.random.seed(1)
for angle in angles:
rotation_axis = np.random.uniform(-1., 1., size=(nb_rotations, 3))
rotation_axis /= np.linalg.norm(rotation_axis,
axis=1,
keepdims=True)
rotation_axis *= angle
rotation_qs = quaternion.from_rotation_vector(rotation_axis)
rotation_qs = quaternion.as_float_array(rotation_qs)
# print(rotation_qs)
r = average_quaternion(rotation_qs)
averages_q.append(r.tolist())
averages_q = np.array(averages_q)
# normalizes rotation (resolve the opposite ambiguity)
averages_q *= np.sign(averages_q[:, 0]).reshape((-1, 1))
# convert quaternions to angle-axis representation
averages_q = quaternion.from_float_array(averages_q)
averages_v = quaternion.as_rotation_vector(averages_q)
# retrieve angle amplitudes.
errors_rad = np.linalg.norm(averages_v, axis=1)
# # plot for debug
# import matplotlib.pyplot as plt
# plt.plot(np.rad2deg(errors_rad), 'x', label='Errors (in deg.) as a function of angle amplitude (in deg.)')
# plt.show()
maximum_error_deg = np.rad2deg(np.max(errors_rad))
self.assertLess(maximum_error_deg, 3.6)
def __init__(self, tcp_pos=None, tcp_quat=None):
self.d_bs = 0.36
self.d_se = 0.42
self.d_ew = 0.4
self.d_wf = 0.151
# DH_paramters a_i, alpha_i, d_i
self.dh_a = np.array([0., 0., 0., 0., 0., 0., 0.])
self.dh_alpha = np.array([-np.pi / 2, np.pi / 2, np.pi / 2, -np.pi / 2, -np.pi / 2, np.pi / 2, 0.])
self.dh_d = np.array([self.d_bs, 0., self.d_se, 0., self.d_ew, 0., self.d_wf])
self.joint_limits = np.vstack([IIWA_JOINT_MIN_LIMITS, IIWA_JOINT_MAX_LIMITS]).T
self.joint_vel_limits = np.array([[-85., 85.],
[-85., 85.],
[-100., 100.],
[-75., 75.],
[-130., 130.],
[-135., 135.],
[-135., 135.]]) / 180. * np.pi
self.singularity_eps = 0.1
self.T_ee = np.eye(4)
if tcp_pos is not None:
self.T_ee[:3, 3] = tcp_pos
if tcp_quat is not None:
self.T_ee[:3, :3] = quaternion.as_rotation_matrix(quaternion.from_float_array(tcp_quat))
def from_quat(self, joint_angles):
"""
Get joint positions from quaternion representations in shape (N, H36M_NR_JOINTS*4)
"""
qs = quaternion.from_float_array(np.reshape(joint_angles, [-1, H36M_NR_JOINTS, 4]))
aa = quaternion.as_rotation_matrix(qs)
return self.fk(np.reshape(aa, [-1, H36M_NR_JOINTS * 3]))
def update_orientation(self, timestep, ext_forces):
"""
Update the orientation of the rigid body according to the ellapsed time and external forces.
Parameters
----------
timestep : scalar
The ellapsed time.
ext_forces : dict
The external forces applied to the rigid body.
Returns
-------
quaternion
A copy of the rigid body's orientation.
"""
cm_torque = sum([
force.torque(self.orientation) for force in ext_forces.values()
]) # M
non_inertial_term = np.cross(self.ang_velocity,
self.inertia.dot(
self.ang_velocity)) # w x (I.w)
ang_acceleration = np.linalg.inv(self.inertia).dot(
cm_torque - non_inertial_term
) #+ from Euler's equations I.w_dot + w x (I.w) = M
self.ang_velocity += ang_acceleration * timestep # dw = w_dot*dt
omega_matrix = self.quaternion_omega_matrix()
d_orientation = omega_matrix.dot(q.as_float_array(
self.orientation)) * 0.5 * timestep
self.orientation += q.from_float_array(
d_orientation) # q_dot=0.5*Omega*q
return np.copy(self.orientation)
def orientation_to_angles(ori):
"""
Covert an array of quaternions to an array of Euler angles. Calculations are from Android source code:
https://android.googlesource.com/platform/frameworks/base/+/master/core/java/android/hardware/SensorManager.java
Function "getOrientation(float[] R, float[] values)"
Note that this function DOES NOT consider singular configurations, such as Gimbal Lock.
Args:
ori: an array of N quaternions.
Returns:
A Nx3 array. With Android's game rotation vector or rotation vector, each group of three values
correspond to: azimuth(yaw), pitch and roll, respectively.
"""
if ori.dtype != quaternion.quaternion:
ori = quaternion.from_float_array(ori)
rm = quaternion.as_rotation_matrix(ori)
angles = np.zeros([ori.shape[0], 3])
angles[:, 0] = adjust_angle_array(np.arctan2(rm[:, 0, 1], rm[:, 1, 1]))
angles[:, 1] = adjust_angle_array(np.arcsin(-rm[:, 2, 1]))
angles[:, 2] = adjust_angle_array(np.arctan2(-rm[:, 2, 0], rm[:, 2, 2]))
return angles
def load(self, data_path):
super().load(data_path)
self.velocities = self.targets[:, :2]
with open(osp.join(data_path, 'info.json')) as f:
info = json.load(f)
rot_tango_to_body = info['align_tango_to_body']
start_frame = info.get('start_frame', 0)
with h5py.File(osp.join(data_path, 'data.hdf5')) as f:
tango_ori = f['pose/tango_ori']
body_ori_q = quaternion.from_float_array(
tango_ori) * quaternion.from_float_array(
rot_tango_to_body).conj()
body_heading = orientation_to_angles(body_ori_q)[start_frame:, 0]
self.targets = np.stack(
[np.sin(body_heading), np.cos(body_heading)], axis=-1)
def align_3dvector_with_gravity_legacy(data,
gravity,
local_g_direction=np.array([0, 1, 0])):
"""
Eliminate pitch and roll from a 3D vector by aligning gravity vector to local_g_direction.
@:param data: N x 3 array
@:param gravity: real gravity direction
@:param local_g_direction: z direction before alignment
@:return
"""
assert data.ndim == 2, 'Expect 2 dimensional array input'
assert data.shape[1] == 3, 'Expect Nx3 array input'
assert data.shape[0] == gravity.shape[0], '{}, {}'.format(
data.shape[0], gravity.shape[0])
epsilon = 1e-03
gravity_normalized = gravity / np.linalg.norm(gravity, axis=1)[:, None]
output = np.copy(data)
for i in range(data.shape[0]):
# Be careful about two singular conditions where gravity[i] and local_g_direction are parallel.
gd = np.dot(gravity_normalized[i], local_g_direction)
if gd > 1. - epsilon:
continue
if gd < -1. + epsilon:
output[i, [1, 2]] *= -1
continue
q = quaternion.from_float_array(
quaternion_from_two_vectors(gravity[i], local_g_direction))
output[i] = (q * quaternion.quaternion(1.0, *data[i]) * q.conj()).vec
return output
def generate_mesh_slices(width, height, depth, center_w, center_x, center_y, center_z, zoom, rotation_theta, rotation_phi, rotation_gamma, rotation_beta, offset_x, offset_y, depth_dither=1):
ct, st = np.cos(rotation_theta), np.sin(rotation_theta)
cp, sp = np.cos(rotation_phi), np.sin(rotation_phi)
cg, sg = np.cos(rotation_gamma), np.sin(rotation_gamma)
cb, sb = np.cos(rotation_beta), np.sin(rotation_beta)
zoom = 2**-zoom
x = np.arange(width, dtype='float64') + offset_x
y = np.arange(height, dtype='float64') + offset_y
z = np.arange(depth, dtype='float64')
x, y = np.meshgrid(x, y)
x = (2 * x - width) * zoom / height
y = (2 * y - height) * zoom / height
for z_ in z:
z_ += np.random.rand(*x.shape) * depth_dither - 0.5*depth_dither
z_ = (2 * z_ - depth) * zoom / depth
# The screen coordinates have x as the real axis, so 'w' is a misnomer here.
y_ = cb*y
w_ = -sb*y
x_, z_ = x*ct + z_*st, z_*ct - x*st
x_, y_ = x_*cp + y*sp, y*cp - x_*sp
y_, z_ = y_*cg + z_*sg, z_*cg - y_*sg
# See above for the misnaming compared to the quaternion library.
x_ += center_w
y_ += center_x
z_ += center_y
w_ += center_z
yield quaternion.from_float_array(np.stack((x_, y_, z_, w_), axis=-1))
def quatdiff_in_euler(quat_curr, quat_des):
"""
Compute difference between quaternions and return
Euler angles as difference
"""
curr_mat = quaternion.as_rotation_matrix(
quaternion.from_float_array(quat_curr))
des_mat = quaternion.as_rotation_matrix(
quaternion.from_float_array(quat_des))
rel_mat = des_mat.T.dot(curr_mat)
rel_quat = quaternion.from_rotation_matrix(rel_mat)
vec = quaternion.as_float_array(rel_quat)[1:]
if rel_quat.w < 0.0:
vec = -vec
return -des_mat.dot(vec)
def load_gt_poses(gt_filename):
indices = []
Ts = []
# Camera to world
# Dirty left 2 right coordinate transform
# https://github.com/theNded/MeshHashing/blob/master/src/io/config_manager.cc#L88
T_l2r = np.eye(4)
T_l2r[1, 1] = -1
with open(gt_filename) as f:
content = f.readlines()
for line in content:
data = np.array(list(map(float, line.strip().split(' '))))
indices.append(int(data[0]))
data = data[1:]
q = data[3:][[3, 0, 1, 2]]
q = quaternion.from_float_array(q)
R = quaternion.as_rotation_matrix(q)
t = data[:3]
T = np.eye(4)
T[0:3, 0:3] = R
T[0:3, 3] = t
Ts.append(T_l2r @ T @ np.linalg.inv(T_l2r))
return indices, Ts
def generate_imaginary_mesh_slices(width, height, depth, center_w, center_x, center_y, center_z, zoom, rotation_theta, rotation_phi, rotation_gamma, offset_x, offset_y, depth_dither=1):
ct, st = np.cos(rotation_theta), np.sin(rotation_theta)
cp, sp = np.cos(rotation_phi), np.sin(rotation_phi)
cg, sg = np.cos(rotation_gamma), np.sin(rotation_gamma)
zoom = 2**-zoom
x = np.arange(width, dtype='float64') + offset_x
y = np.arange(height, dtype='float64') + offset_y
z = np.arange(depth, dtype='float64')
x, y = np.meshgrid(x, y)
x = (2 * x - width) * zoom / height
y = (2 * y - height) * zoom / height
w = 0*x + center_w
for z_ in z:
z_ += np.random.rand(*x.shape) * depth_dither - 0.5*depth_dither
z_ = (2 * z_ - depth) * zoom / depth
x_, z_ = x*ct + z_*st, z_*ct - x*st
x_, y_ = x_*cp + y*sp, y*cp - x_*sp
y_, z_ = y_*cg + z_*sg, z_*cg - y_*sg
x_ += center_x
y_ += center_y
z_ += center_z
yield quaternion.from_float_array(np.stack((w, x_, y_, z_), axis=-1))
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 MakeRotationQuaternion(angle, vec):
a = np.cos(angle/2)
b = np.sin(angle/2)
axis = vec*b
axis = np.append(a, axis)
quat = quaternion.from_float_array(axis)
return quat
def _system_equations(self, state, forces, moments):
"""Euler flat earth equations: linear momentum equations, angular momentum
equations, angular kinematic equations, linear kinematic
equations.
Parameters
----------
velocity, omega, attitude, forces, moments : all nd arrays of size (m,12)
Returns
-------
dstate_dt : array_like, shape(9)
Derivative with respect to time of the state vector.
Current value of absolute acceleration and angular acceleration,
both expressed in body axes, Euler angles derivatives and velocity
with respect to Earth Axis.
(du_dt, dv_dt, dw_dt, dp_dt, dq_dt, dr_dt, dphi_dt, dtheta_dt,
dpsi_dt, dx_dt, dy_dt, dz_dt)
(m/s² , m/s², m/s², rad/s², rad/s², rad/s², rad/s, rad/s, rad/s,
m/s, m/s, m/s).
References
----------
.. [1] B. Etkin, Dynamics of flight, stability and control
"""
state_dot = self.make_state_obj(N=state.N)
# get inertia parameters
mass = self.aircraft.mass
I = self.aircraft.inertia
invI = self.aircraft.inertia_inverse
# get state values
u, v, w = state.velocity.T
p, q, r = state.omega.T
# phi, theta, psi = attitude.T
# Linear momentum equations
Fx, Fy, Fz = forces.T
state_dot.u = Fx / mass + r * v - q * w
state_dot.v = Fy / mass - r * u + p * w
state_dot.w = Fz / mass + q * u - p * v
# Angular momentum equations
state_dot.p, state_dot.q, state_dot.r = (
invI @ (moments - np.cross(state.omega, (I @ state.omega.T).T)).T)
# Angular Kinematic equations
Qomega = npquat.from_float_array(
np.hstack((np.zeros((state.N, 1)), state.omega)))
state_dot.quaternion = 1 / 2 * state.quaternion * Qomega + 1 / 2 * (
1 - np.norm(state.quaternion)) * state.quaternion / .1
# Linear kinematic equations : rotate body velocity back to earth frame
state_dot.position = change_basis(state.velocity,
state.quaternion.conjugate())
self.aircraft.state_dot = state_dot
return state_dot.vec
def respawn_agent(self, position, rotation):
"""Respawns the agent at the position and rotation specified"""
sim = self._robot.base.sim
agent = sim.get_agent(0)
new_agent_state = habitat_sim.AgentState()
new_agent_state.position = position
new_agent_state.rotation = quaternion.from_float_array(rotation)
agent.set_state(new_agent_state)
def compute_attitude_splines(self):
s_w = self.attitude_splines[0]
s_x = self.attitude_splines[1]
s_y = self.attitude_splines[2]
s_z = self.attitude_splines[3]
splines_coeffs = np.array([s_w, s_x, s_y, s_z]).T
self.discretized_attitude = quaternion.from_float_array(splines_coeffs)
return self.discretized_attitude
def minimal_rotation(R, t, iterations=2):
"""Adjust frame so that there is no rotation about z' axis
The output of this function is a frame that rotates the z axis onto the same z' axis as the
input frame, but with minimal rotation about that axis. This is done by pre-composing the input
rotation with a rotation about the z axis through an angle gamma, where
dgamma/dt = 2*(dR/dt * z * R.conjugate()).w
This ensures that the angular velocity has no component along the z' axis.
Note that this condition becomes easier to impose the closer the input rotation is to a
minimally rotating frame, which means that repeated application of this function improves its
accuracy. By default, this function is iterated twice, though a few more iterations may be
called for.
Parameters
==========
R: quaternion array
Time series describing rotation
t: float array
Corresponding times at which R is measured
iterations: int [defaults to 2]
Repeat the minimization to refine the result
"""
from scipy.interpolate import InterpolatedUnivariateSpline as spline
if iterations == 0:
return R
R = quaternion.as_float_array(R)
Rdot = np.empty_like(R)
for i in range(4):
Rdot[:, i] = spline(t, R[:, i]).derivative()(t)
R = quaternion.from_float_array(R)
Rdot = quaternion.from_float_array(Rdot)
halfgammadot = quaternion.as_float_array(Rdot * quaternion.z * R.conjugate())[:, 0]
halfgamma = spline(t, halfgammadot).antiderivative()(t)
Rgamma = np.exp(quaternion.z * halfgamma)
return minimal_rotation(R * Rgamma, t, iterations=iterations-1)
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