Feedback controllers -> class

[ryancoe]

In reviewing #273, I noticed that we are repeating a fair bit of code and docstrings in the pto.py for the PID controllers. We could potentially use a class to handle this. @cmichelenstrofer pointed out a potential concern that doing this might change the workflow between using an unstructured controller vs. structured controller. Let's discuss this during our upcoming meeting.

[cmichelenstrofer]

This is what it could look like. Haven't tested it.

class FeedbackController:

    def __init__(self,
        ndof_pto: int,
        proportional: Optional[bool] = True,
        integral: Optional[bool] = False,
        derivative: Optional[bool] = False,
        saturation: Optional[FloatOrArray] = None,
    ):
        self.proportional = proportional
        self.integral = integral
        self.derivative = derivative
        self.saturation = saturation

        self.ndof = ndof_pto
        self.nterms = self.proportional + self.integral + self.derivative
        self.nstate = self.ndof * self.nterms

    def force(self,
        pto: TPTO,
        wec: TWEC,
        x_wec: ndarray,
        x_opt: ndarray,
        waves: Optional[Dataset] = None,
        nsubsteps: Optional[int] = 1,
    ):
        gain_p, gain_i, gain_d = self._gains(x_opt)

        vel_td = pto.velocity(wec, x_wec, x_opt, waves, nsubsteps)
        pos_td = pto.position(wec, x_wec, x_opt, waves, nsubsteps)
        acc_td = pto.acceleration(wec, x_wec, x_opt, waves, nsubsteps)

        force_td = (
            np.dot(vel_td, gain_p.T) +
            np.dot(pos_td, gain_i.T) +
            np.dot(acc_td, gain_d.T)
        )

        if self.saturation:
            force_td = self._saturation(fore_td)

        return force_td

    def _gains(self, x_opt):
        idx = 0
        ndof = self.ndof

        if self.proportional:
            gain_p = np.diag(x_opt[idx*ndof:(idx+1)*ndof])
            idx = idx +1
        else:
            gain_p = np.zeros([ndof, ndof])

        if self.integral:
            gain_i = np.diag(x_opt[idx*ndof:(idx+1)*ndof])
            idx = idx +1
        else:
            gain_i = np.zeros([ndof, ndof])

        if self.derivative:
            gain_d = np.diag(x_opt[idx*ndof:(idx+1)*ndof])
        else:
            gain_d = np.zeros([ndof, ndof])

        return gain_p, gain_i, gain_d

    def _saturation(self, force_td):
        if saturation is not None:
            saturation = np.atleast_2d(np.squeeze(saturation))
            assert len(saturation)==ndof
        if len(saturation.shape) > 2:
            raise ValueError("`saturation` must have <= 2 dimensions.")

        if saturation.shape[1] == 1:
            f_min, f_max = -1*saturation, saturation
        elif saturation.shape[1] == 2:
            f_min, f_max = saturation[:,0], saturation[:,1]
        else:
            raise ValueError("`saturation` must have 1 or 2 columns.")

        force_td_list = []
        for i in range(self.ndof):
            tmp = np.clip(force_td[:,i], f_min[i], f_max[i])
            force_td_list.append(tmp)
        return np.array(force_td_list).T

The user would do the following when using this:

...
controller = wot.pto.FeedbackController(1, ...)
pto = wot.pto.PTO(..., controller=controller.force, ...)
...

[ryancoe]

From our discussion, we generally agree:

• If we do this, we need to make an overall controllers class that includes the unstructured controller
• Move to a new a submodule (e.g., controllers.py)
• Do this all as a separate PR (don't include in #273)