def get_all_property(dataset, name):
all_data_point = []
# rot_mean = torch.Tensor(dataset.data_statistics['rotation_mean'])
pos_mean = torch.Tensor(dataset.data_statistics['position_mean'])
pos_std = torch.Tensor(dataset.data_statistics['position_std'])
# rot_std = torch.Tensor(dataset.data_statistics['rotation_std'])
for idx in range(len(dataset)):
input_dict, labels = dataset[idx]
if name == 'before_angle':
before_tensor = input_dict['norm_state_tensor'][3:7]
before_angle1 = euler_from_quaternion(before_tensor,
axes='rxyz')[0] / 3.14 * 180
before_angle2 = euler_from_quaternion(before_tensor,
axes='rxyz')[1] / 3.14 * 180
before_angle3 = euler_from_quaternion(before_tensor,
axes='rxyz')[2] / 3.14 * 180
all_data_point += [before_angle1, before_angle2, before_angle3]
elif name == 'position':
before_pos = norm_tensor(
norm_or_denorm=False,
tensor=input_dict['norm_state_tensor'][:3],
mean_tensor=pos_mean,
std_tensor=pos_std)
all_data_point += [before_pos[0], before_pos[1], before_pos[2]]
elif name == 'rot_diff':
before_rot = input_dict['norm_state_tensor'][3:7]
before_angle = euler_from_quaternion(before_rot,
axes='rxyz')[0] / 3.14 * 180
after_rot = labels['norm_state_tensor'][3:7]
after_angle = euler_from_quaternion(after_rot,
axes='rxyz')[0] / 3.14 * 180
diff_rot = after_angle - before_angle
if abs(diff_rot) > 40:
embed()
all_data_point += [diff_rot]
elif name == 'pos_diff':
before_pos = norm_tensor(
norm_or_denorm=False,
tensor=input_dict['norm_state_tensor'][:3],
mean_tensor=pos_mean,
std_tensor=pos_std)
after_pos = norm_tensor(norm_or_denorm=False,
tensor=labels['norm_state_tensor'][:3],
mean_tensor=pos_mean,
std_tensor=pos_std)
diff_pos = after_pos - before_pos
all_data_point += [diff_pos[0], diff_pos[1], diff_pos[2]]
return all_data_point
def quaternion_to_euler_angle(quaternion, degree=True):
'''
Transform quaternion to euler angles.
:param quaternion: in [w, x, y, z] format.
:param degree: whether the result should be represented in degree.
:return: euler angles, in xyz format.
'''
if type(quaternion) == torch.Tensor:
quaternion = quaternion.cpu()
euler_angle = euler_from_quaternion(quaternion, axes='rxyz')
euler_angle = np.array(euler_angle)
if degree:
euler_angle = euler_angle / math.pi * 180
return euler_angle
from utils.conversions import trans_rot_to_hmat,hmat_to_trans_rot
from utils.transformations import euler_from_quaternion
import rospy
import roslib
roslib.load_manifest("tf")
import tf
import numpy as np
class Globals:
listener = None
if rospy.get_name() == "/unnamed":
rospy.init_node("transform_kinect_frames")
Globals.listener = tf.TransformListener()
rospy.sleep(1)
ONIfromCL = trans_rot_to_hmat(*Globals.listener.lookupTransform("/camera_rgb_optical_frame","camera_link", rospy.Time(0)))
GAZfromONI = trans_rot_to_hmat([0, -.15, -.24], [0, 0, 0, 1])
HEADfromGAZ = trans_rot_to_hmat(*Globals.listener.lookupTransform("/head_plate_frame", "/wide_stereo_gazebo_l_stereo_camera_optical_frame",rospy.Time(0)))
HEADfromCL = np.dot(np.dot(HEADfromGAZ, GAZfromONI), ONIfromCL)
trans_final, quat_final = hmat_to_trans_rot(HEADfromCL)
eul_final = euler_from_quaternion(quat_final)
print "translation", trans_final
print "rotation", eul_final
print "%.4f %.4f %.4f %.4f %.4f %.4f %.4f"%(tuple(trans_final) + tuple(quat_final))
def quat_to_yaw(q):
e = transformations.euler_from_quaternion(q)
return e[2]
def _hdf5_add_commands(self):
### all
hdf5_times = list(self._hdf5_dict.time)
hdf5_times.append(hdf5_times[-1] + (hdf5_times[-1] - hdf5_times[-2]))
### steps
csv = pandas.read_csv(self._csv_acc_fname)
times = 1e-3 * np.array(csv['cts'])
accs = np.stack([
csv['Accelerometer (x) [m/s2]'], csv['Accelerometer (y) [m/s2]'],
csv['Accelerometer (z) [m/s2]']
]).T.astype(np.float32)
pedometer = Pedometer(accs, times)
step_rates = pedometer.get_step_rates()
step_rates_chunked = []
for i in range(len(hdf5_times) - 1):
indices = np.logical_and(times >= hdf5_times[i],
times < hdf5_times[i + 1])
step_rate_i = step_rates[indices].mean()
step_rates_chunked.append(step_rate_i)
### camera orientation
csv = pandas.read_csv(self._csv_ori_fname)
times = np.array(1e-3 * csv['cts'])
quats = np.array(
[csv['1'], csv['2'], csv['3'], csv['CameraOrientation']]).T
def smooth_quaternions(quats, frac_range=0.4, frac_bias=0.04):
# https://www.mathworks.com/help/fusion/examples/lowpass-filter-orientation-using-quaternion-slerp.html
smoothed_quats = [quats[0]]
for quat in quats[1:]:
smoothed_quat = smoothed_quats[-1]
delta_quat = transformations.quaternion_multiply(
transformations.quaternion_inverse(smoothed_quat), quat)
delta_quat /= np.linalg.norm(delta_quat)
angle = transformations.quaternion_to_axisangle(
delta_quat)[0][0]
alpha = (angle / np.pi) * frac_range + frac_bias
smoothed_quat = transformations.quaternion_slerp(
smoothed_quats[-1], quat, alpha)
smoothed_quats.append(smoothed_quat)
smoothed_quats = np.array(smoothed_quats)
assert np.all(np.isfinite(smoothed_quats))
return smoothed_quats
quats = smooth_quaternions(quats)
quats_chunked = []
for i in range(len(hdf5_times) - 1):
indices = np.logical_and(times >= hdf5_times[i],
times < hdf5_times[i + 1])
quats_chunked.append(quats[indices])
### integrate
dt = times[1] - times[0]
fps = 1. / dt
turns = []
steps = []
for i in range(len(hdf5_times) - 1):
indices = np.where(
np.logical_and(times >= hdf5_times[i],
times < hdf5_times[i + 1]))[0]
assert len(indices) == (indices[-1] - indices[0] + 1)
start_idx, stop_idx = indices[0], indices[-1]
# step
step_rate = step_rates_chunked[i]
# angles
prepend = int(fps * 1.5)
prestart_idx = np.clip(start_idx - prepend, 0, len(times) - 1)
poststop_idx = np.clip(stop_idx + prepend, 0, len(times) - 1)
origin = quats[start_idx]
origin_inv = transformations.quaternion_inverse(origin)
quats_list_origin = [
transformations.quaternion_multiply(origin_inv, q)
for q in quats[prestart_idx:poststop_idx]
]
angles = -np.array([
transformations.euler_from_quaternion(q)[1]
for q in quats_list_origin
])
angles = np_utils.butter_lowpass_filter(angles,
cutoff=0.8,
fs=30.,
order=5)
# account for delay of
delay_shift = int(0.8 * fps)
angles = np.roll(angles, -delay_shift)
angles = angles[start_idx -
prestart_idx:-(poststop_idx - stop_idx)]
pos = np.zeros(2)
if len(angles) > 0:
dt = (1. / GoproToHdf5.FPS) / len(angles)
for angle in angles:
pos += dt * step_rate * np.array(
[np.cos(angle), np.sin(angle)])
turns.append(np.arctan2(pos[1], pos[0]))
steps.append(np.linalg.norm(pos))
self._hdf5_dict.commands.step = np.array(steps)
self._hdf5_dict.commands.turn = np.array(turns)
def euler_from_quaternion(q):
"""Convert quaternion from (x,y,z,w) returned from pybullet to
euler angles = (roll, pitch, yaw) convention used by transformations.py"""
trans_quat = to_transquat(q)
eul = trans.euler_from_quaternion(trans_quat)
return eul
