Method control_loop

piper_controller.py:461–495

Main robot control loop running at robot_rate Hz. This should be run in a separate thread. Only sends commands when robot is in ENABLED state.

(self)

            return False

    def control_loop(self) -> None:
        """Main robot control loop running at robot_rate Hz.

        This should be run in a separate thread.
        Only sends commands when robot is in ENABLED state.
        """
        loop_period = 1.0 / self.robot_rate

        while self.running.is_set():
            try:
                # Only send commands if robot is enabled
                if self.is_robot_enabled():
                    # Get current control mode
                    current_mode = self.get_control_mode()

                    with self.position_lock:
                        if current_mode == PiperController.ControlMode.END_EFFECTOR:
                            # Send end-effector pose command
                            self._send_end_effector_command(self._target_pose)
                        elif current_mode == PiperController.ControlMode.JOINT_SPACE:
                            # Send joint angles command
                            self._send_joint_command(self._target_joint_angles)

                        # Always send gripper command regardless of control mode
                        self._send_gripper_command(self._gripper_open_value_degrees)
                else:
                    # Robot is not enabled, just sleep without sending commands
                    if self.debug_mode:
                        print("Control loop: Robot not enabled, skipping command")

                time.sleep(loop_period)

            except Exception as e:
                print(f"Robot control loop error: {e}")
                time.sleep(0.01)

    def _send_end_effector_command(self, transform: np.ndarray) -> None:
        """Send end-effector pose command to the robot.