Humanoid Robot Navigation: Code Snippets

class PIDController:
    def __init__(self, kp, ki, kd):
        self.kp, self.ki, self.kd = kp, ki, kd
        self.integral = 0.0
        self.prev_error = 0.0

    def step(self, error, dt):
        self.integral += error * dt
        derivative = (error - self.prev_error) / max(dt, 1e-6)
        self.prev_error = error
        return self.kp * error + self.ki * self.integral + self.kd * derivative

def track_point(target, pose, pid, dt):
    error = target - pose
    control = pid.step(error, dt)
    return pose + control * dt