Draw a coordinate frame (XYZ axes) at the given transform.
(ax, matrix: np.ndarray, label: str, scale: float = 0.1)
| 140 | |
| 141 | |
| 142 | def draw_coordinate_frame(ax, matrix: np.ndarray, label: str, scale: float = 0.1): |
| 143 | """Draw a coordinate frame (XYZ axes) at the given transform.""" |
| 144 | origin = swap_yz(get_position(matrix)) |
| 145 | x_end = swap_yz(get_position(matrix) + matrix[:3, 0] * scale) |
| 146 | y_end = swap_yz(get_position(matrix) + matrix[:3, 1] * scale) |
| 147 | z_end = swap_yz(get_position(matrix) + matrix[:3, 2] * scale) |
| 148 | |
| 149 | # Draw axes (X=red, Y=green, Z=blue) |
| 150 | ax.plot([origin[0], x_end[0]], [origin[1], x_end[1]], [origin[2], x_end[2]], |
| 151 | 'r-', linewidth=2, label=f'{label} X' if label else None) |
| 152 | ax.plot([origin[0], y_end[0]], [origin[1], y_end[1]], [origin[2], y_end[2]], |
| 153 | 'g-', linewidth=2, label=f'{label} Y' if label else None) |
| 154 | ax.plot([origin[0], z_end[0]], [origin[1], z_end[1]], [origin[2], z_end[2]], |
| 155 | 'b-', linewidth=2, label=f'{label} Z' if label else None) |
| 156 | |
| 157 | # Label |
| 158 | if label: |
| 159 | ax.text(origin[0], origin[1], origin[2] + scale * 1.2, label, fontsize=10, fontweight='bold') |
| 160 | |
| 161 | |
| 162 | def draw_hand(ax, hand_positions: dict[str, np.ndarray], color: str, label: str): |
no test coverage detected