def test_derivatives(self): P = PiecewisePolynomial(self.xi, self.yi, 3) m = 4 r = P.derivatives(self.test_xs, m) #print r.shape, r for i in xrange(m): assert_almost_equal(P.derivative(self.test_xs, i), r[i])
def test_derivatives(self): P = PiecewisePolynomial(self.xi, self.yi, 3) m = 4 r = P.derivatives(self.test_xs, m) # print r.shape, r for i in xrange(m): assert_almost_equal(P.derivative(self.test_xs, i), r[i])
def test_scalar(self): P = PiecewisePolynomial(self.xi,self.yi,3) assert_almost_equal(P(self.test_xs[0]),self.spline_ys[0]) assert_almost_equal(P.derivative(self.test_xs[0],1),self.spline_yps[0]) assert_almost_equal(P(np.array(self.test_xs[0])),self.spline_ys[0]) assert_almost_equal(P.derivative(np.array(self.test_xs[0]),1), self.spline_yps[0])
def test_shapes_scalarvalue_derivative(self): P = PiecewisePolynomial(self.xi,self.yi,4) n = 4 assert_array_equal(np.shape(P.derivative(0,1)), ()) assert_array_equal(np.shape(P.derivative(np.array(0),1)), ()) assert_array_equal(np.shape(P.derivative([0],1)), (1,)) assert_array_equal(np.shape(P.derivative([0,1],1)), (2,))
def test_shapes_vectorvalue_derivative(self): P = PiecewisePolynomial(self.xi, np.multiply.outer(self.yi, np.arange(3)), 4) n = 4 assert_array_equal(np.shape(P.derivative(0, 1)), (3, )) assert_array_equal(np.shape(P.derivative([0], 1)), (1, 3)) assert_array_equal(np.shape(P.derivative([0, 1], 1)), (2, 3))
def test_incremental(self): with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial([self.xi[0]], [self.yi[0]], 3) for i in xrange(1,len(self.xi)): P.append(self.xi[i],self.yi[i],3) assert_almost_equal(P(self.test_xs),self.spline_ys)
def test_incremental(self): with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial([self.xi[0]], [self.yi[0]], 3) for i in xrange(1, len(self.xi)): P.append(self.xi[i], self.yi[i], 3) assert_almost_equal(P(self.test_xs), self.spline_ys)
def test_shapes_vectorvalue_derivative(self): with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=DeprecationWarning) P = PiecewisePolynomial(self.xi, np.multiply.outer(self.yi, np.arange(3)), 4) n = 4 assert_array_equal(np.shape(P.derivative(0, 1)), (3,)) assert_array_equal(np.shape(P.derivative([0], 1)), (1, 3)) assert_array_equal(np.shape(P.derivative([0, 1], 1)), (2, 3))
def test_wrapper(self): P = PiecewisePolynomial(self.xi, self.yi) assert_almost_equal(P(self.test_xs), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs)) assert_almost_equal( P.derivative(self.test_xs, 2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=2) ) assert_almost_equal( P.derivatives(self.test_xs, 2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=[0, 1]) )
def test_scalar(self): with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=DeprecationWarning) P = PiecewisePolynomial(self.xi, self.yi, 3) assert_almost_equal(P(self.test_xs[0]), self.spline_ys[0]) assert_almost_equal(P.derivative(self.test_xs[0], 1), self.spline_yps[0]) assert_almost_equal(P(np.array(self.test_xs[0])), self.spline_ys[0]) assert_almost_equal(P.derivative(np.array(self.test_xs[0]), 1), self.spline_yps[0])
def test_vector(self): xs = [0, 1, 2] ys = [[[0, 1]], [[1, 0], [-1, -1]], [[2, 1]]] P = PiecewisePolynomial(xs, ys) Pi = [PiecewisePolynomial(xs, [[yd[i] for yd in y] for y in ys]) for i in xrange(len(ys[0][0]))] test_xs = np.linspace(-1, 3, 100) assert_almost_equal(P(test_xs), np.rollaxis(np.asarray([p(test_xs) for p in Pi]), -1)) assert_almost_equal( P.derivative(test_xs, 1), np.transpose(np.asarray([p.derivative(test_xs, 1) for p in Pi]), (1, 0)) )
def test_shapes_vectorvalue_derivative(self): with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial(self.xi, np.multiply.outer(self.yi, np.arange(3)),4) n = 4 assert_array_equal(np.shape(P.derivative(0,1)), (3,)) assert_array_equal(np.shape(P.derivative([0],1)), (1,3)) assert_array_equal(np.shape(P.derivative([0,1],1)), (2,3))
def test_scalar(self): with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial(self.xi,self.yi,3) assert_almost_equal(P(self.test_xs[0]),self.spline_ys[0]) assert_almost_equal(P.derivative(self.test_xs[0],1),self.spline_yps[0]) assert_almost_equal(P(np.array(self.test_xs[0])),self.spline_ys[0]) assert_almost_equal(P.derivative(np.array(self.test_xs[0]),1), self.spline_yps[0])
def test_derivatives(self): with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial(self.xi,self.yi,3) m = 4 r = P.derivatives(self.test_xs,m) #print r.shape, r for i in xrange(m): assert_almost_equal(P.derivative(self.test_xs,i),r[i])
def test_shapes_scalarvalue_derivative(self): with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial(self.xi,self.yi,4) n = 4 assert_array_equal(np.shape(P.derivative(0,1)), ()) assert_array_equal(np.shape(P.derivative(np.array(0),1)), ()) assert_array_equal(np.shape(P.derivative([0],1)), (1,)) assert_array_equal(np.shape(P.derivative([0,1],1)), (2,))
def test_derivatives(self): with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial(self.xi, self.yi, 3) m = 4 r = P.derivatives(self.test_xs, m) #print r.shape, r for i in xrange(m): assert_almost_equal(P.derivative(self.test_xs, i), r[i])
def evaluate(z): shutil.rmtree(os.path.join(os.getcwd(), "eval/"), ignore_errors=True) os.mkdir(os.path.join(os.getcwd(), "eval")) thresholds = np.linspace(0, 1, 21) mid = np.arange(0, 1, 0.01) precision_set = [] recall_set = [] refset = {} tempx = 0 for i in xrange(len(malorder)): if malorder[i].split("-")[0] not in refset: refset[malorder[i].split("-")[0]] = [i] else: refset[malorder[i].split("-")[0]].append(i) with open("eval/refset.txt", "w") as f: for family in refset: f.write(family + ": " + ' '.join([str(x) for x in refset[family]]) + "\n") for i in thresholds: with open("eval/refset.txt") as f: reflines = f.readlines() hc = fcluster(z, i, 'distance') cdblines = get_clist(hc, i) precision = calculate_precision(reflines, cdblines) recall = calculate_recall(reflines, cdblines) precision_set.append(precision) recall_set.append(recall) global p1 p1 = PiecewisePolynomial(thresholds, np.array(precision_set)[:, np.newaxis]) global p2 p2 = PiecewisePolynomial(thresholds, np.array(recall_set)[:, np.newaxis]) for x in mid: root, infodict, ier, mesg = fsolve(pdiff, x, full_output=True) root = float("%.3f" % root[0]) if ier == 1 and thresholds.min() < root < thresholds.max(): tempx = root break tempy = p2(tempx) if p1(tempx) > p2(tempx): tempy = p1(tempx) print "Best x:", tempx, "Best y:", tempy try: pleg1, = plt.plot(thresholds, precision_set, '-bo', label="Precision") pleg2, = plt.plot(thresholds, recall_set, '-rx', label="Recall") plt.legend([pleg1, pleg2], ["Precision", "Recall"], loc="center right") plt.xlabel('Distance threshold (t)') plt.ylabel("Precision and Recall") plt.show() except: raise finally: plt.close() return tempx, tempy
def test_wrapper(self): with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=DeprecationWarning) P = PiecewisePolynomial(self.xi, self.yi) assert_almost_equal(P(self.test_xs), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs)) assert_almost_equal( P.derivative(self.test_xs, 2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=2) ) assert_almost_equal( P.derivatives(self.test_xs, 2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=[0, 1]) )
def test_vector(self): xs = [0, 1, 2] ys = [[[0,1]],[[1,0],[-1,-1]],[[2,1]]] P = PiecewisePolynomial(xs,ys) Pi = [PiecewisePolynomial(xs,[[yd[i] for yd in y] for y in ys]) for i in xrange(len(ys[0][0]))] test_xs = np.linspace(-1,3,100) assert_almost_equal(P(test_xs), np.rollaxis(np.asarray([p(test_xs) for p in Pi]),-1)) assert_almost_equal(P.derivative(test_xs,1), np.transpose(np.asarray([p.derivative(test_xs,1) for p in Pi]), (1,0)))
def test_vector(self): xs = [0, 1, 2] ys = [[[0, 1]], [[1, 0], [-1, -1]], [[2, 1]]] with warnings.catch_warnings(): warnings.filterwarnings("ignore", category=DeprecationWarning) P = PiecewisePolynomial(xs, ys) Pi = [PiecewisePolynomial(xs, [[yd[i] for yd in y] for y in ys]) for i in xrange(len(ys[0][0]))] test_xs = np.linspace(-1, 3, 100) assert_almost_equal(P(test_xs), np.rollaxis(np.asarray([p(test_xs) for p in Pi]), -1)) assert_almost_equal( P.derivative(test_xs, 1), np.transpose(np.asarray([p.derivative(test_xs, 1) for p in Pi]), (1, 0)) )
def make_pps(traj): ''' take a trajectory object as input return a dictionary of piecewise polynomials, one for each name ''' pps = {} # differential states for name in traj.dvMap._xNames: # make piecewise poly pps[name] = None for timestepIdx in range(traj.dvMap._nk): for nicpIdx in range(traj.dvMap._nicp): ts = [] ys = [] for degIdx in range(traj.dvMap._deg+1): ts.append(traj.tgrid[timestepIdx,nicpIdx,degIdx]) ys.append([traj.dvMap.lookup(name,timestep=timestepIdx,nicpIdx=nicpIdx,degIdx=degIdx)]) if pps[name] is None: pps[name] = PiecewisePolynomial(ts,ys) else: pps[name].extend(ts,ys) pps[name].extend([traj.tgrid[-1,0,0]],[[traj.dvMap.lookup(name,timestep=-1,nicpIdx=0,degIdx=0)]]) # algebraic variables for name in traj.dvMap._zNames: # make piecewise poly pps[name] = None for timestepIdx in range(traj.dvMap._nk): for nicpIdx in range(traj.dvMap._nicp): ts = [] ys = [] for degIdx in range(1,traj.dvMap._deg+1): ts.append(traj.tgrid[timestepIdx,nicpIdx,degIdx]) ys.append([traj.dvMap.lookup(name,timestep=timestepIdx,nicpIdx=nicpIdx,degIdx=degIdx)]) if pps[name] is None: pps[name] = PiecewisePolynomial(ts,ys) else: pps[name].extend(ts,ys) # controls for name in traj.dvMap._uNames: # make piecewise poly ts = [] ys = [] for timestepIdx in range(traj.dvMap._nk): ts.append(traj.tgrid[timestepIdx,0,0]) ys.append([traj.dvMap.lookup(name,timestep=timestepIdx)]) pps[name] = PiecewisePolynomial(ts,ys) return pps
def test_vector(self): xs = [0, 1, 2] ys = [[[0,1]],[[1,0],[-1,-1]],[[2,1]]] with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial(xs,ys) Pi = [PiecewisePolynomial(xs,[[yd[i] for yd in y] for y in ys]) for i in xrange(len(ys[0][0]))] test_xs = np.linspace(-1,3,100) assert_almost_equal(P(test_xs), np.rollaxis(np.asarray([p(test_xs) for p in Pi]),-1)) assert_almost_equal(P.derivative(test_xs,1), np.transpose(np.asarray([p.derivative(test_xs,1) for p in Pi]), (1,0)))
def __init__(self, r, energies, forces, structures, **kwargs): """ Initializes an NEBAnalysis from the cumulative root mean squared distances between structures, the energies, the forces, the structures and the interpolation_order for the analysis. Args: r: Root mean square distances between structures energies: Energies of each structure along reaction coordinate forces: Tangent forces along the reaction coordinate. structures ([Structure]): List of Structures along reaction coordinate. """ self.r = np.array(r) self.energies = np.array(energies) self.forces = np.array(forces) self.structures = structures # We do a piecewise interpolation between the points. Each spline ( # cubic by default) is constrained by the boundary conditions of the # energies and the tangent force, i.e., the derivative of # the energy at each pair of points. if scipy_old_piecewisepolynomial: self.spline = PiecewisePolynomial( self.r, np.array([self.energies, -self.forces]).T, orders=3) else: # New scipy implementation for scipy > 0.18.0 self.spline = CubicSpline(x=self.r, y=self.energies, bc_type=((1, 0.0), (1, 0.0)))
def setup_spline(self, spline_options=None): """ Setup of the options for the spline interpolation Args: spline_options (dict): Options for cubic spline. For example, {"saddle_point": "zero_slope"} forces the slope at the saddle to be zero. """ self.spline_options = spline_options relative_energies = self.energies - self.energies[0] if scipy_old_piecewisepolynomial: if self.spline_options: raise RuntimeError('Option for saddle point not available with' 'old scipy implementation') self.spline = PiecewisePolynomial( self.r, np.array([relative_energies, -self.forces]).T, orders=3) else: # New scipy implementation for scipy > 0.18.0 if self.spline_options.get('saddle_point', '') == 'zero_slope': imax = np.argmax(relative_energies) self.spline = CubicSpline(x=self.r[:imax + 1], y=relative_energies[:imax + 1], bc_type=((1, 0.0), (1, 0.0))) cspline2 = CubicSpline(x=self.r[imax:], y=relative_energies[imax:], bc_type=((1, 0.0), (1, 0.0))) self.spline.extend(c=cspline2.c, x=cspline2.x[1:]) else: self.spline = CubicSpline(x=self.r, y=relative_energies, bc_type=((1, 0.0), (1, 0.0)))
def __init__(self, outcars, structures, interpolation_order=3): """ Initializes an NEBAnalysis from Outcar and Structure objects. Use the static constructors, e.g., :class:`from_dir` instead if you prefer to have these automatically generated from a directory of NEB calculations. Args: outcars ([Outcar]): List of Outcar objects. Note that these have to be ordered from start to end along reaction coordinates. structures ([Structure]): List of Structures along reaction coordinate. Must be same length as outcar. interpolation_order (int): Order of polynomial to use to interpolate between images. Same format as order parameter in scipy.interplotate.PiecewisePolynomial. """ if len(outcars) != len(structures): raise ValueError("# of Outcars must be same as # of Structures") # Calculate cumulative root mean square distance between structures, # which serves as the reaction coordinate. Note that these are # calculated from the final relaxed structures as the coordinates may # have changed from the initial interpolation. r = [0] prev = structures[0] for st in structures[1:]: dists = np.array([s2.distance(s1) for s1, s2 in zip(prev, st)]) r.append(np.sqrt(np.sum(dists ** 2))) prev = st r = np.cumsum(r) energies = [] forces = [] for i, o in enumerate(outcars): o.read_neb() energies.append(o.data["energy"]) if i in [0, len(outcars) - 1]: forces.append(0) else: forces.append(o.data["tangent_force"]) energies = np.array(energies) energies -= energies[0] forces = np.array(forces) self.r = np.array(r) self.energies = energies self.forces = forces self.structures = structures # We do a piecewise interpolation between the points. Each spline ( # cubic by default) is constrained by the boundary conditions of the # energies and the tangent force, i.e., the derivative of # the energy at each pair of points. if scipy_old_piecewisepolynomial: self.spline = PiecewisePolynomial( self.r, np.array([self.energies, -self.forces]).T, orders=interpolation_order) else: # New scipy implementation for scipy > 0.18.0 self.spline = CubicSpline(x=self.r, y=self.energies, bc_type=((1, 0.0), (1, 0.0)))
def test_wrapper(self): P = PiecewisePolynomial(self.xi, self.yi) assert_almost_equal( P(self.test_xs), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs)) assert_almost_equal( P.derivative(self.test_xs, 2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=2)) assert_almost_equal( P.derivatives(self.test_xs, 2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=[0, 1]))
def test_wrapper(self): with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial(self.xi,self.yi) assert_almost_equal(P(self.test_xs), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs)) assert_almost_equal(P.derivative(self.test_xs,2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=2)) assert_almost_equal(P.derivatives(self.test_xs,2), piecewise_polynomial_interpolate(self.xi, self.yi, self.test_xs, der=[0,1]))
def test_shapes_scalarvalue(self): with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial(self.xi, self.yi, 4) assert_array_equal(np.shape(P(0)), ()) assert_array_equal(np.shape(P(np.array(0))), ()) assert_array_equal(np.shape(P([0])), (1, )) assert_array_equal(np.shape(P([0, 1])), (2, ))
def test_shapes_vectorvalue_1d(self): yi = np.multiply.outer(np.asarray(self.yi), np.arange(1)) with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial(self.xi, yi, 4) assert_array_equal(np.shape(P(0)), (1, )) assert_array_equal(np.shape(P([0])), (1, 1)) assert_array_equal(np.shape(P([0, 1])), (2, 1))
def __init__(self, x, y, axis=0): x = np.asarray(x) y = np.asarray(y) axis = axis % y.ndim xp = x.reshape((x.shape[0],) + (1,)*(y.ndim-1)) yp = np.rollaxis(y, axis) data = np.empty((yp.shape[0], 2) + yp.shape[1:], y.dtype) data[:,0] = yp data[:,1] = self._get_derivatives(xp, yp) s = list(range(2, y.ndim + 1)) s.insert(axis, 1) s.insert(axis, 0) data = data.transpose(s) PiecewisePolynomial.__init__(self, x, data, orders=3, direction=None, axis=axis)
def _setup_phi_interpolator(self): """ Setup interpolater for phi, works on scalar and arrays """ # Generate piecewise 3th order polynomials to connect the discrete # values of phi obtained from from Poisson, using dphi/dr self._interpolator_set = True if (self.scale): phi_and_derivs = numpy.vstack([[self.phi],[self.dphidr1]]).T else: phi_and_derivs = numpy.vstack([[self.phihat],[self.dphidrhat1]]).T self._phi_poly = PiecewisePolynomial(self.r,phi_and_derivs,direction=1)
def test_shapes_scalarvalue_derivative(self): P = PiecewisePolynomial(self.xi, self.yi, 4) n = 4 assert_array_equal(np.shape(P.derivative(0, 1)), ()) assert_array_equal(np.shape(P.derivative(np.array(0), 1)), ()) assert_array_equal(np.shape(P.derivative([0], 1)), (1, )) assert_array_equal(np.shape(P.derivative([0, 1], 1)), (2, ))
def pla(data, period=15): N = int(len(data) / period) orig_x = range(0, len(data)) tck = splrep(orig_x, data, s=0) test_xs = np.linspace(0, len(data), N) spline_ys = splev(test_xs, tck) spline_yps = splev(test_xs, tck, der=1) xi = np.unique(tck[0]) yi = [[splev(x, tck, der=j) for j in xrange(3)] for x in xi] P = PiecewisePolynomial(xi, yi, orders=1) test_ys = P(test_xs) #inter_y = interp0(test_xs, test_ys, orig_x) inter_y = interp1(test_xs, test_ys, orig_x) #plt.plot(orig_x, data, orig_x, inter_y) #plt.show() mae = sqrt(mean_absolute_error(inter_y, data)) return [mae]
def _create_from_control_points(self, control_points, tangents, scale): """ Creates the FiberSource instance from control points, and a specified mode to compute the tangents. Parameters ---------- control_points : ndarray shape (N, 3) tangents : 'incoming', 'outgoing', 'symmetric' scale : multiplication factor. This is useful when the coodinates are given dimensionless, and we want a specific size for the phantom. """ # Compute instant points ts, from 0. to 1. # (time interval proportional to distance between control points) nb_points = control_points.shape[0] dists = np.zeros(nb_points) dists[1:] = np.sqrt((np.diff(control_points, axis=0) ** 2).sum(1)) ts = dists.cumsum() length = ts[-1] ts = ts / np.max(ts) # Create interpolation functions (piecewise polynomials) for x, y and z derivatives = np.zeros((nb_points, 3)) # The derivatives at starting and ending points are normal # to the surface of a sphere. derivatives[0, :] = -control_points[0] derivatives[-1, :] = control_points[-1] # As for other derivatives, we use discrete approx if tangents == 'incoming': derivatives[1:-1, :] = (control_points[1:-1] - control_points[:-2]) elif tangents == 'outgoing': derivatives[1:-1, :] = (control_points[2:] - control_points[1:-1]) elif tangents == 'symmetric': derivatives[1:-1, :] = (control_points[2:] - control_points[:-2]) else: raise Error('tangents should be one of the following: incoming, ' 'outgoing, symmetric') derivatives = (derivatives.T / np.sqrt((derivatives ** 2).sum(1))).T \ * length self.x_poly = PiecewisePolynomial(ts, scale * np.vstack((control_points[:, 0], derivatives[:, 0])).T) self.y_poly = PiecewisePolynomial(ts, scale * np.vstack((control_points[:, 1], derivatives[:, 1])).T) self.z_poly = PiecewisePolynomial(ts, scale * np.vstack((control_points[:, 2], derivatives[:, 2])).T)
def read_backscatter_yield_tables(): """ Read backscattering yield tables and intitialize spline functions. """ for material_name, byield_file in zip(par.MATERIAL_NAMES, par.BACKSCATTER_YIELD_FILES): if os.path.exists(byield_file): byield_file_path = byield_file else: byield_file_path = os.path.join(os.path.dirname(__file__), 'tables', byield_file) BYIELD_FILE_EXT[material_name] = os.path.splitext(byield_file_path)[1] if BYIELD_FILE_EXT[material_name] == '.npz': data_from_table = np.load(byield_file_path) t = data_from_table['t'] c = data_from_table['c'] k = data_from_table['k'] tck = [t, c, k] BYIELD_FUN[material_name] = Spline(tck) elif BYIELD_FILE_EXT[material_name] == '.pp': thetas, yields, dyields = np.loadtxt(byield_file_path, skiprows=1, unpack=True) BYIELD_FUN[material_name] = PiecewisePolynomial( thetas, zip(yields, dyields)) else: raise ValueError( "Invalid file extension of backscattering yield file.") # check if all material names have sputtering yields defined for material_name in par.MATERIAL_NAMES: if material_name not in BYIELD_FUN: raise AssertionError( "not for all material names backscatter yields defined.")
def compute_positions_velocities(self, times, x_positions_velocities, y_positions_velocities, z_positions_velocities): xinterpolator = PiecewisePolynomial(times, x_positions_velocities, orders=3, direction=1) yinterpolator = PiecewisePolynomial(times, y_positions_velocities, orders=3, direction=1) zinterpolator = PiecewisePolynomial(times, z_positions_velocities, orders=3, direction=1) T = np.arange(0, times[-1], self.dt) n = len(T) precomputed_positions = np.empty((3,n)) precomputed_positions[0,:] = xinterpolator(T) precomputed_positions[1,:] = yinterpolator(T) precomputed_positions[2,:] = zinterpolator(T) precomputed_velocities = np.empty((3,n)) precomputed_velocities[0,:] = xinterpolator.derivative(T) precomputed_velocities[1,:] = yinterpolator.derivative(T) precomputed_velocities[2,:] = zinterpolator.derivative(T) return T, precomputed_positions, precomputed_velocities, n
def test_derivative(self): with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial(self.xi,self.yi,3) assert_almost_equal(P.derivative(self.test_xs,1),self.spline_yps)
def move_trajectory(self, times, x_positions_velocities, y_positions_velocities, z_positions_velocities): xinterpolator = PiecewisePolynomial(times, x_positions_velocities, orders=3, direction=1) yinterpolator = PiecewisePolynomial(times, y_positions_velocities, orders=3, direction=1) zinterpolator = PiecewisePolynomial(times, z_positions_velocities, orders=3, direction=1) T = np.arange(0, times[-1], self.dt) n = len(T) velocity_and_w = np.zeros(6) velocity_and_w[0] = xinterpolator.derivative(T[0]) velocity_and_w[1] = yinterpolator.derivative(T[0]) velocity_and_w[2] = zinterpolator.derivative(T[0]) precomputed_positions = np.empty((3,n)) precomputed_positions[0,:] = xinterpolator(T) precomputed_positions[1,:] = yinterpolator(T) precomputed_positions[2,:] = zinterpolator(T) precomputed_velocities = np.empty((3,n)) precomputed_velocities[0,:] = xinterpolator.derivative(T) precomputed_velocities[1,:] = yinterpolator.derivative(T) precomputed_velocities[2,:] = zinterpolator.derivative(T) actual_positions = np.empty((3,n)) actual_positions[:,0] = self.get_position() corrector_velocities = np.empty((3,n)) corrector_velocities[:,0] = np.zeros(3) proportional_velocities = np.empty((3,n)) proportional_velocities[:,0] = np.zeros(3) integral_velocities = np.empty((3,n)) integral_velocities[:,0] = np.zeros(3) derivative_velocities = np.empty((3,n)) derivative_velocities[:,0] = np.zeros(3) vx_proportional, vy_proportional, vz_proportional = 0.0, 0.0, 0.0 last_vx_proportional, last_vy_proportional, last_vz_proportional = 0.0, 0.0, 0.0 vx_integral, vy_integral, vz_integral = 0.0, 0.0, 0.0 for i in xrange(1,n): t_start = rospy.get_time() self.limb.set_joint_velocities( self.make_joint_dict( self.get_joint_velocities(velocity_and_w))) time_interval = T[i] - T[i-1] position = self.get_position() vx_proportional = precomputed_positions[0,i] - position[0] vy_proportional = precomputed_positions[1,i] - position[1] vz_proportional = precomputed_positions[2,i] - position[2] proportional_velocities = self.kp * np.array([vx_proportional, vy_proportional, vz_proportional]) vx_integral += vx_proportional * time_interval vy_integral += vy_proportional * time_interval vz_integral += vz_proportional * time_interval integral_velocities = self.ki * np.array([vx_integral, vy_integral, vz_integral]) vx_derivative = (vx_proportional - last_vx_proportional)/time_interval vy_derivative = (vy_proportional - last_vy_proportional)/time_interval vz_derivative = (vz_proportional - last_vz_proportional)/time_interval derivative_velocities = self.kd * np.array([vx_derivative, vy_derivative, vz_derivative]) vx_corrector = self.kp * vx_proportional + self.ki * vx_integral + self.kd * vx_derivative vy_corrector = self.kp * vy_proportional + self.ki * vy_integral + self.kd * vy_derivative vz_corrector = self.kp * vz_proportional + self.ki * vz_integral + self.kd * vz_derivative corrector_velocities[:,i] = np.array([vx_corrector, vy_corrector, vz_corrector]) velocity_and_w[0] = precomputed_velocities[0,i] + vx_corrector velocity_and_w[1] = precomputed_velocities[1,i] + vy_corrector velocity_and_w[2] = precomputed_velocities[2,i] + vz_corrector last_vx_proportional = vx_proportional last_vy_proportional = vy_proportional last_vz_proportional = vz_proportional actual_positions[:,i] = position end_time = time_interval + t_start loginfo("Computation Took: {0} out of {1} seconds".format(rospy.get_time() - t_start, time_interval)) rospy.sleep(end_time - rospy.get_time()) loginfo("exit_control_mode") self.limb.exit_control_mode() paramtext = "%1.4f_%1.4f_%1.4f_%1.4f_%1.4f" % (self.kp, self.ki, self.kd, self.extra_motion_maximum, self.extra_motion_multiple) # date = "" date = str(datetime.now()) folder = "tests" A = np.empty((n,4)) A[:,0] = T A[:,1:] = actual_positions.T # np.savetxt("/home/cs4752/ros_ws/src/cs4752_proj2/{2}/{1}actual-positions-{0}.csv".format(paramtext,date,folder),A) B = np.empty((n,4)) B[:,0] = T B[:,1:] = precomputed_positions.T # np.savetxt("/home/cs4752/ros_ws/src/cs4752_proj2/{2}/{1}precomputed-positions-{0}.csv".format(paramtext,date,folder),B) loginfo("saved errors")
def test_derivative(self): with warnings.catch_warnings(): warnings.filterwarnings('ignore', category=DeprecationWarning) P = PiecewisePolynomial(self.xi, self.yi, 3) assert_almost_equal(P.derivative(self.test_xs, 1), self.spline_yps)
def interpolateInitialGuess(self,filename,force=False,quiet=False,numLoops=1): print "interpolating initial guess..." f=open(filename,'r') traj = pickle.load(f) f.close() assert isinstance(traj,trajectory.Trajectory), "the file \""+filename+"\" doean't have a pickled Trajectory" h = (traj.tgrid[-1,0,0] - traj.tgrid[0,0,0])/float(traj.dvMap._nk*traj.dvMap._nicp) h *= traj.dvMap._nk*traj.dvMap._nicp/float(self.nk*self.nicp) h *= numLoops pps = {} missing = [] ############# make piecewise polynomials ########### # differential states for name in traj.dvMap._xNames: # make piecewise poly pps[name] = None for timestepIdx in range(traj.dvMap._nk): for nicpIdx in range(traj.dvMap._nicp): ts = [] ys = [] for degIdx in range(traj.dvMap._deg+1): ts.append(traj.tgrid[timestepIdx,nicpIdx,degIdx]) ys.append([traj.dvMap.lookup(name,timestep=timestepIdx,nicpIdx=nicpIdx,degIdx=degIdx)]) if pps[name] is None: pps[name] = PiecewisePolynomial(ts,ys) else: pps[name].extend(ts,ys) pps[name].extend([traj.tgrid[-1,0,0]],[[traj.dvMap.lookup(name,timestep=-1,nicpIdx=0,degIdx=0)]]) # algebraic variables for name in traj.dvMap._zNames: # make piecewise poly pps[name] = None for timestepIdx in range(traj.dvMap._nk): for nicpIdx in range(traj.dvMap._nicp): ts = [] ys = [] for degIdx in range(1,traj.dvMap._deg+1): ts.append(traj.tgrid[timestepIdx,nicpIdx,degIdx]) ys.append([traj.dvMap.lookup(name,timestep=timestepIdx,nicpIdx=nicpIdx,degIdx=degIdx)]) if pps[name] is None: pps[name] = PiecewisePolynomial(ts,ys) else: pps[name].extend(ts,ys) # controls for name in traj.dvMap._uNames: # make piecewise poly ts = [] ys = [] for timestepIdx in range(traj.dvMap._nk): ts.append(traj.tgrid[timestepIdx,0,0]) ys.append([traj.dvMap.lookup(name,timestep=timestepIdx)]) pps[name] = PiecewisePolynomial(ts,ys) ############# interpolate ########### # interpolate differential states for name in self.dae.xNames(): if name not in pps: missing.append(name) continue # evaluate piecewise poly to set initial guess t0 = 0.0 for timestepIdx in range(self.nk): for nicpIdx in range(self.nicp): for degIdx in range(self.deg+1): time = t0 + h*self.lagrangePoly.tau_root[degIdx] if time > traj.tgrid[-1,0,0]: time -= traj.tgrid[-1,0,0] self.guess(name,pps[name](time),timestep=timestepIdx,nicpIdx=nicpIdx,degIdx=degIdx,force=force,quiet=quiet) t0 += h if t0 > traj.tgrid[-1,0,0]: t0 -= traj.tgrid[-1,0,0] self.guess(name,pps[name](t0),timestep=-1,nicpIdx=0,degIdx=0,force=force,quiet=quiet) # interpolate algebraic variables for name in self.dae.zNames(): if name not in pps: missing.append(name) continue # evaluate piecewise poly to set initial guess t0 = 0.0 for timestepIdx in range(self.nk): for nicpIdx in range(self.nicp): for degIdx in range(1,self.deg+1): time = t0 + h*self.lagrangePoly.tau_root[degIdx] if time > traj.tgrid[-1,0,0]: time -= traj.tgrid[-1,0,0] self.guess(name,pps[name](time),timestep=timestepIdx,nicpIdx=nicpIdx,degIdx=degIdx,force=force,quiet=quiet) t0 += h # interpolate controls for name in self.dae.uNames(): if name not in pps: missing.append(name) continue # evaluate piecewise poly to set initial guess t0 = 0.0 for timestepIdx in range(self.nk): self.guess(name,pps[name](t0),timestep=timestepIdx,force=force,quiet=quiet) t0 += h # set parameters for name in self.dae.pNames(): if name not in traj.dvMap._pNames: missing.append(name) continue if name=='endTime': self.guess(name,traj.dvMap.lookup(name)*numLoops,force=force,quiet=quiet) else: self.guess(name,traj.dvMap.lookup(name),force=force,quiet=quiet) msg = "finished interpolating initial guess" if len(missing) > 0: msg += ", couldn't find fields: "+str(missing) else: msg += ", all fields found" print msg
def test_derivative(self): P = PiecewisePolynomial(self.xi, self.yi, 3) assert_almost_equal(P.derivative(self.test_xs, 1), self.spline_yps)
def test_shapes_vectorvalue_derivative(self): P = PiecewisePolynomial(self.xi, np.multiply.outer(self.yi, np.arange(3)), 4) n = 4 assert_array_equal(np.shape(P.derivative(0, 1)), (3,)) assert_array_equal(np.shape(P.derivative([0], 1)), (1, 3)) assert_array_equal(np.shape(P.derivative([0, 1], 1)), (2, 3))
def test_incremental(self): P = PiecewisePolynomial([self.xi[0]], [self.yi[0]], 3) for i in xrange(1, len(self.xi)): P.append(self.xi[i], self.yi[i], 3) assert_almost_equal(P(self.test_xs), self.spline_ys)
def test_shapes_scalarvalue(self): P = PiecewisePolynomial(self.xi, self.yi, 4) assert_array_equal(np.shape(P(0)), ()) assert_array_equal(np.shape(P(np.array(0))), ()) assert_array_equal(np.shape(P([0])), (1, )) assert_array_equal(np.shape(P([0, 1])), (2, ))
def test_shapes_vectorvalue_1d(self): yi = np.multiply.outer(np.asarray(self.yi), np.arange(1)) P = PiecewisePolynomial(self.xi, yi, 4) assert_array_equal(np.shape(P(0)), (1, )) assert_array_equal(np.shape(P([0])), (1, 1)) assert_array_equal(np.shape(P([0, 1])), (2, 1))
class FiberSource: """ A ``FiberSource`` is a continuous representation of a fiber trajectory, .. math:: f: [0, 1] \\rightarrow \\mathcal{R}^3. The trajectory is modeled by 3 piecewise polynomials (one for each dimension). Note that the fiber is connecting two end points on the "cortical surface". The construction makes sure that the tangents to the fiber are normal to this surface. Parameters ---------- control_points : array-like shape (nb_points, 3) a set of points through which the fiber will go. tangents : optional, default = 'symmetric' Either 'symmetric', 'incoming', 'outgoing'. Controls the way the tangents are computed. scale : optional, default = 1.0 A multiplicative factor for the points positions. This corresponds to the sphere radius in mm. """ def __init__(self, control_points, **kwargs): # The mode to compute tangents, either 'symmetric', 'incoming', or # 'outgoing' tangents = kwargs.get('tangents', 'symmetric') scale = kwargs.get('scale', 1.0) self._create_from_control_points(control_points, tangents, scale) def __call__(self, ts): return self.interpolate(ts) def _create_from_control_points(self, control_points, tangents, scale): """ Creates the FiberSource instance from control points, and a specified mode to compute the tangents. Parameters ---------- control_points : ndarray shape (N, 3) tangents : 'incoming', 'outgoing', 'symmetric' scale : multiplication factor. This is useful when the coodinates are given dimensionless, and we want a specific size for the phantom. """ # Compute instant points ts, from 0. to 1. # (time interval proportional to distance between control points) nb_points = control_points.shape[0] dists = np.zeros(nb_points) dists[1:] = np.sqrt((np.diff(control_points, axis=0) ** 2).sum(1)) ts = dists.cumsum() length = ts[-1] ts = ts / np.max(ts) # Create interpolation functions (piecewise polynomials) for x, y and z derivatives = np.zeros((nb_points, 3)) # The derivatives at starting and ending points are normal # to the surface of a sphere. derivatives[0, :] = -control_points[0] derivatives[-1, :] = control_points[-1] # As for other derivatives, we use discrete approx if tangents == 'incoming': derivatives[1:-1, :] = (control_points[1:-1] - control_points[:-2]) elif tangents == 'outgoing': derivatives[1:-1, :] = (control_points[2:] - control_points[1:-1]) elif tangents == 'symmetric': derivatives[1:-1, :] = (control_points[2:] - control_points[:-2]) else: raise Error('tangents should be one of the following: incoming, ' 'outgoing, symmetric') derivatives = (derivatives.T / np.sqrt((derivatives ** 2).sum(1))).T \ * length self.x_poly = PiecewisePolynomial(ts, scale * np.vstack((control_points[:, 0], derivatives[:, 0])).T) self.y_poly = PiecewisePolynomial(ts, scale * np.vstack((control_points[:, 1], derivatives[:, 1])).T) self.z_poly = PiecewisePolynomial(ts, scale * np.vstack((control_points[:, 2], derivatives[:, 2])).T) def interpolate(self, ts): """ From a ``FiberSource``, which is a continuous representation, to a ``Fiber``, a discretization of the fiber trajectory. Parameters ---------- ts : array-like, shape (N, ) A list of "timesteps" between 0 and 1. Returns ------- trajectory : array-like, shape (N, 3) The trajectory of the fiber, discretized over the provided timesteps. """ N = ts.shape[0] trajectory = np.zeros((N, 3)) trajectory[:, 0] = self.x_poly(ts) trajectory[:, 1] = self.y_poly(ts) trajectory[:, 2] = self.z_poly(ts) return trajectory def tangents(self, ts): """ Get tangents (as unit vectors) at given timesteps. Parameters ---------- ts : array-like, shape (N, ) A list of "timesteps" between 0 and 1. Returns ------- tangents : array-like, shape (N, 3) The tangents (as unit vectors) to the fiber at selected timesteps. """ x_der = self.x_poly.derivative(ts, der=1) y_der = self.y_poly.derivative(ts, der=1) z_der = self.z_poly.derivative(ts, der=1) N = ts.shape[0] tangents = np.zeros((N, 3)) tangents[:, 0] = x_der tangents[:, 1] = y_der tangents[:, 2] = z_der tangents = tangents / np.sqrt(np.sum(tangents ** 2, 1))[:, np.newaxis] return tangents def curvature(self, ts): """ Evaluates the curvature of the fiber at given positions. The curvature is computed with the formula .. math:: \gamma = \\frac{\|f'\wedge f''\|}{\|f'\|^3}\qquad. Parameters ---------- ts : array-like, shape (N, ) A list of "timesteps" between 0 and 1. Returns ------- curvatures : array-like, shape (N, ) The curvatures of the fiber trajectory, at selected timesteps. """ x_der1 = self.x_poly.derivative(ts, der=1) x_der2 = self.x_poly.derivative(ts, der=2) y_der1 = self.y_poly.derivative(ts, der=1) y_der2 = self.y_poly.derivative(ts, der=2) z_der1 = self.z_poly.derivative(ts, der=1) z_der2 = self.z_poly.derivative(ts, der=2) curv = (z_der2*y_der1 - y_der2*z_der1)**2 curv += (x_der2*z_der1 - z_der2*x_der1)**2 curv += (y_der2*x_der1 - x_der2*y_der1)**2 curv /= (x_der1**2 + y_der1**2 + z_der1**2)**3 np.sqrt(curv, out=curv) return curv
def test_construction(self): P = PiecewisePolynomial(self.xi, self.yi, 3) assert_almost_equal(P(self.test_xs), self.spline_ys)