def test_t1(algorithm, const): def odefun(y, _, k): return y[1], y[0] / k[0] def odejac(_, __, k): df_dy = np.array([[0, 1], [1 / k[0], 0]]) df_dp = np.empty((2, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0] - 1, yf[0] algo = Shooting(odefun, None, bcfun, algorithm=algorithm) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[0, 1], [0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = (np.exp(-sol.t / np.sqrt(sol.k)) - np.exp((sol.t - 2) / np.sqrt(sol.k))) / ( 1 - np.exp(-2.e0 / np.sqrt(sol.k))) e2 = (1. / (sol.k ** (1 / 2) * np.exp(sol.t / sol.k ** (1 / 2))) + np.exp( (sol.t - 2) / sol.k ** (1 / 2)) / sol.k ** (1 / 2)) / (1 / np.exp(2 / sol.k ** (1 / 2)) - 1) assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
def test_t9(algorithm, const): def odefun(y, _, k): return 2 * y[1], 2 * (-(4 * y[2] * y[1] + 2 * y[0]) / (k[0] + y[2] ** 2)), 2 def odejac(y, _, k): df_dy = np.array([[0, 2, 0], [-4 / (y[2] ** 2 + k[0]), -(8 * y[2]) / (y[2] ** 2 + k[0]), (4 * y[2] * (2 * y[0] + 4 * y[1] * y[2])) / (y[2] ** 2 + k[0]) ** 2 - (8 * y[1]) / (y[2] ** 2 + k[0])], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, k): return y0[0] - 1 / (1 + k[0]), yf[0] - 1 / (1 + k[0]), y0[2] + 1 algo = Shooting(odefun, None, bcfun, algorithm=algorithm, num_arcs=2) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0., 1., 2) # noinspection PyTypeChecker solinit.y = np.array([[1. / (1. + const), 0., -1.], [1. / (1. + const), 1., 1.]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = 1 / (sol.k[0] + sol.y[:, 2] ** 2) e2 = -(2 * sol.y[:, 2]) / (sol.y[:, 2] ** 2 + sol.k[0]) ** 2 assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
def test_t10(algorithm, const): def odefun(y, _, k): return 2 * y[1], 2 * (-y[2] * y[1] / k[0]), 2 def odejac(y, _, k): df_dy = np.array([[0, 2, 0], [0, 2 * (-y[2]) / k[0], 2 * (-y[1] / k[0])], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0], yf[0] - 2, y0[2] + 1 algo = Shooting(odefun, None, bcfun, algorithm=algorithm) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[0, 0, -1], [2, 0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = 1 + erf(sol.y[:, 2] / np.sqrt(2 * sol.k[0])) / erf(1 / np.sqrt(2 * sol.k[0])) e2 = np.sqrt(2) / (np.sqrt(np.pi) * np.sqrt(sol.k[0]) * np.exp(sol.y[:, 2] ** 2 / (2 * sol.k[0])) * erf( np.sqrt(2) / (2 * np.sqrt(sol.k[0])))) assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
def test_t7(algorithm, const): def odefun(y, _, k): return 2 * y[1], 2 * ((-y[2] * y[1] + y[0] - (1.0e0 + k[0] * np.pi ** 2) * np.cos(np.pi * y[2]) - np.pi * y[2] * np.sin(np.pi * y[2])) / k[0]), 2 def odejac(y, _, k): df_dy = np.array([[0, 2, 0], [2 / k[0], -2 * y[2] / k[0], -(2 * (y[1] + np.pi * np.sin(np.pi * y[2]) + np.pi ** 2 * y[2] * np.cos(np.pi * y[2]) - np.pi * np.sin(np.pi * y[2]) * (k[0] * np.pi ** 2 + 1))) / k[0]], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0] + 1, yf[0] - 1, y0[2] + 1 algo = Shooting(odefun, None, bcfun, algorithm=algorithm) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[-1, 0, -1], [1, 0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = np.cos(np.pi * sol.y[:, 2]) + sol.y[:, 2] + ( sol.y[:, 2] * erf(sol.y[:, 2] / np.sqrt(2.0e0 * sol.k[0])) + np.sqrt(2 * sol.k[0] / np.pi) * np.exp(-sol.y[:, 2] ** 2 / (2 * sol.k[0]))) / ( erf(1.0e0 / np.sqrt(2 * sol.k[0])) + np.sqrt(2.0e0 * sol.k[0] / np.pi) * np.exp(-1 / (2 * sol.k[0]))) e2 = erf((np.sqrt(2) * sol.y[:, 2]) / (2 * np.sqrt(sol.k[0]))) / ( erf(np.sqrt(2) / (2 * np.sqrt(sol.k[0]))) + (np.sqrt(2) * np.sqrt(sol.k[0])) / ( np.sqrt(np.pi) * np.exp(1 / (2 * sol.k[0])))) - np.pi * np.sin(np.pi * sol.y[:, 2]) + 1 assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
def test_t4(algorithm, const): def odefun(y, _, k): return 2 * y[1], 2 * (((1 + k[0]) * y[0] - y[1]) / k[0]), 2 def odejac(_, __, k): df_dy = np.array([[0, 2, 0], [2 * (1 + k[0]) / k[0], 2 * (-1) / k[0], 0], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, k): return y0[0] - 1 - np.exp(-2), yf[0] - 1 - np.exp(-2 * (1 + k[0]) / k[0]), y0[2] + 1 algo = Shooting(odefun, None, bcfun, algorithm=algorithm) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[-1, 0, -1], [-1, 0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = np.exp(sol.y[:, 2] - 1) + np.exp(-((1 + sol.k[0]) * (1 + sol.y[:, 2]) / sol.k[0])) e2 = np.exp(sol.y[:, 2] - 1) - (sol.k[0] + 1) / ( sol.k[0] * np.exp((sol.y[:, 2] + 1) * (sol.k[0] + 1) / sol.k[0])) assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
def test_t5(algorithm, const): def odefun(y, _, k): return (2 * y[1], 2 * ((y[0] + y[2] * y[1] - (1 + k[0] * np.pi ** 2) * np.cos(np.pi * y[2]) + y[2] * np.pi * np.sin(np.pi * y[2])) / k[0]), 2) def odejac(y, _, k): df_dy = np.array([[0, 2, 0], [2 / k[0], 2 * y[2] / k[0], (2 * (y[1] + np.pi * np.sin(np.pi * y[2]) + np.pi * np.sin(np.pi * y[2]) * (k * np.pi ** 2 + 1) + np.pi * np.pi * y[2] * np.cos(np.pi * y[2]))) / k[0]], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0] + 1, yf[0] + 1, y0[2] + 1 algo = Shooting(odefun, None, bcfun, algorithm=algorithm) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[-1, 0, -1], [-1, 0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = np.cos(np.pi * sol.y[:, 2]) e2 = -np.pi * np.sin(np.pi * sol.y[:, 2]) assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
def test_shooting_4(): # This problem contains a quad and tests if the prob solver correctly # integrates the quadfun. Also tests multiple shooting. def odefun(x, _, __): return -x[1], x[0] def quadfun(x, _, __): return x[0] def bcfun(y0, _, __, qf, ___, ____, _____): return y0[0], y0[1] - 1, qf[0] - 1.0 algo = Shooting(odefun, quadfun, bcfun, num_arcs=4) solinit = Trajectory() solinit.t = np.linspace(0, np.pi / 2, 2) solinit.y = np.array([[1, 0], [1, 0]]) solinit.q = np.array([[0], [0]]) solinit.k = np.array([]) out = algo.solve(solinit)['traj'] assert (out.y[0, 0] - 0) < tol assert (out.y[0, 1] - 1) < tol assert (out.q[0, 0] - 2) < tol assert (out.q[-1, 0] - 1) < tol
def inv_map(self, sol: Solution) -> Solution: sol = copy.deepcopy(sol) if self.delta_ind_idx is None: self.delta_ind_idx = np.shape(sol.p)[0] - 1 sol.t = sol.t * sol.p[self.delta_ind_idx] sol.p = np.delete(sol.p, self.delta_ind_idx) return sol
def inv_map(self, sol: Solution) -> Solution: if self.ind_state_idx is None: self.ind_state_idx = np.shape(sol.y)[1] - 1 sol.t = sol.y[:, self.ind_state_idx] sol.y = np.delete(sol.y, self.ind_state_idx, axis=1) sol.lam_t = sol.lam[:, self.ind_state_idx] sol.lam = np.delete(sol.lam, self.ind_state_idx, axis=1) return sol
def map(self, sol: Solution) -> Solution: sol = copy.deepcopy(sol) if self.delta_ind_idx is None: self.delta_ind_idx = np.shape(sol.p)[0] delta_t = sol.t[-1] - sol.t[0] sol.p = np.insert(sol.p, self.delta_ind_idx, delta_t) sol.t = (sol.t - sol.t[0]) / delta_t return sol
def test_t24(const): def odefun(x, _, k): a_mat_x = 1 + x[2]**2 a_mat_xp = 2 * x[2] y = 1.4 return (x[1], (((1 + y) / 2 - k[0] * a_mat_xp) * x[0] * x[1] - x[1] / x[0] - (a_mat_xp / a_mat_x) * (1 - (y - 1) / 2 * x[0]**2)) / (k[0] * a_mat_x * x[0]), 1) def odejac(x, _, k): y = 1.4 df_dy = np.array( [[0, 1, 0], [(x[1] * (y / 2 - 2 * k * x[2] + 1 / 2) + x[1] / x[0]**2 + (4 * x[0] * x[2] * (y / 2 - 1 / 2)) / (x[2]**2 + 1)) / (k[0] * x[0] * (x[2]**2 + 1)) - ((2 * x[2] * ((y / 2 - 1 / 2) * x[0]**2 - 1)) / (x[2]**2 + 1) - x[1] / x[0] + x[0] * x[1] * (y / 2 - 2 * k[0] * x[2] + 1 / 2)) / (k[0] * x[0]**2 * (x[2]**2 + 1)), (x[0] * (y / 2 - 2 * k[0] * x[2] + 1 / 2) - 1 / x[0]) / (k[0] * x[0] * (x[2]**2 + 1)), -((4 * x[2]**2 * ((y / 2 - 1 / 2) * x[0]**2 - 1)) / (x[2]**2 + 1)**2 - (2 * ((y / 2 - 1 / 2) * x[0]**2 - 1)) / (x[2]**2 + 1) + 2 * k[0] * x[0] * x[1]) / (k[0] * x[0] * (x[2]**2 + 1)) - (2 * x[2] * ((2 * x[2] * ((y / 2 - 1 / 2) * x[0]**2 - 1)) / (x[2]**2 + 1) - x[1] / x[0] + x[0] * x[1] * (y / 2 - 2 * k[0] * x[2] + 1 / 2))) / (k[0] * x[0] * (x[2]**2 + 1)**2)], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(x0, xf, _, __, ___): return x0[0] - 0.9129, xf[0] - 0.375, x0[2] algo = SPBVP(odefun, None, bcfun) algo.set_derivative_jacobian(odejac) sol = Trajectory() sol.t = np.linspace(0, 1, 2) sol.y = np.array([[1, 1, 0], [0.1, 0.1, 1]]) sol.k = np.array([const]) cc = np.linspace(const * 10, const, 10) for c in cc: sol = copy.deepcopy(sol) sol.k = np.array([c]) sol = algo.solve(sol)['traj'] assert sol.converged
def test_spbvp_3(): # This problem contains a parameter, but it is not explicit in the BCs. # Since time is buried in the ODEs, this tests if the BVP solver calculates # sensitivities with respect to parameters. def odefun(_, p, __): return 1 * p[0] def bcfun(y0, yf, _, __, ___): return y0[0] - 0, yf[0] - 2 algo = SPBVP(odefun, None, bcfun) solinit = Trajectory() solinit.t = np.linspace(0, 1, 4) solinit.y = np.array([[0], [0], [0], [0]]) solinit.p = np.array([1]) solinit.k = np.array([]) out = algo.solve(solinit)['traj'] assert abs(out.p - 2) < tol
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_t19(algorithm, const): def odefun(y, _, k): return y[1], (-y[1] / k[0]), 1 def odejac(_, __, k): df_dy = np.array([[0, 1, 0], [0, -1 / k[0], 0], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0], yf[0], y0[2] algo = Shooting(odefun, None, bcfun, algorithm=algorithm) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[0, 0, 0], [0, 0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] assert sol.converged
def test_t29(algorithm, const): def odefun(y, _, k): return y[1], (y[0] - y[0]*y[1]) / k[0] def odejac(y, _, k): df_dy = np.array([[0, 1], [(1-y[1]) / k[0], -y[0] / k[0]]]) df_dp = np.empty((2, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0], yf[0] - 3/2 algo = Shooting(odefun, None, bcfun, algorithm=algorithm, num_arcs=1) algo.set_derivative_jacobian(odejac) sol = Trajectory() sol.t = np.linspace(0, 1, 2) sol.y = np.array([[0, 0], [3/2, 0]]) sol.k = np.array([const]) sol = algo.solve(sol)['traj'] assert sol.converged
def test_t23(algorithm, const): def odefun(y, _, k): return y[1], 1 / k[0] * np.sinh(y[0] / k[0]) def odejac(y, _, k): df_dy = np.array([[0, 1], [np.cosh(y[0] / k[0]) / k[0] ** 2, 0]]) df_dp = np.empty((2, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0], yf[0] - 1 algo = Shooting(odefun, None, bcfun, algorithm=algorithm) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[0, 0], [1, 0]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] assert sol.converged
def test_t15(const): def odefun(y, _, k): return 2 * y[1], 2 * (y[2] * y[0] / k[0]), 2 def odejac(y, _, k): df_dy = np.array([[0, 2, 0], [2 * (y[2] / k[0]), 0, 2 * (y[0] / k[0])], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0] - 1, yf[0] - 1, y0[2] + 1 algo = SPBVP(odefun, None, bcfun) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[1, 0, -1], [0, 0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] assert sol.converged
def test_t22(const): def odefun(y, _, k): return y[1], -(y[1] + y[0] * y[0]) / k[0] def odejac(y, _, k): df_dy = np.array([[0, 1], [-(2 * y[0]) / k[0], -1 / k[0]]]) df_dp = np.empty((2, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0], yf[0] - 1 / 2 algo = SPBVP(odefun, None, bcfun) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[0, 0], [0, 0]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] assert sol.converged
def test_t6(): # This is a "special" case not using the difficulty settings above. def odefun(y, _, k): return (2 * y[1], 2 * ((-y[2] * y[1] - k[0] * np.pi**2 * np.cos(np.pi * y[2]) - np.pi * y[2] * np.sin(np.pi * y[2])) / k[0]), 2) def odejac(y, _, k): df_dy = np.array( [[0, 2, 0], [ 0, -2 * y[2] / k[0], -(2 * (y[1] + np.pi * np.sin(np.pi * y[2]) - k[0] * np.pi**3 * np.sin(np.pi * y[2]) + np.pi**2 * y[2] * np.cos(np.pi * y[2]))) / k[0] ], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0] + 2, yf[0], y0[2] + 1 algo = SPBVP(odefun, None, bcfun) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[-1, 0, -1], [-1, 0, 1]]) solinit.k = np.array([1]) sol = algo.solve(solinit)['traj'] e1 = np.cos(np.pi * sol.y[:, 2]) + erf( sol.y[:, 2] / np.sqrt(2 * sol.k[0])) / erf(1 / np.sqrt(2 * sol.k[0])) e2 = np.sqrt(2) / ( np.sqrt(np.pi) * np.sqrt(sol.k[0]) * np.exp(sol.y[:, 2]**2 / (2 * sol.k[0])) * erf(np.sqrt(2) / (2 * np.sqrt(sol.k[0])))) - np.pi * np.sin(np.pi * sol.y[:, 2]) assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
def test_r18(const): def odefun(y, _, k): return -y[0] / k[0] def quadfun(y, _, __): return y[0] def bcfun(_, q0, __, qf, ___, ____, k): return q0[0] - 1, qf[0] - np.exp(-1 / k[0]) algo = SPBVP(odefun, quadfun, bcfun) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[1], [1]]) solinit.q = np.array([[0], [0]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = np.exp(-sol.t / sol.k[0]) e2 = -1 / (sol.k[0] * np.exp(sol.t / sol.k[0])) assert all(e1 - sol.q[:, 0] < tol) assert all(e2 - sol.y[:, 0] < tol)
def test_r8(const): def odefun(y, _, k): return -y[0] / k[0] def quadfun(y, _, __): return y[0] def bcfun(_, q0, __, qf, ___, ____, _____): return q0[0] - 1, qf[0] - 2 algo = Shooting(odefun, quadfun, bcfun) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[1], [1]]) solinit.q = np.array([[0], [0]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = (1.e0 - np.exp((sol.t - 1.e0) / sol.k)) / (1.e0 - np.exp(-1.e0 / sol.k)) e2 = np.exp((sol.t - 1) / sol.k) / (sol.k * (1 / np.exp(1 / sol.k) - 1)) assert all(e1 - sol.q[:, 0] < tol) assert all(e2 - sol.y[:, 0] < tol)
def test_t3(const): def odefun(y, _, k): return (2 * y[1], 2 * (-(2 + np.cos(np.pi * y[2])) * y[1] + y[0] - (1 + k[0] * np.pi * np.pi) * np.cos(np.pi * y[2]) - (2 + np.cos(np.pi * y[2])) * np.pi * np.sin(np.pi * y[2])) / k[0], 2) def odejac(y, _, k): df_dy = np.array( [[0, 2, 0], [ 2 / k[0], -(2 * np.cos(np.pi * y[2]) + 4) / k[0], (2 * np.pi**2 * np.sin(np.pi * y[2])**2 + 2 * np.pi * np.sin(np.pi * y[2]) * (k[0] * np.pi**2 + 1) - 2 * np.pi**2 * np.cos(np.pi * y[2]) * (np.cos(np.pi * y[2]) + 2) + 2 * y[1] * np.pi * np.sin(np.pi * y[2])) / k[0] ], [0, 0, 0]], dtype=np.float) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0] + 1, yf[0] + 1, y0[2] + 1 algo = SPBVP(odefun, None, bcfun) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[-1, 0, -1], [-1, 0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = np.cos(np.pi * sol.y[:, 2]) e2 = -np.pi * np.sin(np.pi * sol.y[:, 2]) assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
def test_spbvp_1(): # Full 2PBVP test problem # This is the simplest BVP def odefun(y, _, __): return y[1], -abs(y[0]) def bcfun(y0, yf, _, __, ___): return y0[0], yf[0] + 2 algo = SPBVP(odefun, None, bcfun) solinit = Trajectory() solinit.t = np.linspace(0, 4, 4) solinit.y = np.array([[0, 1], [0, 1], [0, 1], [0, 1]]) solinit.k = np.array([]) out = algo.solve(solinit)['traj'] assert out.y[0][0] < tol assert out.y[0][1] - 2.06641646 < tol assert out.y[-1][0] + 2 < tol assert out.y[-1][1] + 2.87588998 < tol assert out.t[-1] - 4 < tol assert abs(out.y[0][1] - solinit.y[0][1]) > tol assert abs(out.y[-1][0] - solinit.y[-1][0]) - 2 < tol
def test_t8(algorithm, const): def odefun(y, _, k): return y[1], (-y[1] / k[0]), 1 def odejac(_, __, k): df_dy = np.array([[0, 1, 0], [0, -1 / k[0], 0], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0] - 1, yf[0] - 2, y0[2] algo = Shooting(odefun, None, bcfun, algorithm=algorithm) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[1, 0, -1], [2, 0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = (2 - np.exp(-1 / sol.k[0]) - np.exp(-sol.y[:, 2] / sol.k[0])) / (1 - np.exp(-1 / sol.k[0])) e2 = -1 / (sol.k[0] * np.exp(sol.y[:, 2] / sol.k[0]) * (1 / np.exp(1 / sol.k[0]) - 1)) assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
def test_t18(const): def odefun(y, _, k): return y[1], (-y[1] / k[0]), 1 def odejac(_, __, k): df_dy = np.array([[0, 1, 0], [0, -1 / k[0], 0], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, k): return y0[0] - 1, yf[0] - np.exp(-1 / k[0]), y0[2] algo = SPBVP(odefun, None, bcfun) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[0, 0, 0], [0, 0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = np.exp(-sol.y[:, 2] / sol.k[0]) e2 = -1 / (sol.k[0] * np.exp(sol.y[:, 2] / sol.k[0])) assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
def test_t14(const): def odefun(y, _, k): return 2 * y[1], 2 * ((y[0] - k[0] * np.pi**2 * np.cos(np.pi * y[2]) - np.cos(np.pi * y[2])) / k[0]), 2 def odejac(y, _, k): df_dy = np.array([[0, 2, 0], [ 2 / k[0], 0, (2 * (np.pi * np.sin(np.pi * y[2]) + k[0] * np.pi**3 * np.sin(np.pi * y[2]))) / k[0] ], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, ___): return y0[0], yf[0], y0[2] + 1 algo = SPBVP(odefun, None, bcfun) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[0, 0, -1], [0, 0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = np.cos(np.pi * sol.y[:, 2]) + np.exp( -(1 + sol.y[:, 2]) / np.sqrt(sol.k[0])) + np.exp( -(1 - sol.y[:, 2]) / np.sqrt(sol.k[0])) e2 = np.exp((sol.y[:, 2] - 1) / np.sqrt(sol.k[0])) / np.sqrt( sol.k[0]) - np.pi * np.sin( np.pi * sol.y[:, 2]) - 1 / (np.sqrt(sol.k[0]) * np.exp( (sol.y[:, 2] + 1) / np.sqrt(sol.k[0]))) assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
def test_t16(algorithm, const): def odefun(y, _, k): return 1 * y[1], 1 * (-y[0] * np.pi ** 2 / (4 * k[0])), 1 def odejac(_, __, k): df_dy = np.array([[0, 1, 0], [-np.pi**2 / (4 * k[0]), 0, 0], [0, 0, 0]]) df_dp = np.empty((3, 0)) return df_dy, df_dp def bcfun(y0, yf, _, __, k): return y0[0], yf[0] - np.sin(np.pi / (2 * np.sqrt(k[0]))), y0[2] algo = Shooting(odefun, None, bcfun, algorithm=algorithm) algo.set_derivative_jacobian(odejac) solinit = Trajectory() solinit.t = np.linspace(0, 1, 2) solinit.y = np.array([[0, 0, 0], [0, 0, 1]]) solinit.k = np.array([const]) sol = algo.solve(solinit)['traj'] e1 = np.sin(np.pi * sol.y[:, 2] / (2 * np.sqrt(sol.k[0]))) e2 = (np.pi * np.cos((np.pi * sol.y[:, 2]) / (2 * np.sqrt(sol.k[0])))) / (2 * np.sqrt(sol.k[0])) assert all(e1 - sol.y[:, 0] < tol) assert all(e2 - sol.y[:, 1] < tol)
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()