Saturated feedback controllers

tests/test_integration.py

@@ -4,9 +4,10 @@
 from pytest import approx
 import wecopttool as wot
 import capytaine as cpy
-import numpy as np
+import autograd.numpy as np
 import matplotlib.pyplot as plt
 from scipy.optimize import Bounds
+import xarray as xr
 
 
 kplim = -1e1
@@ -180,7 +181,7 @@ def test_solve_bounds(bounds_opt, wec_from_bem, regular_wave,
     """Confirm that bounds are not violated and scale correctly when
     passing bounds argument as both as Bounds object and a tuple"""
 
-    # replace unstructured controller with propotional controller
+    # replace unstructured controller with proportional controller
     wec_from_bem.forces['PTO'] = p_controller_pto.force_on_wec
 
     res = wec_from_bem.solve(waves=regular_wave,
@@ -302,7 +303,6 @@ def test_pi_controller_regular_wave(self,
                          ).squeeze().sum('omega').item()
 
         assert power_sol == approx(power_optimal, rel=1e-4)
-
     def test_unstructured_controller_irregular_wave(self,
                                                     fb,
                                                     bem,
@@ -333,4 +333,86 @@ def test_unstructured_controller_irregular_wave(self,
         power_optimal = (np.abs(Fex)**2/8 / np.real(hydro_impedance.squeeze())
                          ).squeeze().sum('omega').item()
 
-        assert power_sol == approx(power_optimal, rel=1e-2)
+        assert power_sol == approx(power_optimal, rel=1e-3)
+
+    def test_saturated_pi_controller(self,
+                                    bem,
+                                    regular_wave,
+                                    pto,
+                                    nfreq):
+        """Saturated PI controller matches constrained unstructured controller
+        for a regular wave
+        """
+
+        pto_tmp = pto
+        pto = {}
+        wec = {}
+        nstate_opt = {}
+
+        # Constraint
+        f_max = 2000.0
+
+        nstate_opt['us'] = 2*nfreq
+        pto['us'] = pto_tmp
+        def const_f_pto(wec, x_wec, x_opt, waves):
+            f = pto['us'].force_on_wec(wec, x_wec, x_opt, waves, 
+                                       nsubsteps=4)
+            return f_max - np.abs(f.flatten())
+        wec['us'] = wot.WEC.from_bem(bem,
+                                     f_add={"PTO": pto['us'].force_on_wec},
+                                     constraints=[{'type': 'ineq',
+                                                   'fun': const_f_pto, }])
+
+
+        ndof = 1
+        nstate_opt['pi'] = 2
+        def saturated_pi(pto, wec, x_wec, x_opt, waves=None, nsubsteps=1):
+            return wot.pto.controller_pi(pto, wec, x_wec, x_opt, waves, 
+                                         nsubsteps, 
+                                         saturation=[-f_max, f_max])
+        pto['pi'] = wot.pto.PTO(ndof=ndof,
+                                kinematics=np.eye(ndof),
+                                controller=saturated_pi,)
+        wec['pi'] = wot.WEC.from_bem(bem,
+                                     f_add={"PTO": pto['pi'].force_on_wec},
+                                     constraints=[])
+
+        x_opt_0 = {'us': np.ones(nstate_opt['us'])*0.1,
+                   'pi': [-1e3, 1e4]}
+        scale_x_wec = {'us': 1e1,
+                       'pi': 1e1}
+        scale_x_opt = {'us': 1e-3,
+                       'pi': 1e-3}
+        scale_obj = {'us': 1e-2,
+                     'pi': 1e-2}
+        bounds_opt = {'us': None,
+                      'pi': ((-1e4, 0), (0, 2e4),)}
+
+        res = {}
+        pto_fdom = {}
+        pto_tdom = {}
+        for key in wec.keys():
+            res[key] = wec[key].solve(waves=regular_wave,
+                            obj_fun=pto[key].average_power,
+                            nstate_opt=nstate_opt[key],
+                            x_wec_0=1e-1*np.ones(wec[key].nstate_wec),
+                            x_opt_0=x_opt_0[key],
+                            scale_x_wec=scale_x_wec[key],
+                            scale_x_opt=scale_x_opt[key],
+                            scale_obj=scale_obj[key],
+                            optim_options={'maxiter': 200},
+                            bounds_opt=bounds_opt[key]
+                            )
+
+            nsubstep_postprocess = 4
+            pto_fdom[key], pto_tdom[key] = pto[key].post_process(wec[key], 
+                                                                 res[key], 
+                                                                 regular_wave, 
+                                                                 nsubstep_postprocess)
+
+        xr.testing.assert_allclose(pto_tdom['pi'].power.squeeze().mean('time'), 
+                                   pto_tdom['us'].power.squeeze().mean('time'),
+                                   rtol=1e-1)
+
+        xr.testing.assert_allclose(pto_tdom['us'].force.max(),
+                                   pto_tdom['pi'].force.max())

tests/test_pto.py

@@ -260,4 +260,25 @@ def test_controller_pid(
         x_wec = [0, amp, 0, 0]
         x_opt = [pid_p, pid_i, pid_d]
         calculated = pto.force(wec, x_wec, x_opt, None)
-        assert np.allclose(force, calculated)
+        assert np.allclose(force, calculated)
+
+    def test_controller_pid_saturated(
+            self, wec, ndof, kinematics, omega, pid_p, pid_i, pid_d,
+        ):
+        """Test the PID controller."""
+        saturation = np.array([[-4,5]])
+        def controller(p,w,xw,xo,wa,ns):
+            return wot.pto.controller_pid(p,w,xw,xo,wa,ns,saturation=saturation)
+        pto = wot.pto.PTO(ndof, kinematics, controller)
+        amp = 2.3
+        w = omega[-2]
+        pos = amp * np.cos(w * wec.time)
+        vel = -1 * amp * w * np.sin(w * wec.time)
+        acc = -1 * amp * w**2 * np.cos(w * wec.time)
+        force = vel*pid_p + pos*pid_i + acc*pid_d
+        force = np.clip(force, saturation[0,0], saturation[0,1])
+        force = force.reshape(-1, 1)
+        x_wec = [0, amp, 0, 0]
+        x_opt = [pid_p, pid_i, pid_d]
+        calculated = pto.force(wec, x_wec, x_opt, None)
+        assert np.allclose(force, calculated)

wecopttool/pto.py

@@ -84,7 +84,7 @@ def __init__(self,
             kinematics).
         controller
             Function with signature
-            :python:`def fun(wec, x_wec, x_opt, waves, nsubsteps):`
+            :python:`def fun(pto, wec, x_wec, x_opt, waves, nsubsteps):`
             or matrix with shape (PTO DOFs, WEC DOFs) that converts
             from the WEC DOFs to the PTO DOFs.
         impedance
@@ -923,6 +923,7 @@ def controller_pid(
     proportional: Optional[bool] = True,
     integral: Optional[bool] = True,
     derivative: Optional[bool] = True,
+    saturation: Optional[FloatOrArray] = None,
 ) -> ndarray:
     """Proportional-integral-derivative (PID) controller that returns
     a time history of PTO forces.
@@ -941,25 +942,30 @@ def controller_pid(
         :py:class:`xarray.Dataset` with the structure and elements shown
         by :py:mod:`wecopttool.waves`.
     nsubsteps
-            Number of steps between the default (implied) time steps.
-            A value of :python:`1` corresponds to the default step
-            length.
+        Number of steps between the default (implied) time steps.
+        A value of :python:`1` corresponds to the default step length.
     proportional
         True to include proportional gain
     integral
         True to include integral gain
     derivative
         True to include derivative gain
+    saturation
+        Maximum and minimum control value.
+        Can be symmetric ([ndof]) or asymmetric ([ndof, 2]).
     """
     ndof = pto.ndof
-    force_td = np.zeros([wec.nt*nsubsteps, ndof])
+    force_td_tmp = np.zeros([wec.nt*nsubsteps, ndof])
+
+    # PID force
     idx = 0
 
     def update_force_td(response):
-        nonlocal idx, force_td
-        gain = np.reshape(x_opt[idx*ndof:(idx+1)*ndof], [1, ndof])
-        force_td = force_td + gain*response
+        nonlocal idx, force_td_tmp
+        gain = np.diag(x_opt[idx*ndof:(idx+1)*ndof])
+        force_td_tmp = force_td_tmp + np.dot(response, gain.T)
         idx = idx + 1
+        return
 
     if proportional:
         vel_td = pto.velocity(wec, x_wec, x_opt, waves, nsubsteps)
@@ -970,6 +976,28 @@ def update_force_td(response):
     if derivative:
         acc_td = pto.acceleration(wec, x_wec, x_opt, waves, nsubsteps)
         update_force_td(acc_td)
+
+    # Saturation
+    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(ndof):
+            tmp = np.clip(force_td_tmp[:,i], f_min[i], f_max[i])
+            force_td_list.append(tmp)
+        force_td = np.array(force_td_list).T
+    else:
+        force_td = force_td_tmp
+
     return force_td
 
 
@@ -980,6 +1008,7 @@ def controller_pi(
     x_opt: ndarray,
     waves: Optional[Dataset] = None,
     nsubsteps: Optional[int] = 1,
+    saturation: Optional[FloatOrArray] = None,
 ) -> ndarray:
     """Proportional-integral (PI) controller that returns a time
     history of PTO forces.
@@ -998,12 +1027,16 @@ def controller_pi(
         :py:class:`xarray.Dataset` with the structure and elements shown
         by :py:mod:`wecopttool.waves`.
     nsubsteps
-            Number of steps between the default (implied) time steps.
-            A value of :python:`1` corresponds to the default step
-            length.
+        Number of steps between the default (implied) time steps.
+        A value of :python:`1` corresponds to the default step length.
+    saturation
+        Maximum and minimum control value.
+        Can be symmetric ([ndof]) or asymmetric ([ndof, 2]).
     """
-    force_td = controller_pid(pto, wec, x_wec, x_opt, waves, nsubsteps,
-                              True, True, False)
+    force_td = controller_pid(
+        pto, wec, x_wec, x_opt, waves, nsubsteps,
+        True, True, False, saturation,
+    )
     return force_td
 
 
@@ -1014,6 +1047,7 @@ def controller_p(
     x_opt: ndarray,
     waves: Optional[Dataset] = None,
     nsubsteps: Optional[int] = 1,
+    saturation: Optional[FloatOrArray] = None,
 ) -> ndarray:
     """Proportional (P) controller that returns a time history of
     PTO forces.
@@ -1034,7 +1068,44 @@ def controller_p(
     nsubsteps
         Number of steps between the default (implied) time steps.
         A value of :python:`1` corresponds to the default step length.
+    saturation
+        Maximum and minimum control value. Can be symmetric ([ndof]) or
+        asymmetric ([ndof, 2]).
     """
-    force_td = controller_pid(pto, wec, x_wec, x_opt, waves, nsubsteps,
-                               True, False, False)
+    force_td = controller_pid(
+        pto, wec, x_wec, x_opt, waves, nsubsteps,
+        True, False, False, saturation,
+    )
     return force_td
+
+
+# utilities
+def nstate_unstructured(nfreq: int, ndof: int) -> int:
+    """
+    Number of states needed to represent an unstructured controller.
+
+    Parameters
+    ----------
+    nfreq
+        Number of frequencies.
+    ndof
+        Number of degrees of freedom.
+    """
+    return 2*nfreq*ndof
+
+
+def nstate_pid(
+        nterm: int,
+        ndof: int,
+) -> int:
+    """
+    Number of states needed to represent an unstructured controller.
+
+    Parameters
+    ----------
+    nterm
+        Number of terms (e.g. 1 for P, 2 for PI, 3 for PID).
+    ndof
+        Number of degrees of freedom.
+    """
+    return int(nterm*ndof)