Open-Loop:
Command ──▶ Controller ──▶ Actuator ──▶ Output

Closed-Loop:
                    ┌────────────────────────┐
                    │                        │
                    ▼                        │
Command ──▶ [+] ──▶ Controller ──▶ Actuator ──▶ Output
             │-                              │
             │                               │
             └────── Sensor ◀────────────────┘

u(t) = Kp × e(t) + Ki × ∫e(τ)dτ + Kd × de(t)/dt

u(t) = Control output
e(t) = Error (setpoint - measured)
Kp, Ki, Kd = Gains

class PIDController:
    def __init__(self, kp, ki, kd, output_limits=None):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.output_limits = output_limits

        self.integral = 0.0
        self.prev_error = 0.0
        self.prev_time = None

    def update(self, setpoint, measured, current_time):
        """
        Calculate PID output.

        Args:
            setpoint: Desired value
            measured: Current measured value
            current_time: Current timestamp

        Returns:
            Control output
        """
        error = setpoint - measured

        # Calculate dt
        if self.prev_time is None:
            dt = 0.01  # Default 10ms
        else:
            dt = current_time - self.prev_time

        # Proportional
        p_term = self.kp * error

        # Integral (with anti-windup)
        self.integral += error * dt
        i_term = self.ki * self.integral

        # Derivative
        if dt > 0:
            derivative = (error - self.prev_error) / dt
        else:
            derivative = 0
        d_term = self.kd * derivative

        # Total output
        output = p_term + i_term + d_term

        # Apply limits
        if self.output_limits:
            output = max(self.output_limits[0],
                        min(self.output_limits[1], output))

        # Store for next iteration
        self.prev_error = error
        self.prev_time = current_time

        return output

    def reset(self):
        """Reset controller state."""
        self.integral = 0.0
        self.prev_error = 0.0
        self.prev_time = None

        ●  CoM (Center of Mass)
        │
        │ Body
        │
    ────┴────  Ground
       CoP (Center of Pressure)

class LIPMController:
    def __init__(self, com_height, gravity=9.81):
        self.z_c = com_height
        self.g = gravity
        self.omega = np.sqrt(gravity / com_height)

    def calculate_zmp(self, com_pos, com_accel):
        """
        Calculate Zero Moment Point (ZMP).

        ZMP = CoM - (z_c / g) × CoM_acceleration
        """
        zmp_x = com_pos[0] - (self.z_c / self.g) * com_accel[0]
        zmp_y = com_pos[1] - (self.z_c / self.g) * com_accel[1]
        return np.array([zmp_x, zmp_y])

    def capture_point(self, com_pos, com_vel):
        """
        Calculate Capture Point (Instantaneous Capture Point).

        CP = CoM + CoM_velocity / omega
        """
        cp_x = com_pos[0] + com_vel[0] / self.omega
        cp_y = com_pos[1] + com_vel[1] / self.omega
        return np.array([cp_x, cp_y])

class BalanceController:
    def __init__(self, kp_hip=100, kd_hip=10, kp_ankle=50, kd_ankle=5):
        self.hip_controller = PIDController(kp_hip, 0, kd_hip)
        self.ankle_controller = PIDController(kp_ankle, 0, kd_ankle)

    def update(self, imu_pitch, imu_roll, target_pitch=0, target_roll=0, time=None):
        """
        Calculate hip and ankle corrections for balance.

        Returns:
            (hip_pitch_correction, ankle_pitch_correction,
             hip_roll_correction, ankle_roll_correction)
        """
        # Pitch control (forward/backward)
        hip_pitch = self.hip_controller.update(target_pitch, imu_pitch, time)
        ankle_pitch = self.ankle_controller.update(target_pitch, imu_pitch, time)

        # Roll control (left/right)
        hip_roll = self.hip_controller.update(target_roll, imu_roll, time)
        ankle_roll = self.ankle_controller.update(target_roll, imu_roll, time)

        return hip_pitch, ankle_pitch, hip_roll, ankle_roll

class JointPositionController:
    def __init__(self, port_handler, packet_handler):
        self.port = port_handler
        self.packet = packet_handler

        # Dynamixel addresses (Protocol 2.0)
        self.ADDR_GOAL_POSITION = 116
        self.ADDR_PRESENT_POSITION = 132

    def set_position(self, dxl_id, goal_rad):
        """Set joint to target position in radians."""
        # Convert radians to Dynamixel units (0-4095)
        goal_unit = int((goal_rad / (2 * np.pi)) * 4095) % 4095

        self.packet.write4ByteTxRx(
            self.port, dxl_id, self.ADDR_GOAL_POSITION, goal_unit
        )

    def get_position(self, dxl_id):
        """Get current position in radians."""
        pos, _, _ = self.packet.read4ByteTxRx(
            self.port, dxl_id, self.ADDR_PRESENT_POSITION
        )
        return (pos / 4095.0) * 2 * np.pi

from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from builtin_interfaces.msg import Duration

class TrajectoryPublisher:
    def __init__(self, node):
        self.pub = node.create_publisher(
            JointTrajectory,
            '/joint_trajectory_controller/joint_trajectory',
            10
        )

    def send_trajectory(self, joint_names, positions, duration_sec=1.0):
        """
        Send joint trajectory command.

        Args:
            joint_names: List of joint names
            positions: List of target positions (radians)
            duration_sec: Time to reach target
        """
        msg = JointTrajectory()
        msg.joint_names = joint_names

        point = JointTrajectoryPoint()
        point.positions = positions
        point.time_from_start = Duration(sec=int(duration_sec),
                                         nanosec=int((duration_sec % 1) * 1e9))

        msg.points = [point]
        self.pub.publish(msg)

from enum import Enum

class WalkingState(Enum):
    IDLE = 0
    INIT = 1
    LEFT_SUPPORT = 2
    RIGHT_SUPPORT = 3
    DOUBLE_SUPPORT = 4
    STOP = 5

class WalkingController:
    def __init__(self):
        self.state = WalkingState.IDLE
        self.phase = 0.0  # 0-1 within each step
        self.step_time = 0.5  # seconds per step

    def update(self, dt):
        """Update walking state machine."""
        self.phase += dt / self.step_time

        if self.phase >= 1.0:
            self.phase = 0.0
            self._transition_state()

        return self._calculate_foot_trajectory()

    def _transition_state(self):
        """Transition to next state."""
        if self.state == WalkingState.LEFT_SUPPORT:
            self.state = WalkingState.DOUBLE_SUPPORT
        elif self.state == WalkingState.DOUBLE_SUPPORT:
            self.state = WalkingState.RIGHT_SUPPORT
        elif self.state == WalkingState.RIGHT_SUPPORT:
            self.state = WalkingState.DOUBLE_SUPPORT

    def _calculate_foot_trajectory(self):
        """Calculate foot position based on current phase."""
        # Cubic spline or sinusoidal trajectory
        # ... implementation depends on gait parameters
        pass