PID

src.python_motion_planning.controller.path_tracker.pid.PID

Bases: PathTracker

PID-based path-tracking controller.

Parameters:

Name	Type	Description	Default
*args		see the parent class.	()
Kp	float	proportional gain	1.0
Ki	float	integral gain	0.1
Kd	float	derivative gain	0.1
**kwargs		see the parent class.	{}

Source code in src\python_motion_planning\controller\path_tracker\pid.py

Python

class PID(PathTracker):
    """
    PID-based path-tracking controller.

    Args:
        *args: see the parent class.
        Kp: proportional gain
        Ki: integral gain
        Kd: derivative gain
        **kwargs: see the parent class.
    """
    def __init__(self,
                 *args,
                 Kp: float = 1.0,
                 Ki: float = 0.1,
                 Kd: float = 0.1,
                 **kwargs):
        super().__init__(*args, **kwargs)
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd

        # integral and previous error for PID
        self.integral_error = np.zeros(self.action_space.shape[0])
        self.prev_error = np.zeros(self.action_space.shape[0])

    def reset(self):
        """
        Reset the controller to initial state.
        """
        super().reset()
        self.integral_error = np.zeros(self.action_space.shape[0])
        self.prev_error = np.zeros(self.action_space.shape[0])

    def _get_desired_action(self, desired_vel: np.ndarray, vel: np.ndarray, orient: np.ndarray) -> np.ndarray:
        """
        Calculates the action to be taken using PID control to reach the desired velocity.

        Args:
            desired_vel: Desired velocity in world frame.
            vel: Current velocity in world frame.
            orient: Current orientation in world frame.

        Returns:
            np.ndarray: Action to be taken ([lin_acc, ang_acc]).
        """
        # velocity error
        error = desired_vel - vel

        # PID terms
        self.integral_error += error * self.dt
        derivative_error = (error - self.prev_error) / self.dt

        control = (self.Kp * error +
                   self.Ki * self.integral_error +
                   self.Kd * derivative_error)

        self.prev_error = error

        action = self.clip_action(control)

        return action

reset()

Reset the controller to initial state.

Source code in src\python_motion_planning\controller\path_tracker\pid.py

Python

def reset(self):
    """
    Reset the controller to initial state.
    """
    super().reset()
    self.integral_error = np.zeros(self.action_space.shape[0])
    self.prev_error = np.zeros(self.action_space.shape[0])