def test_is_symplectic():
basis = [sympy.Symbol('x'), sympy.Symbol('y')]
make_standard_symplectic_form([basis[0]], [basis[1]])
omega = make_standard_symplectic_form([basis[0]], [basis[1]])
assert is_symplectic(omega)
omega[0, 0] = 1
assert not is_symplectic(omega)
omega = make_standard_symplectic_form([basis[0]], [basis[1]])
omega[0, 1] = -omega[0, 1]
assert not is_symplectic(omega)
def test_make_standard_symplectic_form():
basis = [sympy.Symbol('x'), sympy.Symbol('y')]
omega = make_standard_symplectic_form([basis[0]], [basis[1]])
assert len(omega.shape) == 2
assert omega.shape[0] == omega.shape[1] == 2
assert abs(omega[0, 1]) == 1
assert abs(omega[1, 0]) == 1
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
