td_to_fd_to_td_bug.py

import autograd.numpy as np
from autograd.numpy import ndarray
import capytaine as cpy
import matplotlib.pyplot as plt
from typing import Optional, TypeVar, Callable, Union
from xarray import DataArray, Dataset
import wecopttool as wot
############################################################################
# Most of this comes from the WaveBot Tutorial, the couple of weird things #
# I do are first. The main wierd thing I'm doing is creating a new PTO     #
# class that is the same as the original, except it passes velocity to the #
# kinematics function instead of position. The other wierd things I'm doing#
# are creating a kinematics function that essentially takes the absolute   #
# value of the velocity (think 2-way piston) and running a wierd controller#
# that is only used to show the bug and then calls the unstructured        #
# controller.                                                              #
############################################################################

############################################################################
#                                                                          #
#                       Velocity Kinematics PTO                            #
#                                                                          #
############################################################################
from wecopttool.core import TWEC, TStateFunction, FloatOrArray
from wecopttool.pto import PTO,controller_unstructured,_make_abcd,_make_mimo_transfer_mat
TPTO = TypeVar("TPTO", bound="PTO")
TLOSS = Callable[[FloatOrArray, FloatOrArray], FloatOrArray]
class vPTO(PTO):
    # Most of init is the same, just the kinematics function definition...
    def __init__(self,
        ndof: int,
        kinematics: Union[TStateFunction, ndarray],
        controller: Optional[TStateFunction] = None,
        impedance: Optional[ndarray] = None,
        loss: Optional[TLOSS] = None,
        names: Optional[list[str]] = None,
    ) -> None:

        self._ndof = ndof
        # names
        if names is None:
            names = [f'PTO_{i}' for i in range(ndof)]
        elif ndof == 1 and isinstance(names, str):
            names = [names]
        self._names = names
        # kinematics - this is where things change...
        if callable(kinematics):
            def kinematics_fun(wec, x_wec, x_opt, waves, nsubsteps=1):
                pos_wec = wec.vec_to_dofmat(x_wec)
                vel_wec = np.dot(wec.derivative_mat, pos_wec)   # added this
                tmat = self._tmat(wec, nsubsteps)
                vel_wec_td = np.dot(tmat, vel_wec)              # swapped vel in for pos
                return kinematics(vel_wec_td)
        else:
            def kinematics_fun(wec, x_wec, x_opt, waves, nsubsteps=1):
                n = wec.nt*nsubsteps
                return np.repeat(kinematics[:, :, np.newaxis], n, axis=-1)
        self._kinematics = kinematics_fun
        # controller
        if controller is None:
            controller = controller_unstructured

        def force(wec, x_wec, x_opt, waves, nsubsteps=1):
            return controller(self, wec, x_wec, x_opt, waves, nsubsteps)

        self._force = force

        # power
        if impedance is not None:
            check_1 = impedance.shape[0] == impedance.shape[1] == 2*self.ndof
            check_2 = len(impedance.shape) == 3
            if not (check_1 and check_2):
                raise TypeError(
                    "Impedance should have size [2*ndof, 2*ndof, nfreq]"
                )
            for i in range(impedance.shape[2]-1):
                check_3 = (
                    np.allclose(np.real(impedance[:, :, i+1]), np.real(impedance[:, :, 0]))
                )
                check_3 = True  # disable check 3 bc IDK why it's here, asking Sandia... sounds like this is a safe thing to do
                if not check_3:
                    raise ValueError(
                        "Real component of impedance must be constant for " +
                        "all frequencies."
                    )
            impedance_abcd = _make_abcd(impedance, ndof)
            #impedance_abcd = impedance
            self._transfer_mat = _make_mimo_transfer_mat(impedance_abcd, ndof)
        else:
            self._transfer_mat = None
        self._impedance = impedance
        self._loss = loss

############################################################################
#                                                                          #
#                       Kinematics Function "abs()"                        #
#                                                                          #
############################################################################
def k_fun(v):
    piston_area = 1
    signs = np.array([np.sign(np.transpose(v))])  # returns -1, 0, or 1 based on sign
    k_mat = signs*piston_area       # multiply it by the area
    return k_mat

############################################################################
#                                                                          #
#                       Bug creating "Controller"                          #
#                                                                          #
############################################################################
# not a real controller, I'm just using it to show the bug
def bug_control(pto,wec,x_wec,x_opt,waves,nsubsteps):
    vel_td = pto.velocity(wec,x_wec,x_opt,waves,nsubsteps)  # get time domain velocity
    print(f'td... {vel_td[0]}')
    vel_fd = np.transpose(wot.td_to_fd(vel_td))             # convert it to frequency domain
    vel_td = wot.fd_to_td(vel_fd[0])                        # convert back to time domain
    print(f'td->fd->td... {vel_td[0]}')                     # prints only first entry for ease of viewing
    return wot.pto.controller_unstructured(pto,wec,x_wec,x_opt,waves,nsubsteps)
############################################################################
############################################################################



# The rest of this comes from the WaveBot Tutorial
wot.set_loglevel("error")  
# Create WEC geometry data
wb = wot.geom.WaveBot()  # use standard dimensions
mesh_size_factor = 0.5 # 1.0 for default, smaller to refine mesh
mesh = wb.mesh(mesh_size_factor)
fb = cpy.FloatingBody.from_meshio(mesh, name="WaveBot")
fb.add_translation_dof(name="Heave")

# WEC Hydrostatics
ndof = fb.nb_dofs
stiffness = wot.hydrostatics.stiffness_matrix(fb).values
mass = wot.hydrostatics.inertia_matrix(fb).values

# BEM stuff, does the capytaine
f1 = 0.05
nfreq = 50
freq = wot.frequency(f1, nfreq, False) # False -> no zero frequency
bem_data = wot.run_bem(fb, freq)

# Create PTO
name = ["PTO_Heave",]
kinematics = k_fun
controller = bug_control
loss = None
pto_impedance = None
pto = vPTO(ndof, kinematics, controller, pto_impedance, loss, name)

# PTO dynamics forcing function
f_add = {'PTO': pto.force_on_wec}

# Constraint
f_max = 2000.0
nsubsteps = 4

# constraints
def const_f_pto(wec, x_wec, x_opt, waves):
    f = pto.force_on_wec(wec, x_wec, x_opt, waves, nsubsteps)
    return f_max - np.abs(f.flatten())

ineq_cons = {'type': 'ineq',
             'fun': const_f_pto,
             }
constraints = [ineq_cons]

# Make the WEC, take hydrostatics, bem stuff, and combine
wec = wot.WEC.from_bem(
    bem_data,
    inertia_matrix=mass,
    hydrostatic_stiffness=stiffness,
    constraints=constraints,
    friction=None,
    f_add=f_add,
)

# Make a wave
amplitude = 0.0625
wavefreq = 0.3
phase = 30
wavedir = 0
waves = wot.waves.regular_wave(f1, nfreq, wavefreq, amplitude, phase, wavedir)

# we want to get lots of net power out
obj_fun = pto.mechanical_average_power
nstate_opt = 2*nfreq
options = {'maxiter': 200}
scale_x_wec = 1e1
scale_x_opt = 1e-3
scale_obj = 1e-2

# solve...
results = wec.solve(
    waves,
    obj_fun,
    nstate_opt,
    optim_options=options,
    scale_x_wec=scale_x_wec,
    scale_x_opt=scale_x_opt,
    scale_obj=scale_obj,
    )

opt_mechanical_average_power = results.fun
print(f'Optimal average mechanical power: {opt_mechanical_average_power} W')