def terminal_cost(self, function, unit):
r"""Sets the terminal cost of the OCP.
Examples
========
>>> from beluga.problem import OCP
>>> sigma = OCP()
>>> sigma.terminal_cost('x^2', 'm^2')
terminal_cost: {'function': x**2, 'unit': m**2}
.. seealso::
initial_cost, path_cost, get_terminal_cost
"""
if isinstance(function, str):
function = sympify(function)
if isinstance(unit, str):
unit = sympify(unit)
if not isinstance(function, Expr):
raise ValueError
if not isinstance(unit, Expr):
raise ValueError
temp = {'function': function, 'unit': unit}
self._properties['terminal_cost'] = temp
return self
def make_boundary_conditions(
constraints,
states,
costates,
cost,
derivative_fn,
location,
prefix_map=(('initial', (r'([\w\d\_]+)_0', r"_x0['\1']", sympify('-1'))),
('terminal', (r'([\w\d\_]+)_f', r"_xf['\1']", sympify('1'))))):
"""
Creates boundary conditions for initial and terminal constraints.
:param constraints: List of boundary constraints.
:param states: List of state variables.
:param costates: List of costate variables.
:param cost: Cost function.
:param derivative_fn: Total derivative function.
:param location: Location of each boundary constraint.
:param prefix_map: Prefix mapping.
:return: List of boundary conditions.
"""
prefix_map = dict(prefix_map)
bc_list = [
sanitize_constraint_expr(x, states, location, prefix_map)
for x in constraints[location]
]
*_, sign = dict(prefix_map)[location]
cost_expr = sign * cost
bc_list += [
str(costate - derivative_fn(cost_expr, state))
for state, costate in zip(states, costates)
]
return bc_list
def make_ctrl_dae(self, problem):
"""!
\brief Compute EOMs for controls.
\author Michael Grant
\author Thomas Antony
\version 0.1
\date 04/10/16
"""
if len(self.equality_constraints) > 0:
self.mu_vars = [sympify('mu'+str(i+1)) for i in range(len(self.equality_constraints))]
self.mu_lhs = [sympify(c.expr) for c in self.equality_constraints]
else:
self.mu_vars = self.mu_lhs = []
g = self.ham_ctrl_partial + self.mu_lhs
X = [state.sym for state in problem.states()] + [Symbol(costate) for costate in self.costates]
U = [c.sym for c in problem.controls()] + self.mu_vars
xdot = Matrix([sympify(state.process_eqn) for state in problem.states()] + self.costate_rates)
# Compute Jacobian
dgdX = Matrix([[self.derivative(g_i, x_i, self.quantity_vars) for x_i in X] for g_i in g])
dgdU = Matrix([[self.derivative(g_i, u_i, self.quantity_vars) for u_i in U] for g_i in g])
udot = dgdU.LUsolve(-dgdX*xdot); # dgdU * udot + dgdX * xdot = 0
self.dae_states = U
self.dae_equations = list(udot)
self.dae_bc = g
def make_state_dep_ham(self, state, problem):
"""!
\brief Symbolically create the Hamiltonian.
\author Michael Grant
\author Sean Nolan
\version 0.1
\date 06/22/16
"""
H = sympify(0)
new_terms = sympify2(problem.cost['path'].expr)
atoms = new_terms.atoms()
if state in atoms:
H += new_terms
for i in range(len(problem.states())):
new_terms = sympify2(self.costates[i]) * (sympify2(problem.states()[i].process_eqn))
atoms = new_terms.atoms()
if state in atoms:
H += new_terms
# Adjoin equality constraints
for i in range(len(self.equality_constraints)):
new_terms = sympify2('mu' + str(i + 1)) * (sympify2(self.equality_constraints[i].expr))
atoms = new_terms.atoms()
if state in atoms:
H += new_terms
raw_terms = H.as_terms()
H_parts = [raw_terms[0][k][0] for k in range(len(raw_terms[0]))]
new_H = sympify(0)
for part in H_parts:
store = sympify(0)
tries = 0
while (part != store) & (tries <= 5):
store = part
part = part.expand()
tries += 1
raw_terms = part.as_terms()
sub_parts = [raw_terms[0][k][0] for k in range(len(raw_terms[0]))]
new_part = sympify(0)
for sub_part in sub_parts:
atoms = sub_part.atoms()
if state in atoms:
# sub_part = sub_part.factor()
new_part += sub_part
new_part = new_part.factor()
if len(str(new_part)) < 50:
new_part = new_part.simplify(ratio=1.0)
new_H += new_part
# print('New:' + str(len(str(new_H))))
# print('Old:' + str(len(str(H))))
return new_H
def make_ctrl_analytic(self, controls):
"""!
\brief Symbolically compute the solutions for the control along control-unconstrained arcs.
\author Michael Grant
\author Thomas Antony
\version 0.1
\date 06/30/15
"""
# Solve all controls simultaneously
logging.info("Finding optimal control law ...")
# If equality constraints are present
# We need to solve for 'mu's as well
lhs = self.ham_ctrl_partial
vars = [c.sym for c in controls]
self.mu_vars = []
self.mu_lhs = []
if len(self.equality_constraints) > 0:
self.mu_vars = [sympify('mu'+str(i+1)) for i in range(len(self.equality_constraints))]
self.mu_lhs = [sympify(c.expr) for c in self.equality_constraints]
try:
var_list = list(vars + self.mu_vars)
# var_list = vars
eqn_list = []
for eqn in list(lhs + self.mu_lhs ):
eqn_list.append(self.selective_expand(eqn, controls, self.quantity_vars))
logging.info("Attempting using SymPy ...")
logging.debug("dHdu = "+str(eqn_list))
ctrl_sol = solve(eqn_list, var_list, dict=True)
#TODO: ^ uncomment once done with the aircraft noise problem
#ctrl_sol = CtrlSols() #I'm going to hell for this
#logging.info("Grabbed the control laws for the aircraft noise!")
# raise ValueError() # Force mathematica
except ValueError as e: # FIXME: Use right exception name here
logging.debug(e)
logging.info("No control law found")
from beluga.utils.pythematica import mathematica_solve
logging.info("Attempting using Mathematica ...")
#var_sol = mathematica_solve(lhs+self.mu_lhs,vars+self.mu_vars)
# TODO: Extend numerical control laws to mu's
ctrl_sol = var_sol
if ctrl_sol == []:
logging.info("No analytic control law found, switching to numerical method")
#logging.info("Done")
# solve() returns answer in the form
# [ {ctrl1: expr11, ctrl2:expr22},
# {ctrl1: expr21, ctrl2:expr22}]
# Convert this to format required by template
self.control_options = [ [{'name':str(ctrl), 'expr':str(expr)}
for (ctrl,expr) in option.items()]
for option in ctrl_sol]
def terminal(self, function, unit):
if not isinstance(function, str):
raise ValueError
if not isinstance(unit, str):
raise ValueError
temp = self.get('terminal', [])
temp.append({'function': sympify(function), 'unit': sympify(unit)})
self['terminal'] = temp
return self
def __init__(self, sym_ocp):
self.sym_ocp = sym_ocp
self.name = None
# Compile the cost functions from the original OCP
independent = [sym_ocp.get_independent()['symbol']]
states = [s['symbol'] for s in sym_ocp.states()]
controls = [s['symbol'] for s in sym_ocp.controls()]
params = [s['symbol'] for s in sym_ocp.parameters()]
consts = [s['symbol'] for s in sym_ocp.get_constants()]
if sym_ocp.get_initial_cost() is not None:
initial_compiled = lambdify_(
[independent, states, controls, params, consts],
sym_ocp.get_initial_cost()['function'])
else:
initial_compiled = lambdify_(
[independent, states, controls, params, consts], sympify('0'))
if sym_ocp.get_path_cost() is not None:
path_compiled = lambdify_(
[independent, states, controls, params, consts],
sym_ocp.get_path_cost()['function'])
else:
path_compiled = lambdify_(
[independent, states, controls, params, consts], sympify('0'))
if sym_ocp.get_terminal_cost() is not None:
terminal_compiled = lambdify_(
[independent, states, controls, params, consts],
sym_ocp.get_terminal_cost()['function'])
else:
terminal_compiled = lambdify_(
[independent, states, controls, params, consts], sympify('0'))
self.initial_cost = initial_compiled
self.path_cost = path_compiled
self.terminal_cost = terminal_compiled
x_dot = [s['eom'] for s in self.sym_ocp.states()]
self.deriv_func = lambdify_(
[independent, states, controls, params, consts], x_dot)
bc0 = [s['function'] for s in self.sym_ocp.constraints()['initial']]
bcf = [s['function'] for s in self.sym_ocp.constraints()['terminal']]
self.bc_initial = lambdify_(
[independent, states, controls, params, consts], bc0)
self.bc_terminal = lambdify_(
[independent, states, controls, params, consts], bcf)
def path_constraint(self, function, unit, lower, upper, activator, method):
r"""Defines a path constraint for the OCP.
Examples
========
>>> from beluga.problem import OCP
>>> sigma = OCP()
>>> sigma.path_constraint('x^2', 'm^2', lower='x_min', upper='x_max', activator='eps', method='utm')
path_constraints: [{'function': x**2, 'unit': m**2, 'lower': x_min, 'upper': x_max, 'activator': eps, 'method': 'utm'}]
.. seealso::
get_path_constraints
"""
if isinstance(function, str):
function = sympify(function)
if isinstance(unit, str):
unit = sympify(unit)
if isinstance(lower, str):
lower = sympify(lower)
if isinstance(upper, str):
upper = sympify(upper)
if isinstance(activator, str):
activator = sympify(activator)
if not isinstance(function, Expr):
raise ValueError
if not isinstance(unit, Expr):
raise ValueError
if not isinstance(lower, Expr):
raise ValueError
if not isinstance(upper, Expr):
raise ValueError
if not isinstance(activator, Expr):
raise ValueError
if not isinstance(method, str):
raise ValueError
temp = self._properties.get('path_constraints', [])
temp.append({
'function': function,
'unit': unit,
'lower': lower,
'upper': upper,
'activator': activator,
'method': method
})
self._properties['path_constraints'] = temp
return self
def compute_base_scaling(self, sol, scale_expr):
if isinstance(scale_expr, num.Number):
# If scaling factor is a number, use it
return scale_expr
else:
variables = [
(const_name, const_val)
for (const_name,
const_val) in zip(self.problem_data['constants'],
self.problem_data['constants_values'])
]
# Have to do in this order to override state values with arrays
variables += [
(state, max(abs(sol.y[:, idx])))
for idx, state in enumerate(self.problem_data['states'])
]
variables += [
(quad, max(abs(sol.q[:, idx])))
for idx, quad in enumerate(self.problem_data['quads'])
]
var_dict = dict(variables)
# Evaluate expression to get scaling factor
return float(
sympify(scale_expr).subs(var_dict, dtype=float).evalf())
def mathematica_solve(expr, vars):
if isinstance(expr, list):
m_expr = mcode(
['Exp[dummyfoobar]*(' + mcode(e) + ') == 0' for e in expr])
else:
m_expr = 'Exp[dummyfoobar]*(' + mcode(expr) + ') == 0'
cmd = 'Quiet[Simplify[Solve[%s,%s]]]' % (m_expr, mcode(vars)
) # Suppress warnings for now
sol_str = mathematica_run(cmd).strip()
# print(sol_str)
if sol_str == '{}' or "Solve[" in sol_str: # No solution found
return []
else:
# Convert solution to dictionary
out = [
dict([varsol.split(' -> ') for varsol in s.split(', ')])
for s in sol_str[2:-2].split('}, {')
]
# Convert solution strings to sympy expressions
out = [
dict([(sympify(var), mathematica_parse(expr))
for (var, expr) in sol.items()]) for sol in out
]
# print(out)
return out
def independent(self, symbol, unit):
r"""Sets the independent variable.
Examples
========
>>> from beluga.problem import OCP
>>> sigma = OCP()
>>> sigma.independent('t', 's')
independent: {'symbol': t, 'unit': s}
.. seealso::
get_independent
"""
if isinstance(symbol, str):
symbol = Symbol(symbol)
if isinstance(unit, str):
unit = sympify(unit)
if not isinstance(symbol, Symbol):
raise ValueError
if not isinstance(unit, Expr):
raise ValueError
temp = {'symbol': symbol, 'unit': unit}
self._properties['independent'] = temp
return self
def make_ham(self, problem):
"""!
\brief Symbolically create the Hamiltonian.
\author Michael Grant
\author Thomas Antony
\version 0.1
\date 06/30/15
"""
#TODO: Make symbolic
self.ham = sympify(problem.cost['path'].expr)
for i in range(len(problem.states())):
self.ham += sympify(self.costates[i]) * (sympify(problem.states()[i].process_eqn))
# Adjoin equality constraints
for i in range(len(self.equality_constraints)):
self.ham += sympify('mu'+str(i+1)) * (sympify(self.equality_constraints[i].expr))
def make_costate_rate(self, states):
"""!
\brief Creates the symbolic differential equations for the costates.
\author Michael Grant
\author Thomas Antony
\version 0.1
\date 06/30/15
"""
# TODO: Automate partial derivatives of numerical functions
# for state in states:
# rate = diff(sympify('-1*(' + self.ham + ')'),state)
# # numerical_diff = rate.atoms(Derivative)
# self.costate_rates.append(str(rate))
self.costate_rates = [self.derivative(-1*(self.ham),state, self.quantity_vars) for state in states]
# self.costate_rates.append(str(diff(sympify(
# '-1*(' + self.ham + ')'),state)))
for state in states:
corate=self.derivative(-1*(self.ham),state,self.quantity_vars)
custom_diff=corate.atoms(Derivative)
repl = [] #Replacements for custom functions
for d in custom_diff:
for f in d.atoms(AppliedUndef): #Just extracts (never more than 1 f)
for v in f.atoms(Symbol):
if str(v)==str(state): #This should only happen once!
replv=sympify('im('+str(f.subs(v,v+_h))+')/1e-30')
repl.append((d,replv))
self.costate_rates.append(corate.subs(repl))
self.costate_rates=sympify2(self.costate_rates)
def __init__(self, problem, cached=True):
"""!
\brief Initializes all of the relevant necessary conditions of opimality.
\author Michael Grant
\author Thomas Antony
\version 0.2
\date 05/15/16
"""
self.problem = problem
self.get_actions()
self.aug_cost = {}
self.costates = []
self.costate_rates = []
self.ham = sympify('0')
self.ham_ctrl_partial = []
self.ctrl_free = []
self.parameter_list = []
self.bc_initial = []
self.bc_terminal = []
# from .. import Beluga # helps prevent cyclic imports
self.compile_list = ['deriv_func','bc_func','compute_control']
# self.template_prefix = Beluga.config.getroot()+'/beluga/bvpsol/templates/'
self.template_suffix = '.py.mu'
self.dae_states = []
self.dae_equations = []
def __init__(self, param_dict, sym_key='name', excluded=()):
self.__dict__ = {k: sympify(v) if k not in excluded else v
for k, v in param_dict.items()}
self.param_list = list(param_dict.keys())
if sym_key is not None:
self._sym = self.__dict__[sym_key]
else:
self._sym = None
def make_costate_names(states):
r"""
Makes a list of variables representing each costate.
:param states: List of state variables, :math:`x`.
:return: List of costate variables, :math:`\lambda_x`.
"""
return [sympify('lam' + str(s.name).upper()) for s in states]
def make_costate_bc(self, states, location):
"""!
\brief Symbolically create the boundary conditions at initial and final locations.
\author Michael Grant
\author Thomas Antony
\version 0.1
\date 06/30/15
"""
if location is 'initial':
sign = sympify('-1')
elif location is 'terminal':
sign = sympify('1')
cost_expr = sign * (self.aug_cost[location])
#TODO: Fix hardcoded if conditions
#TODO: Change to symbolic
if location == 'initial':
# Using list comprehension instead of loops
# lagrange_ changed to l. Removed hardcoded prefix
self.bc_initial += [str(sympify(state.make_costate()) - self.derivative(sympify(cost_expr),state.sym, self.quantity_vars))
for state in states]
else:
# Using list comprehension instead of loops
self.bc_terminal += [str(sympify(state.make_costate()) - self.derivative(sympify(cost_expr),state.sym,self.quantity_vars))
for state in states]
def mathematica_parse(expr):
rules = (
(r'(\w+)\[', lambda m: m.group(0).lower()
), # Convert all function calls to lowercase
(r'\[', '('), # Replace square brackets with parenthesis
(r'\]', ')'), # Replace square brackets with parenthesis
(r'arc(\w+)', 'a\\1'), # Replace inverse trig functions
(r'sec\((\w+)\)', '(1/cos(\\1))') # Change sec to 1/cos
)
for rule, replacement in rules:
expr, n = re.subn(rule, replacement, expr)
return sympify(expr)
def make_ctrl_partial(self, controls):
"""!
\brief Symbolically compute dH/du where H is the Hamiltonian and u is the control.
\author Michael Grant
\author Thomas Antony
\version 0.1
\date 06/30/15
"""
self.ham_ctrl_partial = []
_h = sympify2('1j*(1e-30)')
for ctrl in controls:
dHdu = self.derivative(sympify(self.ham), ctrl, self.quantity_vars)
custom_diff = dHdu.atoms(Derivative)
for d in custom_diff:
for f in d.atoms(AppliedUndef):
for v in f.atoms(Symbol):
if str(v)==str(ctrl):
replv=sympify('im('+str(f.subs(v,v+_h))+')/1e-30')
repl.append((d,replv))
self.ham_ctrl_partial.append(dHdu.subs(repl))
def make_constrained_arc_fns(states, costates, controls, parameters, constants,
quantity_vars, hamiltonian):
"""
Creates constrained arc control functions.
:param workspace:
:return:
"""
tf_var = sympify('tf')
costate_eoms = [{
'eom': [str(_.eom * tf_var) for _ in costates],
'arctype': 0
}]
bc_list = [] # Unconstrained arc placeholder
return costate_eoms, bc_list
def compute_base_scaling(self, sol, scale_expr):
if isinstance(scale_expr, num.Number):
# If scaling factor is a number, use it
return scale_expr
else:
variables = [(aux_name, aux_val) for aux_type in sol.aux
if isinstance(sol.aux[aux_type], dict)
for (aux_name, aux_val) in sol.aux[aux_type].items()]
# Have to do in this order to override state values with arrays
variables += [
(state, max(abs(sol.y[:, idx])))
for idx, state in enumerate(self.problem_data['state_list'])
]
var_dict = dict(variables)
# Evaluate expression to get scaling factor
return float(
sympify(scale_expr).subs(var_dict, dtype=float).evalf())
def quantity(self, symbol, function):
r"""
:param symbol:
:param function:
:return:
"""
if isinstance(symbol, str):
symbol = Symbol(symbol)
if isinstance(function, str):
function = sympify(function)
if not isinstance(symbol, Symbol):
raise ValueError
if not isinstance(function, Expr):
raise ValueError
temp = self._properties.get('switches', [])
temp.append({'symbol': symbol, 'function': function})
self._properties['switches'] = temp
return self
def make_costate_rate_numeric(self, states):
"""!
\brief Creates the symbolic differential equations for the costates.
\author Michael Grant
\author Sean Nolan
\version 0.1
\date 06/22/16
"""
h = 1e-100
j = symbols('1j')
self.costate_rates = []
for state in states:
state = sympify(state)
H = self.make_state_dep_ham(state, self.problem)
if H != 0:
self.costate_rates.append('-np.imag(' + str((H.subs(state, (state + h * j)))) + ')/' + str(h))
else:
self.costate_rates.append('0')
def control(self, symbol, unit):
r"""Defines a control variable.
Examples
========
>>> from beluga.problem import OCP
>>> sigma = OCP()
>>> sigma.control('u', 'rad')
controls: [{'symbol': u, 'unit': rad}]
.. seealso::
get_controls
"""
if not isinstance(symbol, str):
raise ValueError
if not isinstance(unit, str):
raise ValueError
temp = self._properties.get('controls', [])
temp.append({'symbol': Symbol(symbol), 'unit': sympify(unit)})
self._properties['controls'] = temp
return self
def make_lagrange_mult(c, ind=1):
return sympify('lagrange_' + location + '_' + str(ind))
def create_scale_fn(self, unit_expr):
return lambdify(self.units_sym, sympify(unit_expr))
def get_bvp(self,problem,mode='dae'):
"""Perform variational calculus calculations on optimal control problem
and returns an object describing the boundary value problem to be solved
Returns: bvpsol.BVP object
"""
# Process intermediate variables
self.process_quantities(problem)
# Regularize path constraints using saturation functions
self.process_path_constraints(problem)
# self.state_subs = [(state.sym, sympify(state.process_eqn)) for state in problem.states()]
## Create costate list
# self.costates = [state.make_costate() for state in problem.states()]
self.make_costates(problem)
# for i in range(len(self.problem.states())):
# self.costates.append(self.problem.states()[i].make_costate())
# Build augmented cost strings
# aug_cost_init = sympify(problem.cost['initial'].expr)
self.make_aug_cost('initial')
# self.make_aug_cost(aug_cost_init, problem.constraints(), 'initial')
# aug_cost_term = sympify(problem.cost['terminal'].expr)
self.make_aug_cost('terminal')
# self.make_aug_cost(aug_cost_term, problem.constraints(), 'terminal')
# Add state boundary conditions
self.bc_initial = [self.sanitize_constraint(x,problem).expr
for x in problem.constraints().get('initial')]
self.bc_terminal = [self.sanitize_constraint(x,problem).expr
for x in problem.constraints().get('terminal')]
self.equality_constraints = problem.constraints().get('equality')
## Unconstrained arc calculations
# Construct Hamiltonian
self.make_ham(problem)
logging.debug('Hamiltonian : '+str(self.ham))
# Get list of all custom functions in the problem
# TODO: Check in places other than the Hamiltonian?
# TODO: Move to separate method?
func_list = sympify(self.ham).atoms(AppliedUndef)
# TODO: Change this to not be necessary
self.problem = problem
# Load required functions from the input file
new_functions = {(str(f.func),getattr(problem.input_module,str(f.func)))
for f in func_list
if hasattr(problem.input_module,str(f.func)) and
inspect.isfunction(getattr(problem.input_module,str(f.func)))}
problem.functions.update(new_functions)
undefined_func = [f.func for f in func_list if str(f.func) not in problem.functions]
if not all(x is None for x in undefined_func):
raise ValueError('Invalid function(s) specified: '+str(undefined_func))
# Compute costate conditions
self.make_costate_bc(problem.states(),'initial')
self.make_costate_bc(problem.states(),'terminal')
# TODO: Make this more generalized free final time condition
# HARDCODED tf variable
time_constraints = problem.constraints().get('independent')
if len(time_constraints) > 0:
self.bc_terminal.append('tf - 1')
else:
# Add free final time boundary condition
self.bc_terminal.append('_H - 0')
# Compute costate process equations
if mode == 'numerical':
self.make_costate_rate_numeric(problem.states())
else:
self.make_costate_rate(problem.states())
self.make_ctrl_partial(problem.controls())
# # Add support for state and control constraints
# problem.state('xi11','xi12','m')
# problem.state('xi12','ue1','m')
# self.costates += ['eta11','eta12'] # Costates for xi
#
# # Add new states to hamiltonian
# h1_3 = '(psi12*xi12^2 + psi11*ue1)'; # xi12dot = ue1
# c1_2 = 'u'
# self.ham += sympify('eta11*xi12 + eta12*ue1') #
# self.ham += sympify('mu1 * ('+c1_2+' - '+h1_3')')
#
# # TODO: Compute these automatically
# self.costate_rates += ['mu1*(xi12**2*psi1_3 + psi12*ue1)',
# 'mu1*(2*psi12*xi12) - eta1']
#
# Compute unconstrained control law
# (need to add singular arc and bang/bang smoothing, numerical solutions)
self.make_ctrl(problem, mode)
# Create problem dictionary
# NEED TO ADD BOUNDARY CONDITIONS
# bc1 = [self.sanitize_constraint(x) for x in initial_bc]
self.problem_data = {
'aux_list': [
{
'type' : 'const',
'vars': [const.var for const in problem.constants()]
},
{
'type' : 'constraint',
'vars': []
},
{
'type' : 'function',
'vars' : [func_name for func_name in problem.functions]
}
],
# TODO: Generalize 'tf' to independent variable for current arc
'state_list':
[str(state) for state in problem.states()] +
[str(costate) for costate in self.costates] +
['tf']
,
'parameter_list': [str(param) for param in self.parameter_list],
'deriv_list':
['(tf)*(' + str(sympify(state.process_eqn)) + ')' for state in problem.states()] +
['(tf)*(' + str(costate_rate) + ')' for costate_rate in self.costate_rates] +
# ['(tf)*((' + str(costate_rate) + ').imag)' for costate_rate in self.costate_rates] +
['tf*0'] # TODO: Hardcoded 'tf'
,
'state_rate_list':
['(tf)*(' + str(sympify2(state.process_eqn)) + ')' for state in problem.states()]
,
'dae_var_list':
[str(dae_state) for dae_state in self.dae_states],
'dae_eom_list':
['(tf)*('+str(dae_eom)+')' for dae_eom in self.dae_equations],
'dae_var_num': len(self.dae_states),
'num_states': 2*len(problem.states()) + 1,
'dHdu': [str(dHdu) for dHdu in self.ham_ctrl_partial] + self.mu_lhs,
'left_bc_list': self.bc_initial + (self.dae_bc if (mode == 'dae') else []),
'right_bc_list': self.bc_terminal,
'control_options': [] if (mode == 'dae') else self.control_options,
'control_list': [str(u) for u in problem.controls()] + [str(mu) for mu in self.mu_vars],
'num_controls': len(problem.controls()) + len(self.mu_vars), # Count mu multipliers
'ham_expr':self.ham,
# 'contr_dep_ham': [],
'quantity_list': self.quantity_list,
# 'dae_mode': mode == 'dae',
}
# problem.constraints[i].expr for i in range(len(problem.constraints))
# Create problem functions by importing from templates
self.compiled = imp.new_module('_probobj_'+problem.name)
if mode == 'dae':
# self.template_suffix = '_dae' + self.template_suffix
self.template_suffix = '_dae_num' + self.template_suffix
if mode == 'num':
self.template_suffix = '_num' + self.template_suffix
compile_result = [self.compile_function(self.template_prefix+func+self.template_suffix, verbose=True)
for func in self.compile_list]
dhdu_fn = self.compiled.get_dhdu_func
dae_num = len(problem.controls()) + len(self.mu_vars)
self.bvp = BVP(self.compiled.deriv_func,self.compiled.bc_func,dae_func_gen=dhdu_fn,dae_num_states=dae_num)
self.bvp.solution.aux['const'] = dict((const.var,const.val) for const in problem.constants())
self.bvp.solution.aux['parameters'] = self.problem_data['parameter_list']
self.bvp.solution.aux['function'] = problem.functions
# TODO: Fix hardcoding of function handle name (may be needed for multivehicle/phases)?
self.bvp.control_func = self.compiled.compute_control
self.bvp.problem_data = self.problem_data
# TODO: ^^ Do same for constraint values
return self.bvp
def process_path_constraints(self, problem):
constraints = problem.constraints().get('path')
quantity_subs = self.quantity_vars.items()
path_cost_expr = sympify(problem.cost['path'].expr)
path_cost_unit = sympify(problem.cost['path'].unit)
if path_cost_expr == 0:
logging.debug('No path cost specified, using unit from terminal cost function')
problem.cost['path'].unit = problem.cost['terminal'].unit
path_cost_unit = sympify(problem.cost['terminal'].unit)
logging.debug('Path cost is of unit: '+str(path_cost_unit))
time_unit = Symbol('s')
for (ind,c) in enumerate(constraints):
# Determine order of constraint
logging.debug('Processing path constraint: '+c.label)
order = 0
cq = [sympify(c.expr)]
dxdt = [sympify(state.process_eqn) for state in problem.states()]
# Zeroth order constraints have no 'xi' state
xi_vars = []
h = []
while True:
control_found = False
for u in problem.controls():
if u.sym in cq[-1].subs(quantity_subs).atoms():
logging.info('Constraint is of order '+str(order))
control_found = True
break
if control_found:
break
dcdx = [self.derivative(cq[-1], state.sym, self.quantity_vars) for state in problem.states()]
# Chain rule (Assume there is no explciit time-dependence) to find dcdt
cq.append(sum(d1*d2 for d1,d2 in zip(dcdx, dxdt)))
order = order + 1
# Create the auxiliary state variables
xi_vars.append(Symbol('xi'+str(ind+1)+str(order)))
# Create the smoothing control variable
xi_vars.append(Symbol('ue'+str(ind+1)))
# TODO: Fix constraint object to accept two limits
c_limit = sympify(c.limit)
if c_limit.is_Number:
# TODO: Allow continuation on constraints
# Define new hidden constant
c_limit = sympify('_'+c.label)
print(c.limit)
problem.constant(str(c_limit),float(c.limit),c.unit)
logging.debug('Added constant '+str(c_limit))
if c.direction == '>':
c.lbound = c_limit
c.ubound = -c_limit
elif c.direction == '<':
c.ubound = c_limit
c.lbound = -c_limit
else:
raise ValueError('Invalid direction specified for constraint')
psi = self.get_satfn(xi_vars[0], ubound=c.ubound, lbound=c.lbound, slopeAtZero=1)
psi_vars = [(Symbol('psi'+str(ind+1)), psi)]
# Add to quantity list
self.quantity_vars[Symbol('psi'+str(ind+1))] = psi
self.quantity_list.append({'name':('psi'+str(ind+1)), 'expr':str(psi)})
# m-th order constraint needs up to m-th derivative of psi to be defined
psi_i = psi
for i in range(order):
psi_i = diff(psi_i, xi_vars[0])
# psi_vars.append((Symbol('psi'+str(ind+1)+str(i+1)+'('+str(xi_vars[0])+')'), psi_i))
psi_vars.append((Symbol('psi'+str(ind+1)+str(i+1)), psi_i))
self.quantity_vars[Symbol('psi'+str(ind+1)+str(i+1))] = psi_i
self.quantity_list.append({'name':('psi'+str(ind+1)+str(i+1)), 'expr':str(psi_i)})
# psi_vars = psi_vars + []
psi_var_sub = [(v,k) for k,v in psi_vars]
# FIXME: Hardcoded h derivatives for now
# h = [psi_vars[0][0]]
# h.append(psi_vars[1][0]*xi_vars[1]) # psi'*xi12
# h.append(psi_vars[2][0]*xi_vars[1] + psi_vars[1][0]*xi_vars[2]) # psi''*xi12 + psi'*xi13
# psi'''*xi12 + xi13*psi12'' + psi12*xi13 + psi11*ue1
# h.append(psi_vars[3][0]*xi_vars[1] + 2 * psi_vars[2][0]*xi_vars[2] + psi_vars[1][0]*xi_vars[3] )
#TODO: Hardcoded 't' as independent variable with unit of 's'
# c_vals = [80e3, -5000, 9.539074102210087] # third number is vdot at zero approx
c_vals = np.ones(order)*0.1
h = [psi_vars[0][1]]
for i in range(order):
# Add 'xi' state
problem.state(str(xi_vars[i]), str(xi_vars[i+1]),'('+c.unit+')/s^('+str(i)+')')
# Constraint all cq at initial point (forms constraints for xi_ij)
problem.constraints().initial(str(cq[i] - h[i]),'('+c.unit+')/s^('+str(i)+')')
# Add to initial guess vector
problem.guess.start.append(c_vals[i])
dhdxi = [diff(h[i], xi_v) for xi_v in xi_vars[:-1]]
dhdt = sum(d1*d2 for d1,d2 in zip(dhdxi,xi_vars[1:])) # xi11dot = xi12 etc.
dhdt = dhdt.subs(psi_var_sub)
h.append(dhdt)
# Add the smoothing control with the right unit
ue_unit = sympify('('+c.unit+')/(s^('+str(order)+'))')
problem.control(str(xi_vars[-1]), str(ue_unit))
logging.debug('Adding control '+str(xi_vars[-1])+' with unit '+str(ue_unit))
# Add equality constraint
cqi_unit = ue_unit*time_unit
problem.constraints().equality(str(cq[-1] - h[-1]),str(cqi_unit))
# Add smoothing factor
eps_const = Symbol('eps_'+c.label)
eps_unit = (path_cost_unit/ue_unit**2)/time_unit #Unit of integrand
problem.constant(str(eps_const), 1e-6, str(eps_unit))
logging.debug('Adding smoothing factor '+str(eps_const)+' with unit '+str(eps_unit))
# Append new control to path cost
path_cost_expr = path_cost_expr + eps_const*xi_vars[-1]**2
logging.debug('Updated path cost is: '+str(path_cost_expr))
problem.cost['path'].expr = str(path_cost_expr)
u_constraints = problem.constraints().get('control')
for (ind,c) in enumerate(u_constraints):
w_i = sympify('uw'+str(ind+1))
psi = self.get_satfn(w_i, ubound=sympify(c.ubound), lbound = sympify(c.lbound))
# Add the smoothing control
problem.control(str(w_i), c.unit)
# Add equality constraint
csym = sympify(c.expr)
problem.constraints().equality(str(csym - psi),c.unit)
uw_unit = symipfy2(c.unit)
eps_const = Symbol('eps_'+str(ind+1))
eps_unit = (path_cost_unit/uw_unit**2)/time_unit #Unit of integrand
problem.constant(str(eps_const), 1, str(eps_unit))
def __init__(self, problem_data):
self.raw = problem_data
# Unpack and sympify problem data
# self.t = sympify(problem_data['independent'])
self.x = sympify(problem_data['states'])
self.u = sympify(problem_data['controls'])
self.p_d = sympify(problem_data['dynamical_parameters'])
self.k = sympify(problem_data['constants'])
self.q = sympify(problem_data['quads'])
self.p_n = sympify(problem_data['nondynamical_parameters'])
self.p = self.p_d + self.p_n # TODO: The parameter usage is inconsistent. Should clean-up down the line
self.x_dot = sympify(problem_data['states_rates'])
self.q_dot = sympify(problem_data['quads_rates'])
self.ham = sympify(problem_data['hamiltonian'])
self.bc_0 = sympify(problem_data['bc_initial'])
self.bc_f = sympify(problem_data['bc_terminal'])
self.dhdu = sympify(problem_data['dHdu'])
self.initial_cost = sympify(problem_data['initial_cost'])
self.path_cost = sympify(problem_data['path_cost'])
self.terminal_cost = sympify(problem_data['terminal_cost'])
self.df_dy = sympify(problem_data['states_jac'][0])
self.df_dp = sympify(problem_data['states_jac'][1])
self.dbc_0_dy = sympify(problem_data['bc_initial_jac'])
self.dbc_f_dy = sympify(problem_data['bc_terminal_jac'])
self.dbc_0_dp = sympify(problem_data['bc_initial_parameter_jac'])
self.dbc_f_dp = sympify(problem_data['bc_terminal_parameter_jac'])
self.path_constraints = problem_data['path_constraints']
control_options = problem_data['control_options']
if control_options is None:
self.algebraic_control_options = []
else:
self.algebraic_control_options = \
[[sympify(option[str(u_i)]) for u_i in self.u] for option in control_options]
# self.name = problem_data['problem_name']
self.name = None
def ocp_to_bvp(ocp, guess):
ws = init_workspace(ocp, guess)
problem_name = ws['problem_name']
independent_variable = ws['indep_var']
states = ws['states']
controls = ws['controls']
constants = ws['constants']
constants_of_motion = ws['constants_of_motion']
constraints = ws['constraints']
quantities = ws['quantities']
initial_cost = ws['initial_cost']
terminal_cost = ws['terminal_cost']
path_cost = ws['path_cost']
quantity_vars, quantity_list, derivative_fn = process_quantities(
quantities)
augmented_initial_cost = make_augmented_cost(initial_cost,
constraints,
location='initial')
initial_lm_params = make_augmented_params(constraints, location='initial')
augmented_terminal_cost = make_augmented_cost(terminal_cost,
constraints,
location='terminal')
terminal_lm_params = make_augmented_params(constraints,
location='terminal')
hamiltonian, costates = make_ham_lamdot(states, path_cost, derivative_fn)
bc_initial = make_boundary_conditions(constraints,
states,
costates,
augmented_initial_cost,
derivative_fn,
location='initial')
bc_terminal = make_boundary_conditions(constraints,
states,
costates,
augmented_terminal_cost,
derivative_fn,
location='terminal')
bc_terminal = make_time_bc(constraints, bc_terminal)
dHdu = make_dhdu(hamiltonian, controls, derivative_fn)
nond_parameters = initial_lm_params + terminal_lm_params
control_law = make_control_law(dHdu, controls)
# Generate the problem data
tf_var = sympify('tf')
out = ws
out['costates'] = costates
out['initial_lm_params'] = initial_lm_params
out['terminal_lm_params'] = terminal_lm_params
out['problem_data'] = {
'method':
'brysonho',
'problem_name':
problem_name,
'aux_list': [{
'type': 'const',
'vars': [str(k) for k in constants]
}],
'state_list': [str(x) for x in it.chain(states, costates)],
'deriv_list':
[tf_var * state.eom
for state in states] + [tf_var * costate.eom for costate in costates],
'states':
states,
'costates':
costates,
'constants':
constants,
'constants_of_motion':
constants_of_motion,
'dynamical_parameters': [tf_var],
'nondynamical_parameters':
nond_parameters,
'control_list': [str(x) for x in it.chain(controls)],
'controls':
controls,
'quantity_vars':
quantity_vars,
'hamiltonian':
hamiltonian,
'num_states':
2 * len(states),
'num_params':
len(nond_parameters) + 1,
'dHdu':
dHdu,
'bc_initial': [_ for _ in bc_initial],
'bc_terminal': [_ for _ in bc_terminal],
'control_options':
control_law,
'num_controls':
len(controls),
'quantity_list':
quantity_list,
'nOdes':
2 * len(states)
}
return out
