Optimization failing when trying to enforce periodicity in Dymos
I am trying to model a simplified wave energy converter with optimal control using Dymos. The linear model of the wave energy converter is characterized by mass, damping, and stiffness terms. There is also a sine-wave excitation term. The control term is an arbitrary force at each timestep and the objective is to minimize the energy (maximize energy harvested) which is the product of control force and velocity. The system itself should be periodic and if I fix the initial and final values of x and v, it optimizes successfully. However, if I try to enforce periodicity using traj.link_phases() from the racecar lap example, I get optimization failed. I'd like to enforce periodicity because I don't necessarily know the initial x and v and it should have a specific phase offset based on the parameters and excitation. Is there anything I'm missing in terms of enforcing periodicity? Or something that is over-constraining this problem?
import numpy as np
import openmdao.api as om
import dymos as dm
import matplotlib.pyplot as plt
class OscillatorODE(om.ExplicitComponent):
"""
A Dymos ODE for a damped harmonic oscillator with wave excitation and optimal
controls.
"""
def initialize(self):
self.options.declare('num_nodes', types=int)
def setup(self):
nn = self.options['num_nodes']
ar = np.arange(nn)
# Inputs
self.add_input('x', shape=(nn,), desc='displacement', units='m')
self.add_input('v', shape=(nn,), desc='velocity', units='m/s')
self.add_input('k', desc='spring constant', units='N/m')
self.add_input('c', desc='damping coefficient', units='N*s/m')
self.add_input('m', desc='mass', units='kg')
self.add_input('exc_amp', desc='exc force amplitude', units='N')
self.add_input('time',shape=(nn,),desc='time',units='s') # New input for time
self.add_input("u", shape=(nn,), units="N", desc="control force applied") # control force
# frequency inputs
self.add_input('wave_freq', desc='fundamental frequency', units='Hz')
#self.add_input('phi', desc='excitation phase offset', units='rad') # add phase offset to optimize
self.add_output('v_dot', val=np.zeros(nn), desc='rate of change of velocity', units='m/s**2')
self.add_output("power", shape=(nn,), units="W", desc="power to be integrated")
self.add_output("x_dot", shape=(nn,), desc="rate of change of displacement", units="m/s") # for integration
# v_dot wrt vector inputs (diagonal)
self.declare_partials('v_dot', 'x', rows=ar, cols=ar)
self.declare_partials('v_dot', 'v', rows=ar, cols=ar)
self.declare_partials('v_dot', 'u', rows=ar, cols=ar)
self.declare_partials('v_dot', 'time', rows=ar, cols=ar)
# power wrt vector inputs (diagonal)
self.declare_partials('power', 'v', rows=ar, cols=ar)
self.declare_partials('power', 'u', rows=ar, cols=ar)
# v_dot wrt scalar-ish inputs (dense (nn x 1) column)
self.declare_partials('v_dot', 'k')
self.declare_partials('v_dot', 'c')
self.declare_partials('v_dot', 'm')
self.declare_partials('v_dot', 'exc_amp')
self.declare_partials('v_dot', 'wave_freq')
self.declare_partials('x_dot', 'v', rows=ar, cols=ar)
#self.declare_partials('v_dot', 'phi')
def compute(self, inputs, outputs):
x = inputs['x']
v = inputs['v']
k = inputs['k']
c = inputs['c']
m = inputs['m']
exc_amp = inputs['exc_amp']
wave_freq = inputs['wave_freq']
t = inputs['time']
u = inputs['u']
f_spring = -k * x
f_damper = -c * v
f_control = u
#phi = inputs['phi']
w = 2*np.pi*wave_freq
f_ext = exc_amp*np.cos(w*t) # or sin, as you intend
#outputs['f_dot'] = -wave_freq*2*np.pi*exc_amp*np.sin(wave_freq*2*np.pi*t) # force is steadily increasing
outputs['v_dot'] = (f_spring + f_damper + f_ext + f_control) / m # this is the residual
outputs['power'] = f_control*v # power is control force*velocity
outputs['x_dot'] = v # for integration
#outputs['t_check'] = 1
def compute_partials(self, inputs, partials):
nn = self.options['num_nodes']
x = inputs['x']
v = inputs['v']
k = inputs['k']
c = inputs['c']
m = inputs['m']
exc_amp = inputs['exc_amp']
wave_freq = inputs['wave_freq']
t = inputs['time']
u = inputs['u']
partials['v_dot', 'x'] = (-k / m) * np.ones(nn)
partials['v_dot', 'v'] = (-c / m) * np.ones(nn)
partials['v_dot', 'k'] = -x / m
partials['v_dot', 'c'] = -v / m
#phi = inputs['phi']
w = 2*np.pi*wave_freq
partials['v_dot', 'm'] = -( -k * x - c * v + exc_amp*np.cos(w*t) + u ) / m**2
partials['v_dot', 'exc_amp'] = np.cos(w*t) / m
partials['v_dot', 'wave_freq'] = -exc_amp*2*np.pi*t*np.sin(w*t) / m
partials['v_dot', 'time'] = -exc_amp*w*np.sin(w*t) / m
partials['v_dot', 'u'] = (1 / m) * np.ones(nn)
partials['power', 'v'] = u
partials['power', 'u'] = v
partials['x_dot', 'v'] = np.ones(nn)
wavefreq = 0.1 # Hz
f1 = wavefreq
nfreq = 10
t_vec = np.linspace(0, 1/f1, 2*nfreq, endpoint=True)
# Instantiate an OpenMDAO Problem instance.
prob = om.Problem()
# We need an optimization driver. To solve this simple problem ScipyOptimizerDriver will work.
prob.driver = om.ScipyOptimizeDriver(maxiter=500)
prob.driver.options['optimizer'] = 'SLSQP'
prob.driver.declare_coloring()
# Instantiate a Dymos Trajectory and add it to the Problem model.
traj = dm.Trajectory()
prob.model.add_subsystem('traj', traj)
# Instantiate a Phase and add it to the Trajectory.
phase = dm.Phase(ode_class=OscillatorODE, transcription=dm.GaussLobatto(num_segments=24,order=3))
traj.add_phase('phase0', phase)
# Tell Dymos that the duration of the phase is bounded.
phase.set_time_options(fix_initial=True, fix_duration=True, targets=['time'])
# Tell Dymos the states to be propagated using the given ODE.
phase.add_state('x', fix_initial=False, fix_final=False, rate_source='x_dot', scaler=1, targets=['x'], units='m')
phase.add_state('v', fix_initial=False, fix_final=False, rate_source='v_dot', targets=['v'], units='m/s')
phase.add_state('energy', fix_initial=True, rate_source='power', ref=10, defect_ref=1, units='J') # integration of power
phase.add_control('u', fix_initial=False, rate_continuity=False, opt=True, continuity=True, targets=['u'], val=0, lower=-30, upper=30, scaler=1e-1, units='N')
# Enforce periodic states: final = initial
traj.link_phases(phases=['phase0', 'phase0'], vars=['x','v'], locs=('final', 'initial'))
# The spring constant, damping coefficient, and mass are inputs to the system that are
# constant throughout the phase.
phase.add_parameter('k', units='N/m', targets=['k'], static_target=True)
phase.add_parameter('c', units='N*s/m', targets=['c'], static_target=True)
phase.add_parameter('m', units='kg', targets=['m'], static_target=True)
phase.add_parameter('exc_amp', units='N', targets=['exc_amp'], static_target=True)
phase.add_parameter('wave_freq', units='Hz', targets=['wave_freq'], static_target=True)
phase.add_timeseries_output('time', output_name='time')
# Since we're using an optimization driver, an objective is required. We'll minimize
# the final time in this case.
phase.add_objective('energy', loc='final',scaler=1e-2)
prob.model.linear_solver = om.DirectSolver()
# Setup the OpenMDAO problem
prob.setup()
# Assign values to the times and states
t_final = t_vec[-1]
phase.set_time_val(0.0, t_final) # 1 period
phase.set_state_val('x', vals=[1.0, 1.0], time_vals=[0.0, t_final])
phase.set_state_val('v', vals=[0.0,0.0], time_vals=[0.0, t_final])
phase.set_control_val('u', vals=[10, 10], time_vals=[0.0, t_final])
phase.set_state_val('energy', vals=[0.0,-1000], time_vals=[0.0, t_final]) # the final value is a guess
phase.set_parameter_val('k', 24452)
phase.set_parameter_val('c', 71)
phase.set_parameter_val('m', 876 + 1492)
phase.set_parameter_val('exc_amp', np.abs(23518+45*1j))
phase.set_parameter_val('wave_freq', 0.1)
#phase.set_parameter_val('u', 1.0)
#phase.set_parameter_val('f', 1.0)
# Now we're using the optimization driver to iteratively run the model and vary the
dm.run_problem(prob)
# Perform an explicit simulation of our ODE from the initial conditions.
sim_out = traj.simulate(times_per_seg=10)
# Plot the state values obtained from the phase timeseries objects in the simulation output.
t_sol = prob.get_val('traj.phase0.timeseries.time')
t_sim = sim_out.get_val('traj.phase0.timeseries.time')