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