| 40 | |
| 41 | |
| 42 | def hand2pose(hand, side = "right", euler = [0, 0, 0] ): |
| 43 | |
| 44 | wrist = hand[f"{side}_wrist"] |
| 45 | finger = wrist @ hand[f"{side}_fingers"] |
| 46 | |
| 47 | pinch_distance = hand["right_pinch_distance"] |
| 48 | |
| 49 | thumb_tip = finger[4, :3, -1] |
| 50 | thumb_base = finger[2, :3, -1] |
| 51 | index_tip = finger[9, :3, -1] |
| 52 | index_base = finger[7, :3, -1] |
| 53 | |
| 54 | base_middle = (thumb_base + index_base) * 0.5 |
| 55 | tip_middle = (thumb_tip + index_tip) * 0.5 |
| 56 | |
| 57 | z_axis = tip_middle - base_middle |
| 58 | z_axis /= np.linalg.norm(z_axis) |
| 59 | |
| 60 | |
| 61 | # use index→thumb direction as x |
| 62 | x_axis = thumb_base - index_base |
| 63 | x_axis -= np.dot(x_axis, z_axis) * z_axis # make y ⟂ z |
| 64 | x_axis /= np.linalg.norm(x_axis) |
| 65 | |
| 66 | y_axis = np.cross(z_axis, x_axis) |
| 67 | y_axis /= np.linalg.norm(y_axis) |
| 68 | |
| 69 | rot = np.column_stack((x_axis, y_axis, z_axis)) |
| 70 | rot /= np.cbrt(np.linalg.det(rot)) # ensure det = 1 |
| 71 | |
| 72 | rot = rot @ R.from_euler("xyz", euler, degrees=True).as_matrix() |
| 73 | |
| 74 | mat = np.eye(4) |
| 75 | mat[:3, :3] = rot |
| 76 | mat[:3, 3] = tip_middle |
| 77 | |
| 78 | return mat, pinch_distance |
| 79 | |
| 80 | |
| 81 | |