def quater2ang_v(quater_1, quater_2, dt): delta_q = qmult(quater_2, qconjugate(quater_1)) delta_q_len = np.linalg.norm(delta_q[1:]) delta_q_angle = 2 * np.arctan2(delta_q_len, delta_q[0]) w = delta_q[1:] * delta_q_angle * 1 / dt return w
def _update_coordinate_system(self, origin_device_id: Optional[DeviceId], device_positions: Dict[DeviceId, np.ndarray], device_rotations: Dict[DeviceId, np.ndarray], num_samples: int = 10): """Updates the coordinate system origin.""" if origin_device_id: # Collect samples for the devices. pos_samples = [] quat_samples = [] for _ in range(num_samples): self._refresh_poses() state = self._get_tracker_state(origin_device_id, ignore_device_transform=True) pos_samples.append(state.pos) quat_samples.append(state.rot_quat) global_translation = -np.mean(pos_samples, axis=0) global_rotation = qconjugate(average_quaternions(quat_samples)) origin_rx, origin_ry, origin_rz = quat2euler(global_rotation, axes='rxyz') logging.info('Have origin rotation: %1.2f %1.2f %1.2f', origin_rx, origin_ry, origin_rz) self._coord_system.set_global_transform(global_translation, euler2mat(0, 0, origin_rz)) for device_id, position in device_positions.items(): self._coord_system.set_local_transform(device_id, translation=position) for device_id, rotation in device_rotations.items(): self._coord_system.set_local_transform(device_id, rotation=rotation)
def test_qnorm(): qi = tq.qeye() assert tq.qnorm(qi) == 1 assert tq.qisunit(qi) qi[1] = 0.2 assert not tq.qisunit(qi) # Test norm is sqrt of scalar for multiplication with conjugate. # https://en.wikipedia.org/wiki/Quaternion#Conjugation,_the_norm,_and_reciprocal for q in quats: q_c = tq.qconjugate(q) exp_norm = np.sqrt(tq.qmult(q, q_c)[0]) assert np.allclose(tq.qnorm(q), exp_norm)
def difference_quat(q1, q2): """ Calculates the difference between two quaternions Args: q1 (np.array): quaternion q2 (np.array): quaternion Returns: float: difference """ q2_conj = quat.qconjugate(q2) return quat.qmult(q1, q2_conj)
def quar_axis_error(q_sp, q_state): """Compute the error in quaternions from the setpoints and robot state Args: q_sp (np.array): Reference signal Set point quaternion q_state (np.array): Sensor Feedback quaternion Returns: exponential angle (np.array) """ # Quaternion multiplication q_set * (q_state)' target - state q_state_conj = quaternions.qconjugate(q_state) q_error = quaternions.qmult(q_sp, q_state_conj) # Nearest rotation if (q_error[0] < 0): q_error = -1. * q_error axis_error = quaternions.quat2axangle(q_error) return axis_error[0] * axis_error[1]
def test_qconjugate(): # Takes sequence cq = tq.qconjugate((1, 0, 0, 0)) # Returns float type assert cq.dtype.kind == 'f'
def quat_sub(q1, q2): """ Returns q1 * cong(q2) """ return qops.qmult(q2, qops.qconjugate(q1))
def quat_frame_transform_inv(axes): return qops.qconjugate(quat_frame_transform(axes))
def kinematic_tree(poses, j2d, cam): #, proc_param): joints = { 'Pelvis': 0, 'Neck': 12, 'Chest': 3, 'L_Shoulder': 16, 'L_Elbow': 18, 'R_Shoulder': 17, 'R_Elbow': 19, 'L_Hip': 1, 'L_Knee': 4, 'L_Ankle': 7, 'R_Hip': 2, 'R_Knee': 5, 'R_Ankle': 8 } # LR reverse for deepmimic target_joints = { 'Pelvis': [4, 5, 6, 7], 'Neck': [12, 13, 14, 15], 'Chest': [8, 9, 10, 11], 'L_Shoulder': [39, 40, 41, 42], 'L_Elbow': [43], 'R_Shoulder': [25, 26, 27, 28], 'R_Elbow': [29], 'L_Hip': [30, 31, 32, 33], 'L_Knee': [34], 'L_Ankle': [35, 36, 37, 38], 'R_Hip': [16, 17, 18, 19], 'R_Knee': [20], 'R_Ankle': [21, 22, 23, 24], } """ # motions[:, 0] = 0.0625 # motions[:, 1:4] # root position # motions[:, 4:8] # root rotation # motions[:, 8:12] # chest rotation # motions[:, 12:16] # neck rotation # motions[:, 16:20] # right hip rot # motions[:, 20] # right knee # motions[:, 21:25] # right ankle rot # motions[:, 25:29] # right shoulder rotation # motions[:, 30] # right elbow # motions[:, 30:34] # left hip rot # motions[:, 34] # left knee # motions[:, 35:39] # left ankle # motions[:, 39:43] # left shoulder rot # motions[:, 43] # left elbow rot """ poses = poses.reshape((-1, 3)) motions = np.zeros(44) r = [0.7071, 0, 0.7071, 0] # # Quaternion that represents 90 degrees around Y # qconjugate(r) = [ 0.7071, 0, -0.7071, 0] motions[1:4] = cam #root_position(j2d, cam, proc_param) for joi, num in joints.items(): x = poses[num] # change of basis: SMPL to DeepMimic if joi in ['R_Knee', 'L_Knee']: q = -vec_to_angle(x) elif joi in ['L_Elbow', 'R_Elbow']: q = vec_to_angle(x) elif joi in ['Pelvis']: q = vec_to_quaternion(x) q = qq.qmult(r, q) q = qq.qmult(q, qq.qconjugate(r)) q = qq.qmult([0, 1, 0, 0], q) elif joi in ['L_Shoulder']: q = vec_to_quaternion(x) q = qq.qmult( q, [0.7071, 0, 0, 0.7071] ) # [0.7071, 0, 0, 0.7071] Quaternion that represents 90 degrees around Z q = qq.qmult(r, q) q = qq.qmult(q, qq.qconjugate(r)) elif joi in ['R_Shoulder']: q = vec_to_quaternion(x) q = qq.qmult( q, [0.7071, 0, 0, -0.7071] ) #[0.7071, 0, 0, -0.7071] Quaternion that represents 90 degrees around -Z q = qq.qmult(r, q) q = qq.qmult(q, qq.qconjugate(r)) else: q = vec_to_quaternion(x) q = qq.qmult(r, q) q = qq.qmult(q, qq.qconjugate(r)) motions[target_joints[joi]] = q return motions
def rotate(q, pt): qmult1 = tfq.qmult(q, (np.append(0, pt).transpose())) qmult2 = tfq.qmult(qmult1, tfq.qconjugate(q)) return (np.asarray(qmult2))[1:]
def build_kinematic_tree(theta, j2d, cam, proc_param): """ z: 3D joint coordinates 14x3 v: vectors """ # 0 r foot # 1 r knee # 2 r hip # 3 l hip # 4 l knee # 5 l foot # 6 r hand # 7 r elbow # 8 r shoulder # 9 l shoulder # 10 l elbow # 11 l hand # 12 thorax # 13 head # GYM joints # motions[:, 0] = 0.0625 # motions[:, 4:8] = [1, 0,0,0] # root rotation # motions[:, 8:12] = [1, 0,0,0] # chest rotation # motions[:, 12:16] = [1, 0, 0, 0] # neck rotation # motions[:, 16:20] = [1, 0, 0, 0] # right hip rot # motions[:, 20] = [1, 0, 0, 0] # right knee # motions[:, 21:25] = [1, 0, 0, 0] # right ankle rot # motions[:, 25:29] = [1, 0, 0, 0] # right shoulder rotation # motions[:, 30] = [1, 0, 0, 0] # right elbow # motions[:, 30:34] = [1, 0, 0, 0] # left hip rot # motions[:, 34] = [1, 0, 0, 0] # left knee # motions[:, 35:39] = [1, 0, 0, 0] # left ankle # motions[:, 39:43] = [1, 0, 0, 0] # left shoulder rot # motions[:, 43] = [1, 0, 0, 0] # left elbow rot # theta = theta[self.num_cam:(self.num_cam + self.num_theta)] theta = theta.reshape((-1, 3)) z = np.zeros(44) r = [0.7071, 0, 0.7071, 0] # SMPL to DeepMimic z[1:4] = calcRootTranslation(j2d, cam, proc_param) for joi, num in joints.items(): x = theta[num] # change of basis: SMPL to DeepMimic if joi in ['R_Knee', 'L_Knee']: q = -vec_to_angle(x) elif joi in ['L_Elbow', 'R_Elbow']: q = vec_to_angle(x) elif joi in ['Pelvis']: q = vec_to_quaternion(x) q = qq.qmult(r, q) q = qq.qmult(q, qq.qconjugate(r)) q = qq.qmult([0, 1, 0, 0], q) elif joi in ['L_Shoulder']: q = vec_to_quaternion(x) q = qq.qmult(q, [0.7071, 0, 0, 0.7071]) q = qq.qmult(r, q) q = qq.qmult(q, qq.qconjugate(r)) elif joi in ['R_Shoulder']: q = vec_to_quaternion(x) q = qq.qmult(q, [0.7071, 0, 0, -0.7071]) q = qq.qmult(r, q) q = qq.qmult(q, qq.qconjugate(r)) else: q = vec_to_quaternion(x) q = qq.qmult(r, q) q = qq.qmult(q, qq.qconjugate(r)) z[target_joints[joi]] = q return z