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 compile_scaling(self):
lambdify = self.lambdify
units = self.prob.units
units_syms = getattr_from_list(units, 'sym')
units_func = getattr_from_list(units, 'expr')
compute_unit_factors = lambdify(self._units_args, units_func)
scalable_components_list = [
self.prob.independent_variable, self.prob.states, self.prob.quads,
self.prob.controls, self.prob.parameters,
self.prob.constraint_parameters, self.prob.constants
]
compute_factors = [
lambdify(units_syms, getattr_from_list(component, 'units'))
for component in scalable_components_list
]
def compute_scale_factors(sol: Trajectory):
ref_vals = tuple(
[max_mag(_arr) for _arr in [sol.t, sol.y, sol.q]] +
[np.fabs(_arr) for _arr in [sol.p, sol.nu, sol.k]])
unit_factors = compute_unit_factors(*ref_vals)
return tuple([
compute_factor(*unit_factors)
for compute_factor in compute_factors
])
def scale_sol(sol: Trajectory, scale_factors, inv=False):
sol = copy.deepcopy(sol)
if inv:
op = np.multiply
else:
op = np.divide
sol.t = op(sol.t, scale_factors[0])
sol.y = op(sol.y, scale_factors[1])
sol.q = op(sol.q, scale_factors[2])
if sol.u.size > 0:
sol.u = op(sol.u, scale_factors[3])
sol.p = op(sol.p, scale_factors[4])
sol.nu = op(sol.nu, scale_factors[5])
sol.k = op(sol.k, scale_factors[6])
return sol
self.compute_scale_factors = compute_scale_factors
self.scale_sol = scale_sol
return self
def init(self, gamma, bvp):
self.bvp = bvp
# for var_name in self.var_iterator():
# if var_name not in [str(s['symbol']) for s in self.prob.get_constants()]:
# raise ValueError('Variable ' + var_name + ' not found in boundary value problem')
gamma_in = copy.deepcopy(gamma)
gamma_in.converged = False
self.gammas = [gamma_in]
self.constant_names = getattr_from_list(bvp.constants, 'name')
def compile_deriv_func(self, use_control_arg=False):
sym_eom = getattr_from_list(self.prob.states, 'eom')
sym_eom_q = getattr_from_list(self.prob.quads, 'eom')
if self.compute_u is None:
if use_control_arg:
_args = self._dynamic_args_w_controls
else:
_args = self._dynamic_args
self.compute_y_dot = self.lambdify(_args, sym_eom)
if len(sym_eom_q) > 0:
self.compute_q_dot = self.lambdify(_args, sym_eom_q)
self.deriv_func, self.quad_func = self.compute_y_dot, self.compute_q_dot
else:
self.compute_y_dot = self.lambdify(self._dynamic_args_w_controls,
sym_eom)
compute_u = self.compute_u
compute_y_dot = self.compute_y_dot
def deriv_func(_y, _p, _k):
_u = compute_u(_y, _p, _k)
return np.array(compute_y_dot(_y, _u, _p, _k))
self.deriv_func = jit_compile_func(deriv_func, self._dynamic_args)
if len(sym_eom_q) > 0:
self.compute_q_dot = self.lambdify(
self._dynamic_args_w_controls, sym_eom_q)
compute_q_dot = self.compute_q_dot
def quad_func(_y, _p, _k):
_u = compute_u(_y, _p, _k)
return np.array(compute_q_dot(_y, _u, _p, _k))
self.quad_func = jit_compile_func(quad_func,
self._dynamic_args)
return self.deriv_func, self.quad_func
def match_constants_to_states(prob: SymbolicProblem, sol: trajectory):
state_names = getattr_from_list(prob.states + [prob.independent_variable], 'name')
initial_states = np.hstack((sol.y[0, :], sol.t[0]))
terminal_states = np.hstack((sol.y[-1, :], sol.t[-1]))
initial_bc = dict(zip(state_names, initial_states))
terminal_bc = dict(zip(state_names, terminal_states))
constant_names = getattr_from_list(prob.constants, 'name')
for ii, bc0 in enumerate(initial_bc):
if bc0 + '_0' in constant_names:
jj = getattr_from_list(prob.constants, 'name').index(bc0 + '_0')
sol.k[jj] = initial_bc[bc0]
for ii, bcf in enumerate(terminal_bc):
if bcf + '_f' in constant_names:
jj = getattr_from_list(prob.constants, 'name').index(bcf + '_f')
sol.k[jj] = terminal_bc[bcf]
return sol
def init(self, sol, bvp):
super(SparseProductStrategy, self).init(sol, bvp)
lower_bounds = []
upper_bounds = []
num_vars = len(self.vars)
for var_name in self.vars.keys():
jj = getattr_from_list(bvp.constants, 'name').index(var_name)
self.vars[var_name].value = sol.k[jj]
lower_bounds.append(sol.k[jj])
upper_bounds.append(self.vars[var_name].target)
major_num_steps = self._num_subdivisions[0]
minor_num_steps = self._num_subdivisions[1]
step_list = [lower_bounds]
major_point_cloud = [lower_bounds]
minor_point_cloud = []
for n in range(num_vars):
new_maj_pnts = []
maj_step_series = np.linspace(lower_bounds[n], upper_bounds[n],
major_num_steps[n])
for maj_point in major_point_cloud:
maj_pnt_line = []
for maj_step in maj_step_series:
new_pnt = list(maj_point)
new_pnt[n] = maj_step
maj_pnt_line.append(tuple(new_pnt))
for maj_pnt_0, maj_pnt_1 in zip(maj_pnt_line[:-1],
maj_pnt_line[1:]):
min_step_series = np.linspace(maj_pnt_0[n],
maj_pnt_1[n],
minor_num_steps[n] + 1,
endpoint=False)[1:]
for min_step in min_step_series:
new_point = list(maj_pnt_0)
new_point[n] = min_step
step_list.append(tuple(new_point))
minor_point_cloud.append(tuple(new_point))
step_list.append(maj_pnt_1)
new_maj_pnts += maj_pnt_line[1:]
major_point_cloud += new_maj_pnts
self.num_cases(len(step_list))
for step in step_list:
for n, var_name in enumerate(self.vars.keys()):
self.vars[var_name].steps.append(step[n])
def init(self, sol, bvp):
super(ProductStrategy, self).init(sol, bvp)
num_vars = len(self.vars)
self.num_cases(self.num_subdivisions()**num_vars)
for var_name in self.vars.keys():
jj = getattr_from_list(bvp.constants, 'name').index(var_name)
self.vars[var_name].value = sol.k[jj]
ls_set = [
np.linspace(self.vars[var_name].value, self.vars[var_name].target,
self._num_subdivisions)
for var_name in self.vars.keys()
]
for val in itertools.product(*ls_set):
ii = 0
for var_name in self.vars.keys():
self.vars[var_name].steps.append(val[ii])
ii += 1
def solve(autoscale=True, bvp=None, bvp_algorithm=None, guess_generator=None, initial_helper=False,
method='traditional', n_cpus=1, ocp=None, ocp_transform=None, ocp_inv_transform=None,
ocp_inv_transform_many=None, optim_options=None, steps=None, save_sols=True):
if optim_options is None:
optim_options = {}
if logger.level <= logging.DEBUG:
logger.debug(make_a_splash())
"""
Error checking
"""
if n_cpus < 1:
raise ValueError('Number of cpus must be greater than 1.')
if n_cpus > 1:
pool = pathos.multiprocessing.Pool(processes=n_cpus)
else:
pool = None
if ocp is None:
raise NotImplementedError('\"ocp\" must be defined.')
"""
Main code
"""
# f_ocp = compile_direct(ocp)
logger.debug('Using ' + str(n_cpus) + '/' + str(pathos.multiprocessing.cpu_count()) + ' CPUs. ')
if bvp is None:
preprocessor = make_preprocessor()
processed_ocp = preprocessor(copy.deepcopy(ocp))
if method.lower() in ['indirect', 'traditional', 'brysonho']:
method = 'traditional'
if method.lower() in ['traditional', 'diffyg']:
RFfunctor = make_indirect_method(copy.deepcopy(processed_ocp), method=method, **optim_options)
prob = RFfunctor(copy.deepcopy(processed_ocp))
elif method == 'direct':
functor = make_direct_method(copy.deepcopy(processed_ocp), **optim_options)
prob = functor(copy.deepcopy(processed_ocp))
else:
raise NotImplementedError
postprocessor = make_postprocessor()
bvp = postprocessor(copy.deepcopy(prob))
logger.debug('Resulting BVP problem:')
logger.debug(bvp.__repr__())
ocp_transform = bvp.map_sol
ocp_inv_transform = bvp.inv_map_sol
else:
if ocp_transform is None or ocp_inv_transform is None or ocp_inv_transform_many is None:
raise ValueError('BVP problem must have an associated \'ocp_map\' and \'ocp_map_inverse\'')
solinit = Trajectory()
solinit.k = np.array(getattr_from_list(bvp.constants, 'default_val'))
solinit = guess_generator.generate(bvp.functional_problem, solinit, ocp_transform, ocp_inv_transform)
if initial_helper:
sol_ocp = copy.deepcopy(solinit)
sol_ocp = match_constants_to_states(ocp, ocp_inv_transform(sol_ocp))
solinit.k = sol_ocp.k
if bvp.functional_problem.compute_u is not None:
u = np.array([bvp.functional_problem.compute_u(solinit.y[0], solinit.p, solinit.k)])
for ii in range(len(solinit.t) - 1):
u = np.vstack(
(u, bvp.functional_problem.compute_u(solinit.y[ii + 1], solinit.p, solinit.k)))
solinit.u = u
"""
Main continuation process
"""
time0 = time.time()
continuation_set = run_continuation_set(bvp_algorithm, steps, solinit, bvp, pool, autoscale)
total_time = time.time() - time0
logger.info('Continuation process completed in %0.4f seconds.\n' % total_time)
bvp_algorithm.close()
"""
Post processing and output
"""
out = postprocess_continuations(continuation_set, ocp_inv_transform)
if pool is not None:
pool.close()
if save_sols or (isinstance(save_sols, str)):
if isinstance(save_sols, str):
filename = save_sols
else:
filename = 'data.json'
save(out, filename=filename)
return out
def compile_bc(self, use_quad_arg=False):
sym_initial_bc = getattr_from_list(self.prob.constraints['initial'],
'expr')
sym_terminal_bc = getattr_from_list(self.prob.constraints['terminal'],
'expr')
if use_quad_arg:
if self.compute_u is None:
_args = self._bc_args_w_quads
compute_initial_bc = self.lambdify(_args, sym_initial_bc)
compute_terminal_bc = self.lambdify(_args, sym_terminal_bc)
def bc_func(_y0, _q0, _yf, _qf, _p, _p_con, _k):
bc_0 = np.array(
compute_initial_bc(_y0, _q0, _p, _p_con, _k))
bc_f = np.array(
compute_terminal_bc(_yf, _qf, _p, _p_con, _k))
return np.concatenate((bc_0, bc_f))
else:
_args = self._bc_args_w_quads_controls
compute_initial_bc = self.lambdify(_args, sym_initial_bc)
compute_terminal_bc = self.lambdify(_args, sym_terminal_bc)
compute_u = self.compute_u
def bc_func(_y0, _q0, _yf, _qf, _p, _p_con, _k):
_u0 = compute_u(_y0, _p, _k)
_uf = compute_u(_yf, _p, _k)
bc_0 = np.array(
compute_initial_bc(_y0, _q0, _u0, _p, _p_con, _k))
bc_f = np.array(
compute_terminal_bc(_yf, _qf, _uf, _p, _p_con, _k))
return np.concatenate((bc_0, bc_f))
_combined_args = \
([self.prob.independent_variable.sym] + self._state_syms + self._quad_syms) * 2 \
+ self._parameter_syms + self._constraint_parameters_syms + self._constant_syms
self.bc_func = jit_compile_func(bc_func, _combined_args)
self.compute_initial_bc, self.compute_terminal_bc = compute_initial_bc, compute_terminal_bc
else:
if self.compute_u is None:
_args = self._bc_args
compute_initial_bc = self.lambdify(_args, sym_initial_bc)
compute_terminal_bc = self.lambdify(_args, sym_terminal_bc)
def bc_func(_y0, _yf, _p, _p_con, _k):
bc_0 = np.array(compute_initial_bc(_y0, _p, _p_con, _k))
bc_f = np.array(compute_terminal_bc(_yf, _p, _p_con, _k))
return np.concatenate((bc_0, bc_f))
else:
_args = self._bc_args_w_controls
compute_initial_bc = self.lambdify(_args, sym_initial_bc)
compute_terminal_bc = self.lambdify(_args, sym_terminal_bc)
compute_u = self.compute_u
def bc_func(_y0, _yf, _p, _p_con, _k):
_u0 = compute_u(_y0, _p, _k)
_uf = compute_u(_yf, _p, _k)
bc_0 = np.array(
compute_initial_bc(_y0, _u0, _p, _p_con, _k))
bc_f = np.array(
compute_terminal_bc(_yf, _uf, _p, _p_con, _k))
return np.concatenate((bc_0, bc_f))
_combined_args = [self._state_syms] * 2 + [self._parameter_syms] + [self._constraint_parameters_syms] \
+ [self._constant_syms]
self.bc_func = jit_compile_func(bc_func, _combined_args)
self.compute_initial_bc, self.compute_terminal_bc = compute_initial_bc, compute_terminal_bc
return self.bc_func