Best way to handle singularaties in Dymos equations of motion?

Question

Hi all I am working on rocket trajectory optimization with Dymos. The equations of motion for my model are for a spherical non-rotating Earth

where v is speed of the rocket, gamma is the flightpath angle to local horizontal, h is height above surface, theta is angle along the great circle of the trajectory, m is mass, epsilon is angle of attack, T is trust, D is drag, L is lift. These are quite similar to the equations of motion in the Dymos water rocket example. I used check_partials and my analytic derivatives seem to be correct.

As you can see gamma's rate of change is divided by v, so you have a singularity where v=0 at the beginning of the trajectory. You can get around that by starting v at some small number that's a small fraction of the intended final velocity -- like 1 to m/s for an orbital rocket.

However, the lower I set my initial velocity, the harder I can see the optimizer and grid refinement needs to work to get to a converged solution. For example,

setting final conditions of h=200 km, v= 7668 m/s and gamma=0 (a circular orbit), with v initial = 10 m/s, I have to start with 20 segments of order 6 to be able to reach convergence, and the final grid is like this

Number of Segments = 25 Segment Ends = [-1.0, -0.9833, -0.9667, -0.95, -0.9333, -0.9, -0.85, -0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0] Segment Order = [9, 9, 9, 9, 9, 9, 9, 9, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6]

The optimization time on this converged grid is 45 seconds

if I use v_initial = 1000 m/s, same final conditions, and a starting grid of 20 segments of order 3, the final grid is

Segment Ends = [-1.0, -0.95, -0.9, -0.8, -0.7, -0.6, -0.4, -0.2, 0.0, 0.2, 0.4, 0.6, 0.8, 1.0] Segment Order = [6, 6, 6, 6, 6, 3, 3, 3, 3, 3, 3, 3, 3]

the optimization time on the converged grid is 3.18 seconds.

Given the way grid points are being concentrated towards the beginning of the trajectory makes me think that the singularity in gamma_dot caused by v is the problem.

What can I do to improve performance?

import matplotlib.pyplot as plt

import openmdao.api as om
from openmdao.utils.assert_utils import assert_near_equal

import dymos as dm
import numpy as np

from dymos.examples.plotting import plot_results

import sys

from rocket_ode import rocketODE

#
# Instantiate the problem and configure the optimization driver
#
p = om.Problem(model=om.Group())

p.driver = om.pyOptSparseDriver()
p.driver.options['optimizer'] = 'IPOPT'
p.driver.declare_coloring()

h_target = 200E3
v_circ = (3.986004418E14/(h_target + 6378E3))**0.5

#
# Instantiate the trajectory and phase
#
traj = dm.Trajectory()


seg_ends = [-1.0, -0.9833, -0.9667, -0.95, -0.9333, -0.9, -0.85, -0.8, -0.7, -0.6, -0.5, -0.4,  -0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0]
seg_order = [9, 9, 9, 9, 9, 9, 9, 9, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6]
phase = dm.Phase(ode_class=rocketODE,
                   transcription=dm.Radau(num_segments=25, order=seg_order,
                   segment_ends=seg_ends, compressed=False))



traj.add_phase('phase0', phase)

p.model.add_subsystem('traj', traj)

#
# Set the options on the optimization variables
# Note the use of explicit state units here since much of the ODE uses imperial units
# and we prefer to solve this problem using metric units.
# #
phase.set_time_options(fix_initial=True, duration_bounds=(50, 1000))

# phase.set_time_options(fix_initial=True, fix_duration=True, duration_ref=100.0)

phase.add_state('v', fix_initial=True, lower=10.0, units='m/s',
                ref=1.0E3, targets=['eom.v'], rate_source='eom.v_dot')

phase.add_state('h', fix_initial=True, lower=0, upper=1.0E6, units='m',
                ref=1.0E5, targets=['eom.h'], rate_source='eom.h_dot')

phase.add_state('gamma', fix_initial=True, lower=0, upper=np.pi, units='rad',
                ref=1.0, targets=['eom.gamma'], rate_source='eom.gamma_dot')

