def create_spline(y, yp, x, h):
"""Create a cubic spline given values and derivatives.
Formulas for the coefficients are taken from interpolate.CubicSpline.
Returns
-------
sol : PPoly
Constructed spline as a PPoly instance.
"""
from scipy.interpolate import PPoly
n, m = y.shape
c = num.empty((4, n, m - 1), dtype=y.dtype)
slope = (y[:, 1:] - y[:, :-1]) / h
t = (yp[:, :-1] + yp[:, 1:] - 2 * slope) / h
c[0] = t / h
c[1] = (slope - yp[:, :-1]) / h - t
c[2] = yp[:, :-1]
c[3] = y[:, :-1]
c = num.rollaxis(c, 1)
return PPoly(c, x, extrapolate=True, axis=1)
def trim_start(poly, start):
from scipy.interpolate import PPoly, CubicHermiteSpline #, BPoly
#PPoly.from_bernstein_basis
#BPoly.from_derivatives
times = poly.x
if start <= times[0]:
return poly
if start >= times[-1]:
return None
first = find(lambda i: times[i] >= start, range(len(times)))
ts = [start, times[first]] # + start) / 2.]
ps = [poly(t) for t in ts]
derivative = poly.derivative()
vs = [derivative(t) for t in ts]
correction = CubicHermiteSpline(ts, ps, dydx=vs)
times = [start] + list(times[first:])
c = poly.c[:, first - 1:, ...]
c[:, 0, ...] = correction.c[-poly.c.shape[0]:, 0,
...] # TODO: assert that the rest are zero
poly = PPoly(c=c, x=times)
return poly
def test_antiderivative_simple(self):
np.random.seed(1234)
# [ p1(x) = 3*x**2 + 2*x + 1,
# p2(x) = 1.6875]
c = np.array([[3, 2, 1], [0, 0, 1.6875]]).T
# [ pp1(x) = x**3 + x**2 + x,
# pp2(x) = 1.6875*(x - 0.25) + pp1(0.25)]
ic = np.array([[1, 1, 1, 0], [0, 0, 1.6875, 0.328125]]).T
# [ ppp1(x) = (1/4)*x**4 + (1/3)*x**3 + (1/2)*x**2,
# ppp2(x) = (1.6875/2)*(x - 0.25)**2 + pp1(0.25)*x + ppp1(0.25)]
iic = np.array([[1 / 4, 1 / 3, 1 / 2, 0, 0],
[0, 0, 1.6875 / 2, 0.328125, 0.037434895833333336]]).T
x = np.array([0, 0.25, 1])
pp = PPoly(c, x)
ipp = pp.antiderivative()
iipp = pp.antiderivative(2)
iipp2 = ipp.antiderivative()
assert_allclose(ipp.x, x)
assert_allclose(ipp.c.T, ic.T)
assert_allclose(iipp.c.T, iic.T)
assert_allclose(iipp2.c.T, iic.T)
def linear_interp(obs_t, cum_obs):
"""
Construct a linear count interpolant
(which for monotonic counts is a stepwise rate)
"""
obs_t = np.asarray(obs_t)
cum_obs = np.asarray(cum_obs)
coeffs = np.zeros((2, cum_obs.size + 1))
knots = np.zeros(obs_t.size + 2)
# interpolating integral construction is awkward
# because the spline constructors only like cubics,
# so I build the PPoly manually
knots[1:-1] = obs_t
# extend with null counts
# so that it extrapolates, but conservatively
knots[0] = obs_t[0] - 1
knots[-1] = obs_t[-1] + 1
# rate
coeffs[0, 1:-1] = (
cum_obs[1:] - cum_obs[:-1]
) / (
obs_t[1:] - obs_t[:-1]
)
# step
coeffs[1, 1:] = cum_obs
# edge
coeffs[1, 0] = cum_obs[0]
big_n_hat = PPoly(
coeffs,
knots,
extrapolate=True)
return big_n_hat
def __init__(self, breaks, coefs):
self.coefs = coefs
self.breaks = breaks.reshape(1, -1)
self.__f = PPoly(self.coefs.T, self.breaks[0, :])
def _p(self) -> PPoly:
return PPoly(c=1.0 / self.Ne[None], x=self.t)
def naturalCubicSpline(X, Y, xx):
#===============================================================
# Cubic Spline Interpolation with the Free Run-out End Condition
#===============================================================
# Syntax
# --------------------------------------------------------------
# yy = naturalCubicSpline(X,Y,xx)
# pp = naturalCubicSpline(X,Y)
#
# Input Parameters
# --------------------------------------------------------------
# X : interpolation points
# Y : value of f(X)
# xx : points where we want an evaluation of P(x),
# where P is the interpolator polynomial
#
# Description
# --------------------------------------------------------------
# yy = naturalCubicSpline(X,Y,xx) uses a cubic spline interpolation
# to find yy at the values of the interpolant xx. The end
# condition is Free run-out (see p.6 in the text). The values
# in x must be distinct.
#===============================================================
# Sort [X,Y] to avoid errors in ppval()
Xsorted = sort(X, axis=0)
I = unravel_index(argsort(X, axis=0), X.shape)
Ysorted = Y[I]
n = size(Xsorted)
delta = Xsorted[1:n] - Xsorted[0:n - 1]
# Construct the tri-diagonal matrix to find g'' (with free run-out b.c.)
matSize = n - 2
M = zeros([matSize, matSize])
b = zeros(matSize)
for i in range(matSize):
if i > 0:
M[i, i - 1] = delta[i] / 6.
M[i, i] = (delta[i] + delta[i + 1]) / 3.
if i < matSize - 1:
M[i, i + 1] = delta[i + 1] / 6.
b[i] = (Ysorted[i + 2] - Ysorted[i + 1]) / delta[i + 1] - (
Ysorted[i + 1] - Ysorted[i]) / delta[i]
# Solve the system for g'' and add boundary points
gpp = linalg.solve(M, b)
gpp = hstack((0., gpp, 0.))
# Construct the piecewise polynomials
coefs = zeros([n - 1, 4])
for i in range(n - 1):
Delta = delta[i]
coefs[i, 0] = (gpp[i + 1] - gpp[i]) / (6. * Delta)
coefs[i, 1] = gpp[i] / 2.
coefs[i, 2] = -(gpp[i + 1] + 2. * gpp[i]) / 6. * Delta + (
Ysorted[i + 1] - Ysorted[i]) / Delta
coefs[i, 3] = Ysorted[i]
coefs = coefs.T
pp = PPoly(coefs, Xsorted)
# Return
return pp(xx)
def test_size_history_call(rnd_eta):
p = rnd_eta
q = PPoly(x=p.t, c=[1.0 / p.Ne])
for t in np.random.rand(100) * len(p.t):
assert abs(p(t) - q(t)) < 1e-4
def jump_spline2(x,
w,
interp_x=[-1, -0.5, 0, 0.3, 1],
interp_y=[0, 4, 0, 1, -1],
bc_left=[(1, 0), (1, 0)],
bc_right=[(1, 0), (1, 0)]):
"""Stepping model.
Parameters
----------
x : numpy array (N,), dtype=float
Grid points in which the model will be evaluated. N is the number of grid points.
w : numpy array (N,), dtype=float
Weights used to evaluate integrals by the Gaussian quadrature.
interp_x: numpy array (5,), dtype=float, or list
The x positions of the boundaries, maixma and minima of the potential,
sorted from the left to the right. Contains 5 values: the left boundary,
the first maximum, the minimum, the second maximum and the right boundary.
The default is [-1, -0.5, 0, 0.3, 1], which corresponds to the stepping
potential used in M. Genkin, O. Hughes, T.A. Engel, Nat Commun 12, 5986 (2021).
interp_y: numpy array (5,), dtype=float, or list
The corresponding y-values of the potential at the points specified by
interp_x. The default is [0, 4, 0, 1, -1], which corresponds to the stepping
potential used in M. Genkin, O. Hughes, T.A. Engel paper, Nat Commun 12, 5986 (2021).
bc_left: list
A list that contains two tuples that specify boundary conditions for the potential
on the left boundary. The format is the same as in bc_type argument of
scipy.interpolate.CubicSpline function. The default is [(1, 0), (1, 0)],
which corresponds to a zero-derivative (Neumann) BC for the potential.
bc_right: list
Same as bc_left, but for a right boundary. The default is [(1, 0), (1, 0)],
which corresponds to a zero-derivative (Neumann) BC for the potential.
Returns
-------
peq : numpy array, dtype=float
Probability density distribution evaluated at grid ponits x.
"""
xv = np.array(interp_x)
yv = np.array(interp_y)
cs = np.zeros((xv.shape[0] + 1, 4), dtype=x.dtype)
#Find additonal anchoring points on left and right boundaries
x_add_l, y_add_l = add_anchor_point(xv[:3], yv[:3])
x_add_r, y_add_r = add_anchor_point(xv[-3:][::-1], yv[-3:][::-1])
#Add these poionts to the x and y arrays
xv_new = np.concatenate(([xv[0], x_add_l], xv[1:-1], [x_add_r, xv[-1]]))
yv_new = np.concatenate(([yv[0], y_add_l], yv[1:-1], [y_add_r, yv[-1]]))
#Use three points for boundary splines, and two points for splines in the bulk
cs[0:2, :] = CubicSpline(xv_new[:3], yv_new[:3], bc_type=bc_left).c.T
for i in range(1, xv.shape[0] - 2):
cs[i + 1, :] = CubicSpline(xv[i:i + 2],
yv[i:i + 2],
bc_type=[(1, 0), (1, 0)]).c.T
cs[-2:] = CubicSpline(xv_new[-3:], yv_new[-3:], bc_type=bc_right).c.T
poly = PPoly(cs.T, xv_new)
peq = np.exp(-poly(x))
# normalization
peq /= sum(w * peq)
return peq
def __init__(self, traj, robot):
self.traj = traj #: init
self.spec = traj.GetConfigurationSpecification()
self.dof = robot.GetActiveDOF()
self._interpolation = self.spec.GetGroupFromName('joint').interpolation
assert self._interpolation == 'quadratic' or self._interpolation == "cubic", "This class only handles trajectories with quadratic or cubic interpolation"
self._duration = traj.GetDuration()
all_waypoints = traj.GetWaypoints(0, traj.GetNumWaypoints()).reshape(
traj.GetNumWaypoints(), -1)
valid_wp_indices = [0]
self.ss_waypoints = [0.0]
for i in range(1, traj.GetNumWaypoints()):
dt = self.spec.ExtractDeltaTime(all_waypoints[i])
if dt > 1e-5: # If delta is too small, skip it.
valid_wp_indices.append(i)
self.ss_waypoints.append(self.ss_waypoints[-1] + dt)
self.n_waypoints = len(valid_wp_indices)
self.ss_waypoints = np.array(self.ss_waypoints)
self.s_start = self.ss_waypoints[0]
self.s_end = self.ss_waypoints[-1]
self.waypoints = np.array([
self.spec.ExtractJointValues(all_waypoints[i], robot,
robot.GetActiveDOFIndices())
for i in valid_wp_indices
])
self.waypoints_d = np.array([
self.spec.ExtractJointValues(all_waypoints[i], robot,
robot.GetActiveDOFIndices(), 1)
for i in valid_wp_indices
])
# Degenerate case: there is only one waypoint.
if self.n_waypoints == 1:
pp_coeffs = np.zeros((1, 1, self.dof))
for idof in range(self.dof):
pp_coeffs[0, 0, idof] = self.waypoints[0, idof]
# A constant function
self.ppoly = PPoly(pp_coeffs, [0, 1])
elif self._interpolation == "quadratic":
self.waypoints_dd = []
for i in range(self.n_waypoints - 1):
qdd = ((self.waypoints_d[i + 1] - self.waypoints_d[i]) /
(self.ss_waypoints[i + 1] - self.ss_waypoints[i]))
self.waypoints_dd.append(qdd)
self.waypoints_dd = np.array(self.waypoints_dd)
# Fill the coefficient matrix for scipy.PPoly class
pp_coeffs = np.zeros((3, self.n_waypoints - 1, self.dof))
for idof in range(self.dof):
for iseg in range(self.n_waypoints - 1):
pp_coeffs[:, iseg, idof] = [
self.waypoints_dd[iseg, idof] / 2,
self.waypoints_d[iseg, idof], self.waypoints[iseg,
idof]
]
self.ppoly = PPoly(pp_coeffs, self.ss_waypoints)
elif self._interpolation == "cubic":
self.waypoints_dd = np.array([
self.spec.ExtractJointValues(all_waypoints[i], robot,
robot.GetActiveDOFIndices(), 2)
for i in valid_wp_indices
])
self.waypoints_ddd = []
for i in range(self.n_waypoints - 1):
qddd = ((self.waypoints_dd[i + 1] - self.waypoints_dd[i]) /
(self.ss_waypoints[i + 1] - self.ss_waypoints[i]))
self.waypoints_ddd.append(qddd)
self.waypoints_ddd = np.array(self.waypoints_ddd)
# Fill the coefficient matrix for scipy.PPoly class
pp_coeffs = np.zeros((4, self.n_waypoints - 1, self.dof))
for idof in range(self.dof):
for iseg in range(self.n_waypoints - 1):
pp_coeffs[:, iseg, idof] = [
self.waypoints_ddd[iseg, idof] / 6,
self.waypoints_dd[iseg, idof] / 2,
self.waypoints_d[iseg, idof], self.waypoints[iseg,
idof]
]
self.ppoly = PPoly(pp_coeffs, self.ss_waypoints)
self.ppoly_d = self.ppoly.derivative()
self.ppoly_dd = self.ppoly.derivative(2)
def to_pp(self) -> PPoly:
x = np.concatenate([self.intervals[0, :1], self.intervals[:, 1]])
return PPoly(x=x, c=self.heights[None])
from matplotlib.pyplot import plot
import numpy as np
from matplotlib import pyplot as plt
from scipy.interpolate import CubicHermiteSpline, PPoly
N = 3
time = np.linspace(0, 3, 2 * N + 1)
gains = np.random.random((2 * N + 1, 2))
quadratic_polys = np.array([
np.polyfit(time[:3], gains[i:i + 3], 2) for i in range(0, 2 * N - 1, 2)
]).swapaxes(0, 1)
# quadratic_polys = np.flip(quadratic_polys, 0)
i = 0
poly1 = np.polyfit(time[i:i + 3], gains[i:i + 3], 2)
i = 2
poly2 = np.polyfit(time[i:i + 3], gains[i:i + 3], 2)
i = 4
poly3 = np.polyfit(time[i:i + 3], gains[i:i + 3], 2)
gain_spline = PPoly(quadratic_polys, time[::2])
plot_time = np.linspace(0, 3, 100)
# plt.plot(plot_time, np.polyval(poly1[:, 0], plot_time))
# plt.plot(plot_time, np.polyval(poly2[:, 0], plot_time))
# plt.plot(plot_time, np.polyval(poly3[:, 0], plot_time))
plt.plot(time, gains[:, 0])
plt.plot(plot_time, gain_spline(plot_time))
plt.show()
def separate_poly(poly):
from scipy.interpolate import PPoly
k, m, d = poly.c.shape
return [PPoly(c=poly.c[:, :, i], x=poly.x) for i in range(d)]
def to_pp(self):
return PPoly(x=np.r_[self.t, np.inf], c=[self.Ne])
def test_simple(self):
c = np.array([[1, 4], [2, 5], [3, 6]])
x = np.array([0, 0.5, 1])
p = PPoly(c, x)
assert_allclose(p(0.3), 1 * 0.3**2 + 2 * 0.3 + 3)
assert_allclose(p(0.7), 4 * (0.7 - 0.5)**2 + 5 * (0.7 - 0.5) + 6)
def __init__(self, x, y, axis=0, bc_type='clamped', extrapolate=None):
x, y = map(np.asarray, (x, y))
if np.issubdtype(x.dtype, np.complexfloating):
raise ValueError("`x` must contain real values.")
if np.issubdtype(y.dtype, np.complexfloating):
dtype = complex
else:
dtype = float
y = y.astype(dtype, copy=False)
axis = axis % y.ndim
if x.ndim != 1:
raise ValueError("`x` must be 1-dimensional.")
if x.shape[0] < 2:
raise ValueError("`x` must contain at least 2 elements.")
if x.shape[0] != y.shape[axis]:
raise ValueError("The length of `y` along `axis`={0} doesn't "
"match the length of `x`".format(axis))
if not np.all(np.isfinite(x)):
raise ValueError("`x` must contain only finite values.")
if not np.all(np.isfinite(y)):
raise ValueError("`y` must contain only finite values.")
dx = np.diff(x)
if np.any(dx <= 0):
raise ValueError("`x` must be strictly increasing sequence.")
n = x.shape[0]
y = np.rollaxis(y, axis)
bc, y = self._validate_bc(bc_type, y, y.shape[1:], axis)
if extrapolate is None:
if bc[0] == 'periodic':
extrapolate = 'periodic'
else:
extrapolate = True
dxr = dx.reshape([dx.shape[0]] + [1] * (y.ndim - 1))
slope = np.diff(y, axis=0) / dxr
# If bc is 'not-a-knot' this change is just a convention.
# If bc is 'periodic' then we already checked that y[0] == y[-1],
# and the spline is just a constant, we handle this case in the same
# way by setting the first derivatives to slope, which is 0.
if n == 2:
if bc[0] in ['not-a-knot', 'periodic']:
bc[0] = (1, slope[0])
if bc[1] in ['not-a-knot', 'periodic']:
bc[1] = (1, slope[0])
# This is a very special case, when both conditions are 'not-a-knot'
# and n == 3. In this case 'not-a-knot' can't be handled regularly
# as both conditions are identical. We handle this case by
# constructing a parabola passing through given points.
if n == 3 and bc[0] == 'not-a-knot' and bc[1] == 'not-a-knot':
A = np.zeros((3, 3)) # This is a standard matrix.
b = np.empty((3, ) + y.shape[1:], dtype=y.dtype)
A[0, 0] = 1
A[0, 1] = 1
A[1, 0] = dx[1]
A[1, 1] = 2 * (dx[0] + dx[1])
A[1, 2] = dx[0]
A[2, 1] = 1
A[2, 2] = 1
b[0] = 2 * slope[0]
b[1] = 3 * (dxr[0] * slope[1] + dxr[1] * slope[0])
b[2] = 2 * slope[1]
s = solve(A,
b,
overwrite_a=False,
overwrite_b=False,
check_finite=False)
else:
# Find derivative values at each x[i] by solving a tridiagonal
# system.
A = np.zeros((3, n)) # This is a banded matrix representation.
b = np.empty((n, ) + y.shape[1:], dtype=y.dtype)
# Filling the system for i=1..n-2
# (x[i-1] - x[i]) * s[i-1] +\
# 2 * ((x[i] - x[i-1]) + (x[i+1] - x[i])) * s[i] +\
# (x[i] - x[i-1]) * s[i+1] =\
# 3 * ((x[i+1] - x[i])*(y[i] - y[i-1])/(x[i] - x[i-1]) +\
# (x[i] - x[i-1])*(y[i+1] - y[i])/(x[i+1] - x[i]))
A[1, 1:-1] = 2 * (dx[:-1] + dx[1:]) # The diagonal
A[0, 2:] = dx[:-1] # The upper diagonal
A[-1, :-2] = dx[1:] # The lower diagonal
b[1:-1] = 3 * (dxr[1:] * slope[:-1] + dxr[:-1] * slope[1:])
bc_start, bc_end = bc
if bc_start == 'periodic':
# Due to the periodicity, and because y[-1] = y[0], the linear
# system has (n-1) unknowns/equations instead of n:
A = A[:, 0:-1]
A[1, 0] = 2 * (dx[-1] + dx[0])
A[0, 1] = dx[-1]
b = b[:-1]
# Also, due to the periodicity, the system is not tri-diagonal.
# We need to compute a "condensed" matrix of shape (n-2, n-2).
# See http://www.cfm.brown.edu/people/gk/chap6/node14.html for
# more explanations.
# The condensed matrix is obtained by removing the last column
# and last row of the (n-1, n-1) system matrix. The removed
# values are saved in scalar variables with the (n-1, n-1)
# system matrix indices forming their names:
a_m1_0 = dx[-2] # lower left corner value: A[-1, 0]
a_m1_m2 = dx[-1]
a_m1_m1 = 2 * (dx[-1] + dx[-2])
a_m2_m1 = dx[-2]
a_0_m1 = dx[0]
b[0] = 3 * (dxr[0] * slope[-1] + dxr[-1] * slope[0])
b[-1] = 3 * (dxr[-1] * slope[-2] + dxr[-2] * slope[-1])
Ac = A[:, :-1]
b1 = b[:-1]
b2 = np.zeros_like(b1)
b2[0] = -a_0_m1
b2[-1] = -a_m2_m1
# s1 and s2 are the solutions of (n-2, n-2) system
s1 = solve_banded((1, 1),
Ac,
b1,
overwrite_ab=False,
overwrite_b=False,
check_finite=False)
s2 = solve_banded((1, 1),
Ac,
b2,
overwrite_ab=False,
overwrite_b=False,
check_finite=False)
# computing the s[n-2] solution:
s_m1 = ((b[-1] - a_m1_0 * s1[0] - a_m1_m2 * s1[-1]) /
(a_m1_m1 + a_m1_0 * s2[0] + a_m1_m2 * s2[-1]))
# s is the solution of the (n, n) system:
s = np.empty((n, ) + y.shape[1:], dtype=y.dtype)
s[:-2] = s1 + s_m1 * s2
s[-2] = s_m1
s[-1] = s[0]
else:
if bc_start == 'not-a-knot':
A[1, 0] = dx[1]
A[0, 1] = x[2] - x[0]
d = x[2] - x[0]
b[0] = ((dxr[0] + 2 * d) * dxr[1] * slope[0] +
dxr[0]**2 * slope[1]) / d
elif bc_start[0] == 1:
A[1, 0] = 1
A[0, 1] = 0
b[0] = bc_start[1]
elif bc_start[0] == 2:
A[1, 0] = 2 * dx[0]
A[0, 1] = dx[0]
b[0] = -0.5 * bc_start[1] * dx[0]**2 + 3 * (y[1] - y[0])
if bc_end == 'not-a-knot':
A[1, -1] = dx[-2]
A[-1, -2] = x[-1] - x[-3]
d = x[-1] - x[-3]
b[-1] = ((dxr[-1]**2 * slope[-2] +
(2 * d + dxr[-1]) * dxr[-2] * slope[-1]) / d)
elif bc_end[0] == 1:
A[1, -1] = 1
A[-1, -2] = 0
b[-1] = bc_end[1]
elif bc_end[0] == 2:
A[1, -1] = 2 * dx[-1]
A[-1, -2] = dx[-1]
b[-1] = 0.5 * bc_end[1] * dx[-1]**2 + 3 * (y[-1] - y[-2])
s = solve_banded((1, 1),
A,
b,
overwrite_ab=False,
overwrite_b=False,
check_finite=False)
# Compute coefficients in PPoly form.
t = (s[:-1] + s[1:] - 2 * slope) / dxr
c = np.empty((4, n - 1) + y.shape[1:], dtype=t.dtype)
c[0] = t / dxr
c[1] = (slope - s[:-1]) / dxr - t
c[2] = s[:-1]
c[3] = y[:-1]
super(CubicSplineWithNodeSens, self).__init__(c,
x,
extrapolate=extrapolate)
self.axis = axis
# Compute y-derivatives at nodes
if n < 3:
raise NotImplementedError(
'node_derivatives requires more than 3 x')
else:
'''
At this point A and b have been constructed with boundary conditions.
A: stays the same.
b: b becomes a matrix, d(rhs_i) / dy_j
'''
if y.ndim > 1: # TODO - Confirm solution when y has more than 1 axis
raise NotImplementedError(
"Solution of node_derivatives currently only allows 1D y")
# Find vector of cross-derivative values, d/dy_j (dy(x_i)/dx)
# Take derivative of Linear system to compute node derivatives
# A is the same as before. New RHS is tridiagonal.
rhs = np.zeros((n, n)) # TODO: Test for other dimensionalities
# obtain diagonal indices for internal points
ij_diag = tuple([np.diag_indices(n - 2)[i] + 1 for i in range(2)])
minus_over_plus = 3 * dx[:-1] / dx[1:]
plus_over_minus = 3 * dx[1:] / dx[:-1]
rhs[ij_diag] = plus_over_minus - minus_over_plus # The diagonal
rhs[ij_diag[0], ij_diag[1] +
1] = minus_over_plus # upper diagonal # Confirm (+). Confirm slice
rhs[ij_diag[0], ij_diag[1] -
1] = -plus_over_minus # lower diagonal # Confirm (-). Confirm slice
if bc_start[0] == 1:
rhs[0, 0] = 0
elif bc_start[0] == 2:
raise NotImplementedError(
'bc_start not implemented. '
'We only handle fixed 1st derivatives.')
# Below is the boundary condition for dy/dx|x_0
# b[0] = -0.5 * bc_start[1] * dx[0] ** 2 + 3 * (y[1] - y[0])
else:
raise NotImplementedError(
'bc_start not implemented. '
'We only handle fixed 1st derivatives.')
if bc_end[0] == 1:
rhs[-1, -1] = 0
elif bc_end[0] == 2:
raise NotImplementedError(
'bc_end not implemented. '
'We only handle fixed 1st derivatives.')
# Below is the boundary condition for dy/dx|x_{n-1}
# b[-1] = 0.5 * bc_end[1] * dx[-1] ** 2 + 3 * (y[-1] - y[-2])
else:
raise NotImplementedError(
'bc_end not implemented. '
'We only handle fixed 1st derivatives.')
d2ydydx = solve_banded((1, 1),
A,
rhs,
overwrite_ab=False,
overwrite_b=False,
check_finite=False)
# The y_derivative dq(x)/dy_j
# Create an additional vector Piecewise Polynomial
# The PPoly weights are very similar to q(x), both fcns of x, y, y'
inv_dx = 1 / dx
inv_dx_rhs = inv_dx.reshape([dx.shape[0]] + [1] * (rhs.ndim - 1))
d2_sum = (d2ydydx[:-1] + d2ydydx[1:])
d = np.zeros((4, n - 1) + rhs.shape[1:], dtype=t.dtype)
# Start with portion common to all j
d[0] = (inv_dx_rhs**2) * d2_sum
d[1] = -inv_dx_rhs * (d2_sum + d2ydydx[:-1])
d[2] = d2ydydx[:-1]
# Adjust when j==i: dq_i / dy_i
ij_diag = np.diag_indices(n - 1) + y.shape[1:]
d[0][ij_diag] += 2.0 * inv_dx**3
d[1][ij_diag] -= 3.0 * inv_dx**2
d[3][ij_diag] += 1.0
# Adjust when j=i+1: dq_i / dy_{i+1}
idx_upper = (ij_diag[0], ij_diag[1] + 1)
d[0][idx_upper] -= 2.0 * inv_dx**3
d[1][idx_upper] += 3.0 * inv_dx**2
self._ysens = PPoly(d, x, extrapolate=extrapolate)
def perform(self, t0, tau, left_domain, right_domain):
"""
@type left_domain Domain
@type right_domain Domain
:param t0:
"""
assert issubclass(type(left_domain.time_integration_scheme), ContinuousRepresentationScheme)
assert issubclass(type(right_domain.time_integration_scheme), ContinuousRepresentationScheme)
# use boundary conditions of t_n-1 as initial guess for t_n
u_neumann_continuous = lambda tt: left_domain.right_BC["neumann"] * np.ones_like(tt)
# enforce boundary conditions
left_domain.u[0] = left_domain.left_BC["dirichlet"]
right_domain.u[-1] = right_domain.right_BC["dirichlet"]
t1 = t0+tau
# do fixed number of sweeps
for window_sweep in range(5):
# subcycling parameters
max_approximation_order = 5
# operator for left participant
t_sub, tau_sub = np.linspace(t0, t1, self.n_substeps_left + 1, retstep=True)
u0_sub = left_domain.u
coeffs_m1 = np.zeros([max_approximation_order + 1, self.n_substeps_left])
coeffs_m2 = np.zeros([max_approximation_order + 1, self.n_substeps_left])
for ii in range(self.n_substeps_left):
t0_sub = t_sub[ii]
sampling_times_substep = left_domain.time_integration_scheme.get_sampling_times(t0_sub, tau_sub)
for i in range(sampling_times_substep.shape[0]):
# f(u,t_n) with boundary conditions from this timestep
A, R = second_derivative_matrix(left_domain.grid, dirichlet_l=left_domain.left_BC["dirichlet"], neumann_r=u_neumann_continuous(sampling_times_substep[i]))
# use most recent coupling variables for all
left_domain.time_integration_scheme.set_rhs(A, R, i)
# time stepping
u1_sub = left_domain.time_integration_scheme.do_step(u0_sub, tau_sub)
# do time continuous reconstruction of Nodal values
u_dirichlet_continuous_sub_m1 = left_domain.time_integration_scheme.get_continuous_representation_for_component(
-1, t0_sub, u0_sub, u1_sub, tau_sub)
u_dirichlet_continuous_sub_m2 = left_domain.time_integration_scheme.get_continuous_representation_for_component(
-2, t0_sub, u0_sub, u1_sub, tau_sub)
coeffs_m1[:u_dirichlet_continuous_sub_m1.coef.shape[0], ii] = u_dirichlet_continuous_sub_m1.coef
coeffs_m2[:u_dirichlet_continuous_sub_m2.coef.shape[0], ii] = u_dirichlet_continuous_sub_m2.coef
u0_sub = u1_sub
if self.n_substeps_left == 1:
u_dirichlet_continuous_m1 = u_dirichlet_continuous_sub_m1
u_dirichlet_continuous_m2 = u_dirichlet_continuous_sub_m2
else:
u_dirichlet_continuous_m1 = PPoly(coeffs_m1[::-1,:], t_sub) # we have to reverse the order of the coefficients for PPoly
u_dirichlet_continuous_m2 = PPoly(coeffs_m2[::-1,:], t_sub) # we have to reverse the order of the coefficients for PPoly
u_left_new = u1_sub # use result of last subcycle for result of window
# operator for right participant
t_sub, tau_sub = np.linspace(t0, t1, self.n_substeps_right + 1, retstep=True)
u0_sub = right_domain.u
coeffs_p1 = np.zeros([max_approximation_order + 1, self.n_substeps_right])
for ii in range(self.n_substeps_right):
t0_sub = t_sub[ii]
sampling_times_substep = right_domain.time_integration_scheme.get_sampling_times(t0_sub, tau_sub)
for i in range(sampling_times_substep.shape[0]):
# f(u,t_n) with boundary conditions from this timestep
A, R = second_derivative_matrix(right_domain.grid, dirichlet_l=u_dirichlet_continuous_m1(sampling_times_substep[i]), dirichlet_r=right_domain.right_BC["dirichlet"])
# use most recent coupling variables for all
right_domain.time_integration_scheme.set_rhs(A, R, i)
# time stepping
u1_sub = right_domain.time_integration_scheme.do_step(u0_sub, tau_sub)
u_dirichlet_continuous_sub_p1 = right_domain.time_integration_scheme.get_continuous_representation_for_component(
1, t0_sub, u0_sub, u1_sub, tau_sub)
u1_sub[0] = u_dirichlet_continuous_m1(t0_sub+tau_sub) # we have to set the (known and changing) dirichlet value manually, since this value is not changed by the timestepping
coeffs_p1[:u_dirichlet_continuous_sub_p1.coef.shape[0], ii] = u_dirichlet_continuous_sub_p1.coef
u0_sub = u1_sub
if self.n_substeps_right == 1:
u_dirichlet_continuous_p1 = u_dirichlet_continuous_sub_p1
else:
u_dirichlet_continuous_p1 = PPoly(coeffs_p1[::-1,:], t_sub) # we have to reverse the order of the coefficients for PPoly
u_right_new = u1_sub # use result of last subcycle for result of window
u_right_new[0] = u_dirichlet_continuous_m1(t0+tau) # we have to set the (known and changing) dirichlet value manually, since this value is not changed by the timestepping
if numeric_parameters.neumann_coupling_order != 2:
print("Operator of order %d is not implemented!!!" % numeric_parameters.neumann_coupling_order)
quit()
fraction = left_domain.grid.h / right_domain.grid.h # normalize to right domain's meshwidth
p = np.array([-fraction, 0, 1.0])
c = compute_finite_difference_scheme_coeffs(evaluation_points=p, derivative_order=1)
# for u_stencil[1] we have to use the left_domain's continuous representation, because the right_domain's
# representation is constant in time. This degrades the order to 1 for a irregular mesh.
u_stencil = [
u_dirichlet_continuous_m2,
u_dirichlet_continuous_m1,
u_dirichlet_continuous_p1
]
# compute continuous representation for Neumann BC
u_neumann_continuous = lambda x: 1.0/right_domain.grid.h * (u_stencil[0](x) * c[0] + u_stencil[1](x) * c[1] + u_stencil[2](x) * c[2])
# update solution
left_domain.update_u(u_left_new)
right_domain.update_u(u_right_new)
# update coupling variables
left_domain.right_BC["neumann"] = u_neumann_continuous(t1)
right_domain.left_BC["dirichlet"] = u_dirichlet_continuous_m1(t1)
return True
Pe = np.array(40.5)
Ve = np.array(8.0)
Ae = np.array(20.1)
x, c = ThreeSegmentSpline(Ps, Vs, As, Pe, Ve, Ae, Jmax)
print("x =\n", x)
print("c =\n", c)
print("c.shape=\n", c.shape)
# Prepend 0
xr = np.insert(x, 0, 0)
print(xr)
# Increase Dimensions
cr = np.expand_dims(c, axis=2)
print(cr)
cspl = PPoly(cr, xr)
t_sampled = np.linspace(0, xr[-1], 100)
y = cspl(t_sampled)
yd = cspl(t_sampled, 1)
ydd = cspl(t_sampled, 2)
assert_almost_equal(y[0], Ps)
assert_almost_equal(yd[0], Vs)
assert_almost_equal(ydd[0], As)
assert_almost_equal(y[-1], Pe)
assert_almost_equal(yd[-1], Ve)
assert_almost_equal(ydd[-1], Ae)
plt.plot(t_sampled, cspl(t_sampled))
plt.show()
### Multi Dimensional Test
def add_anchor_point(x, y):
"""Auxilliary function to create stepping potential used by energy_model.peq_models.jump_spline2 module
Find additional anchoring point (x_add, y_add), such that x[0] < x_add < x[1], and y_add = y_spline (x_add_mirror),
where y_spline is spline between x[1] and x[2], and x_add_mirror is a mirror point of x_add w.r.t. x[1], e.g. |x_add-x[1]|=|x[1]-x_add_mirror|.
This additional point will force the barriers to have symmetrical shape.
Params:
x: x - values at three right or three left boundary points, e.g. interp_x[0:3], or interp_x[-3:][::-1] (reverse order on the right boundary)
y: corresponding y values
Returns:
x_add, y_add - additional point in between x[0] < x_add < x[1] that can be used for spline interpolation
"""
#Start with the middle point
x_add_mirror = 0.5 * (x[1] + x[2])
not_found = True
cpoly = PPoly(
CubicSpline(np.sort(x[1:3]), np.sort(y[1:3]), bc_type=[(1, 0),
(1, 0)]).c,
np.sort([x[1], x[2]]))
first_peak_is_maximum = True if y[0] <= y[1] else False
while not_found:
#Check if y-value at anchor point exceeds value at the boundary and that x-value is within an interval
if first_peak_is_maximum:
if cpoly(x_add_mirror) > y[0] and np.abs(
x_add_mirror - x[1]) < np.abs(x[1] - x[0]):
x_add = 2 * x[1] - x_add_mirror
if x[1] > x[0]:
poly2 = PPoly(
CubicSpline([x[0], x_add, x[1]],
[y[0], cpoly(x_add_mirror), y[1]],
bc_type=[(1, 0), (1, 0)]).c,
[x[0], x_add, x[1]])
else:
poly2 = PPoly(
CubicSpline([x[1], x_add, x[0]],
[y[1], cpoly(x_add_mirror), y[0]],
bc_type=[(1, 0), (1, 0)]).c,
[x[1], x_add, x[0]])
x_dense = np.linspace(x[0], x[1], 100)
if all(poly2(x_dense) <= y[1]):
not_found = False
else:
if cpoly(x_add_mirror) < y[0] and np.abs(
x_add_mirror - x[1]) < np.abs(x[1] - x[0]):
x_add = 2 * x[1] - x_add_mirror
if x[1] > x[0]:
poly2 = PPoly(
CubicSpline([x[0], x_add, x[1]],
[y[0], cpoly(x_add_mirror), y[1]],
bc_type=[(1, 0), (1, 0)]).c,
[x[0], x_add, x[1]])
else:
poly2 = PPoly(
CubicSpline([x[1], x_add, x[0]],
[y[1], cpoly(x_add_mirror), y[0]],
bc_type=[(1, 0), (1, 0)]).c,
[x[1], x_add, x[0]])
x_dense = np.linspace(x[0], x[1], 100)
if all(poly2(x_dense) >= y[1]):
not_found = False
x_add_mirror = 0.5 * (x[1] + x_add_mirror)
return x_add, cpoly(2 * x[1] - x_add)
