class Interpolated(object):
def __init__(self, x, y, sigma):
self.x = x
self.y = y
self.sigma = sigma
self.handles = []
def override(self, x, y, sigma):
self.x = x
self.y = y
self.sigma = sigma
def getHighCurv(self, threshold):
# points of max / min curvature, thresholded based on magnitude of curvature
roots = self._curv_roots[~np.isnan(
self._curv_roots)] # remove nans -- TODO address root problem (!)
return roots[(np.abs(self._curv(roots)) > threshold)]
# return self._curv_roots[(np.abs(self._curv(self._curv_roots)) > threshold)]
def getInflectionPoints(self, threshold):
# points of zero curvature, thresholded based on magnitude of gradient
roots = self._grad_roots[~np.isnan(
self._grad_roots)] # remove nans -- TODO address root problem (!)
return roots[(np.abs(self._grad(roots)) > threshold)]
# return self._grad_roots[(np.abs(self._grad(self._grad_roots)) > threshold)]
@property
def spline(self):
return self._spline
@property
def grad(self):
return self._grad
@property
def curv(self):
return self._curv
@property
def sigma(self):
return self._sigma
@sigma.setter
def sigma(self, s):
self._sigma = s
if (self._sigma == 0):
self._spline = CubicSpline(self.x, self.y, bc_type='not-a-knot')
else:
self._spline = CubicSpline(self.x,
gaussian_filter1d(self.y, self._sigma),
bc_type='not-a-knot')
self._grad = self._spline.derivative(1)
self._curv = self._spline.derivative(2)
self._curv_roots = self._spline.derivative(3).roots(discontinuity=True)
self._grad_roots = self._curv.roots(discontinuity=True)
def interpolate_path(robot,
joints,
path,
velocity_fraction=DEFAULT_SPEED_FRACTION,
k=1,
bspline=False,
dump=False,
**kwargs):
from scipy.interpolate import CubicSpline, interp1d
#from scipy.interpolate import CubicHermiteSpline, KroghInterpolator
# https://scikit-learn.org/stable/auto_examples/linear_model/plot_polynomial_interpolation.html
# TODO: local search to retime by adding or removing waypoints
# TODO: iteratively increase spacing until velocity / accelerations meets limits
# https://docs.scipy.org/doc/scipy/reference/tutorial/interpolate.html
# Waypoints are followed perfectly, twice continuously differentiable
# TODO: https://pythonrobotics.readthedocs.io/en/latest/modules/path_tracking.html#mpc-modeling
path, time_from_starts = retime_trajectory(
robot,
joints,
path,
velocity_fraction=velocity_fraction,
sample_step=None)
if k == 3:
if bspline:
positions = approximate_spline(time_from_starts,
path,
k=k,
**kwargs)
else:
# bc_type= clamped | natural | ((1, 0), (1, 0))
positions = CubicSpline(time_from_starts,
path,
bc_type='clamped',
extrapolate=False)
else:
kinds = {1: 'linear', 2: 'quadratic', 3: 'cubic'} # slinear
positions = interp1d(time_from_starts,
path,
kind=kinds[k],
axis=0,
assume_sorted=True)
if not dump:
return positions
# TODO: only if CubicSpline
velocities = positions.derivative()
accelerations = positions.derivative()
for i, t in enumerate(positions.x):
print(i, round(t, 3), positions(t), velocities(t), accelerations(t))
# TODO: compose piecewise functions
# TODO: ramp up and ramp down path
# TODO: quadratic interpolation between endpoints
return positions
def get_angle(s):
qm = s[:, int(s.shape[1] / 2), int(s.shape[2] / 2) - 3, 0, 1]
qp = s[:, int(s.shape[1] / 2), int(s.shape[2] / 2) + 3, 0, 1]
sp_qm = CubicSpline(range(len(qm)), qm)
sp_qp = CubicSpline(range(len(qp)), qp)
roots_qm = sp_qm.derivative().roots().tolist()
roots_qp = sp_qp.derivative().roots().tolist()
roots_qm = np.array(
[i for i in roots_qm if i >= 0 and sp_qm(i) > 0.8 * qm.max() and sp_qm(i) < 1.2 * qm.max()])
roots_qp = np.array(
[i for i in roots_qp if i >= 0 and sp_qp(i) > 0.8 * qp.max() and sp_qp(i) < 1.2 * qp.max()])
idx = 0 if len(roots_qm)==1 else 1 if len(roots_qm)==2 else 2
return np.abs(np.arctan(6/(roots_qm[idx] - roots_qp[idx]))*360/(2*np.pi))
def interp(t, X, teval, Xeval, kind):
"""Do interpolation on previous calculated solution.
It handles case when t is None, in which case, teval is not used and we use a uniform grid in [0, 1]
:param t: array-like, user-specified time stamps
:param X: ndarray, (x, x), variable to be interpolated
:param teval: array-like, where to evaluate for the interpolation
:param Xeval: ndarray, (x, x), where to store the evaluation. It might has more columns than X, in which case we fill higher-order derivative
:param kind: str, interpolation type for scipy.interpolate.interp1d, can be (‘linear’, ‘nearest’, ‘zero’, ‘slinear’, ‘quadratic’, ‘cubic’)
"""
targetN = Xeval.shape[0]
giveN = X.shape[0]
Xcol = X.shape[1]
XevalCol = Xeval.shape[1]
order = XevalCol // Xcol
if t is None:
teval = np.linspace(0, 1, targetN)
t = np.linspace(0, 1, giveN)
# construct object
if order == 1:
interp_ = interp1d(t, X, kind=kind, axis=0)
Xeval[:] = interp_(teval)
else:
interp_ = CubicSpline(t, X)
Xeval[:, :X.shape[1]] = interp_(teval)
curind = 1
while order > 1:
interp_ = interp_.derivative()
Xeval[:, curind*Xcol: (curind+1)*Xcol] = interp_(teval)
order -= 1
curind += 1
return
def _get_feature_min(self, x_values, y_values, ey_values, feature):
'''compute location and flux of feature minimum'''
# find deepest absorption
min_pos = y_values.argmin()
if ((min_pos < 5) or
(min_pos > y_values.shape[0] - 5)) and (not self.emission):
return np.nan, np.nan, np.nan, np.nan
# interpolate the feature with a Cubic Spline and use it to derive the absorption minimum
cs = CubicSpline(x_values, y_values, extrapolate=False)
extrema = cs.derivative().roots()
if self.emission:
lambda_m = extrema[np.argmax(cs(extrema))]
else:
lambda_m = extrema[np.argmin(cs(extrema))]
flux_m = cs(lambda_m)
# rough calculation of flux minimum uncertainty
flux_m_err = np.median(ey_values)
# compute wavelength error has std of all wavelengths corresponding to fluxes within noise from minimum if not overridden
if self.emission:
lambda_m_err = np.std(x_values[y_values > (flux_m - flux_m_err)])
else:
lambda_m_err = np.std(x_values[y_values < (flux_m + flux_m_err)])
# optionally override lambda uncertainty with a specified value
if (self.lambda_m_err != 'measure') and (
(type(self.lambda_m_err) == type(1)) or
(type(self.lambda_m_err) == type(1.1))):
lambda_m_err = self.lambda_m_err
return lambda_m, lambda_m_err, flux_m, flux_m_err
def generate_dist_vels(v0, vt, accel, jerk):
"""
Use a cubic spline to fit the distance vs speed
:param v0: start velocity
:param vt: target velocity
:param accel: maximum acceleration
:param jerk: maximum jerk
:return: list of calculated distances, list of calculated velocities
"""
ds, ts = WaypointUpdater.get_min_distance(
v0, vt, accel, jerk, return_list=True)
if len(ds) < 2:
return [0], [v0]
cs_d = CubicSpline(ts, ds, bc_type='natural')
cs_v = cs_d.derivative(nu=1)
# generate sampled points from spline
ts_samples = np.arange(T_STEP_SIZE, ts[-1] + T_STEP_SIZE, T_STEP_SIZE)
ds_samples = cs_d(ts_samples)
vs_samples = cs_v(ts_samples)
if LOG:
accel_s = cs_d(ts_samples, 2)
js = cs_d(ts_samples, 3)
rospy.loginfo(
"max accel %.03f, jerk %.03f", np.max(
np.abs(accel_s)), np.max(
np.abs(js)))
return ds_samples, vs_samples
def interpolate(self, new_time, derivative_order=0, out=None):
new_time = np.asarray(new_time)
if new_time.ndim != 1:
raise ValueError(f"New time array must have exactly 1 dimension; it has {new_time.ndim}.")
new_shape = self.shape[:-2] + (new_time.size, self.shape[-1])
if out is not None:
out = np.asarray(out)
if out.shape != new_shape:
raise ValueError(
f"Output array should have shape {new_shape} for consistency with new time array and modes array"
)
if out.dtype != np.complex:
raise ValueError(f"Output array should have dtype `complex`; it has dtype {out.dtype}")
result = out or np.empty(new_shape, dtype=complex)
if derivative_order > 3:
raise ValueError(
f"{type(self)} interpolation uses CubicSpline, and cannot take a derivative of order {derivative_order}"
)
spline = CubicSpline(self.u, self.view(np.ndarray), axis=-2)
if derivative_order < 0:
spline = spline.antiderivative(-derivative_order)
elif 0 < derivative_order <= 3:
spline = spline.derivative(derivative_order)
result[:] = spline(new_time)
metadata = self._metadata.copy()
metadata["time"] = new_time
return type(self)(result, **metadata)
def stationary_rotation(dt, lat, alt, Cnb, Cbs=None):
"""Simulate readings on a stationary bench.
Parameters
----------
dt : float
Time step.
lat : float
Latitude of the place.
alt : float
Altitude of the place.
Cnb : ndarray, shape (n_points, 3, 3)
Body attitude matrix.
Cbs : ndarray with shape (3, 3) or (n_points, 3, 3) or None
Sensor assembly attitude matrix relative to the body axes. If None,
(default) identity attitude is assumed.
Returns
-------
gyro, accel : ndarray, shape (n_points - 1, 3)
Gyro and accelerometer readings.
"""
n_points = Cnb.shape[0]
time = dt * np.arange(n_points)
lon_inertial = np.rad2deg(earth.RATE) * time
lat = np.full_like(lon_inertial, lat)
Cin = dcm.from_llw(lat, lon_inertial)
R = transform.lla_to_ecef(lat, lon_inertial, alt)
v_s = CubicSpline(time, R).derivative()
if Cbs is None:
Cns = Cnb
else:
Cns = util.mm_prod(Cnb, Cbs)
Cis = util.mm_prod(Cin, Cns)
Cib_spline = RotationSpline(time, Rotation.from_matrix(Cis))
a = Cib_spline.interpolator.c[2]
b = Cib_spline.interpolator.c[1]
c = Cib_spline.interpolator.c[0]
g = earth.gravitation_ecef(lat, lon_inertial, alt)
a_s = v_s.derivative()
d = a_s.c[1] - g[:-1]
e = a_s.c[0] - np.diff(g, axis=0) / dt
d = util.mv_prod(Cis[:-1], d, at=True)
e = util.mv_prod(Cis[:-1], e, at=True)
gyros, accels = _compute_readings(dt, a, b, c, d, e)
return gyros, accels
def test_three_points(self):
# gh-11758: Fails computing a_m2_m1
# In this case, s (first derivatives) could be found manually by solving
# system of 2 linear equations. Due to solution of this system,
# s[i] = (h1m2 + h2m1) / (h1 + h2), where h1 = x[1] - x[0], h2 = x[2] - x[1],
# m1 = (y[1] - y[0]) / h1, m2 = (y[2] - y[1]) / h2
x = np.array([1.0, 2.75, 3.0])
y = np.array([1.0, 15.0, 1.0])
S = CubicSpline(x, y, bc_type='periodic')
self.check_correctness(S, 'periodic', 'periodic')
assert_allclose(S.derivative(1)(x), np.array([-48.0, -48.0, -48.0]))
def _siteCharacteristics(t, v):
"""Compute the mean amplitude, cycle, and time of maximum and minimum.
"""
with np.errstate(under='ignore'):
amp = (v.max(axis=1) - v.min(axis=1)).mean()
cyc = v.mean(axis=0)
fun = CubicSpline(np.hstack([t, t[0] + 365.]),
np.hstack([cyc, cyc[0]]),
bc_type="periodic")
troot = fun.derivative().solve()
troot = troot[(troot >= 0) * (troot <= 365.)]
tmax = troot[fun(troot).argmax()]
tmin = troot[fun(troot).argmin()]
vs = fun(np.linspace(0, 365, 366))
return amp, tmax, tmin, vs
def ddot(x, y, interp=True):
"""
Return 2nd order derivative.
"""
if interp:
iinc = np.argsort(x)
_x = x[iinc]
_y = y[iinc]
fy = CubicSpline(_x, _y)
yddot = fy.derivative(2)(x)
else:
ydot = np.gradient(y, x)
yddot = np.gradient(ydot, x)
return yddot
def cubic_spline(self):
""" Cublic Spline for forward and spot curves
Input rates: only spot
Output rates: spot or forward rates
"""
# Compute Cublic Spline for spot rates
func = CubicSpline(self.ir[1, ], self.ir[0, ])
time_points = np.linspace(self.ir[1, 0], self.ir[1, -1], n)
if self.t == "forward":
# Calculate d/d(tau) r(tau)* tau = r'(tau) * tau + r(tau)
func_derivative = func.derivative(nu=1)
interpolation_values = func_derivative(time_points) * time_points + func(time_points)
self.interpolator = np.array([interpolation_values, time_points])
else: # spot rates
self.interpolator = np.array([func(time_points), time_points])
class SplineSignalPreprocessor(SignalPreprocessor):
def __init__(self, t, y, **kwargs):
super().__init__(t, y)
self.cs = None
def interpolate(self, t_new):
self.cs = CubicSpline(self.t, self.y)
return self.cs(t_new)
def calculate_time_derivative(self, t_new):
if self.cs is None:
self.interpolate(t_new=t_new)
pp = self.cs.derivative()
return pp(t_new)
def normalised_spline_Differencing(XX, TT, Normals, keeps=None):
from scipy.interpolate import CubicSpline
XX = np.divide(np.array(XX).T, Normals[:, np.newaxis]) # species X Obs
TT = np.array(TT)
diffs = []
for i in range(XX.shape[0]):
cs = CubicSpline(TT, XX[i, :])
dspl = cs.derivative()
diffs.append(dspl(TT))
if keeps is not None:
diffs[0] = [0.0] * len(TT)
return np.array(diffs)[keeps, :], XX[keeps, :], TT
else:
return np.array(diffs), XX, TT
def lmp_table_text(rang, eev, forces=None, finex=None):
if forces is None:
from scipy.interpolate import CubicSpline
ecs = CubicSpline(rang, eev)
fcs = ecs.derivative()
if finex is not None:
rang = finex
eev = ecs(finex)
forces = -fcs(rang)
nr = len(rang)
text = 'N %d\n\n' % nr
line_fmt = '{ir:12d} {r:>20.6e} {e:>20.6e} {f:>20.6e}\n'
ir = 1
for r, e, f in zip(rang, eev, forces):
line = line_fmt.format(ir=ir, r=r, e=e, f=f)
ir += 1
text += line
return text
def use_cubic_spline(pos_initial, pos_peak, pos_final, n_points):
# LEFT FOOT: points to interpolate over
l_x = [pos_initial[0], pos_peak[0], pos_final[0]]
l_y = [pos_initial[1], pos_peak[1], pos_final[1]]
l_z = [pos_initial[2], pos_peak[2], pos_final[2]]
# generate cubic spline (only using left foot for simplicity and to enforce symmetry)
l_cs = CubicSpline(l_x, [l_y, l_z], axis=1)
#get cubic spline derivative, if needed finish implementing this
dl_cs = l_cs.derivative()
# indices for cubic spline
l_xs = np.linspace(l_x[0], l_x[2], num=n_points)
left_spline = np.array([l_xs, l_cs(l_xs)[0], l_cs(l_xs)[1]])
# import ipdb; ipdb.set_trace()
return left_spline
def sdotmax(cs: interpolate.CubicSpline,
s: np.ndarray,
gmax,
smax,
gamma=4.257):
# [sdot, k, ] = sdotMax(PP, p_of_s, s, gmax, smax)
#
# Given a k-space curve C (in [1/cm] units), maximum gradient amplitude
# (in G/cm) and maximum slew-rate (in G/(cm*ms)).
# This function calculates the upper bound for the time parametrization
# sdot (which is a non scaled max gradient constaint) as a function
# of s.
#
# cs -- spline polynomial
# p_of_s -- parametrization vs arclength
# s -- arclength parametrization (0->1)
# gmax -- maximum gradient (G/cm)
# smax -- maximum slew rate (G/ cm*ms)
#
# returns the maximum sdot (1st derivative of s) as a function of
# arclength s
# Also, returns curvature as a function of s and length of curve (L)
#
# (c) Michael Lustig 2005
# last modified 2006
# Absolute value of 2nd derivative in curve space using cubic splines
cs2 = cs.derivative(2) # spline derivative
cs2_highres = cs2(s) # evaluated along arc length
k = np.linalg.norm(cs2_highres, axis=1) # magnitude
# calc I constraint curve (maximum gradient)
sdot1 = gamma * gmax * np.ones_like(s)
# calc II constraint curve (curve curvature dependent)
sdot2 = np.sqrt(gamma * smax / (k + np.finfo(float).eps))
# calc total constraint
sdot = np.minimum(sdot1, sdot2)
return sdot, k
def spline_Differencing(XX, TT):
from scipy.interpolate import CubicSpline
# redundant code, do change
if len(TT) == XX.shape[0]:
XX = np.array(XX).T # convert to species X Obs
TT = np.array(TT)
diffs = []
for i in range(XX.shape[0]):
cs = CubicSpline(TT, XX[i, :])
dspl = cs.derivative()
diffs.append(dspl(TT[1:]))
Arr = np.array(diffs)
Arr[0, :] = 0.0 # correction for the constant.
return Arr.T
class DiscretePath(PhasePath):
def __init__(self,
points,
seconds_per_point=10,
closed=True,
smooth=False):
if closed:
points.append(points[0])
self.points = np.array(points)
self.num_segments = len(self.points) - 1
self.seconds_per_point = seconds_per_point
self.spline = None
if smooth:
self.spline = CubicSpline(
[i / self.num_segments for i in range(self.num_segments + 1)],
self.points,
axis=0,
bc_type='periodic' if closed else 'not-a-knot',
extrapolate='periodic',
)
self.spline_grad = self.spline.derivative()
def duration(self):
return float(self.num_segments * self.seconds_per_point)
def grad_at_point(self, phase):
if self.spline:
return self.spline_grad(phase)
raise NotImplementedError
def at_point(self, phase):
if self.spline:
return self.spline(phase)
seg_index = min(int(phase * self.num_segments), self.num_segments - 1)
pa = self.points[seg_index]
pb = self.points[seg_index + 1]
seg_phase = phase * self.num_segments - seg_index
return seg_phase * pb + (1 - seg_phase) * pa
def solver_graph(solver, manifold, c0, c1, solution=None):
# The weight matrix
W = solver.New_Graph.todense()
# Find the Euclidean closest points on the graph to be used as fake start and end.
_, c0_indices = solver.kNN_graph.kneighbors(
c0.T) # Find the closest kNN_num+1 points to c0
_, c1_indices = solver.kNN_graph.kneighbors(
c1.T) # Find the closest kNN_num+1 points to c1
ind_closest_to_c0 = np.nan # The index in the training data closer to c0
ind_closest_to_c1 = np.nan
cost_to_c0 = 1e10
cost_to_c1 = 1e10
for n in range(
solver.kNN_num -
1): # We added one extra neighbor when we constructed the graph
# Pick the next point in the training data tha belong in the kNNs of c0 and c1
ind_c0 = c0_indices[0, n] # kNN index from the training data
ind_c1 = c1_indices[0, n] # kNN index from the training data
x_c0 = solver.data[ind_c0, :].reshape(-1,
1) # The kNN point near to c0
x_c1 = solver.data[ind_c1, :].reshape(-1,
1) # The kNN point near to c1
# Construct temporary straight lines
temp_curve_c0 = lambda t: evaluate_failed_solution(c0, x_c0, t)
temp_curve_c1 = lambda t: evaluate_failed_solution(c1, x_c1, t)
# Shortest path on graph prefers "low-weight" connections
temp_cost_c0 = curve_length(manifold, temp_curve_c0, tol=solver.tol)
temp_cost_c1 = curve_length(manifold, temp_curve_c1, tol=solver.tol)
# We found one of the Euclidean kNNs that has closer Riemannian distance from the other kNNs we have checked.
if temp_cost_c0 < cost_to_c0:
ind_closest_to_c0 = ind_c0
cost_to_c0 = temp_cost_c0
if temp_cost_c1 < cost_to_c1:
ind_closest_to_c1 = ind_c1
cost_to_c1 = temp_cost_c1
# The closest points in the graph to the test points c0, c1
source_ind = ind_closest_to_c0
end_ind = ind_closest_to_c1
path = [end_ind]
pairwise_lengths = []
temp_ind = end_ind
# Find the discrete path between source and sink. Each cell [i,j] keeps the previous point path before reaching j from i
while True:
prev_ind = solver.predecessors[
source_ind,
temp_ind] # The previous point to reach the [goal == temp_ind]
if prev_ind == -9999: # There is not any other point in the path
break
else:
path.append(prev_ind)
pairwise_lengths.append(
W[temp_ind, prev_ind]
) # Weight/distance between the current and previous node
temp_ind = prev_ind # Move the pointer to one point close to the source.
path.reverse(
) # Reverse the path from [end, ..., source] -> [source, ..., end]
inds = np.asarray(path)
DiscreteCurve_data = solver.data[
inds.flatten(), :] # The discrete path on the graph
# A heuristic to smooth the discrete path with a mean kernel
DiscreteCurve_data = np.concatenate(
(c0.T, DiscreteCurve_data[1:-1, :], c1.T), axis=0)
DiscreteCurve_new = np.empty((0, c0.shape[0]))
for n in range(1, DiscreteCurve_data.shape[0] - 1):
new_point = (DiscreteCurve_data[n - 1] + DiscreteCurve_data[n + 1] +
DiscreteCurve_data[n]) / 3
DiscreteCurve_new = np.concatenate(
(DiscreteCurve_new, new_point.reshape(1, -1)), axis=0)
DiscreteCurve_data = DiscreteCurve_new.copy()
DiscreteCurve = np.concatenate((c0.T, DiscreteCurve_data, c1.T), axis=0)
# Simple time parametrization of the curve
N_points = DiscreteCurve.shape[
0] # Number of points in the discrete shortest path
t = np.linspace(0, 1, num=N_points,
endpoint=True) # The time steps to construct the spline
# Interpolate the points with a cubic spline.
curve_spline = CubicSpline(
t, DiscreteCurve
) # The continuous curve that interpolates the points on the graph
dcurve_spline = curve_spline.derivative() # The derivative of the curve
curve = lambda t: evaluate_spline_solution(curve_spline, dcurve_spline, t)
# Return the solution
solution = {
'curve': curve,
'solver': solver.name,
'points': DiscreteCurve[1:-1, :],
'time_stamps': t[1:-1]
}
curve_length_eval = curve_length(manifold,
curve,
tol=solver.tol,
limit=solver.limit)
logmap = dcurve_spline(0).reshape(-1, 1) # The initial velocity
logmap = curve_length_eval * logmap.reshape(-1, 1) / np.linalg.norm(
logmap) # Scaling for normal coordinates.
failed = False
return curve, logmap, curve_length_eval, failed, solution
class SplineInterpolator(Interpolator):
"""Interpolate the given waypoints by spline.
This is a simple wrapper over scipy.CubicSpline class.
Parameters
----------
ss_waypoints: array
Shaped (N+1,). Path positions of the waypoints.
waypoints: array
Shaped (N+1, dof). Waypoints.
Attributes
----------
dof : int
Output dimension of the function
cspl : :class:`scipy.interpolate.CubicSpline`
The path.
cspld : :class:`scipy.interpolate.CubicSpline`
The path 1st derivative.
cspldd : :class:`scipy.interpolate.CubicSpline`
The path 2nd derivative.
"""
def __init__(self, ss_waypoints, waypoints, bc_type='not-a-knot'):
super(SplineInterpolator, self).__init__()
assert ss_waypoints[0] == 0, "First index must equals zero."
self.ss_waypoints = np.array(ss_waypoints)
self.waypoints = np.array(waypoints)
if np.isscalar(waypoints[0]):
self.dof = 1
else:
self.dof = self.waypoints[0].shape[0]
self.duration = ss_waypoints[-1]
assert self.ss_waypoints.shape[0] == self.waypoints.shape[0]
self.s_start = self.ss_waypoints[0]
self.s_end = self.ss_waypoints[-1]
if len(ss_waypoints) == 1:
def f1(s):
try:
ret = np.zeros((len(s), self.dof))
ret[:, :] = self.waypoints[0]
except TypeError:
ret = self.waypoints[0]
return ret
def f2(s):
try:
ret = np.zeros((len(s), self.dof))
except TypeError:
ret = np.zeros(self.dof)
return ret
self.cspl = f1
self.cspld = f2
self.cspldd = f2
else:
self.cspl = CubicSpline(ss_waypoints, waypoints, bc_type=bc_type)
self.cspld = self.cspl.derivative()
self.cspldd = self.cspld.derivative()
def get_duration(self):
return self.duration
def eval(self, ss_sam):
return self.cspl(ss_sam)
def evald(self, ss_sam):
return self.cspld(ss_sam)
def evaldd(self, ss_sam):
return self.cspldd(ss_sam)
def compute_rave_trajectory(self, robot):
""" Compute an OpenRAVE trajectory equivalent to this trajectory.
Parameters
----------
robot: OpenRAVE.Robot
Returns
-------
trajectory: OpenRAVE.Trajectory
"""
traj = orpy.RaveCreateTrajectory(robot.GetEnv(), "")
spec = robot.GetActiveConfigurationSpecification('cubic')
spec.AddDerivativeGroups(1, False)
spec.AddDerivativeGroups(2, True)
traj.Init(spec)
deltas = [0]
for i in range(len(self.ss_waypoints) - 1):
deltas.append(self.ss_waypoints[i + 1] - self.ss_waypoints[i])
if len(self.ss_waypoints) == 1:
q = self.eval(0)
qd = self.evald(0)
qdd = self.evaldd(0)
traj.Insert(traj.GetNumWaypoints(),
list(q) + list(qd) + list(qdd) + [0])
else:
qs = self.eval(self.ss_waypoints)
qds = self.evald(self.ss_waypoints)
qdds = self.evaldd(self.ss_waypoints)
for (q, qd, qdd, dt) in zip(qs, qds, qdds, deltas):
traj.Insert(traj.GetNumWaypoints(),
q.tolist() + qd.tolist() + qdd.tolist() + [dt])
return traj
#!/usr/bin/python
import numpy as np
import scipy.interpolate
from scipy.interpolate import CubicSpline
k, p = np.loadtxt('outspec.csv',
dtype=(float, float),
delimiter=",",
unpack=True)
csp = CubicSpline(k, p)
nsamp = 50
kdown = np.logspace(-4, 3, nsamp)
pdown = csp(kdown)
deriv = (csp.derivative(nu=1))(k)
#Setting boundary derivatives to match the finer spline.
#For some reason, the default boundary derivatives caused trouble.
cspdown = CubicSpline(kdown, pdown, bc_type=((1, deriv[0]), (1, deriv[-1])))
np.savez("spline_down", bkpts=cspdown.x, coeffs=cspdown.c)
def create_tildes(self, path): #{{{
# throws error if tildes already exist in path
if (os.path.isfile(path + 'tildes.npz')
and not self.num.debugging) or (self.tilde_arr is not None):
raise RuntimeError('Tildes already exist.')
if (self.convolved_signal_arr is not None):
print 'Doing tilde on convolved signal.'
signal = self.convolved_signal_arr
theta_out = self.theta_out_arr
else:
if os.path.isfile(path + 'convolved_profiles.npz'):
print 'Doing tilde on convolved signal from file.'
f = np.load(path + 'convolved_profiles.npz')
signal = f['convolved_signal']
theta_out = f['theta_out']
elif ((self.signal_arr is not None)
and (self.theta_out_arr is not None)):
print 'Doing tilde on unconvolved signal.'
signal = self.signal_arr
theta_out = self.theta_out_arr
elif os.path.isfile(path + 'profiles.npz'):
print 'Doing tilde on unconvolved signal from file.'
f = np.load(path + 'profiles.npz')
signal = f['signal']
theta_out = f['theta_out']
else:
raise RuntimeError(
'No convolved or unconvolved profiles exist.')
self.tilde_arr = np.empty(
(self.num.Npoints_M, self.num.Npoints_z, len(
self.num.lambda_grid)),
dtype=np.complex64)
for ii in xrange(self.num.Npoints_M):
start = time()
for jj in xrange(self.num.Npoints_z):
try:
spl_interpolator = CubicSpline(
signal[ii, jj, :][::-1],
self.num.scaled_real_theta_grid[::-1],
extrapolate=False) # theta ( signal )
der_interpolator = spl_interpolator.derivative(
) # dtheta/dsignal
theta_of_signal = np.nan_to_num(
spl_interpolator(self.num.signal_grid))
dtheta_dsignal = np.fabs(
np.nan_to_num(der_interpolator(self.num.signal_grid)))
self.tilde_arr[ii, jj, :] = np.fft.rfft(theta_of_signal *
dtheta_dsignal)
self.tilde_arr[
ii, jj, :] *= 2. * np.pi * theta_out[ii, jj]**2. * (
self.num.signal_grid[1] - self.num.signal_grid[0])
self.tilde_arr[ii, jj, :] -= self.tilde_arr[
ii, jj, 0] # subtract the zero mode
except ValueError:
warn(
'FT failed in (' + str(ii) + ',' + str(jj) +
').\nThis is caused by numerical issues in the convolution,\nwhich make the profile not monotonically decreasing.\nFalling back to slower method.',
UserWarning)
self.tilde_arr[ii, jj, :] = theta_out[
ii, jj]**2. * profiles.__tilde_lambda_loop(
self.num.scaled_real_theta_grid, signal[ii, jj, :],
2. * self.num.lambda_grid)
end = time()
if (ii % 4 == 0) and self.num.verbose:
print str((end - start) / 60. *
(self.num.Npoints_M -
ii)) + ' minutes remaining in create_tildes.'
class SplineInterpolator(AbstractGeometricPath):
"""Interpolate the given waypoints by cubic spline.
This interpolator is implemented as a simple wrapper over scipy's
CubicSpline class.
Parameters
----------
ss_waypoints: np.ndarray(m,)
Path positions of the waypoints.
waypoints: np.ndarray(m, d)
Waypoints.
bc_type: optional
Boundary conditions of the spline. Can be 'not-a-knot',
'clamped', 'natural' or 'periodic'.
- 'not-a-knot': The most default option, return the most naturally
looking spline.
- 'clamped': First-order derivatives of the spline at the two
end are clamped at zero.
See scipy.CubicSpline documentation for more details.
"""
def __init__(self, ss_waypoints, waypoints, bc_type="not-a-knot"):
super(SplineInterpolator, self).__init__()
self.ss_waypoints = np.array(ss_waypoints) # type: np.ndarray
self._q_waypoints = np.array(waypoints) # type: np.ndarray
assert self.ss_waypoints.shape[0] == self._q_waypoints.shape[0]
if len(ss_waypoints) == 1:
def _1dof_cspl(s):
try:
ret = np.zeros((len(s), self.dof))
ret[:, :] = self._q_waypoints[0]
except TypeError:
ret = self._q_waypoints[0]
return ret
def _1dof_cspld(s):
try:
ret = np.zeros((len(s), self.dof))
except TypeError:
ret = np.zeros(self.dof)
return ret
self.cspl = _1dof_cspl
self.cspld = _1dof_cspld
self.cspldd = _1dof_cspld
else:
self.cspl = CubicSpline(ss_waypoints, waypoints, bc_type=bc_type)
self.cspld = self.cspl.derivative()
self.cspldd = self.cspld.derivative()
def __call__(self, path_positions, order=0):
if order == 0:
return self.cspl(path_positions)
if order == 1:
return self.cspld(path_positions)
if order == 2:
return self.cspldd(path_positions)
raise ValueError("Invalid order %s" % order)
@property
def waypoints(self):
"""Tuple[np.ndarray, np.ndarray]: Return the waypoints.
The first element is the positions, the second element is the
array of waypoints.
"""
return self.ss_waypoints, self._q_waypoints
@deprecated
def get_duration(self):
"""Return the path's duration."""
return self.duration
@property
def duration(self):
"""Return the duration of the path."""
return self.ss_waypoints[-1] - self.ss_waypoints[0]
@property
def path_interval(self):
"""Return the start and end points."""
return np.array([self.ss_waypoints[0], self.ss_waypoints[-1]])
@deprecated
def get_path_interval(self):
"""Return the path interval."""
return self.path_interval
@property
def dof(self):
if np.isscalar(self._q_waypoints[0]):
return 1
return self._q_waypoints[0].shape[0]
def compute_rave_trajectory(self, robot):
"""Compute an OpenRAVE trajectory equivalent to this trajectory.
Parameters
----------
robot:
Openrave robot.
Returns
-------
trajectory:
Equivalent openrave trajectory.
"""
traj = orpy.RaveCreateTrajectory(robot.GetEnv(), "")
spec = robot.GetActiveConfigurationSpecification("cubic")
spec.AddDerivativeGroups(1, False)
spec.AddDerivativeGroups(2, True)
traj.Init(spec)
deltas = [0]
for i in range(len(self.ss_waypoints) - 1):
deltas.append(self.ss_waypoints[i + 1] - self.ss_waypoints[i])
if len(self.ss_waypoints) == 1:
q = self(0)
qd = self(0, 1)
qdd = self(0, 2)
traj.Insert(traj.GetNumWaypoints(),
list(q) + list(qd) + list(qdd) + [0])
else:
qs = self(self.ss_waypoints)
qds = self(self.ss_waypoints, 1)
qdds = self(self.ss_waypoints, 2)
for (q, qd, qdd, dt) in zip(qs, qds, qdds, deltas):
traj.Insert(
traj.GetNumWaypoints(),
q.tolist() + qd.tolist() + qdd.tolist() + [dt],
)
return traj
t0, x0, y0 = [float(p) for p in lines[2].split('\t')]
t_real_l = []
x_real_l = []
y_real_l = []
for i in range(2, len(lines)):
t_real[i - 2] += float(lines[i].split('\t')[0]) - t0
x_real[i - 2] += float(lines[i].split('\t')[1]) - x0
y_real[i - 2] += float(lines[i].split('\t')[2]) - y0
t_real_l.append(float(lines[i].split('\t')[0]) - t0)
x_real_l.append(float(lines[i].split('\t')[1]) - x0)
y_real_l.append(float(lines[i].split('\t')[2]) - y0)
print(t_real_l)
print(t_real_l)
csr_l_x = CubicSpline(t_real_l, x_real_l)
csr_l_y = CubicSpline(t_real_l, y_real_l)
der_x_l = csr_l_x.derivative()
der_y_l = csr_l_y.derivative()
sluttfarter.append(np.sqrt(csr_l_x(t_n[-1])**2 + csr_l_y(t_n[-1])**2))
f.close()
t_real = [t / 10 for t in t_real]
x_real = [x / 10 for x in x_real]
y_real = [y / 10 for y in y_real]
print("!!!")
print(x_real)
v_r = [0]
for i in range(1, 36):
dx = x_real[i] - x_real[i - 1]
dy = y_real[i] - y_real[i - 1]
dt = t_real[i] - t_real[i - 1]
ds = np.sqrt(dx * dx + dy * dy)
class SplineInterpolator(Interpolator):
"""Interpolate the given waypoints by cubic spline.
This interpolator is implemented as a simple wrapper over scipy's
CubicSpline class.
Parameters
----------
ss_waypoints: array
Shaped (N+1,). Path positions of the waypoints.
waypoints: array
Shaped (N+1, dof). Waypoints.
bc_type: str, optional
Boundary condition. Can be 'not-a-knot', 'clamped', 'natural' or 'periodic'.
See scipy.CubicSpline documentation for more details.
Attributes
----------
dof : int
Output dimension of the function
cspl : :class:`scipy.interpolate.CubicSpline`
The path.
cspld : :class:`scipy.interpolate.CubicSpline`
The path 1st derivative.
cspldd : :class:`scipy.interpolate.CubicSpline`
The path 2nd derivative.
"""
def __init__(self, ss_waypoints, waypoints, bc_type='clamped'):
super(SplineInterpolator, self).__init__()
assert ss_waypoints[0] == 0, "First index must equals zero."
self.ss_waypoints = np.array(ss_waypoints)
self.waypoints = np.array(waypoints)
self.bc_type = bc_type
assert self.ss_waypoints.shape[0] == self.waypoints.shape[0]
self.s_start = self.ss_waypoints[0]
self.s_end = self.ss_waypoints[-1]
if len(ss_waypoints) == 1:
def _1dof_cspl(s):
try:
ret = np.zeros((len(s), self.dof))
ret[:, :] = self.waypoints[0]
except TypeError:
ret = self.waypoints[0]
return ret
def _1dof_cspld(s):
try:
ret = np.zeros((len(s), self.dof))
except TypeError:
ret = np.zeros(self.dof)
return ret
self.cspl = _1dof_cspl
self.cspld = _1dof_cspld
self.cspldd = _1dof_cspld
else:
self.cspl = CubicSpline(ss_waypoints, waypoints, bc_type=bc_type)
self.cspld = self.cspl.derivative()
self.cspldd = self.cspld.derivative()
def get_waypoints(self):
"""Return the appropriate scaled waypoints."""
return self.ss_waypoints, self.waypoints
def get_duration(self):
warnings.warn(
"get_duration is deprecated, use duration (property) instead",
PendingDeprecationWarning)
return self.duration
@property
def duration(self):
return self.ss_waypoints[-1] - self.ss_waypoints[0]
@property
def dof(self):
if np.isscalar(self.waypoints[0]):
return 1
return self.waypoints[0].shape[0]
def get_dof(self): # type: () -> int
warnings.warn("get_dof is deprecated, use dof (property) instead",
PendingDeprecationWarning)
return self.dof
def eval(self, ss_sam):
return self.cspl(ss_sam)
def evald(self, ss_sam):
return self.cspld(ss_sam)
def evaldd(self, ss_sam):
return self.cspldd(ss_sam)
def compute_rave_trajectory(self, robot):
"""Compute an OpenRAVE trajectory equivalent to this trajectory.
Parameters
----------
robot:
Openrave robot.
Returns
-------
trajectory:
Equivalent openrave trajectory.
"""
traj = orpy.RaveCreateTrajectory(robot.GetEnv(), "")
spec = robot.GetActiveConfigurationSpecification('cubic')
spec.AddDerivativeGroups(1, False)
spec.AddDerivativeGroups(2, True)
traj.Init(spec)
deltas = [0]
for i in range(len(self.ss_waypoints) - 1):
deltas.append(self.ss_waypoints[i + 1] - self.ss_waypoints[i])
if len(self.ss_waypoints) == 1:
q = self.eval(0)
qd = self.evald(0)
qdd = self.evaldd(0)
traj.Insert(traj.GetNumWaypoints(),
list(q) + list(qd) + list(qdd) + [0])
else:
qs = self.eval(self.ss_waypoints)
qds = self.evald(self.ss_waypoints)
qdds = self.evaldd(self.ss_waypoints)
for (q, qd, qdd, dt) in zip(qs, qds, qdds, deltas):
traj.Insert(traj.GetNumWaypoints(),
q.tolist() + qd.tolist() + qdd.tolist() + [dt])
return traj
import numpy as np import matplotlib.pyplot as plt import cv2 import SplineRoadNew from scipy.interpolate import CubicSpline SR = SplineRoadNew.SplineRoad(count_cone=150) _, track_points = SR.standard_track_generate_data(s=0.07, a=0) # print(track_points[0,:]) fig, ax = plt.subplots(2, 1) x = track_points[0, :] y = track_points[1, :] print(np.arange(0, x.shape[0]).shape) ax[0].plot(x, y, 'o') tt = np.arange(0, x.shape[0]) cs = CubicSpline(np.arange(0, x.shape[0]), np.c_[x, y], bc_type='periodic') cs.derivative() # new_points = splev(u, tck) ax[1].plot(cs(tt)[:, 0], cs(tt)[:, 1], 'o') plt.show()
def from_position(dt, lat, lon, alt, h, p, r):
"""Generate inertial readings given position and attitude.
Parameters
----------
dt : float
Time step.
lat, lon, alt : array_like, shape (n_points,)
Time series of latitude, longitude and altitude.
h, p, r : array_like, shape (n_points,)
Time series of heading, pitch and roll angles.
Returns
-------
traj : DataFrame
Trajectory. Contains n_points rows.
gyro : ndarray, shape (n_points - 1, 3)
Gyro readings.
accel : ndarray, shape (n_points - 1, 3)
Accelerometer readings.
"""
lat = np.asarray(lat, dtype=float)
lon = np.asarray(lon, dtype=float)
alt = np.asarray(alt, dtype=float)
h = np.asarray(h, dtype=float)
p = np.asarray(p, dtype=float)
r = np.asarray(r, dtype=float)
n_points = lat.shape[0]
time = dt * np.arange(n_points)
lat_inertial = lat.copy()
lon_inertial = lon.copy()
lon_inertial += np.rad2deg(earth.RATE) * time
Cin = dcm.from_llw(lat_inertial, lon_inertial)
R = transform.lla_to_ecef(lat_inertial, lon_inertial, alt)
v_s = CubicSpline(time, R).derivative()
v = v_s(time)
V = v.copy()
V[:, 0] += earth.RATE * R[:, 1]
V[:, 1] -= earth.RATE * R[:, 0]
V = util.mv_prod(Cin, V, at=True)
Cnb = dcm.from_hpr(h, p, r)
Cib = util.mm_prod(Cin, Cnb)
Cib_spline = RotationSpline(time, Rotation.from_matrix(Cib))
a = Cib_spline.interpolator.c[2]
b = Cib_spline.interpolator.c[1]
c = Cib_spline.interpolator.c[0]
g = earth.gravitation_ecef(lat_inertial, lon_inertial, alt)
a_s = v_s.derivative()
d = a_s.c[1] - g[:-1]
e = a_s.c[0] - np.diff(g, axis=0) / dt
d = util.mv_prod(Cib[:-1], d, at=True)
e = util.mv_prod(Cib[:-1], e, at=True)
gyros, accels = _compute_readings(dt, a, b, c, d, e)
traj = pd.DataFrame(index=np.arange(time.shape[0]))
traj['lat'] = lat
traj['lon'] = lon
traj['alt'] = alt
traj['VE'] = V[:, 0]
traj['VN'] = V[:, 1]
traj['VU'] = V[:, 2]
traj['h'] = h
traj['p'] = p
traj['r'] = r
return traj, gyros, accels
#print(spline(xspline))
ax1.plot(xspline, spline(xspline))
#ax1.plot(xspline,uspline(xspline))
#ax1.plot(x1,p1(x1), x2, p2(x2))
ax1.set_title(r"Continuum Spectrum")
ax1.set_xlabel(r"$\log_{10}(\nu$ [GHz])")
ax1.set_ylabel(r"$\log_{10}$(Flux Density [$\mu$Jy])")
ax1.legend(["Cubic Spline", "Measured Flux Density"], loc='best')
ax1.margins(0.05)
fig1.savefig('fluxVsFreq.png', dpi=300, bbox_inches='tight')
#plt.legend()
#plt.show()
fig2 = plt.figure()
ax2 = fig2.add_subplot(111)
ax2.plot(xspline, spline.derivative(1)(xspline))
#ax2.plot(xspline,uspline.derivative()(xspline))
ax2.set_title(r"Spectral index as a function of frequency")
ax2.set_xlabel(r"$\log_{10}(\nu$ [GHz])")
ax2.set_ylabel(r"$\alpha$")
ax2.legend(["Derivative of Cubic Spline"], loc='best')
ax2.margins(0.05)
fig2.savefig('spectral index.png', dpi=300, bbox_inches='tight')
fig3 = plt.figure() #plot fluxes
ax3 = fig3.add_subplot(111)
ax3.errorbar(np.log10(fluxes[:, 0]),
np.log10(fluxes[:, 1]),
yerr=[
np.subtract(np.log10(np.subtract(fluxes[:, 1], fluxes[:, 2])),
np.log10(fluxes[:, 1])),
died = np.asarray(died)
enqm = np.asarray(enqm)
nben = np.asarray(nben)
# subtract the nonbonded energy to get the "QM torsional"
enfit = enqm - nben
died_spline = died.copy()
en_spline = enqm.copy()
died_enfit_spline = died.copy()
enfit_spline = enfit.copy()
f = CubicSpline(died_spline, en_spline)
ffit = CubicSpline(died_enfit_spline, enfit_spline)
cr_pts = f.derivative().roots()
deriv2 = f.derivative(2)
cr_pts = np.delete(cr_pts, np.where(deriv2(cr_pts) < 0.))
# order and remove the points relative to the transition states
npremove = ceil(args.cut_energy*len(died))
if args.cut_from_total:
lowenremove = -np.sort(-enqm)[npremove-1]
filtbar = np.where(enqm >= lowenremove)
filtxc = np.where(f(xc) >= lowenremove)
else:
lowenremove = -np.sort(-enfit)[npremove-1]
filtbar = np.where(enfit >= lowenremove)
filtxc = np.where(ffit(xc) >= lowenremove)
olddied = died.copy()
def plot_mep(atom_pos, mep_energies, image_name = None, show = None):
"""
Used for NEB method
atom_pos (list) - xcart positions of diffusing atom along the path,
mep_energies (list) - full energies of the system corresponding to atom_pos
"""
#Create
atom_pos = np.array(atom_pos)
data = atom_pos.T #
tck, u= interpolate.splprep(data) #now we get all the knots and info about the interpolated spline
path = interpolate.splev(np.linspace(0,1,500), tck) #increase the resolution by increasing the spacing, 500 in this example
path = np.array(path)
diffs = np.diff(path.T, axis = 0)
path_length = np.linalg.norm( diffs, axis = 1).sum()
mep_pos = np.array([p*path_length for p in u])
if 0: #plot the path in 3d
fig = plt.figure()
ax = Axes3D(fig)
ax.plot(data[0], data[1], data[2], label='originalpoints', lw =2, c='Dodgerblue')
ax.plot(path[0], path[1], path[2], label='fit', lw =2, c='red')
ax.legend()
plt.show()
mine = min(mep_energies)
eners = np.array(mep_energies)-mine
xnew = np.linspace(0, path_length)
# ynew = spline(mep_pos, eners, xnew )
# spl = CubicSpline(mep_pos, eners, bc_type = 'natural' ) second-derivative zero
spl = CubicSpline(mep_pos, eners, bc_type = 'clamped' ) #first derivative zero
ynew = spl(xnew)
#minimum now is always zero,
spl_der = spl.derivative()
mi = min(xnew)
ma = max(xnew)
r = spl_der.roots()
print(r)
r = r[ np.logical_and(mi<r, r<ma) ] # only roots inside the interval are interesting
diff_barrier = max( spl(r) ) # the maximum value
print_and_log('plot_mep(): Diffusion barrier =',round(diff_barrier, 2),' eV', imp = 'y')
# sys.exit()
path2saved = fit_and_plot(orig = (mep_pos, eners, 'ro'), spline = (xnew, ynew, 'b-'), xlim = (-0.05, None ),
xlabel = 'Reaction coordinate ($\AA$)', ylabel = 'Energy (eV)', image_name = image_name, show = show)
return path2saved, diff_barrier
