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