def map(self,
sol: Solution,
control_costate: Union[float, np.ndarray] = 0.) -> Solution:
idx_u_list = []
for idx_u, (idx_y,
u) in enumerate(sorted(zip(self.control_idxs, sol.u.T))):
sol.y = np.insert(sol.y, idx_y, u, axis=1)
if isinstance(control_costate, Iterable):
if not isinstance(control_costate, np.ndarray):
control_costate = np.array(control_costate)
costate_insert = control_costate[idx_u] * np.ones_like(sol.t)
else:
costate_insert = control_costate * np.ones_like(sol.t)
sol.lam = np.insert(sol.lam, -1, costate_insert, axis=1)
if len(sol.lam_u) == 0:
sol.lam_u = np.array([costate_insert])
else:
sol.lam_u = np.insert(sol.lam_u, -1, costate_insert, axis=1)
idx_u_list.append(idx_u)
sol.u = np.delete(sol.u, idx_u_list, axis=1)
return sol
def map(self, sol: Solution) -> Solution:
idx_u_list = []
for idx_u, (idx_y,
u) in enumerate(sorted(zip(self.control_idxs, sol.u.T))):
sol.y = np.insert(sol.y, idx_y, u, axis=1)
idx_u_list.append(idx_u)
sol.u = np.delete(sol.u, idx_u_list, axis=1)
return sol
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
def test_composable_functors(method):
problem = Problem()
problem.independent('t', 's')
problem.state('x', 'v*cos(theta)', 'm')
problem.state('y', 'v*sin(theta)', 'm')
problem.state('v', 'g*sin(theta)', 'm/s')
problem.constant_of_motion('c1', 'lamX', 's/m')
problem.constant_of_motion('c2', 'lamY', 's/m')
problem.control('theta', 'rad')
problem.constant('g', -9.81, 'm/s^2')
problem.constant('x_f', 1, 'm')
problem.constant('y_f', -1, 'm')
problem.path_cost('1', '1')
problem.initial_constraint('x', 'm')
problem.initial_constraint('y', 'm')
problem.initial_constraint('v', 'm')
problem.terminal_constraint('x - x_f', 'm')
problem.terminal_constraint('y - y_f', 'm')
problem.scale(m='y', s='y/v', kg=1, rad=1, nd=1)
preprocessor = make_preprocessor()
indirect_method = make_indirect_method(problem, method=method)
bvp = indirect_method(preprocessor(problem))
mapper = bvp.map_sol
mapper_inv = bvp.inv_map_sol
gamma = Trajectory()
gamma.t = np.linspace(0, 1, num=10)
gamma.y = np.vstack([np.linspace(0, 0, num=10) for _ in range(3)]).T
gamma.lam = np.vstack([np.linspace(-1, -1, num=10) for _ in range(3)]).T
gamma.u = -np.pi / 2 * np.ones((10, 1))
gamma.k = np.array([-9.81, 1, -1])
g1 = mapper(copy.deepcopy(gamma))
g2 = mapper_inv(copy.deepcopy(g1))
assert g2.y.shape == gamma.y.shape
assert (g2.y - gamma.y < tol).all()
assert g2.q.shape == gamma.q.shape
assert (g2.q - gamma.q < tol).all()
assert g2.lam.shape == gamma.lam.shape
assert (g2.lam - gamma.lam < tol).all()
assert g2.u.shape == gamma.u.shape
assert (g2.u - gamma.u < tol).all()
assert g2.t.size == gamma.t.size
assert (g2.t - gamma.t < tol).all()
assert g2.p.size == gamma.p.size
assert (g2.p - gamma.p < tol).all()
assert g2.nu.size == gamma.nu.size
assert (g2.nu - gamma.nu < tol).all()
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 inv_map(self, sol: Solution) -> Solution:
sol.u = sol.y[:, self.control_idxs]
sol.y = np.delete(sol.y, self.control_idxs, axis=1)
sol.lam = np.delete(sol.lam, self.control_idxs, axis=1)
return sol
def inv_map(self, sol: Solution) -> Solution:
sol.u = np.array([
self.compute_u(_t, _y, _lam, sol.p, sol.k)
for _t, _y, _lam, in zip(sol.t, sol.y, sol.lam)
])
return sol