def test_T11(const):
def odefun(X, u, p, const):
return 2 * X[1],\
2 * ((X[0] - const[0] * np.pi ** 2 * np.cos(np.pi * X[2]) - np.cos(np.pi * X[2])) / const[0]), 2
def odejac(X, u, p, const):
df_dy = np.array(
[[0, 2, 0],
[
2 / const[0], 0,
(2 * (np.pi * np.sin(np.pi * X[2]) +
const[0] * np.pi**3 * np.sin(np.pi * X[2]))) / const[0]
], [0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] + 1, Xf[0] + 1, X0[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
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_T1(const):
def odefun(X, u, p, const):
return X[1], X[0] / const[0]
def odejac(X, u, p, const):
df_dy = np.array([[0, 1], [1 / const[0], 0]])
df_dp = np.empty((2, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] - 1, Xf[0]
algo = spbvp(odefun, None, bcfun)
algo.set_derivative_jacobian(odejac)
solinit = Trajectory()
solinit.t = np.linspace(0, 1, 2)
solinit.y = np.array([[0, 1], [0, 1]])
solinit.const = np.array([const])
sol = algo.solve(solinit)['sol']
e1 = (np.exp(-sol.t / np.sqrt(sol.const)) - np.exp(
(sol.t - 2) / np.sqrt(sol.const))) / (
1 - np.exp(-2.e0 / np.sqrt(sol.const)))
e2 = (1. /
(sol.const**(1 / 2) * np.exp(sol.t / sol.const**(1 / 2))) + np.exp(
(sol.t - 2) / sol.const**(1 / 2)) / sol.const**
(1 / 2)) / (1 / np.exp(2 / sol.const**(1 / 2)) - 1)
assert all(e1 - sol.y[:, 0] < tol)
assert all(e2 - sol.y[:, 1] < tol)
def test_Trajectory():
t = np.array([0, 1, 2, 3])
y1 = t**2
gam = Trajectory(t, y1)
y, q, u = gam(0.5)
assert y == 0.5
assert len(q) == 0
assert len(u) == 0
y, q, u = gam(0.25)
assert y == 0.25
assert len(q) == 0
assert len(u) == 0
gam.set_interpolate_function('cubic')
y, q, u = gam(0.25)
assert y - 0.0625 < 1e-4
assert len(q) == 0
assert len(u) == 0
t, y, q, u = gam[0]
assert t == 0
assert y == 0
assert len(q) == 0
assert len(u) == 0
def test_T33(const):
def odefun(X, u, p, const):
return X[1], (X[0] * X[3] -
X[2] * X[1]) / const[0], X[3], X[4], X[5], (
-X[2] * X[5] - X[0] * X[1]) / const[0]
def odejac(X, u, p, const):
df_dy = np.array([[0, 1, 0, 0, 0, 0],
[
X[3] / const[0], -X[2] / const[0],
-X[1] / const[0], X[0] / const[0], 0, 0
], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
[
-X[1] / const[0], -X[0] / const[0],
-X[5] / const[0], 0, 0, -X[2] / const[0]
]])
df_dp = np.empty((6, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] + 1, X0[2], X0[3], Xf[0] - 1, Xf[2], Xf[3]
algo = spbvp(odefun, None, bcfun)
algo.set_derivative_jacobian(odejac)
sol = Trajectory()
sol.t = np.linspace(0, 1, 2)
sol.y = np.array([[-1, 0, 0, 0, 0, 0], [1, 0, 0, 0, 0, 0]])
sol.const = np.array([const])
sol = algo.solve(sol)['sol']
assert sol.converged
def test_T13(algorithm, const):
def odefun(X, u, p, const):
return 2 * X[1],\
2 * ((X[0] - const[0] * np.pi ** 2 * np.cos(np.pi * X[2]) - np.cos(np.pi * X[2])) / const[0]),\
2
def odejac(X, u, p, const):
df_dy = np.array(
[[0, 2, 0],
[
2 / const[0], 0,
(2 * (np.pi * np.sin(np.pi * X[2]) +
const[0] * np.pi**3 * np.sin(np.pi * X[2]))) / const[0]
], [0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0], Xf[0] + 1, X0[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)
solinit.y = np.array([[0, 0, -1], [0, 0, 1]])
solinit.const = np.array([const])
sol = algo.solve(solinit)['sol']
e1 = np.cos(np.pi * sol.y[:, 2]) + np.exp(
-(1 + sol.y[:, 2]) / np.sqrt(sol.const[0]))
e2 = -np.pi * np.sin(
np.pi * sol.y[:, 2]) - 1 / (np.sqrt(sol.const[0]) * np.exp(
(sol.y[:, 2] + 1) / np.sqrt(sol.const[0])))
assert all(e1 - sol.y[:, 0] < tol)
assert all(e2 - sol.y[:, 1] < tol)
def test_T8(algorithm, const):
def odefun(X, u, p, const):
return X[1], (-X[1] / const[0]), 1
def odejac(X, u, p, const):
df_dy = np.array([[0, 1, 0], [0, -1 / const[0], 0], [0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] - 1, Xf[0] - 2, X0[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
e1 = (2 - np.exp(-1 / sol.const[0]) - np.exp(
-sol.y[:, 2] / sol.const[0])) / (1 - np.exp(-1 / sol.const[0]))
e2 = -1 / (sol.const[0] * np.exp(sol.y[:, 2] / sol.const[0]) *
(1 / np.exp(1 / sol.const[0]) - 1))
assert all(e1 - sol.y[:, 0] < tol)
assert all(e2 - sol.y[:, 1] < tol)
def test_T4(algorithm, const):
def odefun(X, u, p, const):
return 2 * X[1], 2 * (((1 + const[0]) * X[0] - X[1]) / const[0]), 2
def odejac(X, u, p, const):
df_dy = np.array(
[[0, 2,
0], [2 * (1 + const[0]) / const[0], 2 * (-1) / const[0], 0],
[0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] - 1 - np.exp(-2), Xf[0] - 1 - np.exp(
-2 * (1 + const[0]) / const[0]), X0[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
e1 = np.exp(sol.y[:, 2] - 1) + np.exp(-((1 + sol.const[0]) *
(1 + sol.y[:, 2]) / sol.const[0]))
e2 = np.exp(sol.y[:, 2] - 1) - (sol.const[0] + 1) / (sol.const[0] * np.exp(
(sol.y[:, 2] + 1) * (sol.const[0] + 1) / sol.const[0]))
assert all(e1 - sol.y[:, 0] < tol)
assert all(e2 - sol.y[:, 1] < tol)
def test_T5(algorithm, const):
def odefun(X, u, p, const):
return (2 * X[1],
2 * ((X[0] + X[2] * X[1] -
(1 + const[0] * np.pi**2) * np.cos(np.pi * X[2]) +
X[2] * np.pi * np.sin(np.pi * X[2])) / const[0]), 2)
def odejac(X, u, p, const):
df_dy = np.array(
[[0, 2, 0],
[
2 / const[0], 2 * X[2] / const[0],
(2 * (X[1] + np.pi * np.sin(np.pi * X[2]) +
np.pi * np.sin(np.pi * X[2]) * (const * np.pi**2 + 1) +
np.pi * np.pi * X[2] * np.cos(np.pi * X[2]))) / const[0]
], [0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] + 1, Xf[0] + 1, X0[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
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_T17(algorithm, const):
def odefun(X, u, p, const):
return 0.2 * X[1], 0.2 * (-3 * const[0] * X[0] /
(const[0] + X[2]**2)**2), 0.2
def odejac(X, u, p, const):
df_dy = np.array(
[[0, 0.2, 0],
[
-(3 * const[0]) / (5 * (X[2]**2 + const[0])**2), 0,
(12 * const[0] * X[0] * X[2]) / (5 * (X[2]**2 + const[0])**3)
], [0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] + 0.1 / np.sqrt(const[0] + 0.01), Xf[0] - 0.1 / np.sqrt(
const[0] + 0.01), X0[2] + 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, 0], [0, 0, 1]])
solinit.const = np.array([const])
sol = algo.solve(solinit)['sol']
e1 = sol.y[:, 2] / np.sqrt(sol.const[0] + sol.y[:, 2]**2)
e2 = 1 / np.sqrt(sol.y[:, 2]**2 + sol.const[0]) - sol.y[:, 2]**2 / (
sol.y[:, 2]**2 + sol.const[0])**(3 / 2)
assert all(e1 - sol.y[:, 0] < tol)
assert all(e2 - sol.y[:, 1] < tol)
def test_T16(algorithm, const):
def odefun(X, u, p, const):
return 1 * X[1], 1 * (-X[0] * np.pi**2 / (4 * const[0])), 1
def odejac(X, u, p, const):
df_dy = np.array([[0, 1, 0], [-np.pi**2 / (4 * const[0]), 0, 0],
[0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0], Xf[0] - np.sin(np.pi / (2 * np.sqrt(const[0]))), X0[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
e1 = np.sin(np.pi * sol.y[:, 2] / (2 * np.sqrt(sol.const[0])))
e2 = (np.pi * np.cos(
(np.pi * sol.y[:, 2]) /
(2 * np.sqrt(sol.const[0])))) / (2 * np.sqrt(sol.const[0]))
assert all(e1 - sol.y[:, 0] < tol)
assert all(e2 - sol.y[:, 1] < tol)
def test_T31(const):
def odefun(X, u, p, const):
return np.sin(X[1]), X[2], -X[3]/const[0],\
((X[0]-1)*np.cos(X[1]) - X[2]/np.cos(X[1]) - const[0]*X[3]*np.tan(X[1]))/const[0]
def odejac(X, u, p, const):
df_dy = np.array(
[[0, np.cos(X[1]), 0, 0], [0, 0, 1, 0], [0, 0, 0, -1 / const[0]],
[
np.cos(X[1]) / const[0],
-(np.sin(X[1]) * (X[0] - 1) + const[0] * X[3] *
(np.tan(X[1])**2 + 1) +
(X[2] * np.sin(X[1])) / np.cos(X[1])**2) / const[0],
-1 / (const[0] * np.cos(X[1])), -np.tan(X[1])
]])
df_dp = np.empty((4, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0], X0[2], Xf[0], Xf[2]
algo = spbvp(odefun, None, bcfun)
algo.set_derivative_jacobian(odejac)
sol = Trajectory()
sol.t = np.linspace(0, 1, 2)
sol.y = np.array([[0, 0, 0, 0], [0, 0, 0, 0]])
sol.const = np.array([const])
sol = algo.solve(sol)['sol']
assert sol.converged
def test_T32(algorithm, const):
def odefun(X, u, p, const):
return X[1], X[2], X[3], (X[1] * X[2] - X[0] * X[3]) / const[0]
def odejac(X, u, p, const):
df_dy = np.array([[0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1],
[
-X[3] / const[0], X[2] / const[0],
X[1] / const[0], -X[0] / const[0]
]])
df_dp = np.empty((4, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0], X0[1], Xf[0] - 1, Xf[1]
algo = Shooting(odefun, None, bcfun, algorithm=algorithm, num_arcs=4)
algo.set_derivative_jacobian(odejac)
sol = Trajectory()
sol.t = np.linspace(0, 1, 2)
sol.y = np.array([[0, 0, 0, 0], [1, 0, 0, 0]])
sol.const = np.array([const])
sol = algo.solve(sol)['sol']
assert sol.converged
def test_T2(algorithm, const):
def odefun(X, u, p, const):
return X[1], X[1] / const[0]
def odejac(X, u, p, const):
df_dy = np.array([[0, 1], [0, 1 / const[0]]])
df_dp = np.empty((2, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] - 1, Xf[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
e1 = (1.e0 - np.exp(
(sol.t - 1.e0) / sol.const)) / (1.e0 - np.exp(-1.e0 / sol.const))
e2 = np.exp((sol.t - 1) / sol.const) / (sol.const *
(1 / np.exp(1 / sol.const) - 1))
assert all(e1 - sol.y[:, 0] < tol)
assert all(e2 - sol.y[:, 1] < tol)
def test_T21(algorithm, const):
def odefun(X, u, p, const):
return X[1], (X[0] * (1 + X[0]) -
np.exp(-2 * X[2] / np.sqrt(const[0]))) / const[0], 1
def odejac(X, u, p, const):
df_dy = np.array([[0, 1, 0],
[(2 * X[0] + 1) / const[0], 0,
(2 * np.exp(-(2 * X[2]) / np.sqrt(const[0]))) /
const[0]**(3 / 2)], [0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] - 1, Xf[0] - np.exp(-1 / np.sqrt(const[0])), X0[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
e1 = np.exp(-sol.y[:, 2] / np.sqrt(const))
e2 = -np.exp(-sol.y[:, 2] / np.sqrt(const)) / np.sqrt(const)
assert all(e1 - sol.y[:, 0] < tol)
assert all(e2 - sol.y[:, 1] < tol)
def test_T10(algorithm, const):
def odefun(X, u, p, const):
return 2 * X[1], 2 * (-X[2] * X[1] / const[0]), 2
def odejac(X, u, p, const):
df_dy = np.array([[0, 2, 0],
[0, 2 * (-X[2]) / const[0], 2 * (-X[1] / const[0])],
[0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0], Xf[0] - 2, X0[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
e1 = 1 + erf(sol.y[:, 2] / np.sqrt(2 * sol.const[0])) / erf(
1 / np.sqrt(2 * sol.const[0]))
e2 = np.sqrt(2) / (np.sqrt(np.pi) * np.sqrt(sol.const[0]) * np.exp(
sol.y[:, 2]**2 /
(2 * sol.const[0])) * erf(np.sqrt(2) / (2 * np.sqrt(sol.const[0]))))
assert all(e1 - sol.y[:, 0] < tol)
assert all(e2 - sol.y[:, 1] < tol)
def test_T9(algorithm, const):
def odefun(X, u, p, const):
return 2 * X[1], 2 * (-(4 * X[2] * X[1] + 2 * X[0]) /
(const[0] + X[2]**2)), 2
def odejac(X, u, p, const):
df_dy = np.array(
[[0, 2, 0],
[
-4 / (X[2]**2 + const[0]), -(8 * X[2]) / (X[2]**2 + const[0]),
(4 * X[2] * (2 * X[0] + 4 * X[1] * X[2])) /
(X[2]**2 + const[0])**2 - (8 * X[1]) / (X[2]**2 + const[0])
], [0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] - 1 / (1 + const[0]), Xf[0] - 1 / (1 +
const[0]), X0[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)
solinit.y = np.array([[1 / (1 + const), 0, -1], [1 / (1 + const), 1, 1]])
solinit.const = np.array([const])
sol = algo.solve(solinit)['sol']
e1 = 1 / (sol.const[0] + sol.y[:, 2]**2)
e2 = -(2 * sol.y[:, 2]) / (sol.y[:, 2]**2 + sol.const[0])**2
assert all(e1 - sol.y[:, 0] < tol)
assert all(e2 - sol.y[:, 1] < tol)
def test_Flow_full1():
# Solves a differential equation on SO(3). Taken from Kenthe Engo's `diffman` MATLAB repo github.com/kenthe/diffman
y0 = np.eye(3)
tspan = [0, 2.5]
def M2eom(t, y):
return (t ** 2, 1, -t)
def eom2g(t, y):
x, y, z = M2eom(t, y)
out = so(3)
out.set_vector([x, y, z])
return out
dim = y0.shape[0]
y = HManifold(SO(dim, y0))
vf = VectorField(y)
vf.set_M2g(eom2g)
ts = RKMK()
f = Flow(ts, vf)
ti, yi = f(y, tspan[0], tspan[-1], 0.1)
init = np.array([0, 0, 1])
gamma = Trajectory(ti, np.array([np.dot(_, init) for _ in yi]))
assert gamma.t[0] == tspan[0]
assert gamma.t[-1] == tspan[-1]
assert gamma.y[0,0] == init[0]
assert gamma.y[0,1] == init[1]
assert gamma.y[0,2] == init[2]
assert gamma.y[-1,0] - 0.45901073 < tol
assert gamma.y[-1,1] + 0.22656862 < tol
assert gamma.y[-1,2] - 0.85905518 < tol
def test_T7(algorithm, const):
def odefun(X, u, p, const):
return 2 * X[1], 2 * (
(-X[2] * X[1] + X[0] -
(1.0e0 + const[0] * np.pi**2) * np.cos(np.pi * X[2]) -
np.pi * X[2] * np.sin(np.pi * X[2])) / const[0]), 2
def odejac(X, u, p, const):
df_dy = np.array(
[[0, 2, 0],
[
2 / const[0], -2 * X[2] / const[0],
-(2 * (X[1] + np.pi * np.sin(np.pi * X[2]) + np.pi**2 * X[2] *
np.cos(np.pi * X[2]) - np.pi * np.sin(np.pi * X[2]) *
(const[0] * np.pi**2 + 1))) / const[0]
], [0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] + 1, Xf[0] - 1, X0[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
e1 = np.cos(np.pi * sol.y[:, 2]) + sol.y[:, 2] + (
sol.y[:, 2] * erf(sol.y[:, 2] / np.sqrt(2.0e0 * sol.const[0])) +
np.sqrt(2 * sol.const[0] / np.pi) * np.exp(-sol.y[:, 2]**2 /
(2 * sol.const[0]))
) / (erf(1.0e0 / np.sqrt(2 * sol.const[0])) +
np.sqrt(2.0e0 * sol.const[0] / np.pi) * np.exp(-1 /
(2 * sol.const[0])))
e2 = erf((np.sqrt(2) * sol.y[:, 2]) / (2 * np.sqrt(sol.const[0]))) / (
erf(np.sqrt(2) / (2 * np.sqrt(sol.const[0]))) +
(np.sqrt(2) * np.sqrt(sol.const[0])) /
(np.sqrt(np.pi) * np.exp(1 / (2 * sol.const[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 _jacobian_function(X, deriv_func, quad_func, n_odes, n_quads,
n_dynparams, n_arcs):
g = copy.deepcopy(gamma_set)
_y, _q, _params, _nonparams = self._unwrap_y0(
X, n_odes, n_quads, n_dynparams, n_arcs)
for ii in range(n_arcs):
g[ii].y[0] = _y[ii]
if n_quads > 0:
g[ii].q[0] = _q
phi_full_list = []
for ii in range(n_arcs):
t0 = g[ii].t[0]
_y0g, _q0g, _u0g = g[ii](t0)
tf = g[ii].t[-1]
_yfg, _qfg, _ufg = g[ii](tf)
stm0 = np.hstack(
(np.eye(n_odes), np.zeros((n_odes, n_dynparams)))).reshape(
n_odes * (n_odes + n_dynparams))
y0stm = np.zeros((len(stm0) + n_odes))
stmf = np.hstack(
(np.eye(n_odes), np.zeros((n_odes, n_dynparams)))).reshape(
n_odes * (n_odes + n_dynparams))
yfstm = np.zeros((len(stmf) + n_odes))
y0stm[:n_odes] = _y0g
y0stm[n_odes:] = stm0[:]
yfstm[:n_odes] = _yfg
yfstm[n_odes:] = stmf[:]
g[ii].t = np.hstack((t0, tf))
g[ii].y = np.vstack((y0stm, yfstm))
g[ii].q = np.vstack((_q0g, _qfg))
g[ii].u = np.vstack((_u0g, _ufg))
gamma_set_new = _gamma_maker(deriv_func, quad_func, g, _params,
sol, prop, pool, n_quads)
for ii in range(len(gamma_set_new)):
t_set = gamma_set_new[ii].t
temp = gamma_set_new[ii].y
y_set = temp[:, :n_odes]
q_set = gamma_set_new[ii].q
u_set = gamma_set_new[ii].u
gamma_set_new[ii] = Trajectory(t_set, y_set, q_set, u_set)
phi_temp = np.reshape(
temp[:, n_odes:],
(len(gamma_set_new[ii].t), n_odes, n_odes + n_dynparams))
phi_full_list.append(np.copy(phi_temp))
J = self._bc_jac_multi(gamma_set_new,
phi_full_list,
_params,
_nonparams,
sol.aux,
self.quadrature_function,
self.bc_func_ms,
StepSize=1e-6)
return J
def test_T22(const):
def odefun(X, u, p, const):
return X[1], -(X[1] + X[0] * X[0]) / const[0]
def odejac(X, u, p, const):
df_dy = np.array([[0, 1], [-(2 * X[0]) / const[0], -1 / const[0]]])
df_dp = np.empty((2, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0], Xf[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
assert sol.converged
def test_T23(algorithm, const):
def odefun(X, u, p, const):
return X[1], 1 / const[0] * np.sinh(X[0] / const[0])
def odejac(X, u, p, const):
df_dy = np.array([[0, 1], [np.cosh(X[0] / const[0]) / const[0]**2, 0]])
df_dp = np.empty((2, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0], Xf[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
assert sol.converged
def test_T30(algorithm, const):
def odefun(X, u, p, const):
return X[1], (X[0] - X[0] * X[1]) / const[0]
def odejac(X, u, p, const):
df_dy = np.array([[0, 1], [(1 - X[1]) / const[0], -X[0] / const[0]]])
df_dp = np.empty((2, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] + 7 / 6, Xf[0] - 3 / 2
algo = Shooting(odefun, None, bcfun, algorithm=algorithm, num_arcs=8)
algo.set_derivative_jacobian(odejac)
sol = Trajectory()
sol.t = np.linspace(0, 1, 2)
sol.y = np.array([[-7 / 6, 0], [3 / 2, 0]])
sol.const = np.array([const])
cc = np.linspace(const * 10, const, 10)
for c in cc:
sol = copy.deepcopy(sol)
sol.const = np.array([c])
sol = algo.solve(sol)['sol']
assert sol.converged
def test_T23(const):
def odefun(X, u, p, const):
return X[1], 1 / const[0] * np.sinh(X[0] / const[0])
def odejac(X, u, p, const):
df_dy = np.array([[0, 1], [np.cosh(X[0] / const[0]) / const[0]**2, 0]])
df_dp = np.empty((2, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0], Xf[0] - 1
algo = spbvp(odefun, None, bcfun)
algo.set_derivative_jacobian(odejac)
sol = Trajectory()
sol.t = np.linspace(0, 1, 2)
sol.y = np.array([[0, 0], [1, 0]])
sol.const = np.array([const])
cc = np.linspace(const * 10, const, 10)
for c in cc:
sol = copy.deepcopy(sol)
sol.const = np.array([c])
sol = algo.solve(sol)['sol']
assert sol.converged
def test_T27(const):
def odefun(X, u, p, const):
return X[1], X[0] * (1 - X[1]) / const[0]
def odejac(X, u, p, const):
df_dy = np.array([[0, 1], [(1 - X[1]) / const[0], -X[0] / const[0]]])
df_dp = np.empty((2, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] - 1, Xf[0] - 1 / 3
algo = spbvp(odefun, None, bcfun, max_nodes=1500)
algo.set_derivative_jacobian(odejac)
sol = Trajectory()
sol.t = np.linspace(0, 1, 2)
sol.y = np.array([[1, 1], [1 / 3, 1]])
sol.const = np.array([const])
cc = np.linspace(const * 10, const, 10)
for c in cc:
sol = copy.deepcopy(sol)
sol.const = np.array([c])
sol = algo.solve(sol)['sol']
assert sol.converged
def test_T19(const):
def odefun(X, u, p, const):
return X[1], (-np.exp(X[0]) * X[1] + np.pi / 2 * np.sin(
np.pi * X[2] / 2) * np.exp(2 * X[0])) / const[0], 1
def odejac(X, u, p, const):
df_dy = np.array([[0, 1, 0], [0, -1 / const[0], 0], [0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0], Xf[0], 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([[0, 0, 0], [0, 0, 1]])
sol.const = np.array([const])
cc = np.linspace(const * 100, const, 10)
for c in cc:
sol = copy.deepcopy(sol)
sol.const = np.array([c])
sol = algo.solve(sol)['sol']
assert sol.converged
def test_T15(const):
def odefun(X, u, p, const):
return 2 * X[1], 2 * (X[2] * X[0] / const[0]), 2
def odejac(X, u, p, const):
df_dy = np.array([[0, 2, 0],
[2 * (X[2] / const[0]), 0, 2 * (X[0] / const[0])],
[0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] - 1, Xf[0] - 1, X0[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.const = np.array([const])
sol = algo.solve(solinit)['sol']
assert sol.converged
def test_T6():
# This is a "special" case not using the difficulty settings above.
def odefun(X, u, p, const):
return (2 * X[1], 2 *
((-X[2] * X[1] - const[0] * np.pi**2 * np.cos(np.pi * X[2]) -
np.pi * X[2] * np.sin(np.pi * X[2])) / const[0]), 2)
def odejac(X, u, p, const):
df_dy = np.array(
[[0, 2, 0],
[
0, -2 * X[2] / const[0],
-(2 * (X[1] + np.pi * np.sin(np.pi * X[2]) -
const[0] * np.pi**3 * np.sin(np.pi * X[2]) +
np.pi**2 * X[2] * np.cos(np.pi * X[2]))) / const[0]
], [0, 0, 0]])
df_dp = np.empty((3, 0))
return df_dy, df_dp
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] + 2, Xf[0], X0[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.const = np.array([1])
sol = algo.solve(solinit)['sol']
e1 = np.cos(np.pi * sol.y[:, 2]) + erf(sol.y[:, 2] / np.sqrt(
2 * sol.const[0])) / erf(1 / np.sqrt(2 * sol.const[0]))
e2 = np.sqrt(2) / (
np.sqrt(np.pi) * np.sqrt(sol.const[0]) * np.exp(sol.y[:, 2]**2 /
(2 * sol.const[0])) *
erf(np.sqrt(2) /
(2 * np.sqrt(sol.const[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_Shooting_1():
# Full 2PBVP test problem
# This is the simplest BVP
def odefun(X, u, p, const):
return X[1], -abs(X[0])
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0], Xf[0] + 2
algo = Shooting(odefun, None, bcfun)
solinit = Trajectory()
solinit.t = np.linspace(0, 4, 2)
solinit.y = np.array([[0, 1], [0, 1]])
solinit.const = np.array([])
out = algo.solve(solinit)['sol']
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_T24(algorithm, const):
def odefun(X, u, p, const=None):
Ax = 1 + X[2]**2
Apx = 2 * X[2]
y = 1.4
return (X[1],
(((1 + y) / 2 - const[0] * Apx) * X[0] * X[1] - X[1] / X[0] -
(Apx / Ax) * (1 - (y - 1) / 2 * X[0]**2)) /
(const[0] * Ax * X[0]), 1)
def odejac(X, u, p, const):
y = 1.4
df_dy = np.array([
[0, 1, 0],
[(X[1] * (y / 2 - 2 * const * X[2] + 1 / 2) + X[1] / X[0]**2 +
(4 * X[0] * X[2] * (y / 2 - 1 / 2)) /
(X[2]**2 + 1)) / (const[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 * const[0] * X[2] + 1 / 2)) / (const[0] * X[0]**2 *
(X[2]**2 + 1)),
(X[0] * (y / 2 - 2 * const[0] * X[2] + 1 / 2) - 1 / X[0]) /
(const[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 * const[0] * X[0] * X[1]) / (const[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 * const[0] * X[2] + 1 / 2))) /
(const[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, q0, u0, Xf, qf, uf, p, ndp, const=None):
return X0[0] - 0.9129, Xf[0] - 0.375, X0[2]
algo = Shooting(odefun, None, bcfun, algorithm=algorithm, num_arcs=4)
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.const = np.array([const])
cc = np.linspace(const * 10, const, 10)
for c in cc:
sol = copy.deepcopy(sol)
sol.const = np.array([c])
sol = algo.solve(sol)['sol']
assert sol.converged
def test_Shooting_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(X, u, p, const):
return 1 * p[0]
def bcfun(X0, q0, u0, Xf, qf, uf, p, ndp, const):
return X0[0] - 0, Xf[0] - 2
algo = Shooting(odefun, None, bcfun)
solinit = Trajectory()
solinit.t = np.linspace(0, 1, 2)
solinit.y = np.array([[0], [0]])
solinit.dynamical_parameters = np.array([1])
solinit.const = np.array([])
out = algo.solve(solinit)['sol']
assert abs(out.dynamical_parameters - 2) < tol