phase.add_state('lambda', fix_initial=True, lower=0., upper=np.pi, units='rad',
                ref=0.01, targets=['eom.lambda'], rate_source='eom.lambda_dot')

phase.add_state('alpha', fix_initial=True, lower=-np.pi / 2, upper=np.pi / 2, units='rad',
                ref=1, defect_ref=1, targets=['eom.alpha'],
                rate_source='eom.alpha_dot')


phase.add_state('m', fix_initial=True, lower=100.0, upper=1.0E5, units='kg',
                ref=1.0E4, targets=['eom.m'], rate_source='eom.m_dot')


phase.add_control('alpha_rate', fix_initial=True, units='rad/s', lower=-0.1, upper=0.1, scaler=1.0, targets=['eom.alpha_rate'])

# phase.add_control('alpha', fix_initial=True, units='deg', lower=-89.0, upper=89.0, scaler=1.0, targets=['eom.alpha'])
# phase.add_control('thrust', units='N', lower=0, upper=1.35E5, ref=1E5, targets=['eom.thrust'])


phase.add_parameter('Isp', val=500.0, units='s', opt=False, targets=['eom.Isp'])
phase.add_parameter('thrust', val=1.3E5, opt=False, targets=['eom.thrust'], units='N')

phase.add_boundary_constraint('h', loc='final', ref=1.0E5, equals=h_target)
phase.add_boundary_constraint('v', loc='final', ref=1.0E3, equals=v_circ)
phase.add_boundary_constraint('gamma', loc='final', equals=0.0, units='rad')


# Minimize time at the end of the phase
phase.add_objective('time', loc='final', scaler=1.0)
p.model.linear_solver = om.DirectSolver()

#
# Setup the problem and set the initial guess
#
p.setup(check=True)

p['traj.phase0.t_initial'] = 0.0
p['traj.phase0.t_duration'] = 300
p['traj.phase0.states:v'] = phase.interpolate(ys=[10.0, v_circ], nodes='state_input')
p['traj.phase0.states:h'] = phase.interpolate(ys=[0.0, h_target], nodes='state_input')
p['traj.phase0.states:gamma'] = phase.interpolate(ys=[np.pi/2, 0.0], nodes='state_input')
p['traj.phase0.states:lambda'] = phase.interpolate(ys=[0.0, 0.02], nodes='state_input')
p['traj.phase0.states:m'] = phase.interpolate(ys=[10000.0, 1000.0], nodes='state_input')
p['traj.phase0.states:alpha'] = phase.interpolate(ys=[0.0, 0.0], nodes='state_input')

p['traj.phase0.controls:alpha_rate'] = phase.interpolate(ys=[0.0, 0.0], nodes='control_input')


# Solve for the optimal trajectory
#
# dm.run_problem(p,refine_method='hp',refine_iteration_limit=20)
dm.run_problem(p)
#
# Get the explicitly simulated solution and plot the results
#
exp_out = traj.simulate(times_per_seg=100,method='RK45')

This is the EOM code

import numpy as np

import openmdao.api as om


