def next_quaternion(self, ):
if self.oriind >= self.SmoothCount: # sample a new orientation
rand_ori = self.random_quaternion()
new_ori = rand_ori * self.orientation
new_qtn = Quaternionr(new_ori.x, new_ori.y, new_ori.z, new_ori.w)
(pitch, roll, yaw) = to_eularian_angles(new_qtn)
# print ' %.2f, %.2f, %.2f' %(pitch, roll, yaw )
pitch = np.clip(pitch, self.MinPitch, self.MaxPitch)
roll = np.clip(roll, self.MinRoll, self.MaxRoll)
yaw = np.clip(yaw, self.MinYaw, self.MaxYaw)
# print ' %.2f, %.2f, %.2f' %(pitch, roll, yaw )
new_qtn_clip = to_quaternion(pitch, roll, yaw)
new_ori_clip = Quaternionpy(new_qtn_clip.w_val, new_qtn_clip.x_val,
new_qtn_clip.y_val, new_qtn_clip.z_val)
qtnlist = Quaternionpy.intermediates(self.orientation,
new_ori_clip,
self.SmoothCount - 2,
include_endpoints=True)
self.orientation = new_ori_clip
self.orilist = list(qtnlist)
self.oriind = 1
# print "sampled new", new_ori, ', after clip', self.orientation #, 'list', self.orilist
next_qtn = self.orilist[self.oriind]
self.oriind += 1
# print " return next", next_qtn
return Quaternionr(next_qtn.x, next_qtn.y, next_qtn.z, next_qtn.w)
def save_poses(start_pos, end_pos, start_quat, end_quat, N):
# generate positions
pos_diff = end_pos - start_pos
pos_diff = np.repeat(np.reshape(pos_diff, [1, 3]), N, 0)
inc = np.reshape(np.linspace(0, 1, num=N), [N, 1])
pos_diff *= inc
start_pos = np.repeat(np.reshape(start_pos, [1, 3]), N, 0)
pos = start_pos + pos_diff
# generate orientations
qs = Quaternion.intermediates(start_quat,
end_quat,
N,
include_endpoints=False)
parent_dir = "/home/jayant/monkey/grocery_data/Supermarket/data/simulated_capture"
# combine
for i, q in enumerate(qs):
R = q.rotation_matrix
rx = R[1, :]
ry = -R[2, :]
rz = -R[0, :]
def rotm(rx, ry, rz):
return np.array([-rz, rx, -ry])
savemat("{}/pose{:07d}.mat".format(parent_dir, i + 1), {
'C': pos[i, :],
'R': R
})
# Generate rotations
thetas = [-20, -10, 10, 20]
for theta in thetas:
th = np.radians(theta)
R = rotm(rx * cos(th) + ry * sin(th), -rx * sin(th) + ry * cos(th),
rz)
savemat(
"{}/pose{:07d}_rotz{}.mat".format(parent_dir, i + 1, theta), {
'C': pos[i, :],
'R': R
})
R = rotm(-rz * sin(th) + rx * cos(th), ry,
rz * cos(th) + rx * sin(th))
savemat(
"{}/pose{:07d}_roty{}.mat".format(parent_dir, i + 1, theta), {
'C': pos[i, :],
'R': R
})
R = rotm(rx,
ry * cos(th) + rz * sin(th), -ry * sin(th) + rz * cos(th))
savemat(
"{}/pose{:07d}_rotx{}.mat".format(parent_dir, i + 1, theta), {
'C': pos[i, :],
'R': R
})
def interpolate_quaternions(self, qstart, qend, n, include_endpoints=True):
q1 = Quaternion(a=qstart[3], b=qstart[0], c=qstart[1], d=qstart[2])
q2 = Quaternion(a=qend[3], b=qend[0], c=qend[1], d=qend[2])
return Quaternion.intermediates(q1,
q2,
n,
include_endpoints=include_endpoints)
def test_interpolate(self):
q1 = Quaternion(axis=[1, 0, 0], angle=0.0)
q2 = Quaternion(axis=[1, 0, 0], angle=2*pi/3)
num_intermediates = 3
base = pi/6
list1 = list(Quaternion.intermediates(q1, q2, num_intermediates, include_endpoints=False))
list2 = list(Quaternion.intermediates(q1, q2, num_intermediates, include_endpoints=True))
self.assertEqual(len(list1), num_intermediates)
self.assertEqual(len(list2), num_intermediates+2)
self.assertEqual(list1[0], list2[1])
self.assertEqual(list1[1], list2[2])
self.assertEqual(list1[2], list2[3])
self.assertEqual(list2[0], q1)
self.assertEqual(list2[1], Quaternion(axis=[1, 0, 0], angle=base))
self.assertEqual(list2[2], Quaternion(axis=[1, 0, 0], angle=2*base))
self.assertEqual(list2[3], Quaternion(axis=[1, 0, 0], angle=3*base))
self.assertEqual(list2[4], q2)
def generate_quaternion():
q1 = Quaternion.random()
q2 = Quaternion.random()
while True:
for q in Quaternion.intermediates(q1, q2, 20, include_endpoints=True):
yield q
#q1, q2 = q2, q1
q1 = q2
q2 = Quaternion.random()
def pose_interpolation(start_pose, end_pose, num=10, include_endpoint=False): x0, y0, theta0 = start_pose x1, y1, theta1 = end_pose x = np.linspace(x0, x1, num=num, endpoint=include_endpoint) y = np.linspace(y0, y1, num=num, endpoint=include_endpoint) ## convert to quaternion q0 = Quaternion(axis=[0, -1, 0], angle=theta0) q1 = Quaternion(axis=[0, -1, 0], angle=theta1) pose_list = [] v = np.array([1, 0, 0]) for idx, q in enumerate(Quaternion.intermediates(q0, q1, num-1, include_endpoints=True)): if idx < num: e, d, f = q.rotate(v) theta = atan2(f, e) current_pose = [x[idx], y[idx], theta] pose_list.append(current_pose) return pose_list
def interpolate_nodes(node_1, node_2, n_nodes):
node_list = []
for i, quaternion in enumerate(
Quaternion.intermediates(node_1.quaternion,
node_2.quaternion, (n_nodes - 2),
include_endpoints=True)):
vector = node_2.xyz - node_1.xyz
node_xyz = node_1.xyz + i * vector / (n_nodes - 1)
node_quaternion = quaternion
node_list.append([node_xyz, node_quaternion])
return node_list
def _get_intermediate_delta_quats(self,
init_quat,
des_quat,
num_intermediates=10):
"""
init_quat: initial quaternion
des_quat: desired quaternion
TODO: ideally since the quaternions are like complex numbers in high
dimensions, the multiplication should give addition, but for some reason
the simple addtion is fine here, investigate why this is the case
"""
# class should be pyquaternion.Quaternion
from pyquaternion import Quaternion
quat_to_array = lambda q: np.asarray([q.w, q.x, q.y, q.z])
if isinstance(init_quat, np.ndarray):
init_quat = Quaternion(init_quat)
if isinstance(des_quat, np.ndarray):
des_quat = Quaternion(des_quat)
assert isinstance(init_quat, Quaternion)
assert isinstance(des_quat, Quaternion)
# generator for the intermediates
intermediate_quats = list()
for q in Quaternion.intermediates(init_quat,
des_quat,
num_intermediates,
include_endpoints=True):
qu = quat_to_array(q)
intermediate_quats.append(qu)
# go through the intermediates to generate the delta quats
delta_quats = list()
prev_quat = quat_to_array(init_quat).copy()
for q in intermediate_quats:
delta_quat = q - prev_quat
delta_quats.append(delta_quat)
prev_quat = q.copy()
# now delta quats when combined with initial quat should sum to 1
add_val = quat_to_array(init_quat) + np.sum(delta_quats, axis=0)
assert np.allclose(add_val, quat_to_array(des_quat))
return delta_quats
def interpolate_rotation(states_dict):
"""Interpolate rotation states as quaternions
Parameters
----------
states_dict: list of dicts
list of states dictionaries generated by scriptcommands.create_frame_commandlist()
and naparimovie.create_state_dict()
Returns
-------
all_states: list of pyquaternions
list of rotation states of length N frames
"""
frames_rot = [[x['frame'], x['rotate']] for x in states_dict
if x['rotate']]
all_states = {
x: []
for x in range(frames_rot[0][0], frames_rot[-1][0] + 1)
}
for i in range(len(frames_rot) - 1):
q0 = pyQuaternion(frames_rot[i][1].w, frames_rot[i][1].x,
frames_rot[i][1].y, frames_rot[i][1].z)
q1 = pyQuaternion(frames_rot[i + 1][1].w, frames_rot[i + 1][1].x,
frames_rot[i + 1][1].y, frames_rot[i + 1][1].z)
num_frames = frames_rot[i + 1][0] - frames_rot[i][0] - 1
for ind, q in enumerate(
pyQuaternion.intermediates(q0,
q1,
num_frames,
include_endpoints=True)):
all_states[frames_rot[i][0] + ind] = q
all_states = [all_states[x] for x in all_states.keys()]
return all_states
def rotation():
go_to(True)
target = [
-0.09108, -0.51868, 0.47657, -0.49215, -0.48348, 0.51335, -0.5104
]
target_quaternion = Quaternion(np.roll(target[3:], 1))
current_quaternion = arm.end_effector()[3:]
from_quaternion = Quaternion(np.roll(current_quaternion, 1))
rotate = Quaternion(axis=[1, 0, 0], degrees=00.0)
to_quaternion = from_quaternion * rotate
# rotate = Quaternion(axis=[0,1,0], degrees=-50.0)
# to_quaternion *= rotate
rotate = Quaternion(axis=[0, 0, 1], degrees=50.0)
to_quaternion *= rotate
old_q = from_quaternion
for q in Quaternion.intermediates(q0=from_quaternion,
q1=target_quaternion,
n=10):
w = transformations.angular_velocity_from_quaternions(
old_q, q, 1.0 / 10.0)
old_q = q
delta = np.concatenate((np.zeros(3), w.vector))
to_pose = transformations.pose_euler_to_quaternion(arm.end_effector(),
delta,
dt=1.0 / 10.0)
arm.set_target_pose_flex(to_pose, t=1.0 / 10.0)
rospy.sleep(1.0 / 10.0)
dist = Quaternion.distance(target_quaternion,
Quaternion(np.roll(arm.end_effector()[3:], 1)))
print(dist)
def record(client, start_pos, end_pos, start_quat, end_quat, N):
# generate positions
pos_diff = end_pos - start_pos
pos_diff = np.repeat(np.reshape(pos_diff, [1, 3]), N, 0)
inc = np.reshape(np.linspace(0, 1, num=N), [N, 1])
pos_diff *= inc
start_pos = np.repeat(np.reshape(start_pos, [1, 3]), N, 0)
pos = start_pos + pos_diff
# generate orientations
qs = Quaternion.intermediates(start_quat,
end_quat,
N,
include_endpoints=False)
# combine
for i, q in enumerate(qs):
print(pos[i, :], q.rotation_matrix)
responses = get_image_and_depth(client, pos[i, :], q)
for j, response in enumerate(responses):
if response.pixels_as_float:
airsim.write_pfm(
os.path.normpath('depth{:07d}.pfm'.format(i + 1)),
airsim.get_pfm_array(response))
else:
airsim.write_file(
os.path.normpath('image{:07d}.png'.format(i + 1)),
response.image_data_uint8)
R = q.rotation_matrix
rx = R[1, :]
ry = -R[2, :]
rz = -R[0, :]
def rotm(rx, ry, rz):
return np.array([-rz, rx, -ry])
savemat("pose{:07d}.mat".format(i + 1), {'C': pos[i, :], 'R': R})
thetas = [-20, -10, 10, 20]
for theta in thetas:
th = np.radians(theta)
R = rotm(rx * cos(th) + ry * sin(th), -rx * sin(th) + ry * cos(th),
rz)
response = get_image(client, pos[i, :], Quaternion(matrix=R))
airsim.write_file(
os.path.normpath('image{:07d}_rotz{}.png'.format(i + 1,
theta)),
response.image_data_uint8)
savemat("pose{:07d}_rotz{}.mat".format(i + 1, theta), {
'C': pos[i, :],
'R': R
})
R = rotm(-rz * sin(th) + rx * cos(th), ry,
rz * cos(th) + rx * sin(th))
response = get_image(client, pos[i, :], Quaternion(matrix=R))
airsim.write_file(
os.path.normpath('image{:07d}_roty{}.png'.format(i + 1,
theta)),
response.image_data_uint8)
savemat("pose{:07d}_roty{}.mat".format(i + 1, theta), {
'C': pos[i, :],
'R': R
})
R = rotm(rx,
ry * cos(th) + rz * sin(th), -ry * sin(th) + rz * cos(th))
response = get_image(client, pos[i, :], Quaternion(matrix=R))
airsim.write_file(
os.path.normpath('image{:07d}_rotx{}.png'.format(i + 1,
theta)),
response.image_data_uint8)
savemat("pose{:07d}_rotx{}.mat".format(i + 1, theta), {
'C': pos[i, :],
'R': R
})
def generate_path_orientations(start, end, steps):
qs = Quaternion.intermediates(start, end, steps)
ms = []
for q in qs:
ms.append(q.rotation_matrix)
return ms
