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