def test_products():
assert TensorProduct(R2.dx, R2.dy)(
R2.e_x, R2.e_y) == R2.dx(R2.e_x) * R2.dy(R2.e_y) == 1
assert TensorProduct(R2.dx, R2.dy)(None, R2.e_y) == R2.dx
assert TensorProduct(R2.dx, R2.dy)(R2.e_x, None) == R2.dy
assert TensorProduct(R2.dx, R2.dy)(R2.e_x) == R2.dy
assert TensorProduct(R2.x, R2.dx) == R2.x * R2.dx
assert TensorProduct(R2.e_x, R2.e_y)(
R2.x, R2.y) == R2.e_x(R2.x) * R2.e_y(R2.y) == 1
assert TensorProduct(R2.e_x, R2.e_y)(None, R2.y) == R2.e_x
assert TensorProduct(R2.e_x, R2.e_y)(R2.x, None) == R2.e_y
assert TensorProduct(R2.e_x, R2.e_y)(R2.x) == R2.e_y
assert TensorProduct(R2.x, R2.e_x) == R2.x * R2.e_x
assert TensorProduct(R2.dx, R2.e_y)(
R2.e_x, R2.y) == R2.dx(R2.e_x) * R2.e_y(R2.y) == 1
assert TensorProduct(R2.dx, R2.e_y)(None, R2.y) == R2.dx
assert TensorProduct(R2.dx, R2.e_y)(R2.e_x, None) == R2.e_y
assert TensorProduct(R2.dx, R2.e_y)(R2.e_x) == R2.e_y
assert TensorProduct(R2.x, R2.e_x) == R2.x * R2.e_x
assert TensorProduct(R2.e_x, R2.dy)(
R2.x, R2.e_y) == R2.e_x(R2.x) * R2.dy(R2.e_y) == 1
assert TensorProduct(R2.e_x, R2.dy)(None, R2.e_y) == R2.e_x
assert TensorProduct(R2.e_x, R2.dy)(R2.x, None) == R2.dy
assert TensorProduct(R2.e_x, R2.dy)(R2.x) == R2.dy
assert TensorProduct(R2.e_y, R2.e_x)(R2.x**2 + R2.y**2,
R2.x**2 + R2.y**2) == 4 * R2.x * R2.y
assert WedgeProduct(R2.dx, R2.dy)(R2.e_x, R2.e_y) == 1
assert WedgeProduct(R2.e_x, R2.e_y)(R2.x, R2.y) == 1
def test_functional_diffgeom_ch6():
u0, u1, u2, v0, v1, v2, w0, w1, w2 = symbols('u0:3, v0:3, w0:3', real=True)
u = u0*R2.e_x + u1*R2.e_y
v = v0*R2.e_x + v1*R2.e_y
wp = WedgeProduct(R2.dx, R2.dy)
assert wp(u, v) == u0*v1 - u1*v0
u = u0*R3_r.e_x + u1*R3_r.e_y + u2*R3_r.e_z
v = v0*R3_r.e_x + v1*R3_r.e_y + v2*R3_r.e_z
w = w0*R3_r.e_x + w1*R3_r.e_y + w2*R3_r.e_z
wp = WedgeProduct(R3_r.dx, R3_r.dy, R3_r.dz)
assert wp(
u, v, w) == Matrix(3, 3, [u0, u1, u2, v0, v1, v2, w0, w1, w2]).det()
a, b, c = symbols('a, b, c', cls=Function)
a_f = a(R3_r.x, R3_r.y, R3_r.z)
b_f = b(R3_r.x, R3_r.y, R3_r.z)
c_f = c(R3_r.x, R3_r.y, R3_r.z)
theta = a_f*R3_r.dx + b_f*R3_r.dy + c_f*R3_r.dz
dtheta = Differential(theta)
da = Differential(a_f)
db = Differential(b_f)
dc = Differential(c_f)
expr = dtheta - WedgeProduct(
da, R3_r.dx) - WedgeProduct(db, R3_r.dy) - WedgeProduct(dc, R3_r.dz)
assert expr.rcall(R3_r.e_x, R3_r.e_y) == 0
def test_products():
assert TensorProduct(R2.dx, R2.dy)(R2.e_x, R2.e_y) == R2.dx(R2.e_x)*R2.dy(R2.e_y) == 1
assert WedgeProduct(R2.dx, R2.dy)(R2.e_x, R2.e_y) == 1
assert TensorProduct(R2.dx, R2.dy)(None, R2.e_y) == R2.dx
assert TensorProduct(R2.dx, R2.dy)(R2.e_x, None) == R2.dy
assert TensorProduct(R2.dx, R2.dy)(R2.e_x) == R2.dy
assert TensorProduct(R2.x, R2.dx) == R2.x*R2.dx
def test_helpers_and_coordinate_dependent():
one_form = R2.dr + R2.dx
two_form = Differential(R2.x * R2.dr + R2.r * R2.dx)
three_form = Differential(R2.y * two_form) + Differential(
R2.x * Differential(R2.r * R2.dr))
metric = TensorProduct(R2.dx, R2.dx) + TensorProduct(R2.dy, R2.dy)
metric_ambig = TensorProduct(R2.dx, R2.dx) + TensorProduct(R2.dr, R2.dr)
misform_a = TensorProduct(R2.dr, R2.dr) + R2.dr
misform_b = R2.dr**4
misform_c = R2.dx * R2.dy
twoform_not_sym = TensorProduct(R2.dx, R2.dx) + TensorProduct(R2.dx, R2.dy)
twoform_not_TP = WedgeProduct(R2.dx, R2.dy)
one_vector = R2.e_x + R2.e_y
two_vector = TensorProduct(R2.e_x, R2.e_y)
three_vector = TensorProduct(R2.e_x, R2.e_y, R2.e_x)
two_wp = WedgeProduct(R2.e_x, R2.e_y)
assert covariant_order(one_form) == 1
assert covariant_order(two_form) == 2
assert covariant_order(three_form) == 3
assert covariant_order(two_form + metric) == 2
assert covariant_order(two_form + metric_ambig) == 2
assert covariant_order(two_form + twoform_not_sym) == 2
assert covariant_order(two_form + twoform_not_TP) == 2
assert contravariant_order(one_vector) == 1
assert contravariant_order(two_vector) == 2
assert contravariant_order(three_vector) == 3
assert contravariant_order(two_vector + two_wp) == 2
raises(ValueError, lambda: covariant_order(misform_a))
raises(ValueError, lambda: covariant_order(misform_b))
raises(ValueError, lambda: covariant_order(misform_c))
assert twoform_to_matrix(metric) == Matrix([[1, 0], [0, 1]])
assert twoform_to_matrix(twoform_not_sym) == Matrix([[1, 0], [1, 0]])
assert twoform_to_matrix(twoform_not_TP) == Matrix([[0, -1], [1, 0]])
raises(ValueError, lambda: twoform_to_matrix(one_form))
raises(ValueError, lambda: twoform_to_matrix(three_form))
raises(ValueError, lambda: twoform_to_matrix(metric_ambig))
raises(ValueError, lambda: metric_to_Christoffel_1st(twoform_not_sym))
raises(ValueError, lambda: metric_to_Christoffel_2nd(twoform_not_sym))
raises(ValueError, lambda: metric_to_Riemann_components(twoform_not_sym))
raises(ValueError, lambda: metric_to_Ricci_components(twoform_not_sym))
def ocp_to_bvp(ocp):
ws = init_workspace(ocp)
problem_name = ws['problem_name']
independent_variable = ws['independent_var']
independent_variable_units = ws['independent_var_units']
states = ws['states']
states_rates = ws['states_rates']
states_units = ws['states_units']
controls = ws['controls']
controls_units = ws['controls_units']
constants = ws['constants']
constants_units = ws['constants_units']
constants_values = ws['constants_values']
constants_of_motion = ws['constants_of_motion']
constants_of_motion_values = ws['constants_of_motion_values']
constants_of_motion_units = ws['constants_of_motion_units']
symmetries = ws['symmetries']
constraints = ws['constraints']
constraints_units = ws['constraints_units']
quantities = ws['quantities']
quantities_values = ws['quantities_values']
parameters = ws['parameters']
parameters_units = ws['parameters_units']
initial_cost = ws['initial_cost']
initial_cost_units = ws['initial_cost_units']
terminal_cost = ws['terminal_cost']
terminal_cost_units = ws['terminal_cost_units']
path_cost = ws['path_cost']
path_cost_units = ws['path_cost_units']
if initial_cost != 0:
cost_units = initial_cost_units
elif terminal_cost != 0:
cost_units = terminal_cost_units
elif path_cost != 0:
cost_units = path_cost_units * independent_variable_units
else:
raise ValueError(
'Initial, path, and terminal cost functions are not defined.')
dynamical_parameters = []
quantity_vars, quantity_list, derivative_fn = process_quantities(
quantities, quantities_values)
for var in quantity_vars.keys():
for ii in range(len(states_rates)):
states_rates[ii] = states_rates[ii].subs(Symbol(var),
quantity_vars[var])
Q = Manifold(states, 'State_Space')
E = Manifold(states + controls, 'Input_Space')
R = Manifold([independent_variable], 'Independent_Space')
tau_Q = FiberBundle(Q, R, 'State_Bundle')
tau_E = FiberBundle(E, R, 'Input_Bundle')
J1tau_Q = JetBundle(tau_Q, 1, 'Jet_Bundle')
num_states = len(states)
num_states_total = len(J1tau_Q.vertical.base_coords)
hamiltonian, hamiltonian_units, costates, costates_units = make_hamiltonian(
states, states_rates, states_units, path_cost, cost_units)
setx = dict(zip(states + costates, J1tau_Q.vertical.base_coords))
vector_names = [Symbol('D_' + str(x)) for x in states]
settangent = dict(
zip(vector_names, J1tau_Q.vertical.base_vectors[:num_states]))
# Change original terms to be written on manifolds so the diffy g calculations are handled properly
states = [x.subs(setx, simultaneous=True) for x in states]
states_rates = [x.subs(setx, simultaneous=True) for x in states_rates]
costates = J1tau_Q.vertical.base_coords[num_states:]
constants_of_motion_values = [
x.subs(setx, simultaneous=True) for x in constants_of_motion_values
]
symmetries = [x.subs(setx, simultaneous=True) for x in symmetries]
symmetries = [x.subs(settangent, simultaneous=True) for x in symmetries]
constraints['initial'] = [
x.subs(setx, simultaneous=True) for x in constraints['initial']
]
constraints['terminal'] = [
x.subs(setx, simultaneous=True) for x in constraints['terminal']
]
initial_cost = initial_cost.subs(setx, simultaneous=True)
terminal_cost = terminal_cost.subs(setx, simultaneous=True)
hamiltonian = hamiltonian.subs(setx, simultaneous=True)
coparameters = make_costate_names(parameters)
coparameters_units = [
path_cost_units / parameter_units
for parameter_units in parameters_units
]
coparameters_rates = make_costate_rates(hamiltonian, parameters,
coparameters, derivative_fn)
quads = []
quads_rates = []
quads_units = []
quads += coparameters
quads_rates += coparameters_rates
quads_units += coparameters_units
pi = 0
omega = 0
for ii in range(num_states):
pi += WedgeProduct(J1tau_Q.base_vectors[ii],
J1tau_Q.base_vectors[ii + num_states])
omega += WedgeProduct(J1tau_Q.base_oneforms[ii],
J1tau_Q.base_oneforms[ii + num_states])
state_costate_pairing = 0
for ii in range(num_states):
state_costate_pairing += TensorProduct(
J1tau_Q.base_coords[ii + num_states] *
J1tau_Q.base_vectors[ii]) + TensorProduct(
J1tau_Q.base_coords[ii] *
J1tau_Q.base_vectors[ii + num_states])
constant_2_value = {
c: v
for c, v in zip(constants_of_motion, constants_of_motion_values)
}
constant_2_units = {
c: u
for c, u in zip(constants_of_motion, constants_of_motion_units)
}
reduced_states = states + costates
original_states = copy.copy(reduced_states)
reduced_states_units = states_units + costates_units
state_2_units = {
x: u
for x, u in zip(reduced_states, reduced_states_units)
}
X_H = pi.rcall(None, hamiltonian)
equations_of_motion = [X_H.rcall(x) for x in J1tau_Q.vertical.base_coords]
augmented_initial_cost, augmented_initial_cost_units, initial_lm_params, initial_lm_params_units = make_augmented_cost(
initial_cost,
cost_units,
constraints,
constraints_units,
location='initial')
augmented_terminal_cost, augmented_terminal_cost_units, terminal_lm_params, terminal_lm_params_units = make_augmented_cost(
terminal_cost,
cost_units,
constraints,
constraints_units,
location='terminal')
dV_cost_initial = J1tau_Q.verticalexteriorderivative(
augmented_initial_cost)
dV_cost_terminal = J1tau_Q.verticalexteriorderivative(
augmented_terminal_cost)
bc_initial = constraints['initial'] + [
costate + dV_cost_initial.rcall(D_x) for costate, D_x in zip(
costates, J1tau_Q.vertical.base_vectors[:num_states])
] + [
coparameter - derivative_fn(augmented_initial_cost, parameter)
for parameter, coparameter in zip(parameters, coparameters)
]
bc_terminal = constraints['terminal'] + [
costate - dV_cost_terminal.rcall(D_x) for costate, D_x in zip(
costates, J1tau_Q.vertical.base_vectors[:num_states])
] + [
coparameter - derivative_fn(augmented_terminal_cost, parameter)
for parameter, coparameter in zip(parameters, coparameters)
]
bc_terminal = make_time_bc(constraints, hamiltonian, bc_terminal)
dHdu = make_dhdu(hamiltonian, controls, derivative_fn)
control_law = make_control_law(dHdu, controls)
tf_var = sympify('tf')
dynamical_parameters = [tf_var] + parameters
dynamical_parameters_units = [independent_variable_units
] + parameters_units
nondynamical_parameters = initial_lm_params + terminal_lm_params
nondynamical_parameters_units = initial_lm_params_units + terminal_lm_params_units
subalgebras = []
if len(constants_of_motion) > 0:
logging.info('Checking commutation relations... ')
num_consts = len(constants_of_motion)
commutation_relations = [[None for ii in range(num_consts)]
for jj in range(num_consts)]
for ii, c1 in enumerate(constants_of_motion_values):
for jj, c2 in enumerate(constants_of_motion_values):
commutation_relations[ii][jj] = pi.rcall(c1, c2)
for ii, c1 in enumerate(constants_of_motion_values):
subalgebras.append({constants_of_motion[ii]})
for jj in range(0, num_consts):
if commutation_relations[ii][jj] != 0:
subalgebras[-1] |= {constants_of_motion[jj]}
subalgebras = [
gs for ii, gs in enumerate(subalgebras)
if gs not in subalgebras[ii + 1:]
]
reducible_subalgebras = [len(gs) == 1 for gs in subalgebras]
logging.info('Done. ' + str(sum(reducible_subalgebras)) + ' of ' +
str(len(subalgebras)) +
' subalgebras are double reducible.')
for subalgebra in subalgebras:
free_vars = set()
dim = len(subalgebra)
if dim > 1:
raise NotImplementedError
for c in subalgebra:
free_vars |= constant_2_value[c].atoms(reduced_states[0])
logging.info('Attempting reduction of ' + str(subalgebra) + '...')
eq_set = [c - constant_2_value[c] for ii, c in enumerate(subalgebra)]
constants_sol = sympy.solve(eq_set,
list(free_vars),
dict=False,
minimal=True,
simplify=False)
for x in constants_sol:
reduced_states.remove(x)
quads.append(state_costate_pairing.rcall(x))
quant = [constant_2_value[c] for c in subalgebra][0]
quads_rates.append(J1tau_Q.flat(X_H).rcall(pi.rcall(None, quant)))
quads_units.append(state_2_units[state_costate_pairing.rcall(x)])
reduced_states.remove(state_costate_pairing.rcall(x))
temp = copy.copy(subalgebra)
temp2 = copy.copy(temp)
dynamical_parameters.append(temp.pop())
dynamical_parameters_units.append(constant_2_units[temp2.pop()])
hamiltonian = hamiltonian.subs(constants_sol, simultaneous=True)
pi = pi.subs(constants_sol, simultaneous=True
) # TODO: Also change the vectors and differential forms
X_H = pi.rcall(None, hamiltonian)
equations_of_motion = [X_H.rcall(x) for x in reduced_states]
for ii in range(len(quads_rates)):
quads_rates[ii] = quads_rates[ii].subs(constants_sol,
simultaneous=True)
for ii in range(len(control_law)):
for control in control_law[ii]:
control_law[ii][control] = control_law[ii][control].subs(
constants_sol, simultaneous=True)
for ii in range(len(bc_initial)):
bc_initial[ii] = bc_initial[ii].subs(constants_sol,
simultaneous=True)
for ii in range(len(bc_terminal)):
bc_terminal[ii] = bc_terminal[ii].subs(constants_sol,
simultaneous=True)
logging.info('Done.')
# Generate the problem data
control_law = [{str(u): str(law[u])
for u in law.keys()} for law in control_law]
out = {
'method':
'diffyg',
'problem_name':
problem_name,
'aux_list': [{
'type': 'const',
'vars': [str(k) for k in constants]
}],
'states': [str(x) for x in reduced_states],
'states_units': [str(state_2_units[x]) for x in reduced_states],
'deriv_list': [str(tf_var * rate) for rate in equations_of_motion],
'quads': [str(x) for x in quads],
'quads_rates': [str(tf_var * x) for x in quads_rates],
'quads_units': [str(x) for x in quads_units],
'constants': [str(c) for c in constants],
'constants_units': [str(c) for c in constants_units],
'constants_values': [float(c) for c in constants_values],
'constants_of_motion': [str(c) for c in constants_of_motion],
'dynamical_parameters': [str(c) for c in dynamical_parameters],
'dynamical_parameters_units':
[str(c) for c in dynamical_parameters_units],
'nondynamical_parameters': [str(c) for c in nondynamical_parameters],
'nondynamical_parameters_units':
[str(c) for c in nondynamical_parameters_units],
'independent_variable':
str(independent_variable),
'independent_variable_units':
str(independent_variable_units),
'control_list': [str(x) for x in it.chain(controls)],
'controls': [str(u) for u in controls],
'hamiltonian':
str(hamiltonian),
'hamiltonian_units':
str(hamiltonian_units),
'num_states':
len(reduced_states),
'dHdu':
str(dHdu),
'bc_initial': [str(_) for _ in bc_initial],
'bc_terminal': [str(_) for _ in bc_terminal],
'control_options':
control_law,
'num_controls':
len(controls)
}
states_2_constants_fn = [
make_jit_fn([str(x) for x in original_states], str(c))
for c in constants_of_motion_values
]
states_2_states_fn = [
make_jit_fn([str(x) for x in original_states], str(y))
for y in reduced_states
]
def guess_mapper(sol):
n_c = len(constants_of_motion)
if n_c == 0:
return sol
sol_out = Solution()
sol_out.t = copy.copy(sol.t)
sol_out.y = np.array([[fn(*sol.y[0]) for fn in states_2_states_fn]])
sol_out.q = sol.q
if len(quads) > 0:
sol_out.q = -0.0 * np.array([np.ones((len(quads)))])
sol_out.dynamical_parameters = sol.dynamical_parameters
sol_out.dynamical_parameters[-n_c:] = np.array(
[fn(*sol.y[0]) for fn in states_2_constants_fn])
sol_out.nondynamical_parameters = sol.nondynamical_parameters
sol_out.aux = sol.aux
return sol_out
return out, guess_mapper
Mat = sym.Matrix
Sym = sym.symbols
Half = Rat(1, 2)
Third = Rat(1, 3)
Quarter = Rat(1, 4)
def Rec(n):
return Rat(1, n)
from sympy.diffgeom.rn import R2_r
from sympy.diffgeom import WedgeProduct
ex, ey = R2_r.base_vectors()
dx, dy = R2_r.base_oneforms()
print(WedgeProduct(dx, dy))
print(WedgeProduct(dx, dy)(ex, ey))
J = Mat([[Sym("J_{}{}".format(i, j)) for j in range(1, 3)]
for i in range(1, 4)])
print_matrix(J)
print_matrix(J.T * J)
class k_vector:
def __init__(self, n):
self.n = n
def E(n, *vecs):
assert (len(vecs) <= n)
m.z = rect.coord_function(2)
m.e_x = BaseVectorField(rect, 0)
m.e_y = BaseVectorField(rect, 1)
m.e_z = BaseVectorField(rect, 2)
m.dx = Differential(m.x)
m.dy = Differential(m.y)
m.dz = Differential(m.z)
omega_x = Function('omega_x')
omega_y = Function('omega_y')
omega_z = Function('omega_z')
m.r = polar.coord_function(0)
m.phi = polar.coord_function(1)
m.theta = polar.coord_function(2)
m.e_r = BaseVectorField(polar, 0)
m.e_phi = BaseVectorField(polar, 1)
m.e_theta = BaseVectorField(polar, 2)
m.dr = Differential(m.r)
m.dphi = Differential(m.phi)
m.dtheta = Differential(m.theta)
omega_r = Function('omega_r')
omega_phi = Function('omega_phi')
omega_theta = Function('omega_theta')
omega = omega_x(m.x, m.y, m.z) * WedgeProduct(m.dy, m.dz) + omega_y(
m.x, m.y, m.z) * WedgeProduct(m.dz, m.dx) + omega_z(
m.x, m.y, m.z) * WedgeProduct(m.dx, m.dy)
d_omega = Differential(omega)
pprint(d_omega)
def ocp_to_bvp(ocp, **kwargs):
"""
Converts an OCP to a BVP using diffy G methods.
:param ocp: An OCP.
:return: bvp, map, map_inverse
"""
logging.warning(
'\'diffyg_deprecated\' method is deprecated and will be removed in a later version, use \'indirect\'.'
)
ws = init_workspace(ocp)
problem_name = ws['problem_name']
independent_variable = ws['independent_var']
independent_variable_units = ws['independent_var_units']
states = ws['states']
states_rates = ws['states_rates']
states_units = ws['states_units']
controls = ws['controls']
controls_units = ws['controls_units']
constants = ws['constants']
constants_units = ws['constants_units']
constants_values = ws['constants_values']
constants_of_motion = ws['constants_of_motion']
constants_of_motion_values = ws['constants_of_motion_values']
constants_of_motion_values_original = copy.deepcopy(
constants_of_motion_values)
constants_of_motion_units = ws['constants_of_motion_units']
symmetries = ws['symmetries']
constraints = ws['constraints']
constraints_units = ws['constraints_units']
constraints_lower = ws['constraints_lower']
constraints_upper = ws['constraints_upper']
constraints_activators = ws['constraints_activators']
constraints_method = ws['constraints_method']
switches = ws['switches']
switches_values = ws['switches_values']
switches_conditions = ws['switches_conditions']
switches_tolerance = ws['switches_tolerance']
parameters = ws['parameters']
parameters_units = ws['parameters_units']
initial_cost = ws['initial_cost']
initial_cost_units = ws['initial_cost_units']
terminal_cost = ws['terminal_cost']
terminal_cost_units = ws['terminal_cost_units']
path_cost = ws['path_cost']
path_cost_units = ws['path_cost_units']
if initial_cost != 0:
cost_units = initial_cost_units
elif terminal_cost != 0:
cost_units = terminal_cost_units
elif path_cost != 0:
cost_units = path_cost_units * independent_variable_units
else:
raise ValueError(
'Initial, path, and terminal cost functions are not defined.')
"""
Deal with path constraints
"""
for ii, c in enumerate(constraints['path']):
if constraints_method['path'] is None:
raise NotImplementedError
if constraints_method['path'][ii].lower() == 'utm':
path_cost += utm_path(c, constraints_lower['path'][ii],
constraints_upper['path'][ii],
constraints_activators['path'][ii])
elif constraints_method['path'][ii].lower() == 'epstrig':
path_cost += epstrig_path(c, constraints_lower['path'][ii],
constraints_upper['path'][ii],
constraints_activators['path'][ii])
upper = constraints_upper['path'][ii]
lower = constraints_lower['path'][ii]
subber = dict(
zip([constraints['path'][ii]],
[(upper - lower) / 2 * sympy.sin(constraints['path'][ii]) +
(upper + lower) / 2]))
for ii in range(len(states_rates)):
states_rates[ii] = states_rates[ii].subs(subber,
simultaneous=True)
else:
raise NotImplementedError('Unknown path constraint method \"' +
str(constraints_method['path'][ii]) +
'\"')
dynamical_parameters = []
"""
Deal with staging and switches
"""
for ii in range(len(switches)):
if isinstance(switches_values[ii], list):
true_value = 0
for jj in range(len(switches_values[ii])):
temp_value = switches_values[ii][jj]
for kk in range(len(switches_conditions[ii][jj])):
temp_value *= rash_mult(switches_conditions[ii][jj][kk],
switches_tolerance[ii])
true_value += temp_value
switches_values[ii] = true_value
"""
Make substitutions with the switches
"""
switch_vars, switch_list, derivative_fn = process_quantities(
switches, switches_values)
for var in switch_vars.keys():
initial_cost = initial_cost.subs(Symbol(var), switch_vars[var])
path_cost = path_cost.subs(Symbol(var), switch_vars[var])
terminal_cost = terminal_cost.subs(Symbol(var), switch_vars[var])
for ii in range(len(states_rates)):
states_rates[ii] = states_rates[ii].subs(Symbol(var),
switch_vars[var])
Q = Manifold(states, 'State_Space')
E = Manifold(states + controls, 'Input_Space')
R = Manifold([independent_variable], 'Independent_Space')
tau_Q = FiberBundle(Q, R, 'State_Bundle')
tau_E = FiberBundle(E, R, 'Input_Bundle')
J1tau_Q = JetBundle(tau_Q, 1, 'Jet_Bundle')
num_states = len(states)
num_states_total = len(J1tau_Q.vertical.base_coords)
hamiltonian, hamiltonian_units, costates, costates_units = make_hamiltonian(
states, states_rates, states_units, path_cost, cost_units)
setx = dict(zip(states + costates, J1tau_Q.vertical.base_coords))
vector_names = [Symbol('D_' + str(x)) for x in states]
settangent = dict(
zip(vector_names, J1tau_Q.vertical.base_vectors[:num_states]))
# Change original terms to be written on manifolds so the diffy g calculations are handled properly
states = [x.subs(setx, simultaneous=True) for x in states]
states_rates = [x.subs(setx, simultaneous=True) for x in states_rates]
costates = J1tau_Q.vertical.base_coords[num_states:]
constants_of_motion_values = [
x.subs(setx, simultaneous=True) for x in constants_of_motion_values
]
constants_of_motion_values_original = [
x.subs(setx, simultaneous=True)
for x in constants_of_motion_values_original
]
symmetries = [x.subs(setx, simultaneous=True) for x in symmetries]
symmetries = [x.subs(settangent, simultaneous=True) for x in symmetries]
constraints['initial'] = [
x.subs(setx, simultaneous=True) for x in constraints['initial']
]
constraints['terminal'] = [
x.subs(setx, simultaneous=True) for x in constraints['terminal']
]
initial_cost = initial_cost.subs(setx, simultaneous=True)
terminal_cost = terminal_cost.subs(setx, simultaneous=True)
hamiltonian = hamiltonian.subs(setx, simultaneous=True)
dhdt = derivative_fn(hamiltonian, independent_variable)
# if dhdt == 0:
# constants_of_motion = constants_of_motion + [Symbol('hamiltonian')]
# constants_of_motion_values = constants_of_motion_values + [hamiltonian]
# constants_of_motion_units = constants_of_motion_units + [hamiltonian_units]
coparameters = make_costate_names(parameters)
coparameters_units = [
path_cost_units / parameter_units
for parameter_units in parameters_units
]
coparameters_rates = make_costate_rates(hamiltonian, parameters,
coparameters, derivative_fn)
quads = []
quads_rates = []
quads_units = []
quads += coparameters
quads_rates += coparameters_rates
quads_units += coparameters_units
pi = 0
omega = 0
for ii in range(num_states):
# pi += WedgeProduct(J1tau_Q.base_vectors[ii], J1tau_Q.base_vectors[ii + num_states])
pi += TensorProduct(J1tau_Q.base_vectors[ii], J1tau_Q.base_vectors[ii + num_states]) - \
TensorProduct(J1tau_Q.base_vectors[ii + num_states], J1tau_Q.base_vectors[ii])
omega += WedgeProduct(J1tau_Q.base_oneforms[ii],
J1tau_Q.base_oneforms[ii + num_states])
state_costate_pairing = 0
for ii in range(num_states):
state_costate_pairing += TensorProduct(J1tau_Q.base_coords[ii + num_states]*J1tau_Q.base_vectors[ii]) + \
TensorProduct(J1tau_Q.base_coords[ii]*J1tau_Q.base_vectors[ii + num_states])
constant_2_units = {
c: u
for c, u in zip(constants_of_motion, constants_of_motion_units)
}
reduced_states = states + costates
original_states = copy.copy(reduced_states)
reduced_states_units = states_units + costates_units
state_2_units = {
x: u
for x, u in zip(reduced_states, reduced_states_units)
}
X_H = pi.rcall(None, hamiltonian)
equations_of_motion = [X_H.rcall(x) for x in J1tau_Q.vertical.base_coords]
augmented_initial_cost, augmented_initial_cost_units, initial_lm_params, initial_lm_params_units =\
make_augmented_cost(initial_cost, cost_units, constraints, constraints_units, location='initial')
augmented_terminal_cost, augmented_terminal_cost_units, terminal_lm_params, terminal_lm_params_units =\
make_augmented_cost(terminal_cost, cost_units, constraints, constraints_units, location='terminal')
dV_cost_initial = J1tau_Q.verticalexteriorderivative(
augmented_initial_cost)
dV_cost_terminal = J1tau_Q.verticalexteriorderivative(
augmented_terminal_cost)
bc_initial = constraints['initial'] + \
[costate + dV_cost_initial.rcall(D_x) for costate, D_x
in zip(costates, J1tau_Q.vertical.base_vectors[:num_states])] + \
[coparameter - derivative_fn(augmented_initial_cost, parameter) for parameter, coparameter
in zip(parameters, coparameters)]
bc_terminal = constraints['terminal'] + \
[costate - dV_cost_terminal.rcall(D_x) for costate, D_x
in zip(costates, J1tau_Q.vertical.base_vectors[:num_states])] + \
[coparameter - derivative_fn(augmented_terminal_cost, parameter) for parameter, coparameter
in zip(parameters, coparameters)]
time_bc = make_time_bc(constraints, derivative_fn, hamiltonian,
independent_variable)
if time_bc is not None:
bc_terminal += [time_bc]
dHdu = make_dhdu(hamiltonian, controls, derivative_fn)
control_law = make_control_law(dHdu, controls)
tf = sympify('_tf')
# bc_terminal = [bc.subs(independent_variable, tf, simultaneous=True) for bc in bc_terminal]
dynamical_parameters = parameters + [tf]
dynamical_parameters_units = parameters_units + [
independent_variable_units
]
nondynamical_parameters = initial_lm_params + terminal_lm_params
nondynamical_parameters_units = initial_lm_params_units + terminal_lm_params_units
subalgebras = []
if len(constants_of_motion) > 0:
logging.debug('Checking commutation relations... ')
num_consts = len(constants_of_motion)
commutation_relations = [[None for _ in range(num_consts)]
for _ in range(num_consts)]
for ii, c1 in enumerate(constants_of_motion_values):
for jj, c2 in enumerate(constants_of_motion_values):
commutation_relations[ii][jj] = pi.rcall(c1, c2)
for ii, c1 in enumerate(constants_of_motion_values):
subalgebras.append({constants_of_motion[ii]})
for jj in range(0, num_consts):
if commutation_relations[ii][jj] != 0:
subalgebras[-1] |= {constants_of_motion[jj]}
subalgebras = [
gs for ii, gs in enumerate(subalgebras)
if gs not in subalgebras[ii + 1:]
]
reducible_subalgebras = [len(gs) == 1 for gs in subalgebras]
logging.debug('Done. ' + str(sum(reducible_subalgebras)) + ' of ' +
str(len(subalgebras)) +
' subalgebras are double reducible.')
for subalgebra in subalgebras:
constant_2_value = {
c: v
for c, v in zip(constants_of_motion, constants_of_motion_values)
}
dim = len(subalgebra)
if dim > 1:
raise NotImplementedError
# G_name = 'G_' + str(subalgebra).replace('{','').replace('}','')
# G = LieGroup(list(subalgebra), G_name)
# h = constant_2_value[constants_of_motion[0]]
logging.debug('Attempting reduction of ' + str(subalgebra) + '...')
# M_r, phi = restrictor(M, dict([(x, constant_2_value[x]) for x in list(subalgebra)]))
free_vars = set()
const = dict([(x, constant_2_value[x]) for x in list(subalgebra)])
for c in const.keys():
free_vars |= const[c].atoms(reduced_states[0])
eq_set = [c - const[c] for c in const.keys()]
constants_sol = sympy.solve(eq_set,
list(free_vars),
dict=True,
minimal=True,
simplify=False)[0]
# hamiltonian = phi.pullback(hamiltonian)
# g_ii = 0
# for ii in range(len(M_r.base_coords)):
# for jj in range(len(M_r.base_coords)):
# g_ii += TensorProduct(M_r.base_oneforms[ii], M_r.base_oneforms[jj])
# omega = phi.pullback(omega)
# # pi = M_r.sharp(omega)
# constants_of_motion = [phi.pullback(c) for c in constants_of_motion]
# dH = M_r.exteriorderivative(hamiltonian)
# eoms = omega.rcall(None, M_r.sharp(dH))
# breakpoint()
# eps_c = omega.rcall(None, M_r.sharp(M_r.exteriorderivative(constants_of_motion[0])))
# quad_field = eoms.rcall(M_r.sharp(omega.rcall(None, constants_of_motion[0])))
# breakpoint()
#
# breakpoint()
for x in constants_sol:
reduced_states.remove(x)
quads.append(state_costate_pairing.rcall(x))
quant = [constant_2_value[c] for c in subalgebra][0]
quads_rates.append(J1tau_Q.flat(X_H).rcall(pi.rcall(None, quant)))
quads_units.append(state_2_units[state_costate_pairing.rcall(x)])
reduced_states.remove(state_costate_pairing.rcall(x))
temp = copy.copy(subalgebra)
temp2 = copy.copy(temp)
dynamical_parameters.append(temp.pop())
dynamical_parameters_units.append(constant_2_units[temp2.pop()])
hamiltonian = hamiltonian.subs(constants_sol, simultaneous=True)
pi = pi.subs(constants_sol, simultaneous=True
) # TODO: Also change the vectors and differential forms
X_H = pi.rcall(None, hamiltonian)
equations_of_motion = [X_H.rcall(x) for x in reduced_states]
for ii in range(len(quads_rates)):
quads_rates[ii] = quads_rates[ii].subs(constants_sol,
simultaneous=True)
for ii in range(len(control_law)):
for control in control_law[ii]:
control_law[ii][control] = control_law[ii][control].subs(
constants_sol, simultaneous=True)
for ii in range(len(constants_of_motion_values)):
constants_of_motion_values[ii] = constants_of_motion_values[
ii].subs(constants_sol, simultaneous=True)
for ii in range(len(bc_initial)):
bc_initial[ii] = bc_initial[ii].subs(constants_sol,
simultaneous=True)
for ii in range(len(bc_terminal)):
bc_terminal[ii] = bc_terminal[ii].subs(constants_sol,
simultaneous=True)
logging.debug('Done.')
# Generate the problem data
control_law = [{str(u): str(law[u])
for u in law.keys()} for law in control_law]
out = {
'method':
'diffyg',
'problem_name':
problem_name,
'control_method':
'',
'consts': [str(k) for k in constants],
'initial_cost':
None,
'initial_cost_units':
None,
'path_cost':
None,
'path_cost_units':
None,
'terminal_cost':
None,
'terminal_cost_units':
None,
'states': [str(x) for x in reduced_states],
'states_units': [str(state_2_units[x]) for x in reduced_states],
'states_rates': [str(tf * rate) for rate in equations_of_motion],
'states_jac': [None, None],
'quads': [str(x) for x in quads],
'quads_rates': [str(tf * x) for x in quads_rates],
'quads_units': [str(x) for x in quads_units],
'path_constraints': [],
'path_constraints_units': [],
'constants': [str(c) for c in constants],
'constants_units': [str(c) for c in constants_units],
'constants_values': [float(c) for c in constants_values],
'constants_of_motion': [str(c) for c in constants_of_motion],
'dynamical_parameters': [str(c) for c in dynamical_parameters],
'dynamical_parameters_units':
[str(c) for c in dynamical_parameters_units],
'nondynamical_parameters': [str(c) for c in nondynamical_parameters],
'nondynamical_parameters_units':
[str(c) for c in nondynamical_parameters_units],
'control_list': [str(x) for x in it.chain(controls)],
'controls': [str(u) for u in controls],
'hamiltonian':
str(hamiltonian),
'hamiltonian_units':
str(hamiltonian_units),
'num_states':
len(reduced_states),
'dHdu': [str(x) for x in dHdu],
'bc_initial': [str(_) for _ in bc_initial],
'bc_terminal': [str(_) for _ in bc_terminal],
'control_options':
control_law,
'num_controls':
len(controls)
}
states_2_constants_fn = [
make_jit_fn([str(x) for x in original_states + controls], str(c))
for c in constants_of_motion_values_original
]
states_2_reduced_states_fn = [
make_jit_fn([str(x) for x in original_states], str(y))
for y in reduced_states
]
constants_2_states_fn = [
make_jit_fn([str(x) for x in reduced_states + constants_of_motion],
str(y)) for y in constants_of_motion
]
def guess_map(sol, _compute_control=None):
if _compute_control is None:
raise ValueError(
'Guess mapper not properly set up. Bind the control law to keyword \'_compute_control\''
)
sol_out = copy.deepcopy(sol)
nodes = len(sol.t)
n_c = len(constants_of_motion)
sol_out.t = copy.copy(sol.t / sol.t[-1])
sol_out.y = np.array([[
fn(*sol.y[ii], *sol.dual[ii]) for fn in states_2_reduced_states_fn
] for ii in range(sol.t.size)])
sol_out.q = sol.q
if len(quads) > 0:
sol_out.q = -0.0 * np.array([np.ones((len(quads)))])
sol_out.dynamical_parameters = np.hstack(
(sol.dynamical_parameters, sol.t[-1],
np.array([
fn(*sol.y[0], *sol.dual[0], *sol.u[0])
for fn in states_2_constants_fn
])))
sol_out.nondynamical_parameters = np.ones(len(nondynamical_parameters))
sol_out.u = np.array([]).reshape((nodes, 0))
sol_out.const = sol.const
return sol_out
def guess_map_inverse(sol, _compute_control=None):
if _compute_control is None:
raise ValueError(
'Guess mapper not properly set up. Bind the control law to keyword \'_compute_control\''
)
sol_out = copy.deepcopy(sol)
sol_out.u = np.vstack([
_compute_control(yi, None, sol.dynamical_parameters, sol.const)
for yi in sol.y
])
return sol_out
return out, guess_map, guess_map_inverse
TensorProduct(R2.dx, R2.dy)(R2.e_y, R2.e_x) TensorProduct(R2.dx, R2.x*R2.dy)(R2.x*R2.e_x, R2.e_y) #You can nest tensor products. tp1 = TensorProduct(R2.dx, R2.dy) TensorProduct(tp1, R2.dx)(R2.e_x, R2.e_y, R2.e_x) #You can make partial contraction for instance when ‘raising an index’. Putting None in the second argument of rcall means that the respective position in the tensor product is left as it is. TP = TensorProduct metric = TP(R2.dx, R2.dx) + 3*TP(R2.dy, R2.dy) metric.rcall(R2.e_y, None) # WedgeProduct Examples from sympy import Function from sympy.diffgeom.rn import R2 from sympy.diffgeom import WedgeProduct from sympy import pprint WedgeProduct(R2.dx, R2.dy)(R2.e_x, R2.e_y) WedgeProduct(R2.dx, R2.dy)(R2.e_y, R2.e_x) WedgeProduct(R2.dx, R2.x*R2.dy)(R2.x*R2.e_x, R2.e_y) #You can nest wedge products wp1 = WedgeProduct(R2.dx, R2.dy) WedgeProduct(wp1, R2.dx)(R2.e_x, R2.e_y, R2.e_x) # LieDerivative Examples from sympy.diffgeom import (LieDerivative, TensorProduct) from sympy.diffgeom.rn import R2 LieDerivative(R2.e_x, R2.y) LieDerivative(R2.e_x, R2.x) LieDerivative(R2.e_x, R2.e_x) # The Lie derivative of a tensor field by another tensor field is equal to their commutator: LieDerivative(R2.e_x, R2.e_r) LieDerivative(R2.e_x + R2.e_y, R2.x) tp = TensorProduct(R2.dx, R2.dy)