(self, joints, face_joint_idx, smooth_forward=False)
| 53 | # face_joint_idx should follow the order of right hip, left hip, right shoulder, left shoulder |
| 54 | # joints (batch_size, joints_num, 3) |
| 55 | def inverse_kinematics_np(self, joints, face_joint_idx, smooth_forward=False): |
| 56 | assert len(face_joint_idx) == 4 |
| 57 | '''Get Forward Direction''' |
| 58 | l_hip, r_hip, sdr_r, sdr_l = face_joint_idx |
| 59 | across1 = joints[:, r_hip] - joints[:, l_hip] |
| 60 | across2 = joints[:, sdr_r] - joints[:, sdr_l] |
| 61 | across = across1 + across2 |
| 62 | across = across / np.sqrt((across**2).sum(axis=-1))[:, np.newaxis] |
| 63 | # print(across1.shape, across2.shape) |
| 64 | |
| 65 | # forward (batch_size, 3) |
| 66 | forward = np.cross(np.array([[0, 1, 0]]), across, axis=-1) |
| 67 | if smooth_forward: |
| 68 | forward = filters.gaussian_filter1d(forward, 20, axis=0, mode='nearest') |
| 69 | # forward (batch_size, 3) |
| 70 | forward = forward / np.sqrt((forward**2).sum(axis=-1))[..., np.newaxis] |
| 71 | |
| 72 | '''Get Root Rotation''' |
| 73 | target = np.array([[0,0,1]]).repeat(len(forward), axis=0) |
| 74 | root_quat = qbetween_np(forward, target) |
| 75 | |
| 76 | '''Inverse Kinematics''' |
| 77 | # quat_params (batch_size, joints_num, 4) |
| 78 | # print(joints.shape[:-1]) |
| 79 | quat_params = np.zeros(joints.shape[:-1] + (4,)) |
| 80 | # print(quat_params.shape) |
| 81 | root_quat[0] = np.array([[1.0, 0.0, 0.0, 0.0]]) |
| 82 | quat_params[:, 0] = root_quat |
| 83 | # quat_params[0, 0] = np.array([[1.0, 0.0, 0.0, 0.0]]) |
| 84 | for chain in self._kinematic_tree: |
| 85 | R = root_quat |
| 86 | for j in range(len(chain) - 1): |
| 87 | # (batch, 3) |
| 88 | u = self._raw_offset_np[chain[j+1]][np.newaxis,...].repeat(len(joints), axis=0) |
| 89 | # print(u.shape) |
| 90 | # (batch, 3) |
| 91 | v = joints[:, chain[j+1]] - joints[:, chain[j]] |
| 92 | v = v / np.sqrt((v**2).sum(axis=-1))[:, np.newaxis] |
| 93 | # print(u.shape, v.shape) |
| 94 | rot_u_v = qbetween_np(u, v) |
| 95 | |
| 96 | R_loc = qmul_np(qinv_np(R), rot_u_v) |
| 97 | |
| 98 | quat_params[:,chain[j + 1], :] = R_loc |
| 99 | R = qmul_np(R, R_loc) |
| 100 | |
| 101 | return quat_params |
| 102 | |
| 103 | # Be sure root joint is at the beginning of kinematic chains |
| 104 | def forward_kinematics(self, quat_params, root_pos, skel_joints=None, do_root_R=True): |
no test coverage detected