Process keyboard input - returns delta position, delta rotation, joint angle delta, and control flags
(dpos: float = 0.01, drot: float = 0.05, dangle: float = 0.02, joint_mode: bool = False)
| 259 | |
| 260 | |
| 261 | def process_input(dpos: float = 0.01, drot: float = 0.05, dangle: float = 0.02, joint_mode: bool = False): |
| 262 | """Process keyboard input - returns delta position, delta rotation, joint angle delta, and control flags""" |
| 263 | keys = pygame.key.get_pressed() |
| 264 | |
| 265 | # Initialize persistent state |
| 266 | if not hasattr(process_input, "tab_pressed"): |
| 267 | process_input.tab_pressed = False |
| 268 | process_input.c_pressed = False |
| 269 | process_input.j_pressed = False |
| 270 | process_input.left_pressed = False |
| 271 | process_input.right_pressed = False |
| 272 | |
| 273 | # In joint mode, arrow keys control joints |
| 274 | if joint_mode: |
| 275 | # UP/DOWN for angle adjustment |
| 276 | dangle_val = dangle * (keys[pygame.K_UP] - keys[pygame.K_DOWN]) |
| 277 | |
| 278 | # LEFT/RIGHT for joint selection (single press) |
| 279 | prev_joint = False |
| 280 | if keys[pygame.K_LEFT] and not process_input.left_pressed: |
| 281 | prev_joint = True |
| 282 | process_input.left_pressed = True |
| 283 | elif not keys[pygame.K_LEFT]: |
| 284 | process_input.left_pressed = False |
| 285 | |
| 286 | next_joint = False |
| 287 | if keys[pygame.K_RIGHT] and not process_input.right_pressed: |
| 288 | next_joint = True |
| 289 | process_input.right_pressed = True |
| 290 | elif not keys[pygame.K_RIGHT]: |
| 291 | process_input.right_pressed = False |
| 292 | |
| 293 | return None, None, dangle_val, False, False, True, prev_joint, next_joint |
| 294 | |
| 295 | else: |
| 296 | # Normal mode: position and rotation control |
| 297 | # Movement (follow teleop convention) |
| 298 | dx = dpos * (keys[pygame.K_UP] - keys[pygame.K_DOWN]) # UP/DOWN for X |
| 299 | dy = dpos * (keys[pygame.K_LEFT] - keys[pygame.K_RIGHT]) # LEFT/RIGHT for Y |
| 300 | dz = dpos * (keys[pygame.K_e] - keys[pygame.K_d]) # e/d for Z |
| 301 | |
| 302 | # Rotation (euler angles: roll, pitch, yaw) |
| 303 | droll = drot * (keys[pygame.K_q] - keys[pygame.K_w]) # q/w for roll |
| 304 | dpitch = drot * (keys[pygame.K_a] - keys[pygame.K_s]) # a/s for pitch |
| 305 | dyaw = drot * (keys[pygame.K_z] - keys[pygame.K_x]) # z/x for yaw |
| 306 | |
| 307 | return [dx, dy, dz], [droll, dpitch, dyaw], 0.0, False, False, False, False, False |
| 308 | |
| 309 | # Common controls (always checked after mode-specific logic) |
| 310 | |
| 311 | |
| 312 | def process_common_input(): |