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