def test_derivative(self):
x = [0, 1, 3]
c = [[3, 0], [0, 0], [0, 2]]
bp = BPoly(c, x) # [3*(1-x)**2, 2*((x-1)/2)**2]
bp_der = bp.derivative()
assert_allclose(bp_der(0.4), -6 * (0.6))
assert_allclose(bp_der(1.7), 0.7)
def test_derivative(self):
x = [0, 1, 3]
c = [[3, 0], [0, 0], [0, 2]]
bp = BPoly(c, x) # [3*(1-x)**2, 2*((x-1)/2)**2]
bp_der = bp.derivative()
assert_allclose(bp_der(0.4), -6*(0.6))
assert_allclose(bp_der(1.7), 0.7)
def compute_quartic_spline_of_right_heaviside_function():
r"""
Get spline approximation of step function enforcing all derivatives
at 0 and 1 are zero
"""
from scipy.interpolate import BPoly
poly = BPoly.from_derivatives([0, 1], [[0, 0, 0, 0], [1, 0, 0, 0]],
orders=[4])
poly = BPoly.from_derivatives([0, 1], [[0, 0, 0], [1, 0, 0]], orders=[3])
def basis(x, p):
return x[:, np.newaxis]**np.arange(p + 1)[np.newaxis, :]
interp_nodes = (-np.cos(np.linspace(0, np.pi, 5)) + 1) / 2
basis_mat = basis(interp_nodes, 4)
coef = np.linalg.inv(basis_mat).dot(poly(interp_nodes))
print(coef)
xx = np.linspace(0, 1, 101)
print(np.absolute(basis(xx, 4).dot(coef) - poly(xx)).max())
# plt.plot(xx,basis(xx,4).dot(coef))
# plt.plot(xx,poly(xx))
eps = 0.1
a, b = 0, eps
xx = np.linspace(a, b, 101)
plt.plot(xx, basis((xx - a) / (b - a), 4).dot(coef))
def f(x):
return 6 * ((xx) / eps)**2 - 8 * ((xx) / eps)**3 + 3 * ((xx) / eps)**4
plt.plot(xx, f(xx))
plt.show()
def test_make_poly_12(self):
np.random.seed(12345)
ya = np.r_[0, np.random.random(5)]
yb = np.r_[0, np.random.random(5)]
c = BPoly._construct_from_derivatives(0, 1, ya, yb)
pp = BPoly(c[:, None], [0, 1])
for j in range(6):
assert_allclose([pp(0.), pp(1.)], [ya[j], yb[j]])
pp = pp.derivative()
def test_pp_from_bp(self):
x = [0, 1, 3]
c = [[3, 3], [1, 1], [4, 2]]
bp = BPoly(c, x)
pp = PPoly.from_bernstein_basis(bp)
bp1 = BPoly.from_power_basis(pp)
xp = [0.1, 1.4]
assert_allclose(bp(xp), pp(xp))
assert_allclose(bp(xp), bp1(xp))
def test_make_poly_12(self):
np.random.seed(12345)
ya = np.r_[0, np.random.random(5)]
yb = np.r_[0, np.random.random(5)]
c = BPoly._construct_from_derivatives(0, 1, ya, yb)
pp = BPoly(c[:, None], [0, 1])
for j in range(6):
assert_allclose([pp(0.), pp(1.)], [ya[j], yb[j]])
pp = pp.derivative()
def test_deriv_inplace(self):
np.random.seed(1234)
m, k = 5, 8 # number of intervals, order
x = np.sort(np.random.random(m))
c = np.random.random((k, m-1))
bp = BPoly(c, x)
xp = np.linspace(x[0], x[-1], 21)
for i in range(k):
assert_allclose(bp(xp, i), bp.derivative(i)(xp))
def test_make_poly_2(self):
c1 = BPoly._construct_from_derivatives(0, 1, [1, 0], [1])
assert_allclose(c1, [1., 1., 1.])
# f'(0) = 3
c2 = BPoly._construct_from_derivatives(0, 1, [2, 3], [1])
assert_allclose(c2, [2., 7. / 2, 1.])
# f'(1) = 3
c3 = BPoly._construct_from_derivatives(0, 1, [2], [1, 3])
assert_allclose(c3, [2., -0.5, 1.])
def test_make_poly_2(self):
c1 = BPoly._construct_from_derivatives(0, 1, [1, 0], [1])
assert_allclose(c1, [1., 1., 1.])
# f'(0) = 3
c2 = BPoly._construct_from_derivatives(0, 1, [2, 3], [1])
assert_allclose(c2, [2., 7./2, 1.])
# f'(1) = 3
c3 = BPoly._construct_from_derivatives(0, 1, [2], [1, 3])
assert_allclose(c3, [2., -0.5, 1.])
def test_multi_shape(self):
c = np.random.rand(6, 2, 1, 2, 3)
x = np.array([0, 0.5, 1])
p = BPoly(c, x)
assert_equal(p.x.shape, x.shape)
assert_equal(p.c.shape, c.shape)
assert_equal(p(0.3).shape, c.shape[2:])
assert_equal(p(np.random.rand(5, 6)).shape, (5, 6) + c.shape[2:])
dp = p.derivative()
assert_equal(dp.c.shape, (5, 2, 1, 2, 3))
def test_make_poly_3(self):
# f'(0)=2, f''(0)=3
c1 = BPoly._construct_from_derivatives(0, 1, [1, 2, 3], [4])
assert_allclose(c1, [1., 5./3, 17./6, 4.])
# f'(1)=2, f''(1)=3
c2 = BPoly._construct_from_derivatives(0, 1, [1], [4, 2, 3])
assert_allclose(c2, [1., 19./6, 10./3, 4.])
# f'(0)=2, f'(1)=3
c3 = BPoly._construct_from_derivatives(0, 1, [1, 2], [4, 3])
assert_allclose(c3, [1., 5./3, 3., 4.])
def test_raise_degree(self):
np.random.seed(12345)
x = [0, 1]
k, d = 8, 5
c = np.random.random((k, 1, 2, 3, 4))
bp = BPoly(c, x)
c1 = BPoly._raise_degree(c, d)
bp1 = BPoly(c1, x)
xp = np.linspace(0, 1, 11)
assert_allclose(bp(xp), bp1(xp))
def test_multi_shape(self):
c = np.random.rand(6, 2, 1, 2, 3)
x = np.array([0, 0.5, 1])
p = BPoly(c, x)
assert_equal(p.x.shape, x.shape)
assert_equal(p.c.shape, c.shape)
assert_equal(p(0.3).shape, c.shape[2:])
assert_equal(p(np.random.rand(5,6)).shape,
(5,6)+c.shape[2:])
dp = p.derivative()
assert_equal(dp.c.shape, (5, 2, 1, 2, 3))
def test_make_poly_3(self):
# f'(0)=2, f''(0)=3
c1 = BPoly._construct_from_derivatives(0, 1, [1, 2, 3], [4])
assert_allclose(c1, [1., 5. / 3, 17. / 6, 4.])
# f'(1)=2, f''(1)=3
c2 = BPoly._construct_from_derivatives(0, 1, [1], [4, 2, 3])
assert_allclose(c2, [1., 19. / 6, 10. / 3, 4.])
# f'(0)=2, f'(1)=3
c3 = BPoly._construct_from_derivatives(0, 1, [1, 2], [4, 3])
assert_allclose(c3, [1., 5. / 3, 3., 4.])
def test_derivative(self):
x = [0, 1, 3]
c = [[3, 0], [0, 0], [0, 2]]
bp = BPoly(c, x) # [3*(1-x)**2, 2*((x-1)/2)**2]
bp_der = bp.derivative()
assert_allclose(bp_der(0.4), -6*(0.6))
assert_allclose(bp_der(1.7), 0.7)
# derivatives in-place
assert_allclose([bp(0.4, nu=1), bp(0.4, nu=2), bp(0.4, nu=3)],
[-6*(1-0.4), 6., 0.])
assert_allclose([bp(1.7, nu=1), bp(1.7, nu=2), bp(1.7, nu=3)],
[0.7, 1., 0])
def test_derivative_ppoly(self):
# make sure it's consistent w/ power basis
np.random.seed(1234)
m, k = 5, 8 # number of intervals, order
x = np.sort(np.random.random(m))
c = np.random.random((k, m-1))
bp = BPoly(c, x)
pp = PPoly.from_bernstein_basis(bp)
for d in range(k):
bp = bp.derivative()
pp = pp.derivative()
xp = np.linspace(x[0], x[-1], 21)
assert_allclose(bp(xp), pp(xp))
def test_extrapolate_attr(self):
x = [0, 2]
c = [[3], [1], [4]]
bp = BPoly(c, x)
for extrapolate in (True, False, None):
bp = BPoly(c, x, extrapolate=extrapolate)
bp_d = bp.derivative()
if extrapolate is False:
assert_(np.isnan(bp([-0.1, 2.1])).all())
assert_(np.isnan(bp_d([-0.1, 2.1])).all())
else:
assert_(not np.isnan(bp([-0.1, 2.1])).any())
assert_(not np.isnan(bp_d([-0.1, 2.1])).any())
def test_derivative_ppoly(self):
# make sure it's consistent w/ power basis
np.random.seed(1234)
m, k = 5, 8 # number of intervals, order
x = np.sort(np.random.random(m))
c = np.random.random((k, m - 1))
bp = BPoly(c, x)
pp = PPoly.from_bernstein_basis(bp)
for d in range(k):
bp = bp.derivative()
pp = pp.derivative()
xp = np.linspace(x[0], x[-1], 21)
assert_allclose(bp(xp), pp(xp))
def test_extrapolate_attr(self):
x = [0, 2]
c = [[3], [1], [4]]
bp = BPoly(c, x)
for extrapolate in (True, False, None):
bp = BPoly(c, x, extrapolate=extrapolate)
bp_d = bp.derivative()
if extrapolate is False:
assert_(np.isnan(bp([-0.1, 2.1])).all())
assert_(np.isnan(bp_d([-0.1, 2.1])).all())
else:
assert_(not np.isnan(bp([-0.1, 2.1])).any())
assert_(not np.isnan(bp_d([-0.1, 2.1])).any())
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 = BPoly.from_derivatives(ts,
scale * np.vstack((control_points[:, 0], derivatives[:, 0])).T)
self.y_poly = BPoly.from_derivatives(ts,
scale * np.vstack((control_points[:, 1], derivatives[:, 1])).T)
self.z_poly = BPoly.from_derivatives(ts,
scale * np.vstack((control_points[:, 2], derivatives[:, 2])).T)
def Generate_Random_Trace_Function(self):
# t_pre = arange(0, self.random_length)*self.dt
self.t_max_pre = self.random_length * self.dt
# t_init = linspace(0, self.t_max_pre, num=self.N, endpoint=True)
t_init = np.sort(random.uniform(self.dt, self.t_max_pre, self.N))
t_init = np.insert(t_init, 0, 0.0)
t_init = np.append(t_init, self.t_max_pre)
y = 2.0 * (random.random(self.N) - 0.5)
y = y * 0.8 * self.HalfLength / max(abs(y))
y = np.insert(y, 0, 0.0)
y = np.append(y, 0.0)
# t1 = timeit.default_timer()
# self.random_track_f = interp1d(t_init, y, kind='cubic')
# t2 = timeit.default_timer()
# Try algorithm setting derivative to 0 a each point
yder = [[y[i], 0] for i in range(len(y))]
self.random_track_f = BPoly.from_derivatives(t_init, yder)
# t3 = timeit.default_timer()
# print('Time for simple method: {:.3f}ms'.format((t2-t1)*1000))
# print('Time for complex method: {:.3f}ms'.format((t3 - t2) * 1000))
self.new_track_generated = True
def test_orders_too_high(self):
m, k = 5, 12
xi, yi = self._make_random_mk(m, k)
pp = BPoly.from_derivatives(xi, yi, orders=2*k-1) # this is still ok
assert_raises(ValueError, BPoly.from_derivatives, # but this is not
**dict(xi=xi, yi=yi, orders=2*k))
def test_simple5(self):
x = [0, 1]
c = [[1], [1], [8], [2], [1]]
bp = BPoly(c, x)
assert_allclose(
bp(0.3), 0.7**4 + 4 * 0.7**3 * 0.3 + 8 * 6 * 0.7**2 * 0.3**2 +
2 * 4 * 0.7 * 0.3**3 + 0.3**4)
def Bern(vars, pars):
pars_coef = []
for i in range(len(pars)):
pars_coef.append(pars[i])
pars_coef = np.array(pars_coef).reshape(-1, 1)
return BPoly(pars_coef[0:-2],
[pars_coef[-2][0], pars_coef[-1][0]])(vars[0])
def test_two_intervals(self):
x = [0, 1, 3]
c = [[3, 0], [0, 0], [0, 2]]
bp = BPoly(c, x) # [3*(1-x)**2, 2*((x-1)/2)**2]
assert_allclose(bp(0.4), 3 * 0.6 * 0.6)
assert_allclose(bp(1.7), 2 * (0.7 / 2)**2)
def test_random_12(self):
m, k = 5, 12
xi, yi = self._make_random_mk(m, k)
pp = BPoly.from_derivatives(xi, yi)
for order in range(k // 2):
assert_allclose(pp(xi), [yy[order] for yy in yi])
pp = pp.derivative()
def test_random_12(self):
m, k = 5, 12
xi, yi = self._make_random_mk(m, k)
pp = BPoly.from_derivatives(xi, yi)
for order in range(k//2):
assert_allclose(pp(xi), [yy[order] for yy in yi])
pp = pp.derivative()
def test_interval_length(self):
x = [0, 2]
c = [[3], [1], [4]]
bp = BPoly(c, x)
xval = 0.1
s = xval / 2 # s = (x - xa) / (xb - xa)
assert_allclose(
bp(xval), 3 * (1 - s) * (1 - s) + 1 * 2 * s * (1 - s) + 4 * s * s)
def test_zeros(self):
xi = [0, 1, 2, 3]
yi = [[0, 0], [0], [0, 0], [0, 0]] # NB: will have to raise the degree
pp = BPoly.from_derivatives(xi, yi)
assert_(pp.c.shape == (4, 3))
ppd = pp.derivative()
for xp in [0., 0.1, 1., 1.1, 1.9, 2., 2.5]:
assert_allclose([pp(xp), ppd(xp)], [0., 0.])
def fonction(self):
"""Fonction wrapper vers la fonction de scipy BPoly.from_derivatives
"""
pts = self.points_tries
xl = [P.x for P in pts]
yl = [P.y for P in pts]
yl_cum = list(zip(yl, self._derivees()))
return BPoly.from_derivatives(xl, yl_cum)
def interpolate_setpoints(self):
time, position, velocity = self.get_setpoints_unzipped()
yi = []
for i in range(0, len(time)):
yi.append([position[i], velocity[i]])
bpoly = BPoly.from_derivatives(time, yi)
indices = np.linspace(0, self.duration, self.duration * 100)
return [indices, bpoly(indices)]
def test_zeros(self):
xi = [0, 1, 2, 3]
yi = [[0, 0], [0], [0, 0], [0, 0]] # NB: will have to raise the degree
pp = BPoly.from_derivatives(xi, yi)
assert_(pp.c.shape == (4, 3))
ppd = pp.derivative()
for xp in [0., 0.1, 1., 1.1, 1.9, 2., 2.5]:
assert_allclose([pp(xp), ppd(xp)], [0., 0.])
def half_amp_dur(self, waveforms):
"""
Half amplitude duration of a spike
Parameters
----------
A: ndarray
An nSpikes x nElectrodes x nSamples array
Returns
-------
had: float
The half-amplitude duration for the channel (electrode) that has
the strongest (highest amplitude) signal. Units are ms
"""
from scipy import optimize
best_chan = np.argmax(np.max(np.mean(waveforms, 0), 1))
mn_wvs = np.mean(waveforms, 0)
wvs = mn_wvs[best_chan, :]
half_amp = np.max(wvs) / 2
half_amp = np.zeros_like(wvs) + half_amp
t = np.linspace(0, 1 / 1000., 50)
# create functions from the data using PiecewisePolynomial
from scipy.interpolate import BPoly
p1 = BPoly.from_derivatives(t, wvs[:, np.newaxis])
p2 = BPoly.from_derivatives(t, half_amp[:, np.newaxis])
xs = np.r_[t, t]
xs.sort()
x_min = xs.min()
x_max = xs.max()
x_mid = xs[:-1] + np.diff(xs) / 2
roots = set()
for val in x_mid:
root, infodict, ier, mesg = optimize.fsolve(
lambda x: p1(x) - p2(x), val, full_output=True)
if ier == 1 and x_min < root < x_max:
roots.add(root[0])
roots = list(roots)
if len(roots) > 1:
r = np.abs(np.diff(roots[0:2]))[0]
else:
r = np.nan
return r
def test_orders_too_high(self):
m, k = 5, 12
xi, yi = self._make_random_mk(m, k)
pp = BPoly.from_derivatives(xi, yi,
orders=2 * k - 1) # this is still ok
assert_raises(
ValueError,
BPoly.from_derivatives, # but this is not
**dict(xi=xi, yi=yi, orders=2 * k))
def test_pp_from_bp(self):
x = [0, 1, 3]
c = [[3, 3], [1, 1], [4, 2]]
bp = BPoly(c, x)
pp = PPoly.from_bernstein_basis(bp)
bp1 = BPoly.from_power_basis(pp)
xp = [0.1, 1.4]
assert_allclose(bp(xp), pp(xp))
assert_allclose(bp(xp), bp1(xp))
def _construct_polynomials(self):
polys = []
_yd = self._autofill_yd()
for j in range(self._y.shape[1]):
y_with_derivatives = np.vstack((self._y[:, j], _yd[:, j])).T
poly = BPoly.from_derivatives(self._x, y_with_derivatives)
polys.append(poly)
return polys
def test_orders_local(self):
m, k = 7, 12
xi, yi = self._make_random_mk(m, k)
orders = [o + 1 for o in range(m)]
for i, x in enumerate(xi[1:-1]):
pp = BPoly.from_derivatives(xi, yi, orders=orders)
for j in range(orders[i] // 2 + 1):
assert_allclose(pp(x - 1e-12), pp(x + 1e-12))
pp = pp.derivative()
assert_(not np.allclose(pp(x - 1e-12), pp(x + 1e-12)))
def test_orders_local(self):
m, k = 7, 12
xi, yi = self._make_random_mk(m, k)
orders = [o + 1 for o in range(m)]
for i, x in enumerate(xi[1:-1]):
pp = BPoly.from_derivatives(xi, yi, orders=orders)
for j in range(orders[i] // 2 + 1):
assert_allclose(pp(x - 1e-12), pp(x + 1e-12))
pp = pp.derivative()
assert_(not np.allclose(pp(x - 1e-12), pp(x + 1e-12)))
def test_bp_from_pp_random(self):
np.random.seed(1234)
m, k = 5, 8 # number of intervals, order
x = np.sort(np.random.random(m))
c = np.random.random((k, m-1))
pp = PPoly(c, x)
bp = BPoly.from_power_basis(pp)
pp1 = PPoly.from_bernstein_basis(bp)
xp = np.linspace(x[0], x[-1], 21)
assert_allclose(pp(xp), bp(xp))
assert_allclose(pp(xp), pp1(xp))
def test_raise_degree(self):
np.random.seed(12345)
x = [0, 1]
k, d = 8, 5
c = np.random.random((k, 1, 2, 3, 4))
bp = BPoly(c, x)
c1 = BPoly._raise_degree(c, d)
bp1 = BPoly(c1, x)
xp = np.linspace(0, 1, 11)
assert_allclose(bp(xp), bp1(xp))
def test_bp_from_pp_random(self):
np.random.seed(1234)
m, k = 5, 8 # number of intervals, order
x = np.sort(np.random.random(m))
c = np.random.random((k, m - 1))
pp = PPoly(c, x)
bp = BPoly.from_power_basis(pp)
pp1 = PPoly.from_bernstein_basis(bp)
xp = np.linspace(x[0], x[-1], 21)
assert_allclose(pp(xp), bp(xp))
assert_allclose(pp(xp), pp1(xp))
def test_bernstein(self):
# a special knot vector: Bernstein polynomials
k = 3
t = np.asarray([0] * (k + 1) + [1] * (k + 1))
c = np.asarray([1., 2., 3., 4.])
bp = BPoly(c.reshape(-1, 1), [0, 1])
bspl = BSpline(t, c, k)
xx = np.linspace(-1., 2., 10)
assert_allclose(bp(xx, extrapolate=True),
bspl(xx, extrapolate=True),
atol=1e-14)
assert_allclose(splev(xx, (t, c, k)), bspl(xx), atol=1e-14)
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 = BPoly.from_derivatives(self.r,phi_and_derivs)
def test_orders_global(self):
m, k = 5, 12
xi, yi = self._make_random_mk(m, k)
# ok, this is confusing. Local polynomials will be of the order 5
# which means that up to the 2nd derivatives will be used at each point
order = 5
pp = BPoly.from_derivatives(xi, yi, orders=order)
for j in range(order//2+1):
assert_allclose(pp(xi[1:-1] - 1e-12), pp(xi[1:-1] + 1e-12))
pp = pp.derivative()
assert_(not np.allclose(pp(xi[1:-1] - 1e-12), pp(xi[1:-1] + 1e-12)))
# now repeat with `order` being even: on each interval, it uses
# order//2 'derivatives' @ the right-hand endpoint and
# order//2+1 @ 'derivatives' the left-hand endpoint
order = 6
pp = BPoly.from_derivatives(xi, yi, orders=order)
for j in range(order//2):
assert_allclose(pp(xi[1:-1] - 1e-12), pp(xi[1:-1] + 1e-12))
pp = pp.derivative()
assert_(not np.allclose(pp(xi[1:-1] - 1e-12), pp(xi[1:-1] + 1e-12)))
def test_orders_global(self):
m, k = 5, 12
xi, yi = self._make_random_mk(m, k)
# ok, this is confusing. Local polynomials will be of the order 5
# which means that up to the 2nd derivatives will be used at each point
order = 5
pp = BPoly.from_derivatives(xi, yi, orders=order)
for j in range(order // 2 + 1):
assert_allclose(pp(xi[1:-1] - 1e-12), pp(xi[1:-1] + 1e-12))
pp = pp.derivative()
assert_(not np.allclose(pp(xi[1:-1] - 1e-12), pp(xi[1:-1] + 1e-12)))
# now repeat with `order` being even: on each interval, it uses
# order//2 'derivatives' @ the right-hand endpoint and
# order//2+1 @ 'derivatives' the left-hand endpoint
order = 6
pp = BPoly.from_derivatives(xi, yi, orders=order)
for j in range(order // 2):
assert_allclose(pp(xi[1:-1] - 1e-12), pp(xi[1:-1] + 1e-12))
pp = pp.derivative()
assert_(not np.allclose(pp(xi[1:-1] - 1e-12), pp(xi[1:-1] + 1e-12)))
def calc_obj_dynamics(s0, S, p, index):
# get pose and vel traj from S
offset = 6 * index
pose_traj_K = [s0[offset:offset + 3]] + [
S[offset + k * p.len_s:offset + k * p.len_s + 3] for k in range(p.K)
]
vel_traj_K = [s0[offset + 3:offset + 6]] + [
S[offset + k * p.len_s + 3:offset + k * p.len_s + 6]
for k in range(p.K)
]
# make spline functions
spline_funcs = []
for dim in range(3):
x = np.linspace(0., p.T_final, p.K + 1)
y = np.zeros((p.K + 1, 2))
for k in range(p.K + 1):
y[k, :] = pose_traj_K[k][dim], vel_traj_K[k][dim]
spline_funcs += [
BPoly.from_derivatives(x, y, orders=3, extrapolate=False)
]
# use to interpolate pose
pose_traj_T = []
k = 0
times = np.linspace(0., p.T_final, p.T_steps + 1)
for t in times:
if not t % p.delT_phase: # this is a phase
pose_traj_T += [pose_traj_K[k]]
k += 1
else: # get from spline
pose_traj_T += [[
spline_funcs[0](t), spline_funcs[1](t), spline_funcs[2](t)
]]
# use FD to get velocities and accels
vel_traj_T = [np.zeros(3)]
for t in range(1, p.T_steps + 1):
p_tm1 = pose_traj_T[t - 1]
p_t = pose_traj_T[t]
vel_traj_T += [calc_deriv(p_t, p_tm1, p.delT)]
accel_traj_T = [np.zeros(3)]
for t in range(1, p.T_steps + 1):
v_tm1 = vel_traj_T[t - 1]
v_t = vel_traj_T[t]
accel_traj_T += [calc_deriv(v_t, v_tm1, p.delT)]
return pose_traj_T, vel_traj_T, accel_traj_T
def __init__(self, x, y, axis=0, extrapolate=None):
x = _asarray_validated(x, check_finite=False, as_inexact=True)
y = _asarray_validated(y, check_finite=False, as_inexact=True)
axis = axis % y.ndim
xp = x.reshape((x.shape[0],) + (1,) * (y.ndim - 1))
yp = np.rollaxis(y, axis)
dk = self._find_derivatives(xp, yp)
data = np.hstack((yp[:, None, ...], dk[:, None, ...]))
_b = BPoly.from_derivatives(x, data, orders=None)
super(PchipInterpolator_new, self).__init__(_b.c, _b.x, extrapolate=extrapolate)
self.axis = axis
def interpolate_setpoints(self):
if len(self.setpoints) == 1:
self.interpolated_position = lambda time: self.setpoints[0
].position
self.interpolated_velocity = lambda time: self.setpoints[0
].velocity
return
time, position, velocity = self.get_setpoints_unzipped()
yi = []
for i in range(0, len(time)):
yi.append([position[i], velocity[i]])
# We do a cubic spline here, just like the ros joint_trajectory_action_controller,
# see https://wiki.ros.org/robot_mechanism_controllers/JointTrajectoryActionController
self.interpolated_position = BPoly.from_derivatives(time, yi)
self.interpolated_velocity = self.interpolated_position.derivative()
def __init__(self, x, y, yp=None, method='parabola', monotone=False):
if yp is None:
yp = slopes(x, y, method, monotone=monotone)
yyp = [z for z in zip(y, yp)]
bpoly = BPoly.from_derivatives(x, yyp)
super(StinemanInterp2, self).__init__(bpoly.c, x)
def __init__(self, x, y, yp=None, method='secant'):
if yp is None:
yp = slopes(x, y, method=method, monotone=True)
yyp = [z for z in zip(y, yp)]
bpoly = BPoly.from_derivatives(x, yyp, orders=3)
super(Pchip, self).__init__(bpoly.c, x)
def test_yi_trailing_dims(self):
m, k = 7, 5
xi = np.sort(np.random.random(m+1))
yi = np.random.random((m+1, k, 6, 7, 8))
pp = BPoly.from_derivatives(xi, yi)
assert_equal(pp.c.shape, (2*k, m, 6, 7, 8))
def test_make_poly_1(self):
c1 = BPoly._construct_from_derivatives(0, 1, [2], [3])
assert_allclose(c1, [2., 3.])
def __init__(self, x, y, yp=None, method='Catmull-Rom'):
if yp is None:
yp = slopes(x, y, method, monotone=False)
yyp = [z for z in zip(y, yp)]
bpoly = BPoly.from_derivatives(x, yyp, orders=3)
super(CubicHermiteSpline, self).__init__(bpoly.c, x)
