Core PiPER robot controller that manages robot state and communication. This class is independent of any UI framework and can be used with GUI, VR, or other control interfaces through a command queue.
| 12 | |
| 13 | |
| 14 | class PiperController: |
| 15 | """Core PiPER robot controller that manages robot state and communication. |
| 16 | |
| 17 | This class is independent of any UI framework and can be used with |
| 18 | GUI, VR, or other control interfaces through a command queue. |
| 19 | """ |
| 20 | |
| 21 | class ControlMode(Enum): |
| 22 | """Control mode enumeration.""" |
| 23 | |
| 24 | END_EFFECTOR = "end_effector" |
| 25 | JOINT_SPACE = "joint_space" |
| 26 | |
| 27 | def __init__( |
| 28 | self, |
| 29 | can_interface: str = "can0", |
| 30 | robot_rate: float = 100.0, |
| 31 | control_mode: "PiperController.ControlMode" = ControlMode.JOINT_SPACE, |
| 32 | neutral_joint_angles: np.ndarray | None = None, |
| 33 | neutral_end_effector_pose: np.ndarray | None = None, |
| 34 | debug_mode: bool = False, |
| 35 | ) -> None: |
| 36 | """Initialize the robot controller. |
| 37 | |
| 38 | Args: |
| 39 | can_interface: CAN interface for robot communication (default: 'can0') |
| 40 | robot_rate: Robot control loop rate in Hz (default: 100.0) |
| 41 | control_mode: Initial control mode (END_EFFECTOR or JOINT_SPACE) |
| 42 | neutral_joint_angles: Neutral joint angles [j1, j2, j3, j4, j5, j6] in degrees (default: None) |
| 43 | neutral_end_effector_pose: Neutral end effector pose as 4x4 transformation matrix (default: None) |
| 44 | debug_mode: Enable debug logging (default: False) |
| 45 | """ |
| 46 | self.can_interface = can_interface |
| 47 | self.robot_rate = robot_rate |
| 48 | self.debug_mode = debug_mode |
| 49 | |
| 50 | # Thread synchronization |
| 51 | self.position_lock = threading.Lock() |
| 52 | self.state_lock = threading.Lock() |
| 53 | self.running = threading.Event() |
| 54 | self.running.set() |
| 55 | |
| 56 | # Robot operational state |
| 57 | self._robot_enabled = False |
| 58 | |
| 59 | self._control_loop_thread = threading.Thread( |
| 60 | target=self.control_loop, daemon=True |
| 61 | ) |
| 62 | |
| 63 | # Control mode |
| 64 | self._control_mode = control_mode |
| 65 | |
| 66 | # HOME positions in end effector space and joint space |
| 67 | if neutral_end_effector_pose is not None: |
| 68 | if neutral_end_effector_pose.shape == (4, 4): |
| 69 | self.HOME_POSE = neutral_end_effector_pose.copy().astype(np.float64) |
| 70 | else: |
| 71 | raise ValueError( |
no outgoing calls
no test coverage detected