def transformation(self, prob: Problem) -> Problem:
states = sympy.Matrix(extract_syms(prob.states))
dynamic_parameters = sympy.Matrix(extract_syms(prob.parameters))
parameters = sympy.Matrix(
extract_syms(prob.parameters) +
extract_syms(prob.constraint_parameters))
quads = sympy.Matrix(extract_syms(prob.quads))
eom = sympy.Matrix(getattr_from_list(prob.states, 'eom'))
phi_0 = sympy.Matrix(
getattr_from_list(prob.constraints['initial'], 'expr'))
phi_f = sympy.Matrix(
getattr_from_list(prob.constraints['terminal'], 'expr'))
prob.func_jac['df_dy'] = eom.jacobian(states)
prob.bc_jac['initial']['dbc_dy'] = phi_0.jacobian(states)
prob.bc_jac['terminal']['dbc_dy'] = phi_f.jacobian(states)
if len(parameters) > 0:
prob.func_jac.update({'df_dp': eom.jacobian(dynamic_parameters)})
prob.bc_jac['initial']['dbc_dp'] = phi_0.jacobian(parameters)
prob.bc_jac['terminal']['dbc_dp'] = phi_f.jacobian(parameters)
if len(quads) > 0:
prob.bc_jac['initial']['dbc_dq'] = phi_0.jacobian(quads)
prob.bc_jac['terminal']['dbc_dq'] = phi_f.jacobian(quads)
return prob
def transformation(self, prob: Problem) -> Problem:
if not prob.dualized:
raise ValueError('Problem must be dualized.')
control_syms = extract_syms(prob.controls)
prob.dh_du = [
prob.hamiltonian.expr.diff(control_sym)
for control_sym in control_syms
]
logging.debug("Solving dH/du...")
control_options = sympy.solve(prob.dh_du,
control_syms,
minimal=True,
simplify=False,
dict=True)
logging.debug('Control found')
prob.control_law = [
tuple(option[control_sym] for control_sym in control_syms)
for option in control_options
]
prob.sol_map_chain.append(AlgebraicControlMapper(prob))
prob.prob_type = 'prob'
return prob
def __init__(self, prob: Problem):
num_options = len(prob.control_law)
_args = \
[prob.independent_variable.sym, extract_syms(prob.states), extract_syms(prob.costates),
extract_syms(prob.parameters), extract_syms(prob.constants)]
_args_w_control = copy.copy(_args)
_args_w_control.insert(3, extract_syms(prob.controls))
# self.compute_u = compile_control(prob.control_law, _args, prob.hamiltonian.expr, lambdify_func=prob.lambdify)
if num_options == 0:
raise RuntimeError
elif num_options == 1:
compiled_option = prob.lambdify(_args, prob.control_law[0])
def calc_u(_t, _y, _lam, _p, _k):
return np.array(compiled_option(_t, _y, _lam, _p, _k))
else:
compiled_options = prob.lambdify(_args, prob.control_law)
ham_func = prob.lambdify(_args_w_control, prob.hamiltonian.expr)
def calc_u(_t, _y, _lam, _p, _k):
u_set = np.array(compiled_options(_t, _y, _lam, _p, _k))
u = u_set[0, :]
ham = ham_func(_t, _y, _lam, u, _p, _k)
for n in range(1, num_options):
ham_i = ham_func(_t, _y, _lam, u_set[n, :], _p, _k)
if ham_i < ham:
u = u_set[n, :]
ham = ham_i
return u
self.compute_u = jit_compile_func(calc_u,
_args,
func_name='control_function')
def _initialize(self):
self._state_syms = extract_syms(self.prob.states)
self._control_syms = extract_syms(self.prob.controls)
self._parameter_syms = extract_syms(self.prob.parameters)
self._constant_syms = extract_syms(self.prob.constants)
self._quad_syms = extract_syms(self.prob.quads)
self._constraint_parameters_syms = extract_syms(
self.prob.constraint_parameters)
self._dynamic_args = \
[self._state_syms, self._parameter_syms, self._constant_syms]
self._dynamic_args_w_controls = \
[self._state_syms, self._control_syms, self._parameter_syms, self._constant_syms]
self._bc_args = \
[self._state_syms, self._parameter_syms,
self._constraint_parameters_syms, self._constant_syms]
self._bc_args_w_quads = \
[self._state_syms, self._quad_syms, self._parameter_syms,
self._constraint_parameters_syms, self._constant_syms]
self._bc_args_w_controls = \
[self._state_syms, self._control_syms, self._parameter_syms,
self._constraint_parameters_syms, self._constant_syms]
self._bc_args_w_quads_controls = \
[self._state_syms, self._quad_syms, self._control_syms, self._parameter_syms,
self._constraint_parameters_syms, self._constant_syms]
self._units_args = \
[self.prob.independent_variable.sym, self._state_syms, self._quad_syms,
self._parameter_syms, self._constraint_parameters_syms, self._constant_syms]
self._initialized = True
def __init__(self, dual_len, nu_len, ocp: Problem):
self.dual_len = dual_len
self.nu_len = nu_len
_state_syms = extract_syms(ocp.states)
_control_syms = extract_syms(ocp.controls)
_parameter_syms = extract_syms(ocp.parameters)
_constant_syms = extract_syms(ocp.constants)
_quad_syms = extract_syms(ocp.quads)
_constraint_parameters_syms = extract_syms(ocp.constraint_parameters)
_dynamic_args = [
_state_syms, _control_syms, _parameter_syms, _constant_syms
]
_bc_args = [_state_syms, _quad_syms, _parameter_syms, _constant_syms]
self.compute_cost = compile_cost(ocp.cost, _dynamic_args, _bc_args,
ocp.lambdify)
def transformation(self, prob: Problem) -> Problem:
if not is_symplectic(prob.omega):
logging.warning('BVP is not symplectic. Skipping reduction.')
return prob
constant_of_motion = prob.constants_of_motion[self.com_index]
state_syms = extract_syms(prob.states)
costates_syms = extract_syms(prob.costates)
parameter_syms = extract_syms(prob.parameters)
constant_syms = extract_syms(prob.constants)
fn_p = prob.lambdify(
[state_syms, costates_syms, parameter_syms, constant_syms],
constant_of_motion.expr)
states_and_costates = prob.states + prob.costates
atoms = constant_of_motion.expr.atoms()
atoms2 = set()
for atom in atoms:
if isinstance(atom, sympy.Symbol) and (atom not in {
item.sym
for item in prob.parameters + prob.constants
}):
atoms2.add(atom)
atoms = atoms2
solve_for_p = sympy.solve(constant_of_motion.expr -
constant_of_motion.sym,
atoms,
dict=True,
simplify=False)
if len(solve_for_p) > 1:
raise ValueError
parameter_index, symmetry, replace_p = 0, 0, None
for parameter in solve_for_p[0].keys():
symmetry, symmetry_unit = noether(prob, constant_of_motion)
replace_p = parameter
for idx, state in enumerate(states_and_costates):
if state.sym == parameter:
parameter_index = idx
prob.parameters.append(
NamedDimensionalStruct(constant_of_motion.name,
constant_of_motion.units))
symmetry_index = parameter_index - len(prob.states)
# Derive the quad
# Evaluate int(pdq) = int(PdQ)
n = len(prob.quads)
symmetry_symbol = sympy.Symbol('_q_{}'.format(n))
_lhs = constant_of_motion.expr / constant_of_motion.sym * symmetry
lhs = 0
for idx, state in enumerate(states_and_costates):
lhs += sympy.integrate(_lhs[idx], state.sym)
lhs, _ = recursive_sub(lhs, solve_for_p[0])
state_syms = extract_syms(prob.states)
costates_syms = extract_syms(prob.costates)
parameter_syms = extract_syms(prob.parameters)
constant_syms = extract_syms(prob.constants)
# the_p = [constant_of_motion.sym]
fn_q = prob.lambdify(
[state_syms, costates_syms, parameter_syms, constant_syms], lhs)
replace_q = states_and_costates[symmetry_index].sym
solve_for_q = sympy.solve(lhs - symmetry_symbol,
replace_q,
dict=True,
simplify=False)
# Evaluate X_H(pi(., c)), pi = O^sharp
omega = prob.omega.tomatrix()
rvec = sympy.Matrix(([0] * len(states_and_costates)))
for ii, state_1 in enumerate(states_and_costates):
for jj, state_2 in enumerate(states_and_costates):
rvec[ii] += omega[ii, jj] * sympy.diff(constant_of_motion.expr,
state_2.sym)
symmetry_eom = sym_zero
for idx, state in enumerate(states_and_costates):
symmetry_eom += state.eom * rvec[idx]
# TODO: Figure out how to find units of the quads. This is only works in some specialized cases.
symmetry_unit = states_and_costates[symmetry_index].units
prob.quads.append(
DynamicStruct(str(symmetry_symbol),
symmetry_eom,
symmetry_unit,
local_compiler=prob.local_compiler))
for idx, state in enumerate(states_and_costates):
state.eom, _ = recursive_sub(state.eom, solve_for_p[0])
state.eom, _ = recursive_sub(state.eom, solve_for_q[0])
for idx, bc in enumerate(prob.constraints['initial'] +
prob.constraints['terminal']):
bc.expr, _ = recursive_sub(bc.expr, solve_for_p[0])
bc.expr, _ = recursive_sub(bc.expr, solve_for_q[0])
for idx, law in enumerate(prob.control_law):
for jj, symbol in enumerate(law.keys()):
prob.control_law[idx][symbol], _ = recursive_sub(
prob.sympify(law[symbol]), solve_for_p[0])
# prob.control_law[idx][symbol] = law[symbol]
prob.control_law[idx][symbol], _ = recursive_sub(
prob.sympify(law[symbol]), solve_for_q[0])
# prob.control_law[idx][symbol] = law[symbol]
for idx, com in enumerate(prob.constants_of_motion):
if idx != self.com_index:
com.expr, _ = recursive_sub(com.expr, solve_for_p[0])
com.expr, _ = recursive_sub(com.expr, solve_for_q[0])
remove_parameter = states_and_costates[parameter_index]
remove_symmetry = states_and_costates[symmetry_index]
remove_parameter_dict = {'location': None, 'index': None}
if remove_parameter in prob.states:
remove_parameter_dict = {
'location': 'states',
'index': prob.states.index(remove_parameter)
}
prob.states.remove(remove_parameter)
if remove_parameter in prob.costates:
remove_parameter_dict = {
'location': 'costates',
'index': prob.costates.index(remove_parameter)
}
prob.costates.remove(remove_parameter)
remove_symmetry_dict = {'location': None, 'index': None}
if remove_symmetry in prob.states:
remove_symmetry_dict = {
'location': 'states',
'index': prob.states.index(remove_symmetry)
}
prob.states.remove(remove_symmetry)
if remove_symmetry in prob.costates:
remove_symmetry_dict = {
'location': 'costates',
'index': prob.costates.index(remove_symmetry)
}
prob.costates.remove(remove_symmetry)
omega = prob.omega.tomatrix()
if parameter_index > symmetry_index:
omega.row_del(parameter_index)
omega.col_del(parameter_index)
omega.row_del(symmetry_index)
omega.col_del(symmetry_index)
else:
omega.row_del(symmetry_index)
omega.col_del(symmetry_index)
omega.row_del(parameter_index)
omega.col_del(parameter_index)
prob.omega = sympy.MutableDenseNDimArray(omega)
del prob.constants_of_motion[self.com_index]
state_syms = extract_syms(prob.states)
costates_syms = extract_syms(prob.costates)
parameter_syms = extract_syms(prob.parameters)
constant_syms = extract_syms(prob.constants)
quad_syms = extract_syms(prob.quads)
fn_q_inv = prob.lambdify([
state_syms, costates_syms, quad_syms, parameter_syms, constant_syms
], solve_for_q[0][replace_q])
fn_p_inv = prob.lambdify(
[state_syms, costates_syms, parameter_syms, constant_syms],
solve_for_p[0][replace_p])
prob.sol_map_chain.append(
MF(remove_parameter_dict, remove_symmetry_dict, fn_p, fn_q,
fn_p_inv, fn_q_inv))
return prob
def transformation(self, prob: Problem) -> Problem:
if not prob.dualized:
raise ValueError('Problem must be dualized.')
_dynamic_structs = [prob.states, prob.costates]
state_syms = sympy.Matrix(
extract_syms(combine_component_lists(_dynamic_structs)))
control_syms = sympy.Matrix(extract_syms(prob.controls))
eom = sympy.Matrix(
[state.eom for state in combine_component_lists(_dynamic_structs)])
g = sympy.Matrix(
[prob.hamiltonian.expr.diff(u_k) for u_k in control_syms])
dg_dx = g.jacobian(state_syms)
dg_du = g.jacobian(control_syms)
u_dot = dg_du.LUsolve(-dg_dx *
eom) # dg_du * u_dot + dg_dx * x_dot = 0
if sympy.zoo in u_dot.atoms():
raise NotImplementedError(
'Complex infinity in differential control law. Potential bang-bang solution.'
)
for g_k, control in zip(g, prob.controls):
constraint = DimensionalExpressionStruct(
g_k,
prob.hamiltonian.units / control.units,
local_compiler=prob.local_compiler)
prob.constraints['terminal'].append(constraint)
control_idxs = []
if self.method.lower() == 'traditional':
for control_rate in u_dot:
control = prob.controls.pop(0)
control_idxs.append(len(prob.states))
prob.states.append(
DynamicStruct(
control.name,
control_rate,
control.units,
local_compiler=prob.local_compiler).sympify_self())
prob.sol_map_chain.append(
DifferentialControlMapper(control_idxs=control_idxs))
elif self.method.lower() == 'diffyg':
independent_index = len(prob.states) - 1
control_costates = []
for control, control_rate in zip(prob.controls, u_dot):
control_idxs.append(len(prob.states))
prob.states.append(
DynamicStruct(
control.name,
control_rate,
control.units,
local_compiler=prob.local_compiler).sympify_self())
lam_name = '_lam_{}'.format(control.name)
lam = DynamicStruct(
lam_name,
'0',
prob.cost.units / control.units,
local_compiler=prob.local_compiler).sympify_self()
prob.costates.append(lam)
control_costates.append(lam)
omega_new = make_standard_symplectic_form(prob.states,
prob.costates)
n_states = len(prob.states)
n_controls = len(prob.controls)
# Add (du - u' dt) ^ (dlamU - 0 dt) to omega
for idx, u_dot_i in enumerate(u_dot):
omega_new[int(n_states - n_controls + idx),
int(2 * n_states - n_controls + idx)] = 1
omega_new[int(2 * n_states - n_controls + idx),
int(n_states - n_controls + idx)] = -1
omega_new[independent_index,
int(2 * n_states - n_controls + idx)] = -u_dot_i
omega_new[int(2 * n_states - n_controls + idx),
independent_index] = u_dot_i
prob.omega = omega_new
chi_h = make_hamiltonian_vector_field(
prob.hamiltonian.expr, omega_new,
extract_syms(prob.states + prob.costates))
for idx, state in enumerate(prob.states + prob.costates):
state.eom = chi_h[idx]
for lam_u in control_costates:
prob.constraints['initial'].append(
DimensionalExpressionStruct(
lam_u.sym,
lam_u.units,
local_compiler=prob.local_compiler))
prob.controls = []
prob.sol_map_chain.append(
DifferentialControlMapperDiffyG(control_idxs=control_idxs))
else:
raise NotImplementedError(
'Method {} not implemented for differential control'.format(
self.method))
prob.prob_type = 'prob'
return prob
def transformation(self, prob: Problem) -> Problem:
if not prob.sympified:
raise ValueError(
'Problem must be sympified. Hint: did you preprocess the problem?'
)
ocp = copy.deepcopy(prob)
for idx, constraint in enumerate(prob.constraints['initial']):
nu_name = '_nu_0_{}'.format(idx)
nu = NamedDimensionalStruct(
nu_name,
prob.cost.units / constraint.units,
local_compiler=prob.local_compiler).sympify_self()
prob.constraint_adjoints.append(nu)
prob.cost.initial += nu.sym * constraint.expr
for idx, constraint in enumerate(prob.constraints['terminal']):
nu_name = '_nu_f_{}'.format(idx)
nu = NamedDimensionalStruct(
nu_name,
prob.cost.units / constraint.units,
local_compiler=prob.local_compiler).sympify_self()
prob.constraint_adjoints.append(nu)
prob.cost.terminal += nu.sym * constraint.expr
# Make costates TODO Check if quads need costates
prob.hamiltonian = \
DimensionalExpressionStruct(prob.cost.path, prob.cost.units
/ prob.independent_variable.units).sympify_self()
for state in prob.states:
lam_name = '_lam_{}'.format(state.name)
lam = DynamicStruct(
lam_name,
'0',
prob.cost.units / state.units,
local_compiler=prob.local_compiler).sympify_self()
prob.costates.append(lam)
prob.hamiltonian.expr += lam.sym * state.eom
symmetry_costate_addition = np.array([sym_zero for _ in prob.costates])
for idx, symmetry in enumerate(prob.symmetries):
symmetry.field = np.concatenate(
(symmetry.field, symmetry_costate_addition))
# Handle coparameters
for parameter in prob.parameters:
lam_name = '_lam_{}'.format(parameter.name)
prob.coparameters.append(
DynamicStruct(lam_name, '0',
prob.cost.units / parameter.units))
prob.constants_of_motion.append(
NamedDimensionalExpressionStruct(
'hamiltonian',
prob.hamiltonian.expr,
prob.hamiltonian.units,
local_compiler=prob.local_compiler).sympify_self())
if self.method.lower() == 'traditional':
for state, costate in zip(prob.states + prob.parameters,
prob.costates + prob.coparameters):
costate.eom = -prob.hamiltonian.expr.diff(state.sym)
elif self.method.lower() == 'diffyg':
state_syms = extract_syms(prob.states + prob.parameters)
costate_syms = extract_syms(prob.costates + prob.coparameters)
omega = make_standard_symplectic_form(state_syms, costate_syms)
chi_h = make_hamiltonian_vector_field(prob.hamiltonian.expr, omega,
state_syms + costate_syms)
costate_rates = chi_h[-len(prob.states):]
for costate, rate in zip(prob.costates, costate_rates):
costate.eom = rate
prob.omega = omega
for idx, symmetry in enumerate(prob.symmetries):
g_star, units = noether(prob, symmetry)
prob.constants_of_motion.append(
NamedDimensionalExpressionStruct('com_{}'.format(idx),
g_star, units))
# Make costate constraints
for state, costate in zip(prob.states + prob.parameters,
prob.costates + prob.coparameters):
constraint_expr = costate.sym + prob.cost.initial.diff(state.sym)
prob.constraints['initial'].append(
DimensionalExpressionStruct(
constraint_expr,
costate.units,
local_compiler=prob.local_compiler))
constraint_expr = costate.sym - prob.cost.terminal.diff(state.sym)
prob.constraints['terminal'].append(
DimensionalExpressionStruct(
constraint_expr,
costate.units,
local_compiler=prob.local_compiler))
# TODO: Check Placement of Hamiltonian Constraint Placement
# Make time/Hamiltonian constraints
# constraint_expr = prob.cost.initial.diff(prob.independent_variable.sym) - prob.hamiltonian.expr
# prob.constraints['initial'].append(DimensionalExpressionStruct(
# constraint_expr, prob.hamiltonian.units, local_compiler=prob.local_compiler))
constraint_expr = prob.cost.terminal.diff(
prob.independent_variable.sym) + prob.hamiltonian.expr
prob.constraints['terminal'].append(
DimensionalExpressionStruct(constraint_expr,
prob.hamiltonian.units,
local_compiler=prob.local_compiler))
prob.sol_map_chain.append(
DualizeMapper(len(prob.costates), len(prob.constraint_adjoints),
ocp))
prob.dualized = True
return prob