Feedback controllers -> class

[rgcoe]

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, ...)
...

[rgcoe]

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 Saturated feedback controllers #273)

[rgcoe]

Here's the notebook I was using to test/develop this, which compares unstructured, PI, and P controllers for the WaveBot with a PTO force constraint. I don't want to add it to the repo, but perhaps parts of this might be helpful to someone someday.

Python code

# %%
import autograd.numpy as np
import capytaine as cpy
from capytaine.io.meshio import load_from_meshio
import matplotlib.pyplot as plt
import xarray as xr

import wecopttool as wot

## set colorblind-friendly colormap for plots
plt.style.use('tableau-colorblind10')

# %%
wavefreq = 0.3 # Hz
f1 = wavefreq
nfreq = 10

freq = wot.frequency(f1, nfreq, False) # False -> no zero frequency

# %%
amplitude = 0.0625 # m
phase = 30 # degrees
wavedir = 0 # degrees

waves = wot.waves.regular_wave(f1, nfreq, wavefreq, amplitude, phase, wavedir)

# %%
wb = wot.geom.WaveBot()  # use standard dimensions
mesh_size_factor = 0.2 # 1.0 for default, smaller to refine mesh
mesh = wb.mesh(mesh_size_factor)

# create mesh object for WaveBot and add internal lid
mesh_obj = load_from_meshio(mesh, 'WaveBot')
lid_mesh = mesh_obj.generate_lid(-2e-2)

fb = cpy.FloatingBody(mesh=mesh_obj, lid_mesh=lid_mesh, name="WaveBot")

# %%
fb.add_translation_dof(name="Heave")
ndof = fb.nb_dofs

# %%
bem_data = wot.run_bem(fb, freq)

# %%
hd = wot.add_linear_friction(bem_data, friction=500) 
# we're not actually adding friction, but need the datavariables in hd 
hd = wot.check_radiation_damping(hd)

intrinsic_impedance = wot.hydrodynamic_impedance(hd)
fig, axes = wot.utilities.plot_bode_impedance(intrinsic_impedance,
                                            'WaveBot Intrinsic Impedance')

# %%
ptos = {
  'unstructured': wot.pto.PTO(ndof,
                              kinematics=np.eye(ndof),
                              controller=None,
                              impedance=None,
                              loss=None,
                              names=["PTO_Heave",]),
  'pi': wot.pto.PTO(ndof,
                    kinematics=np.eye(ndof),
                    #   controller=wot.pto.controller_pi, #TODO
                    controller=wot.controllers.pid_controller(1,True,True,False),
                    impedance=None,
                    loss=None,
                    names=["PTO_Heave",]),
  'p': wot.pto.PTO(ndof,
                   kinematics=np.eye(ndof),
                   #   controller=wot.pto.controller_pi, #TODO
                   controller=wot.controllers.pid_controller(1,True,False,False),
                   impedance=None,
                   loss=None,
                   names=["PTO_Heave",]),
}

# %%
# Constraint
f_max = 750.0
nsubsteps = 4

wecs = {}
obj_funs = {}
nstate_opts = {}
for key in ptos:
  
  def const_f_pto(wec, x_wec, x_opt, waves): # Format for scipy.optimize.minimize
      f = ptos[key].force(wec, x_wec, x_opt, waves, nsubsteps)
      return f_max - np.abs(f.flatten())

  constraints = [{'type': 'ineq', 'fun': const_f_pto,}]
  
  wecs[key] = wot.WEC.from_bem(
      hd,
      constraints=constraints,  # TODO
      friction=None,
      f_add={'PTO': ptos[key].force_on_wec})
  obj_funs[key] = ptos[key].mechanical_average_power
  if key == 'pi':
      nstate_opts[key] = 2
  elif key == 'p':
      nstate_opts[key] = 1
  elif key == 'unstructured':
      nstate_opts[key] = 2*nfreq

# %%
pto_ds_list = []
wec_ds_list = []

results = {}
for key in wecs:
  print('\n-------------------------------')
  print(f'Running case: {key} controller')
  results[key] = wecs[key].solve(
      waves, 
      obj_funs[key], 
      nstate_opts[key],
      optim_options={'maxiter': 200}, 
      # x_wec_0=np.ones(wec.nfreq*2)*1e-1,
      # x_opt_0=np.ones(wec.nfreq*2)*1e-1,
      # scale_x_wec=1e1,
      # scale_x_opt=1e-3,
      # scale_obj=1e0,
      )
  
  x_wec, x_opt = wecs[key].decompose_state(results[key][0].x)
  nsubsteps = 5
  pto_f, pto_t = ptos[key].post_process(wecs[key], results[key], waves, nsubsteps=nsubsteps)
  wec_f, wec_t = wecs[key].post_process(wecs[key], results[key], waves, nsubsteps=nsubsteps)
  
  pto_ds_list.append(pto_t[0].expand_dims({'controller':[key]}))
  wec_ds_list.append(wec_t[0].expand_dims({'controller':[key]}))
  
  # opt_mechanical_average_power = results[0].fun
  # print(f'Optimal average mechanical power: {opt_mechanical_average_power} W')
  
pto_ds = xr.merge(pto_ds_list)
wec_ds = xr.merge(wec_ds_list)

# %%
fig, ax = plt.subplots(nrows=3, sharex=True)
wec_ds['pos'].plot(ax=ax[0],
                 hue='controller',
                 add_legend=True)
ax[0].set_ylabel('Position [m]')

pto_ds['force'].plot(ax=ax[1],
                   hue='controller',
                   add_legend=False)
ax[1].set_ylabel('Force [N]')

pto_ds['power'].sel(type='mech').squeeze().plot(ax=ax[2],
                                              hue='controller',
                                              add_legend=False)

for axi in ax:
  axi.set_title('')
  axi.label_outer()
  axi.spines[['right', 'top']].set_visible(False)
  axi.autoscale(enable=True, axis='x', tight=True)

# %%
pto_ds.squeeze().power.sel(type='mech').mean(dim='time')=