def test_ParamPoly(self):
tangentX = np.array([9.389829642616592, -7.596531772501544])
tangentY = np.array([0.0, 5.5192033616035365])
t = np.array([0, 1])
x = np.array([0, 17.8605173461395])
y = np.array([0, -5.803233839653106])
hermiteX = CubicHermiteSpline(t, x, tangentX)
hermiteY = CubicHermiteSpline(t, y, tangentY)
xCoeffs = hermiteX.c.flatten()
yCoeffs = hermiteY.c.flatten()
# scipy coefficient and open drive coefficents have opposite order.
myRoad = self.roadBuilder.curveBuilder.createParamPoly3(
0,
isJunction=False,
au=xCoeffs[3],
bu=xCoeffs[2],
cu=xCoeffs[1],
du=xCoeffs[0],
av=yCoeffs[3],
bv=yCoeffs[2],
cv=yCoeffs[1],
dv=yCoeffs[0])
odr = pyodrx.OpenDrive("test")
odr.add_road(myRoad)
odr.adjust_roads_and_lanes()
extensions.printRoadPositions(odr)
extensions.view_road(
odr, os.path.join('..', self.configuration.get("esminipath")))
def poststep_estimate_spike_cspline(self, solver):
"""Use cublic spline through previous points to find spike.
Can be use for all methods, but is not as precise as using the dense output."""
spiked = solver.y_new[0] >= 30.
if spiked:
assert solver.adaptive
knot_ys = np.vstack(
[np.array(solver.solution.ys[-9:]), solver.y_new])
knot_ts = np.append(np.array(solver.solution.ts[-9:]),
solver.t_new)
assert knot_ys.shape[0] == knot_ts.shape[0], knot_ys.shape
if len(solver.solution.events[0]) > 0:
last_event_t = solver.solution.events[0][-1]
else:
last_event_t = np.NINF
knot_idxs = knot_ts > last_event_t
knot_ts = knot_ts[knot_idxs]
knot_ys = knot_ys[knot_idxs, :]
knot_ydots = np.array([
solver.eval_odefun(t, y, count=False)
for t, y in zip(knot_ts, knot_ys)
])
assert knot_ydots.shape == knot_ys.shape, knot_ydots.shape
assert knot_ys.shape[0] >= 2, knot_ys
v_spline = CubicHermiteSpline(x=knot_ts,
y=knot_ys[:, 0] - 30,
dydx=knot_ydots[:, 0],
extrapolate=False)
roots = v_spline.roots()
t_est = roots[(roots >= solver.t) & (roots <= solver.t_new)][0]
solver.t_new = t_est
solver.h = solver.t_new - solver.t
u_spline = CubicHermiteSpline(x=knot_ts,
y=knot_ys[:, 1],
dydx=knot_ydots[:, 1],
extrapolate=False)
solver.y_new = np.array([30., float(u_spline(solver.t_new))])
solver.ydot_new = None
if solver.DEBUG:
self._plot_spike_estimate_cspline(solver, knot_ts, knot_ys,
v_spline)
solver.set_event(event_idxs=[0], event_ts=[t_est])
else:
solver.reset_event()
def test_roots_extrapolate_gh_11185():
x = np.array([0.001, 0.002])
y = np.array([1.66066935e-06, 1.10410807e-06])
dy = np.array([-1.60061854, -1.600619])
p = CubicHermiteSpline(x, y, dy)
# roots(extrapolate=True) for a polynomial with a single interval
# should return all three real roots
r = p.roots(extrapolate=True)
assert_equal(p.c.shape[1], 1)
assert_equal(r.size, 3)
def generate_data(self, output=True):
# 1 Генерация случайных данных
# np.random.seed(1000)
data_x = np.linspace(0, 1, 7, endpoint=True)
data_y = np.concatenate([[0], np.random.rand(5), [1]])
self.xxx = np.linspace(0, 1, 1000)
self.cs = CubicHermiteSpline(data_x, data_y, np.zeros(7) + 1)
self._maxfunc = np.max(self.cs(self.xxx))
self.cs_norm = lambda x: self.cs(x)
self.xxx = self.a * self.xxx
# В UnrealEngine4 в см
# Конусы
self.xxc = np.linspace(self.start, self.end, self.count_cone)[0::]
self.ycc = self._function(self.xxc)
deriv = derivative(self._function, self.xxc)
self.lcx = self.xxc # + _koefx(self.s, self.xxc, deriv)
self.lcy = self._function(
self.xxc) + self.s / 2 # _coef_y(self.s, self.xxc, deriv)
self.rcx = self.xxc # - _koefx(self.s, self.xxc, deriv)
self.rcy = self._function(
self.xxc) - self.s / 2 # - _coef_y(self.s, self.xxc, deriv)
self.is_generated_data = True
if output:
return np.array([[self.lcx, self.lcy], [self.rcx, self.rcy]
]), np.array([self.xxc,
self._function(self.xxc)])
def weighted_average_slopes(data, old_start, old_dt, new_start, new_dt,
new_npts): #, *args, **kwargs):
# In almost all cases the unit will be in time.
new_end = new_start + (new_npts - 1) * new_dt
new_time_array = np.linspace(new_start, new_end, new_npts)
m = np.diff(data) / old_dt
w = np.abs(m)
w = 1.0 / np.clip(w, np.spacing(1), w.max())
slope = np.empty(len(data), dtype=np.float64)
slope[0] = m[0]
slope[1:-1] = (w[:-1] * m[:-1] + w[1:] * m[1:]) / (w[:-1] + w[1:])
slope[-1] = m[-1]
# If m_i and m_{i+1} have opposite signs then set the slope to zero.
# This forces the curve to have extrema at the sample points and not
# in-between.
sign_change = np.diff(np.sign(m)).astype(np.bool)
slope[1:-1][sign_change] = 0.0
derivatives = np.empty((len(data), 2), dtype=np.float64)
print(data[0:30])
print(slope[0:30])
derivatives[:, 0] = data
derivatives[:, 1] = slope
x = np.linspace(old_start, (len(x) - 1) * old_dt, len(x))
interp = CubicHermiteSpline(x, data, derivatives)
def hermite_spline(self, **kwargs):
from scipy.interpolate import CubicHermiteSpline
times = self.x
positions = [self(x) for x in times]
derivative = self.derivative()
velocities = [derivative(x) for x in times]
return CubicHermiteSpline(times, positions, dydx=velocities, **kwargs)
def get_movement_velocities(self, actions, mode='minimalist', span=10):
if mode == 'minimalist':
ramp = 0.5 - 0.5 * np.cos(np.linspace(0, 2 * np.pi, span))
velocities = actions * ramp[:, np.
newaxis] * self._upper_velocity_limits[
np.newaxis]
velocities = velocities[np.newaxis] # shape [1, span, 7]
elif mode == "cubic_hermite":
shape_factor = 0.2
x = [0, 0.5, 1]
actions_speeds = actions[:, :2 * self._n_joints]
actions_speeds = actions_speeds.reshape((2, self._n_joints))
actions_accelerations = actions[:, 2 * self._n_joints:]
actions_accelerations = actions_accelerations.reshape(
(2, self._n_joints))
speeds = np.vstack([self._previous_hermite_speeds, actions_speeds])
accelerations = np.vstack(
[self._previous_hermite_accelerations, actions_accelerations])
speeds[-1] *= shape_factor
accelerations[-1] *= shape_factor
eval = np.linspace(0, 1, span)
poly = CubicHermiteSpline(x, speeds, accelerations)
velocities = poly(eval) * self._upper_velocity_limits[np.newaxis]
velocities = velocities[np.newaxis] # shape [1, span, 7]
self._previous_hermite_speeds = speeds[-1]
self._previous_hermite_accelerations = accelerations[-1]
elif mode == "full_raw":
velocities = actions * self._upper_velocity_limits[np.newaxis]
velocities = velocities[:, np.newaxis] # shape [span, 1, 7]
elif mode == "one_raw":
velocities = actions * self._upper_velocity_limits[np.newaxis]
velocities = velocities[np.newaxis] # shape [1, 1, 7]
else:
raise ValueError("Unrecognized movement mode ({})".format(mode))
return velocities
def test_CubicHermiteSpline_correctness():
x = [0, 2, 7]
y = [-1, 2, 3]
dydx = [0, 3, 7]
s = CubicHermiteSpline(x, y, dydx)
assert_allclose(s(x), y, rtol=1e-15)
assert_allclose(s(x, 1), dydx, rtol=1e-15)
def __init__(self, gate_poses):
self.gates = gate_poses
self.n_gates = np.size(gate_poses, 0)
positions = np.array(
[pose.position.to_numpy_array() for pose in gate_poses])
dists = np.linalg.norm(positions[1:, :] - positions[:-1, :], axis=1)
self.arc_length = np.zeros(shape=self.n_gates)
self.arc_length[1:] = np.cumsum(dists)
# tangents from quaternion
# by rotating default gate direction with quaternion
self.tangents = np.zeros(shape=(self.n_gates, 3))
for i, pose in enumerate(gate_poses):
self.tangents[i, :] = rotate_vector(
pose.orientation, gate_facing_vector).to_numpy_array()
self.track_spline = CubicHermiteSpline(self.arc_length,
positions,
self.tangents,
axis=0)
# gate width to track (half) width
gate_widths = [gate_dimensions[0] / 2.0 for gate in gate_poses]
gate_heights = [gate_dimensions[1] / 2.0 for gate in gate_poses]
self.track_width_spline = CubicSpline(self.arc_length,
gate_widths,
axis=0)
self.track_height_spline = CubicSpline(self.arc_length,
gate_heights,
axis=0)
# sample 2048 points, the 2048 are arbitrary and should really be a parameter
taus = np.linspace(self.arc_length[0], self.arc_length[-1], 2**12)
self.track_centers = self.track_spline(taus)
self.track_tangents = self.track_spline.derivative(nu=1)(taus)
self.track_tangents /= np.linalg.norm(self.track_tangents,
axis=1)[:, np.newaxis]
self.track_normals = np.zeros_like(self.track_tangents)
self.track_normals[:, 0] = -self.track_tangents[:, 1]
self.track_normals[:, 1] = self.track_tangents[:, 0]
self.track_normals /= np.linalg.norm(self.track_normals,
axis=1)[:, np.newaxis]
self.track_widths = self.track_width_spline(taus)
self.track_heights = self.track_height_spline(taus)
def init_history_function(self):
""" Initialization of the historical state according to the type of
history given by the user as :
1. function for simple evaluation
2. tuple of (t_past, y_past, yp_past) for cubic Hermite
interpolation with scipy.interpolate.CubicHermiteSpline
3. constant
4. previous integration
Returns
-------
h : callable
The history function as a callable. Depending of the h_info
attribute, the function can be Hermite interpolation, ....
"""
if (self.h_info == 'from tuple'):
# unpack of time value, state and state's derivative
(self.t_past, self.y_past, self.yp_past) = self.h
self.t_oldest = self.t_past[0]
if (self.t_oldest < (self.t0 - self.delayMax)):
raise ("history tuple give in history not enough to describe\
all past values")
self.y_oldest = self.y_past[:, 0]
self.yp_oldest = self.yp_past[:, 0]
# construction of the history attribute self.h with
# CubicHermiteSpline
self.h = []
for k in range(self.n):
# extrapolation not possible
p = CubicHermiteSpline(self.t_past,
self.y_past[k, :],
self.yp_past[k, :],
extrapolate=False)
self.h.append(p)
elif (self.h_info == 'from function'):
self.t_oldest = self.t0 - self.delayMax
self.t_past = [self.t_oldest, self.t0]
self.y_oldest = self.h(self.t_oldest)
self.yp_oldest = np.zeros(self.y_oldest.shape)
elif (self.h_info == 'from constant'):
self.t_oldest = self.t0 - self.delayMax
self.y_oldest = self.h(self.t0)
self.yp_oldest = self.h(self.t0) * 0.0
self.t_past = [self.t_oldest, self.t0]
elif (self.h_info == 'from DdeResult'):
self.t_oldest = self.t0 - self.delayMax
self.solver_old = self.h
if self.solver_old.sol == None:
if self.solver_old.CE_cyclic.t_min > self.t_oldest:
raise ValueError(
'Z_cyclic can not assess past values. Use dense output'
)
self.h = self.solver_old.CE_cyclic
else:
self.h = self.solver_old.sol
else:
raise ValueError(
"wrong initialization of the dde history, h_info = %s" %
self.h_info)
def by_date(df):
r_map = df.iloc[0, 1:].values
r_map = [0] + r_map.tolist()
chs = CubicHermiteSpline(t_map, r_map, [0] * len(r_map))
ratemap = pd.DataFrame()
ratemap['days_to_expiry'] = np.arange(0, 365 * 10 + 1).astype(int)
ratemap['rate'] = chs(ratemap.days_to_expiry.values)
return ratemap
def _find_spike(knot_ts,
knot_vs,
thresh,
knot_vdots=None,
return_spline=False):
"""Find a spike for knot points."""
if knot_vdots is not None:
spline = CubicHermiteSpline(x=knot_ts,
y=knot_vs - thresh,
dydx=knot_vdots,
extrapolate=False)
else:
spline = CubicSpline(x=knot_ts, y=knot_vs - thresh, extrapolate=False)
roots = spline.roots()
spike_time = roots[(roots >= knot_ts[0]) & (roots <= knot_ts[-1])][0]
if not return_spline:
return spike_time
else:
return spike_time, spline
def test_complex():
x = [1, 2, 3, 4]
y = [1, 2, 1j, 3]
for ip in [KroghInterpolator, BarycentricInterpolator, pchip, CubicSpline]:
p = ip(x, y)
assert_allclose(y, p(x))
dydx = [0, -1j, 2, 3j]
p = CubicHermiteSpline(x, y, dydx)
assert_allclose(y, p(x))
assert_allclose(dydx, p(x, 1))
def Hermite(x, y, dydx):
li = x[0] - 0.5
ls = (x[len(x) - 1]) + 0.5
cs = CubicHermiteSpline(x, y, dydx)
xs = np.arange(li, ls, 0.1)
fig, ax = plt.subplots(figsize=(6.5, 4))
ax.plot(x, y, 'o', label='puntos')
ax.plot(xs, cs(xs), label="Spline")
ax.set_xlim(li, ls)
ax.legend(loc='lower left', ncol=2)
plt.savefig('hermite.jpg')
def get_custom_pitch(length, keypoints):
x = np.arange(1, keypoints.shape[0] + 1,
1) # x coordinates of turning points
xx = np.linspace(x.min(), x.max(), length)
y = np.array(keypoints) # y coordinates of turning points
cspline = CubicHermiteSpline(x=x, y=y,
dydx=np.zeros_like(y)) # interpolator
pitch = torch.Tensor(cspline(xx)).unsqueeze(0)
print(pitch.shape)
print(pitch)
return pitch
class SplinedTrack:
"""This class represents a Track defined by Gates.
A spline is fitted through the Gates with tangential constraints.
This spline is then sampled at 2048 points.
"""
def __init__(self, gate_poses):
self.gates = gate_poses
self.n_gates = np.size(gate_poses, 0)
positions = np.array([pose.position.to_numpy_array() for pose in gate_poses])
dists = np.linalg.norm(positions[1:, :] - positions[:-1, :], axis=1)
self.arc_length = np.zeros(shape=self.n_gates)
self.arc_length[1:] = np.cumsum(dists)
# tangents from quaternion
# by rotating default gate direction with quaternion
self.tangents = np.zeros(shape=(self.n_gates, 3))
for i, pose in enumerate(gate_poses):
self.tangents[i, :] = rotate_vector(pose.orientation, gate_facing_vector).to_numpy_array()
self.track_spline = CubicHermiteSpline(self.arc_length, positions, self.tangents, axis=0)
# gate width to track (half) width
gate_widths = [gate_dimensions[0] / 2.0 for gate in gate_poses]
gate_heights = [gate_dimensions[1] / 2.0 for gate in gate_poses]
self.track_width_spline = CubicSpline(self.arc_length, gate_widths, axis=0)
self.track_height_spline = CubicSpline(self.arc_length, gate_heights, axis=0)
# sample 2048 points, the 2048 are arbitrary and should really be a parameter
taus = np.linspace(self.arc_length[0], self.arc_length[-1], 2**12)
self.track_centers = self.track_spline(taus)
self.track_tangents = self.track_spline.derivative(nu=1)(taus)
self.track_tangents /= np.linalg.norm(self.track_tangents, axis=1)[:, np.newaxis]
self.track_normals = np.zeros_like(self.track_tangents)
self.track_normals[:, 0] = -self.track_tangents[:, 1]
self.track_normals[:, 1] = self.track_tangents[:, 0]
self.track_normals /= np.linalg.norm(self.track_normals, axis=1)[:, np.newaxis]
self.track_widths = self.track_width_spline(taus)
self.track_heights = self.track_height_spline(taus)
def track_frame_at(self, p):
"""Find closest track frame to a reference point p.
:param p: Point of reference
:return: Index of track frame, track center, tangent and normal.
"""
i = np.linalg.norm(self.track_centers - p, axis=1).argmin()
return i, self.track_centers[i], self.track_tangents[i], self.track_normals[i], \
self.track_widths[i], self.track_heights[i]
def by_date(df):
t_map = [
0, 30, 60, 90, 180, 12 * 30, 24 * 30, 36 * 30, 60 * 30, 72 * 30,
120 * 30, 240 * 30, 360 * 30
]
t_map = np.array(t_map)
r_map = df.iloc[-1, 1:].values
r_map = np.array([0] + r_map.tolist())
chs = CubicHermiteSpline(t_map, r_map, [0] * len(t_map))
rm_df = pd.DataFrame()
rm_df['days_to_expiry'] = np.arange(0, 365 * 10 + 1).astype(int)
rm_df['rate'] = chs(rm_df.days_to_expiry.values)
return rm_df
def mpc(x0,
v0,
curve,
dt_max=0.5,
max_time=INF,
max_iterations=INF,
v_max=None,
**kwargs):
assert (max_time < INF) or (max_iterations < INF)
from scipy.interpolate import CubicHermiteSpline
start_time = time.time()
best_cost, best_spline = INF, None
for iteration in irange(max_iterations):
if elapsed_time(start_time) >= max_time:
break
t1 = random.uniform(curve.x[0], curve.x[-1])
future = (curve.x[-1] - t1) # TODO: weighted
if future >= best_cost:
continue
x1 = curve(t1)
if (v_max is not None) and (max((x1 - x0) / v_max) > dt_max):
continue
# if quickest_inf_accel(x0, x1, v_max=v_max) > dt_max:
# continue
v1 = curve(t1, nu=1)
#dt = dt_max
dt = random.uniform(0, dt_max)
times = [0., dt]
positions = [x0, x1]
velocities = [v0, v1]
spline = CubicHermiteSpline(times, positions, dydx=velocities)
if not check_spline(spline, **kwargs):
continue
# TODO: optimize to find the closest on the path within a range
cost = future + (spline.x[-1] - spline.x[0])
if cost < best_cost:
best_cost, best_spline = cost, spline
print('Iteration: {} | Cost: {:.3f} | T: {:.3f} | Time: {:.3f}'.
format(iteration, cost, t1, elapsed_time(start_time)))
print(best_cost, t1, elapsed_time(start_time))
return best_cost, best_spline
def collect():
logger.info(f"Downloading Table: {URL}")
df = pd.read_html(URL, attrs=attrs)
logger.info(f"Number of tables found: {len(df)}")
if len(df) != 1:
return
df = df[0]
df.columns = t_names
df['date_current'] = pd.to_datetime(df.date_current)
df = df.sort_values('date_current', ascending=False)
df = df.reset_index(drop=True)
###############################################################################################
df = df[df.date_current == DATE]
logger.info(f"Number of items after filter: {len(df)}")
if len(df) == 0:
raise Exception("Data not up to date.")
_connector.write("treasuryrates", df)
df.to_csv(f"{DATA}.csv", index=False)
###############################################################################################
r_map = df.iloc[-1, 1:].values
r_map = np.array([0] + r_map.tolist())
chs = CubicHermiteSpline(t_map, r_map, [0]*len(t_map))
rm_df = pd.DataFrame()
rm_df['days_to_expiry'] = np.arange(0, 365 * 10 + 1).astype(int)
rm_df['rate'] = chs(rm_df.days_to_expiry.values)
rm_df['date_current'] = DATE
_connector.write("treasuryratemap", rm_df)
return df
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 generateMinutelyValuesFor(self, day):
startVal = self.dailyValues[day]
endVal = self.dailyValues[day + 1]
xToFit = []
yToFit = []
numPoints = random.randint(3, 7)
xToFit.append(0)
yToFit.append(startVal)
xToFit.append(509)
yToFit.append(endVal)
for i in range(numPoints - 2):
xToFit.append(random.uniform(1, 508))
yToFit.append(
random.uniform(startVal + (startVal - endVal),
endVal - (startVal - endVal)))
zipped = zip(xToFit, yToFit)
zipped = sorted(zipped)
xToFit, yToFit = zip(*zipped)
fit = CubicHermiteSpline(x=xToFit, y=yToFit, dydx=np.zeros(numPoints))
self.minutelyValues = fit(minute_list)
for i in range(self.minutelyValues.size):
self.minutelyValues[i] = self.minutelyValues[i] * random.gauss(
1, 0.0002)
def _fast_numerical_inverse(dist, tol=1e-12, max_intervals=100000):
"""
Generate fast, approximate PPF (inverse CDF) of probability distribution.
`_fast_numerical_inverse` accepts `dist`, an object representing the
distribution for which a fast approximate PPF is desired, and returns an
object `fni` with methods that approximate `dist.ppf` and `dist.rvs`.
For some distributions, these methods may be faster than those of `dist`
itself.
Parameters
----------
dist : object
Object representing distribution for which fast approximate PPF is
desired; e.g., a frozen instance of `scipy.stats.rv_continuous`.
tol : float, optional
u-error tolerance. The default is 1e-12.
max_intervals : int, optional
Maximum number of intervals in the cubic Hermite Spline used to
approximate the percent point function. The default is 100000.
Returns
-------
H : scipy.interpolate.CubicHermiteSpline
Interpolant of the distributions's PPF.
intervals : int
The number of intervals of the interpolant.
midpoint_error : float
The maximum u-error at an interpolant interval midpoint.
a, b : float
The left and right endpoints of the valid domain of the interpolant.
"""
dist, tol, max_intervals = _fni_input_validation(dist, tol, max_intervals)
# [1] Section 2.1: "For distributions with unbounded domain, we have to
# chop off its tails at [a] and [b] such that F(a) and 1-F(b) are small
# compared to the maximal tolerated approximation error."
p = np.array([dist.ppf(tol/10), dist.isf(tol/10)]) # initial interval
# [1] Section 2.3: "We then halve this interval recursively until
# |u[i+1]-u[i]| is smaller than some threshold value, for example, 0.05."
u = dist.cdf(p)
while p.size-1 <= np.ceil(max_intervals/2):
i = np.nonzero(np.diff(u) > 0.05)[0]
if not i.size:
break
p_mid = (p[i] + p[i+1])/2
# Compute only the new values and insert them in the right places
# [1] uses a linked list; we can't do that efficiently
u_mid = dist.cdf(p_mid)
p = np.concatenate((p, p_mid))
u = np.concatenate((u, u_mid))
i_sort = np.argsort(p)
p = p[i_sort]
u = u[i_sort]
# [1] Section 2.3: "Now we continue with checking the error estimate in
# each of the intervals and continue with splitting them until [it] is
# smaller than a given error bound."
u = dist.cdf(p)
f = dist.pdf(p)
while p.size-1 <= max_intervals:
# [1] Equation 4-8
try:
H = CubicHermiteSpline(u, p, 1/f)
except ValueError:
message = ("The interpolating spline could not be created. This "
"is often caused by inaccurate CDF evaluation in a "
"tail of the distribution. Increasing `tol` can "
"resolve this error at the expense of lower accuracy.")
raise ValueError(message)
# To improve performance, add update feature to CubicHermiteSpline
# [1] Equation 12
u_mid = (u[:-1] + u[1:])/2
eu = np.abs(dist.cdf(H(u_mid)) - u_mid)
i = np.nonzero(eu > tol)[0]
if not i.size:
break
p_mid = (p[i] + p[i+1])/2
u_mid = dist.cdf(p_mid)
f_mid = dist.pdf(p_mid)
p = np.concatenate((p, p_mid))
u = np.concatenate((u, u_mid))
f = np.concatenate((f, f_mid))
i_sort = np.argsort(p)
p = p[i_sort]
u = u[i_sort]
f = f[i_sort]
# todo: add test for monotonicity [1] Section 2.4
# todo: deal with vanishing density [1] Section 2.5
return H, eu, p.size-1, u[0], u[-1]
def plot_resolution(Rp,
dY,
Np,
dist='blend',
cf=0.0,
ntor=None,
fig=None,
ax=None):
try:
sns.set_style('white')
sns.set_palette('colorblind')
except NameError:
pass
if (fig is None) and (ax is None):
fig, ax = plt.subplots()
th = np.linspace(-np.pi, np.pi, 100001)
# plot location of uniformly spaced planes
ps = np.linspace(-np.pi, np.pi, Np + 1)
ymin = [-1, 0][dist != 'fourier']
for p in ps:
ax.plot([p, p], [ymin, 1], 'k--', lw=1)
ax.plot([-np.pi, np.pi], [0, 0], 'k', lw=1)
# plot the von Mises distribution through the center of the pellet
if dist == 'vonmises':
f = np.exp(-(Rp / dY)**2 * (1. - np.cos(th)))
dfdt = -(Rp / dY)**2 * np.sin(th) * f
elif dist == 'cauchy':
gamma = dY / Rp
f = (np.cosh(gamma) - 1.) / (np.cosh(gamma) - np.cos(th))
dfdt = -(np.cosh(gamma) - 1.) * np.sin(th) / (np.cosh(gamma) -
np.cos(th))**2
elif dist == 'blend':
fv = np.exp(-(Rp / dY)**2 * (1. - np.cos(th)))
sv = np.trapz(fv, th)
fv /= sv
dfvdt = -(Rp / dY)**2 * np.sin(th) * fv
gamma = dY / Rp
fc = (np.cosh(gamma) - 1.) / (np.cosh(gamma) - np.cos(th))
sc = np.trapz(fc, th)
fc /= sc
dfcdt = -fc * np.sin(th) / (np.cosh(gamma) - np.cos(th))
f = (1. - cf) * fv + cf * fc
sf = np.trapz(f, th)
f /= sf
dfdt = ((1. - cf) * dfvdt + cf * dfcdt) / sf
elif dist == 'fourier':
f = np.cos(ntor * th)
dfdt = -ntor * np.sin(ntor * th)
ax.plot(th, f, lw=3)
# plot Cubic Hermite interpolation using plane locations
fp = interp1d(th, f)(ps)
dfpdt = interp1d(th, dfdt)(ps)
fi = CubicHermiteSpline(ps, fp, dfpdt)(th)
if dist != 'fourier' and np.any(fi < 0):
print("Warning: interpolated pellet distribution goes negative: %.2e" %
fi.min())
ax.plot(th, fi, lw=3)
ax.set_xlim([-np.pi, np.pi])
ax.set_xticks([-np.pi, -np.pi / 2, 0., np.pi / 2, np.pi])
ax.set_xticklabels(
[r'$-\pi$', r'$-\frac{\pi}{2}$', '0', r'$\frac{\pi}{2}$', r'$-\pi$'])
fig.tight_layout()
plt.show()
return (fig, ax)
def getCoeffsForParamPoly(x1,
y1,
h1,
x2,
y2,
h2,
cp1,
cp2,
vShiftForSamePoint=0):
""" Assumes traffice goes from point1 to point2. By default if the contact point is start, traffic is going into the road, and end, traffic is going out. """
if cp1 == pyodrx.ContactPoint.start:
h1 = h1 + np.pi
if cp2 == pyodrx.ContactPoint.end:
h2 = h2 + np.pi
h1 = h1 % (np.pi * 2)
h2 = h2 % (np.pi * 2)
# TODO we need to solve the problem with param poly, not a straight road, as there can still be some angles near threshold for which it can fail.
# if Geometry.headingsTooClose(h1, h2):
# # return a straight road. This is flawed because heading is assumed to be 0 for straight roads in pyodrx
# return self.getStraightRoadBetween(newRoadId, road1, road2, incomingCp, ioutgoingCp,
# isJunction=isJunction,
# n_lanes=n_lanes,
# lane_offset=lane_offset,
# laneSides=laneSides)
# TODO return a curve because points can have the same heading, but big translation which creates problem.
tangentMagnitude = math.sqrt((x1 - x2)**2 + (y1 - y2)**2)
if tangentMagnitude < 3: # it too short, for U-turns
tangentMagnitude = 3
localRotation = h1 # rotation of local frame wrt inertial frame.
u1 = 0
v1 = 0
u2, v2 = Geometry.inertialToLocal((x1, y1), localRotation, (x2, y2))
if u1 == u2 and v1 == v2:
v1 -= vShiftForSamePoint
v2 += vShiftForSamePoint
localStartTangent = Geometry.headingToTangent(0, tangentMagnitude)
localEndHeading = Geometry.getRelativeHeading(h1, h2)
localEndTangent = Geometry.headingToTangent(localEndHeading,
tangentMagnitude)
X = [u1, u2]
Y = [v1, v2]
tangentX = [localStartTangent[0], localEndTangent[0]]
tangentY = [localStartTangent[1], localEndTangent[1]]
# print(f"connecting road #{road1.id} and # {road2.id}: {x1, y1, x2, y2}")
# print(f"connecting road #{road1.id} and # {road2.id}: X, Y, tangentX, tangentY")
# print(X)
# print(Y)
# print(tangentX)
# print(tangentY)
p = [0, 1]
hermiteX = CubicHermiteSpline(p, X, tangentX)
hermiteY = CubicHermiteSpline(p, Y, tangentY)
xCoeffs = hermiteX.c.flatten()
yCoeffs = hermiteY.c.flatten()
return xCoeffs, yCoeffs
import numpy as np
from scipy import optimize
from scipy.special import expit
from scipy.linalg import norm
from scipy.interpolate import interp1d, CubicHermiteSpline
import matplotlib.pyplot as plt
from fitts_law import calc_IP, calc_hit_prob
P_THRESHOLD = 0.02
TP_MIN = 0.1
TP_MAX = 100
correction0_moving_spline = CubicHermiteSpline(
np.array([-1, -0.6, 0.3, 0.5, 1]), np.array([0.6, 1, 1, 0.6, 0]),
np.array([0.8, 0.8, -0.8, -2, -0.8]))
# Data for correction calculation
# Refer to https://www.wolframcloud.com/obj/hebuweitom/Published/Correction.nb
# for the effects of the data in graphs
# obj0 - flow
a0f = [0, 1, 1.5, 2]
k0f = interp1d(a0f, [-14, -7.7, -7, -4.4],
bounds_error=False,
fill_value=(-14, -4.4),
assume_sorted=True)
coeffs0f = np.array([[[0, 0, 1, 6], [0, 0, 1, 3], [0, 0, 1, 3]],
[[-1, 0, 1, 2.5], [-0.5, 1, 1, 1.2], [-0.5, -1, 1, 1.2]],
[[-1.5, 0, 1, 1.5], [-0.75, 1.5, 1, 1],
def interp_sigma():
#beam emittance
eps_x = 20e-9 # m
eps_y = 1.3e-9 # m
df = load_esr()
#range with pressure data
df = df.query("s>-15 and s<10")
interp_x = CubicHermiteSpline(df["s"], df["beta_x"], -2 * df["alpha_x"])
interp_y = CubicHermiteSpline(df["s"], df["beta_y"], -2 * df["alpha_y"])
xs = np.linspace(df["s"][df["s"].index[0]], df["s"][df["s"].index[-1]],
300)
print("range:", xs[0], xs[-1])
#plt.style.use("dark_background")
#col = "lime"
col = "black"
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
set_axes_color(ax, col)
set_grid(plt, col)
#plot data
sigma_x = []
sigma_y = []
for i in xs:
sigma_x.append(get_beam_sigma(eps_x, interp_x(i)))
sigma_y.append(get_beam_sigma(eps_y, interp_y(i)))
#plot in detector coordinates
plt.plot(-1 * df["s"],
get_beam_sigma(eps_x, df["beta_x"]),
"o",
markersize=4,
color="blue",
lw=1)
plt.plot(-1 * xs, sigma_x, "-", color="blue", lw=1)
plt.plot(-1 * df["s"],
get_beam_sigma(eps_y, df["beta_y"]),
"o",
markersize=4,
color="red",
lw=1)
plt.plot(-1 * xs, sigma_y, "-", color="red", lw=1)
leg = legend()
leg.add_entry(leg_txt(), "$E_e$ = 10 GeV")
leg.add_entry(leg_lin("blue"), "$\sigma_x$")
leg.add_entry(leg_lin("red"), "$\sigma_y$")
leg.draw(plt, col)
ax.set_xlabel("$z$ (m)")
ax.set_ylabel("Beam $\sigma$ (mm)")
fig.savefig("01fig.pdf", bbox_inches="tight")
plt.close()
def tvlqr(x, u, dt, func, jax_f):
# interpolation of x
t = [0.]
# obtain the time step for each knot
for i in range(len(dt)):
t.append(t[-1] + dt[i])
xdot = []
for i in range(len(x)-1):
xdot.append(func(x[i], u[i]))
xdot.append(func(x[-1], u[-1]))
# obtain the interpolation for x
xtraj = CubicHermiteSpline(x=t, y=x, dydx=xdot, extrapolate=True)
# interp1d zero-th order interpolation will throw away the last signal
utraj = []
for i in range(len(u)):
utraj.append(u[i])
utraj.append(np.zeros(u[-1].shape))
utraj = interp1d(x=t, y=utraj, kind='zero', axis=0, fill_value='extrapolate')
# local linearization
def jaxfunc(x, u):
return jax.numpy.asarray(jax_f(x, u))
# then to compute the jacobian at x, just call jax.jacfwd(jaxfunc, argnum=0)(x, u)
# for jacobian at u, call the same function with argnum=1
# write down the differential Ricardii equation
def ricartti_f(t, S_):
# obtain A and B
A = jax.jacfwd(jaxfunc, argnums=0)(xtraj(t), utraj(t))
B = jax.jacfwd(jaxfunc, argnums=1)(xtraj(t), utraj(t))
#I = np.identity(len(x[0]))
Q = 1*np.identity(len(x[0]))
S_ = S_.reshape(Q.shape)
res = -(Q - S_ @ B @ B.T @ S_ + S_ @ A + A.T @ S_)
res = res.flatten()
return res
S_0 = 1*np.identity(len(x[0])).flatten()
t_0 = t[-1]
"""
# Here is one way to do it
r = ode(ricardii_f).set_integrator('vode', method='adams', with_jacobian=False)
r.set_initial_value(S_0, t_0)
for i in range(len(dt)-1, -1, -1):
print('going to integrate to time:')
print(r.t-dt[i])
if r.t-dt[i] < 0:
integrate_t = 0.
else:
integrate_t = r.t-dt[i]
r.integrate(integrate_t)
print('after integration:')
print("time:")
print(r.t)
print('y:')
print(r.y)
"""
"""
# another way
time_span = []
for i in range(len(t)):
time_span.append(t[len(t)-1-i])
sol = odeint(ricardii_f, S_0, time_span)
sol = [s.reshape((len(x[0]), len(x[0]))) for s in sol]
S = interp1d(time_span, sol, kind='cubic', axis=0)
"""
# use solve_ivp
sol = solve_ivp(fun=ricartti_f, t_span=[t[-1],0.], y0=S_0, dense_output=True)
S = sol.sol
print(S(t[-1]))
def controller(t, x):
#print('tracking time: %f' % (t))
#print('current state:')
#print(x)
#print('tracking state:')
#print(xtraj(t))
#print('tracking action:')
#print(utraj(t))
B = jax.jacfwd(jaxfunc, argnums=1)(xtraj(t), utraj(t))
K = B.T @ S(t).reshape((len(x),len(x)))
#print(S(t).reshape(len(x),len(x)))
u = -K @ (x - xtraj(t)) + utraj(t)
#print('result control:')
#print(u)
return u
return controller, xtraj, utraj, S
def smooth_curve(start_curve,
v_max,
a_max,
curve_collision_fn,
sample=True,
intermediate=True,
cubic=True,
refit=True,
num=1000,
min_improve=0.,
max_time=INF):
# TODO: rename smoothing.py to shortcutting.py
# TODO: default v_max and a_max
assert (num < INF) or (max_time < INF)
assert refit or intermediate
# TODO: https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.UnivariateSpline.html
#from scipy.interpolate import UnivariateSpline, LSQUnivariateSpline, LSQBivariateSpline
from scipy.interpolate import CubicHermiteSpline
start_time = time.time()
if curve_collision_fn(start_curve, t0=None, t1=None):
#return None
return start_curve
curve = start_curve
for iteration in irange(num):
if elapsed_time(start_time) >= max_time:
break
times = curve.x
durations = [0.] + [t2 - t1
for t1, t2 in get_pairs(times)] # includes start
positions = [curve(t) for t in times]
velocities = [curve(t, nu=1) for t in times]
# ts = [times[0], times[-1]]
# t1, t2 = curve.x[0], curve.x[-1]
t1, t2 = np.random.uniform(times[0], times[-1],
2) # TODO: sample based on position
if t1 > t2: # TODO: minimum distance from a knot
t1, t2 = t2, t1
ts = [t1, t2]
i1 = find(lambda i: times[i] <= t1,
reversed(range(len(times)))) # index before t1
i2 = find(lambda i: times[i] >= t2,
range(len(times))) # index after t2
assert i1 != i2
local_positions = [curve(t) for t in ts]
local_velocities = [curve(t, nu=1) for t in ts]
#print(local_velocities, v_max)
assert all(
np.less_equal(np.absolute(v), v_max + EPSILON).all()
for v in local_velocities)
#if any(np.greater(np.absolute(v), v_max).any() for v in local_velocities):
# continue # TODO: do the same with collisions
x1, x2 = local_positions
v1, v2 = local_velocities
#min_t = 0
min_t = find_lower_bound(x1, x2, v1, v2, v_max=v_max, a_max=a_max)
#min_t = optimistic_time(x1, x2, v_max=v_max, a_max=a_max)
current_t = (t2 - t1) - min_improve
if min_t >= current_t: # TODO: also limit the distance/duration between these two points
continue
#best_t = min_t
if sample:
max_t = current_t
ramp_t = solve_multivariate_ramp(x1, x2, v1, v2, v_max, a_max)
ramp_t = INF if ramp_t is None else ramp_t
max_t = min(max_t, ramp_t)
best_t = random.uniform(min_t, max_t)
else:
best_t = solve_multivariate_ramp(x1, x2, v1, v2, v_max, a_max)
if (best_t is None) or (best_t >= current_t):
continue
#best_t += 1e-3
#print(min_t, best_t, current_t)
local_durations = [t1 - times[i1], best_t, times[i2] - t2]
#local_times = [0, best_t]
local_times = [t1, (t1 + best_t)
] # Good if the collision function is time sensitive
if intermediate:
if cubic:
local_curve = CubicHermiteSpline(local_times,
local_positions,
dydx=local_velocities)
else:
local_curve = solve_multi_poly(times=local_times,
positions=local_positions,
velocities=local_velocities,
v_max=v_max,
a_max=a_max)
if (local_curve is None) or (spline_duration(local_curve) >= current_t) \
or curve_collision_fn(local_curve, t0=None, t1=None):
continue
# print(new_curve.hermite_spline().c[0,...])
local_positions = [local_curve(x) for x in local_curve.x]
local_velocities = [local_curve(x, nu=1) for x in local_curve.x]
local_durations = [t1 - times[i1]] + [
x - local_curve.x[0] for x in local_curve.x[1:]
] + [times[i2] - t2]
if refit:
new_durations = np.concatenate(
[durations[:i1 + 1], local_durations, durations[i2 + 1:]])
# assert len(new_durations) == (i1 + 1) + (len(durations) - i2) + 2
new_times = np.cumsum(new_durations)
# new_times = [new_times[0]] + [t2 for t1, t2 in get_pairs(new_times) if t2 > t1]
new_positions = positions[:i1 +
1] + local_positions + positions[i2:]
new_velocities = velocities[:i1 +
1] + local_velocities + velocities[i2:]
# if not all(np.less_equal(np.absolute(v), v_max).all() for v in new_velocities):
# continue
if cubic:
# new_curve = CubicSpline(new_times, new_positions)
new_curve = CubicHermiteSpline(new_times,
new_positions,
dydx=new_velocities)
else:
new_curve = solve_multi_poly(new_times, new_positions,
new_velocities, v_max, a_max)
if (new_curve is None) or (spline_duration(new_curve) >= spline_duration(curve)) \
or not check_spline(new_curve, v_max, a_max) or \
(not intermediate and curve_collision_fn(new_curve, t0=None, t1=None)):
continue
else:
assert intermediate
# print(curve.x)
# print(curve.c[...,0])
# pre_curve = trim(curve, end=t1)
# post_curve = trim(curve, start=t1)
# curve = append_polys(pre_curve, post_curve)
# print(curve.x)
# print(curve.c[...,0])
# print(new_curve.x)
# print(new_curve.c[...,0])
pre_curve = trim(curve, end=t1)
post_curve = trim(curve, start=t2)
new_curve = append_polys(
pre_curve, local_curve,
post_curve) # TODO: the numerics are throwing this off?
# print(new_curve.x)
# print(new_curve.c[...,0])
#assert(not curve_collision_fn(new_curve, t0=None, t1=None))
if (spline_duration(new_curve) >= spline_duration(curve)) or \
not check_spline(new_curve, v_max, a_max):
continue
print(
'Iterations: {} | Current time: {:.3f} | New time: {:.3f} | Elapsed time: {:.3f}'
.format(iteration, spline_duration(curve),
spline_duration(new_curve), elapsed_time(start_time)))
curve = new_curve
print(
'Iterations: {} | Start time: {:.3f} | End time: {:.3f} | Elapsed time: {:.3f}'
.format(num, spline_duration(start_curve), spline_duration(curve),
elapsed_time(start_time)))
check_spline(curve, v_max, a_max)
return curve
def smooth_cubic(path,
collision_fn,
resolutions,
v_max=None,
a_max=None,
time_step=1e-2,
parabolic=True,
sample=False,
intermediate=True,
max_iterations=1000,
max_time=INF,
min_improve=0.,
verbose=False):
start_time = time.time()
if path is None:
return None
assert (v_max is not None) or (a_max is not None)
assert path and (max_iterations < INF) or (max_time < INF)
from scipy.interpolate import CubicHermiteSpline
def curve_collision_fn(segment, t0=None, t1=None):
#if not within_dynamical_limits(curve, max_v=v_max, max_a=a_max, start_t=t0, end_t=t1):
# return True
_, samples = sample_discretize_curve(segment,
resolutions,
start_t=t0,
end_t=t1,
time_step=time_step)
if any(map(collision_fn, default_selector(samples))):
return True
return False
start_positions = waypoints_from_path(
path
) # TODO: ensure following the same path (keep intermediate if need be)
if len(start_positions) == 1:
start_positions.append(start_positions[-1])
start_durations = [0] + [
solve_linear(
np.subtract(p2, p1), v_max, a_max, t_min=T_MIN, only_duration=True)
for p1, p2 in get_pairs(start_positions)
] # TODO: does not assume continuous acceleration
start_times = np.cumsum(start_durations) # TODO: dilate times
start_velocities = [
np.zeros(len(start_positions[0])) for _ in range(len(start_positions))
]
start_curve = CubicHermiteSpline(start_times,
start_positions,
dydx=start_velocities)
# TODO: directly optimize for shortest spline
if len(start_positions) <= 2:
return start_curve
curve = start_curve
for iteration in irange(max_iterations):
if elapsed_time(start_time) >= max_time:
break
times = curve.x
durations = [0.] + [t2 - t1 for t1, t2 in get_pairs(times)]
positions = [curve(t) for t in times]
velocities = [curve(t, nu=1) for t in times]
t1, t2 = np.random.uniform(times[0], times[-1], 2)
if t1 > t2:
t1, t2 = t2, t1
ts = [t1, t2]
i1 = find(lambda i: times[i] <= t1,
reversed(range(len(times)))) # index before t1
i2 = find(lambda i: times[i] >= t2,
range(len(times))) # index after t2
assert i1 != i2
local_positions = [curve(t) for t in ts]
local_velocities = [curve(t, nu=1) for t in ts]
if not all(
np.less_equal(np.absolute(v),
np.array(v_max) + EPSILON).all()
for v in local_velocities):
continue
x1, x2 = local_positions
v1, v2 = local_velocities
current_t = (t2 - t1) - min_improve # TODO: percent improve
#min_t = 0
min_t = find_lower_bound(x1, x2, v1, v2, v_max=v_max, a_max=a_max)
if parabolic:
# Softly applies limits
min_t = solve_multivariate_ramp(
x1, x2, v1, v2, v_max,
a_max) # TODO: might not be feasible (soft constraint)
if min_t is None:
continue
if min_t >= current_t:
continue
best_t = random.uniform(min_t, current_t) if sample else min_t
local_durations = [t1 - times[i1], best_t, times[i2] - t2]
#local_times = [0, best_t]
local_times = [t1, (t1 + best_t)
] # Good if the collision function is time varying
if intermediate:
local_curve = CubicHermiteSpline(local_times,
local_positions,
dydx=local_velocities)
if curve_collision_fn(local_curve, t0=None,
t1=None): # check_spline
continue
#local_positions = [local_curve(x) for x in local_curve.x]
#local_velocities = [local_curve(x, nu=1) for x in local_curve.x]
local_durations = [t1 - times[i1]] + [
x - local_curve.x[0] for x in local_curve.x[1:]
] + [times[i2] - t2]
new_durations = np.concatenate(
[durations[:i1 + 1], local_durations, durations[i2 + 1:]])
new_times = np.cumsum(new_durations)
new_positions = positions[:i1 + 1] + local_positions + positions[i2:]
new_velocities = velocities[:i1 +
1] + local_velocities + velocities[i2:]
new_curve = CubicHermiteSpline(new_times,
new_positions,
dydx=new_velocities)
if not intermediate and curve_collision_fn(new_curve, t0=None,
t1=None):
continue
if verbose:
print(
'Iterations: {} | Current time: {:.3f} | New time: {:.3f} | Elapsed time: {:.3f}'
.format(iteration, spline_duration(curve),
spline_duration(new_curve), elapsed_time(start_time)))
curve = new_curve
if verbose:
print(
'Iterations: {} | Start time: {:.3f} | End time: {:.3f} | Elapsed time: {:.3f}'
.format(max_iterations, spline_duration(start_curve),
spline_duration(curve), elapsed_time(start_time)))
return curve
# julia
path_ju = 'data_julia'
t_ju = np.load('%s/solMackeyGlassBS3_t.npz' % path_ju)
y_ju = np.load('%s/solMackeyGlassBS3_u.npz' % path_ju)[:,0]
# sol matlab
import scipy.io as spio
path_matlab = 'data_dde23/mackeyGlass_dde23.mat'
mat = spio.loadmat(path_matlab, squeeze_me=True)
t_mat = mat['t']
y_mat = mat['y']
yp_mat = mat['yp']
p_dev = CubicHermiteSpline(t,y,yp)
y_dev_ju = p_dev(t_ju)
p_mat = CubicHermiteSpline(t_mat,y_mat,yp_mat)
y_mat_ju = p_mat(t_ju)
idx = np.searchsorted(t_mat,t_j[0]) - 1
p_mat_jit = CubicHermiteSpline(t_mat[idx:],y_mat[idx:],yp_mat[idx:])
y_mat_jit = p_mat_jit(t_j)
err_mat_ju = np.abs(y_mat_ju - y_ju)/y_ju
err_mat_jit = np.abs(y_mat_jit - y_jit)/y_jit
err_dev_ju = np.abs(y_dev_ju - y_ju)/y_ju
plt.figure()
plt.plot(y,yp,'o', label='solve_dde')