class rocketEOM(om.ExplicitComponent):

    def initialize(self):
        self.options.declare('num_nodes', types=int)

        self.options.declare('CD', types=float, default=0.5,
                             desc='coefficient of drag')

        self.options.declare('S', types=float, default=7.069,
                             desc='aerodynamic reference area (m**2)')

        self.options.declare('r0', types=float, default=6.3781E6,
                             desc='Earth radius m')

        self.options.declare('g0', types=float, default=9.80665,
                             desc='acceleration at earth surface')

        self.options.declare('omega', types=float, default=7.292115E-5,
                             desc='rate of Earth rotation, rad/s')

    def setup(self):
        nn = self.options['num_nodes']

        # Inputs
        self.add_input('v',
                       val=np.zeros(nn),
                       desc='relative velocity',
                       units='m/s')

        self.add_input('h',
                       val=np.zeros(nn),
                       desc='altitude',
                       units='m')

        self.add_input('gamma',
                       val=np.zeros(nn),
                       desc='flight path angle',
                       units='rad')

        self.add_input('lambda',
                       val=np.zeros(nn),
                       desc='longitude',
                       units='rad')

        self.add_input('alpha',
                       val=np.zeros(nn),
                       desc='angle of attack',
                       units='rad')

        self.add_input('alpha_rate',
                       val=np.zeros(nn),
                       desc='angle of attack rate',
                       units='rad/s')

        self.add_input('m',
                       val=np.zeros(nn),
                       desc='mass',
                       units='kg')

        self.add_input('rho',
                       val=np.zeros(nn),
                       desc='density',
                       units='kg/m**3')

        self.add_input('thrust',
                       val=1.4E5 * np.ones(nn),
                       desc='thrust',
                       units='N')

        self.add_input('Isp',
                       val=400.0 * np.ones(nn),
                       desc='specific impulse',
                       units='s')

        # Outputs
        self.add_output('v_dot',
                        val=np.zeros(nn),
                        desc='acceleraton',
                        units='m/s**2')

        self.add_output('gamma_dot',
                        val=np.zeros(nn),
                        desc='flight path angle rate of change',
                        units='rad/s')

        self.add_output('h_dot',
                        val=np.zeros(nn),
                        desc='height rate of change',
                        units='m/s')

        self.add_output('lambda_dot',
                        val=np.zeros(nn),
                        desc='longitude rate of change',
                        units='rad/s')

        self.add_output('alpha_dot',
                        val=np.zeros(nn),
                        desc='angle of attack rate of change',
                        units='rad/s')

        self.add_output('m_dot',
                        val=np.zeros(nn),
                        desc='mass rate of change',
                        units='kg/s')

        # Setup partials
        ar = np.arange(self.options['num_nodes'])

        self.declare_partials(of='v_dot', wrt='h', rows=ar, cols=ar)
        self.declare_partials(of='v_dot', wrt='gamma', rows=ar, cols=ar)
        self.declare_partials(of='v_dot', wrt='m', rows=ar, cols=ar)
        self.declare_partials(of='v_dot', wrt='alpha', rows=ar, cols=ar)
        self.declare_partials(of='v_dot', wrt='thrust', rows=ar, cols=ar)

        self.declare_partials(of='gamma_dot', wrt='v', rows=ar, cols=ar)
        self.declare_partials(of='gamma_dot', wrt='h', rows=ar, cols=ar)
        self.declare_partials(of='gamma_dot', wrt='gamma', rows=ar, cols=ar)
        self.declare_partials(of='gamma_dot', wrt='m', rows=ar, cols=ar)
        self.declare_partials(of='gamma_dot', wrt='alpha', rows=ar, cols=ar)
        self.declare_partials(of='gamma_dot', wrt='thrust', rows=ar, cols=ar)

        self.declare_partials(of='h_dot', wrt='v', rows=ar, cols=ar)
        self.declare_partials(of='h_dot', wrt='gamma', rows=ar, cols=ar)

        self.declare_partials(of='lambda_dot', wrt='v', rows=ar, cols=ar)
        self.declare_partials(of='lambda_dot', wrt='h', rows=ar, cols=ar)
        self.declare_partials(of='lambda_dot', wrt='gamma', rows=ar, cols=ar)

        self.declare_partials(of='m_dot', wrt='Isp', rows=ar, cols=ar)
        self.declare_partials(of='m_dot', wrt='thrust', rows=ar, cols=ar)

        self.declare_partials(of='alpha_dot', wrt='alpha_rate', rows=ar, cols=ar, val=1.0)

    def compute(self, inputs, outputs):

        alpha = inputs['alpha']
        v = inputs['v']
        h = inputs['h']
        gamma = inputs['gamma']
        m = inputs['m']
        T = inputs['thrust']
        ISP = inputs['Isp']

        g = self.g_func(h)
        r0 = self.options['r0']

        D = 0  # calc drag here
        L = 0  # calc lift here
        g = self.g_func(h)
        r = r0 + h

        sin_gamma = np.sin(gamma)
        cos_gamma = np.cos(gamma)

        v_dot = (T * np.cos(alpha) - D) / m - g * sin_gamma

        gamma_dot = ((T * np.sin(alpha) + L) / m - (g - v**2 / r) * cos_gamma) / v

        h_dot = v * sin_gamma

        lambda_dot = v * cos_gamma / r

        m_dot = -T / (9.80665 * ISP)

        outputs['v_dot'] = v_dot
        outputs['gamma_dot'] = gamma_dot
        outputs['h_dot'] = h_dot
        outputs['lambda_dot'] = lambda_dot
        outputs['m_dot'] = m_dot
        outputs['alpha_dot'] = inputs['alpha_rate']

    def g_func(self, h):
        r0 = self.options['r0']
        g0 = self.options['g0']
        return g0 * (r0 / (r0 + h))**2

    def compute_partials(self, inputs, jacobian):

        r0 = self.options['r0']
        g0 = self.options['g0']

        alpha = inputs['alpha']
        v = inputs['v']
        h = inputs['h']
        gamma = inputs['gamma']
        m = inputs['m']
        T = inputs['thrust']
        ISP = inputs['Isp']

        g = self.g_func(h)
        r0 = self.options['r0']

        D = 0  # calc drag here
        L = 0  # calc lift here
        # g = self.g_func(h)
        r = r0 + h

        sin_gamma = np.sin(gamma)
        cos_gamma = np.cos(gamma)
        sin_alpha = np.sin(alpha)
        cos_alpha = np.cos(alpha)

        jacobian['v_dot', 'h'] = 2 * g0 * r0**2 * sin_gamma / r**3

        jacobian['v_dot', 'gamma'] = -g0 * r0**2 * cos_gamma / r**2

        jacobian['v_dot', 'm'] = (D - T * cos_alpha) / m**2

        jacobian['v_dot', 'alpha'] = -T * sin_alpha / m

        jacobian['v_dot', 'thrust'] = cos_alpha / m

        jacobian['gamma_dot', 'v'] = cos_gamma * (g / v**2 + 1 / r) + (L - T * sin_alpha) / (m * v**2)

        jacobian['gamma_dot', 'h'] = (2 * g / (r * v) - v / r**2) * cos_gamma

        jacobian['gamma_dot', 'gamma'] = ((g0 * r0**2 - v**2 * r) * sin_gamma) / (r**2 * v)

        jacobian['gamma_dot', 'm'] = -(-L + T * sin_alpha) / (m**2 * v)

        jacobian['gamma_dot', 'alpha'] = (T * cos_alpha) / (m * v)

        jacobian['gamma_dot', 'thrust'] = sin_alpha / (m * v)

        jacobian['h_dot', 'v'] = sin_gamma

        jacobian['h_dot', 'gamma'] = v * cos_gamma

        jacobian['lambda_dot', 'v'] = cos_gamma / r

        jacobian['lambda_dot', 'h'] = -v * cos_gamma / r**2

        jacobian['lambda_dot', 'gamma'] = -v * sin_gamma / r

        jacobian['m_dot', 'Isp'] = T / (9.80665 * ISP**2)

        jacobian['m_dot', 'thrust'] = - 1 / (9.80665 * ISP)

Answer 1

I agree that the singularity here is a problem, and your approach of setting it to some small initial value is generally a good way to go.

While they're generally pretty reasonable, the grid refinement algorithms currently in Dymos might have issues when the EOM start going singular, and I think you're seeing that here.

What else can you try?

-You could break the problem into multiple phases. The first phase would cover the vertical ascent before pitchover begins. By using a set of EOM that are non-singular for this phase of the trajectory, you could allow the vehicle to build up some speed before transitioning to the second phase, using your current EOM.

-You could reformulate the problem to use EOM that don't have a singularity near your operating conditions. Flight path EOM are generally nice and benign, but they suffer from singularities at v=0 (and gamma=+/-90 deg in the 3D case).

A Cartesian formulation to the EOM, where the vehicle inertial position and velocity are [x, y, vx, vy], would provide EOM that are only singular at the center of the planet. Parameterizing the initial states is somewhat more tricky (especially in the 3D case) because the initial velocity is the initial velocity of the launch pad fixed to the rotating Earth.

You could also try treating flight path angle as a fixed parameter for the first phase, fixing it to 90 degrees until some adequate velocity is established. In this case you'd be accepting some small inaccuracy in your dynamics in order to get better behavior.