def deriv_bspline3D(order, parameter, u, course, k):
xd = BSpline(u, course.x, k)
yd = BSpline(u, course.y, k)
zd = BSpline(u, course.z, k)
deriv_xd = xd.derivative(order)
deriv_yd = yd.derivative(order)
deriv_zd = zd.derivative(order)
deriv_bspline_x = deriv_xd(parameter)
deriv_bspline_y = deriv_yd(parameter)
deriv_bspline_z = deriv_zd(parameter)
deriv_bspline_course = CourseClass(deriv_bspline_x, deriv_bspline_y,
deriv_bspline_z)
return deriv_bspline_course
def Q_complexity(glmv):
#take log of V and S
v = [math.log(i[0]) for i in glmv]
s = [math.log(i[1]) for i in glmv]
#find limits for integral
Smax = max(s)
Smin = min(s)
#reverse to increasing order for Bspline()
v.reverse()
s.reverse()
#convert to arrays for splrep
x = np.array(s)
y = np.array(v)
#find vector knots for Bspline
t, c, k = interpolate.splrep(x=x, y=y, s= 0, k=4)
spl = BSpline(t, c, k)
dvds = BSpline.derivative(spl)
integral = quad(lambda s: ramp(1-(0.25*dvds(s)**2)), Smin, Smax)
Q = (1/(Smax-Smin))*integral[0] #eq5
return Q
def buildDerivativeBSpline(self, u, k, order):
xd = BSpline(u, self.course.x, k)
yd = BSpline(u, self.course.y, k)
zd = BSpline(u, self.course.z, k)
deriv_xd = xd.derivative(order)
deriv_yd = yd.derivative(order)
deriv_zd = zd.derivative(order)
deriv_bspline_course = CourseClass()
parameter = np.linspace(0, 1, self.n_waypoints)
deriv_bspline_course.x = deriv_xd(parameter)
deriv_bspline_course.y = deriv_yd(parameter)
deriv_bspline_course.z = deriv_zd(parameter)
return deriv_bspline_course
def create_spline_basis(
x, knot_list=None, num_knots=None, degree: int = 3, add_intercept=True
):
"""
Creates a spline basis functions.
:param x: array of size num time steps
:param knot_list: indices of knots
:param num_knots: number of knots
:param degree: degree of the spline function
:param add_intercept: appends an additional column of ones, defaults
to False
:return: list of knots, basis functions
:rtype: Tuple[np.ndarray, np.ndarray]
"""
assert ((knot_list is None) and (num_knots is not None)) or (
(knot_list is not None) and (num_knots is None)
), "Define knot_list OR num_knot"
if knot_list is None:
knot_list = np.quantile(x, q=np.linspace(0, 1, num=num_knots))
else:
num_knots = len(knot_list)
knots = np.pad(knot_list, (degree, degree), mode="edge")
B0 = BSpline(knots, np.identity(num_knots + 2), k=degree)
# B0 = BSpline(knot_list, np.identity(num_knots), k=degree)
B = B0(x)
Bdiff = B0.derivative()(x)
if add_intercept:
B = np.hstack([np.ones(B.shape[0]).reshape(-1, 1), B])
Bdiff = np.hstack([np.zeros(B.shape[0]).reshape(-1, 1), Bdiff])
return knot_list, np.stack([B, Bdiff])
def get_spline_basis(self):
self.knots = np.pad(self.knot_list, (self.degree, self.degree), mode="edge")
B0 = BSpline(self.knots, np.identity(len(self.knots)), k=self.degree)
self.B_pad = B0(self.t)
self.B = self.B_pad[self.padding : -self.padding]
if self.degree > 0:
self.B_diff = B0.derivative()(self.t)[self.padding : -self.padding]
else:
self.B_diff = np.zeros(self.B.shape)[self.padding : -self.padding]
def deriv_bspline_position(order, position, parameter, u, course, k):
xd = BSpline(u, course.x, k)
yd = BSpline(u, course.y, k)
zd = BSpline(u, course.z, k)
deriv_xd = xd.derivative(order)
deriv_yd = yd.derivative(order)
deriv_zd = zd.derivative(order)
deriv_parameter = []
for i in range(0, len(position)):
deriv_parameter.append(parameter[position[i]])
deriv_bspline_x = deriv_xd(deriv_parameter)
deriv_bspline_y = deriv_yd(deriv_parameter)
deriv_bspline_z = deriv_zd(deriv_parameter)
deriv_bspline_course = CourseClass(deriv_bspline_x, deriv_bspline_y,
deriv_bspline_z)
return deriv_bspline_course
def B(order, knots, number, space, deriv):
c = zeros(len(knots))
c[number - 1] = 1
Bspl = BSpline(knots, c, order - 1, extrapolate=False)
if deriv == 0:
rBspl = Bspl(space)
else:
Bspl = Bspl.derivative(deriv)
rBspl = Bspl(space)
nanIdx = isnan(rBspl)
rBspl[nanIdx] = 0
return rBspl
def create_spline_basis(x,
knot_list=None,
num_knots=None,
degree=3,
add_intercept=True):
assert ((knot_list is None) and (num_knots is not None)) or (
(knot_list is not None) and
(num_knots is None)), "Define knot_list OR num_knot"
if knot_list is None:
knot_list = jnp.quantile(x, q=jnp.linspace(0, 1, num=num_knots))
else:
num_knots = len(knot_list)
knots = jnp.pad(knot_list, (degree, degree), mode="edge")
B0 = BSpline(knots, jnp.identity(num_knots + 2), k=degree)
B = B0(x)
Bdiff = B0.derivative()(x)
if add_intercept:
B = jnp.hstack([jnp.ones(B.shape[0]).reshape(-1, 1), B])
Bdiff = jnp.hstack([jnp.zeros(B.shape[0]).reshape(-1, 1), Bdiff])
return knot_list, B, Bdiff
def torsion(
x: np.ndarray,
t: np.ndarray,
c: np.ndarray,
k: np.integer,
aux_outputs: bool = False,
) -> np.ndarray:
r"""Compute the torsion of a B-Spline.
The torsion measures the failure of a curve, `r(u)`, to be planar.
If the curvature `k` of a curve is not zero, then the torsion is defined as
.. math::
\tau = -n \cdot b',
where `n` is the principal normal vector, and `b'` the derivative w.r.t. the
arc length `s` of the binormal vector.
The torsion can also be computed as
.. math::
\tau = \lvert r'(t), r''(t), r'''(t) \rvert / \lVert r'(t) \times r''(t) \rVert^2,
where `r(u)` is the position vector as a function of time.
Arguments:
x: A `1xL` array of parameter values where to evaluate the curve.
It contains the parameter values where the torsion of the B-Spline will
be evaluated. It is required to be non-empty, one-dimensional, and
real-valued.
t: A `1xm` array representing the knots of the B-spline.
It is required to be a non-empty, non-decreasing, and one-dimensional
sequence of real-valued elements. For a B-Spline of degree `k`, at least
`2k + 1` knots are required.
c: A `dxn` array representing the coefficients/control points of the B-spline.
Given `n` real-valued, `d`-dimensional points ::math::`x_k = (x_k(1),...,x_k(d))`,
`c` is the non-empty matrix which columns are ::math::`x_1^T,...,x_N^T`. For a
B-Spline of order `k`, `n` cannot be less than `m-k-1`.
k: A non-negative integer representing the degree of the B-spline.
Returns:
torsion: A `1xL` array containing the torsion of the B-Spline evaluated at `x`
References:
.. [1] Máté Attila, The Frenet–Serret formulas.
http://www.sci.brooklyn.cuny.edu/~mate/misc/frenet_serret.pdf
"""
# convert arguments to desired type
x = np.ascontiguousarray(x)
t = np.ascontiguousarray(t)
c = np.ascontiguousarray(c)
k = operator.index(k)
if k < 0:
raise ValueError("The order of the spline must be non-negative")
check_type(t, np.ndarray)
t_dim = t.ndim
if t_dim != 1:
raise ValueError("t must be one-dimensional")
if len(t) == 0:
raise ValueError("t must be non-empty")
check_iterable_type(t, (np.integer, np.float))
if (np.diff(t) < 0).any():
raise ValueError("t must be a non-decreasing sequence")
check_type(c, np.ndarray)
c_dim = c.ndim
if c_dim > 2:
raise ValueError("c must be 2D max")
if len(c.flatten()) == 0:
raise ValueError("c must be non-empty")
if c_dim == 1:
check_iterable_type(c, (np.integer, np.float))
# expand dims so that we can cycle through a single dimension
c = np.expand_dims(c, axis=0)
if c_dim == 2:
for d in c:
check_iterable_type(d, (np.integer, np.float))
n_dim = len(c)
check_type(x, np.ndarray)
x_dim = x.ndim
if x_dim != 1:
raise ValueError("x must be one-dimensional")
if len(x) == 0:
raise ValueError("x must be non-empty")
check_iterable_type(x, (np.integer, np.float))
L = len(x)
# evaluate first, second, and third derivatives
# deriv, dderiv, ddderiv are (d, L) arrays
deriv = np.empty((n_dim, L))
dderiv = np.empty((n_dim, L))
ddderiv = np.empty((n_dim, L))
for i, dim in enumerate(c):
spl = BSpline(t, dim, k)
deriv[i, :] = spl.derivative(nu=1)(x) if k - 1 >= 0 else np.zeros(L)
dderiv[i, :] = spl.derivative(nu=2)(x) if k - 2 >= 0 else np.zeros(L)
ddderiv[i, :] = spl.derivative(nu=3)(x) if k - 3 >= 0 else np.zeros(L)
# transpose derivs
deriv = deriv.T
dderiv = dderiv.T
ddderiv = ddderiv.T
cross = np.cross(deriv, dderiv)
# Could be more efficient by only computing dot products of corresponding rows
num = np.diag((cross @ ddderiv.T))
denom = np.linalg.norm(cross, axis=1)**2
torsion = np.nan_to_num(num / denom)
if aux_outputs == True:
return torsion, deriv, dderiv, ddderiv
else:
return torsion
def speed(
x: np.ndarray,
t: np.ndarray,
c: np.ndarray,
k: np.integer,
aux_outputs: bool = False,
) -> np.ndarray:
r"""Compute the speed of a B-Spline.
The speed is the norm of the first derivative of the B-Spline.
Arguments:
x: A `1xL` array of parameter values where to evaluate the curve.
It contains the parameter values where the speed of the B-Spline will
be evaluated. It is required to be non-empty, one-dimensional, and
real-valued.
t: A `1xm` array representing the knots of the B-spline.
It is required to be a non-empty, non-decreasing, and one-dimensional
sequence of real-valued elements. For a B-Spline of degree `k`, at least
`2k + 1` knots are required.
c: A `dxn` array representing the coefficients/control points of the B-spline.
Given `n` real-valued, `d`-dimensional points ::math::`x_k = (x_k(1),...,x_k(d))`,
`c` is the non-empty matrix which columns are ::math::`x_1^T,...,x_N^T`. For a
B-Spline of order `k`, `n` cannot be less than `m-k-1`.
k: A non-negative integer representing the degree of the B-spline.
Returns:
speed: A `1xL` array containing the speed of the B-Spline evaluated at `x`
References:
.. [1] Kouba, Parametric Equations.
https://www.math.ucdavis.edu/~kouba/Math21BHWDIRECTORY/ArcLength.pdf
"""
# convert arguments to desired type
x = np.ascontiguousarray(x)
t = np.ascontiguousarray(t)
c = np.ascontiguousarray(c)
k = operator.index(k)
if k < 0:
raise ValueError("The order of the spline must be non-negative")
check_type(t, np.ndarray)
t_dim = t.ndim
if t_dim != 1:
raise ValueError("t must be one-dimensional")
if len(t) == 0:
raise ValueError("t must be non-empty")
check_iterable_type(t, (np.integer, np.float))
if (np.diff(t) < 0).any():
raise ValueError("t must be a non-decreasing sequence")
check_type(c, np.ndarray)
c_dim = c.ndim
if c_dim > 2:
raise ValueError("c must be 2D max")
if len(c.flatten()) == 0:
raise ValueError("c must be non-empty")
if c_dim == 1:
check_iterable_type(c, (np.integer, np.float))
# expand dims so that we can cycle through a single dimension
c = np.expand_dims(c, axis=0)
if c_dim == 2:
for d in c:
check_iterable_type(d, (np.integer, np.float))
n_dim = len(c)
check_type(x, np.ndarray)
x_dim = x.ndim
if x_dim != 1:
raise ValueError("x must be one-dimensional")
if len(x) == 0:
raise ValueError("x must be non-empty")
check_iterable_type(x, (np.integer, np.float))
L = len(x)
# evaluate first and second derivatives
# deriv, dderiv are (d, L) arrays
deriv = np.empty((n_dim, L))
for i, dim in enumerate(c):
spl = BSpline(t, dim, k)
deriv[i, :] = spl.derivative(nu=1)(x) if k - 1 >= 0 else np.zeros(L)
# tranpose deriv
deriv = deriv.T
speed = np.linalg.norm(deriv, axis=1)
if aux_outputs == False:
return speed
else:
return speed, deriv
class SmoothingSpline(object):
""" Main cubic smoothing spline class. """
def __init__(self, x, y, lam=10., max_knots=250, order=3):
""" Initialize and fit the cubic smoothing spline, using the Bspline basis from
scipy.
x: A numpy array of univariate independent variables.
y: A numpy array of univariate response variables.
lam: smoothing parameter
max_knots: max number of knots (i.e. max number of spline basis functions)."""
## Start by storing the data
assert len(x) == len(
y
), "Independent and response variable vectors must have the same length."
self.x = x
self.y = y
self.order = 3
self.lam = lam
## Then compute the knots, refactoring to evenly
## spaced knots if the number of unique values is too
## large.
self.knots = np.sort(np.unique(self.x))
if len(self.knots) > max_knots:
self.knots = np.linspace(self.knots[0], self.knots[-1], max_knots)
## Construct the spline basis given the knots
self._aug_knots = np.hstack([
self.knots[0] * np.ones((order, )), self.knots,
self.knots[-1] * np.ones((order, ))
])
self.splines = [
BSpline(self._aug_knots, coeffs, order)
for coeffs in np.eye(len(self.knots) + order - 1)
]
## Finally, with the basis functions, we can fit the
## smoother.
self._fit()
def _fit(self):
""" Subroutine for fitting, called by init. """
## Start the fit procedure by constructing the matrix B
B = np.array([sp(self.x) for sp in self.splines]).T
## Then, construct the penalty matrix by first computing second derivatives
## on the knots and then approximating the integral of second derivative products
## with the trapezoid rule.
d2B = np.array([sp.derivative(2)(self.knots) for sp in self.splines])
weights = np.ones(self.knots.shape)
weights[1:-1] = 2.
Omega = np.dot(d2B, np.dot(np.diag(weights), d2B.T))
## Finally, invert the matrices to construct values of interest.
self.ridge_op = np.linalg.inv(np.dot(B.T, B) + self.lam * Omega)
## From there, we can compute the coefficient matrix H and the
## S matrix (i.e. the hat matrix).
H = np.dot(self.ridge_op, B.T)
self.S = np.dot(B, H)
self.edof = np.trace(self.S)
self.gamma = np.dot(H, self.y)
## Finally, construct the smoothing spline
self.smoother = BSpline(self._aug_knots, self.gamma, self.order)
## And compute the covariance matrix of the coefficients
y_hat = self.smoother(self.x)
self.rss = np.sum((self.y - y_hat)**2)
self.var = self.rss / (len(self.y) - self.edof)
self.cov = self.var * self.ridge_op
return
def __call__(self, x, cov=False):
""" Evaluate it """
## if you want the covariance matrix
if cov:
## Set up the matrix of splines evaluations and similarity transform the
## covariance in the coefficients
F = np.array([BSpline(self._aug_knots, g, self.order)(x) \
for g in np.eye(len(self.knots) + self.order - 1)]).T
covariance_matrix = np.dot(F, np.dot(self.cov, F.T))
## Evaluate the mean using the point estimate
## of the coefficients
point_estimate = np.dot(F, self.gamma)
return point_estimate, covariance_matrix
else:
return self.smoother(x)
def derivative(self, x, degree=1):
""" Evaluate the derivative of degree = degree. """
return self.smoother.derivative(degree)(x)
def correlation_time(self):
""" Use the inferred covariance matrix to compute and estimate of the correlation time
by approximating the width of correlation for a central knot with it's neighbors. """
## Select a central knot
i_mid = int(len(self.knots) / 2)
## Compute a normalized distribution
distribution = np.abs(self.cov[i_mid][1:-1])
distribution = distribution / trapz(distribution, x=self.knots)
## Compute the mean and variance
avg = self.knots[i_mid - 1]
var = trapz(distribution * (self.knots**2), x=self.knots) - avg**2
return np.sqrt(var)
class TrajectoryGenerator:
def __init__(self):
pass
def create_sin_traj(self, period, amplitude, phase, offset):
self.type = "sinusoid"
self.period = period
self.amplitude = amplitude
self.phase = phase
self.offset = offset
def create_bspline_traj(self, degree, knots, coeffs):
self.type = "bspline"
self.knots = knots
self.x_spline = BSpline(knots, coeffs[0, :], degree)
self.z_spline = BSpline(knots, coeffs[1, :], degree)
self.x_derivs = []
self.z_derivs = []
for i in range(3):
self.x_derivs.append(self.x_spline.derivative(i + 1))
self.z_derivs.append(self.z_spline.derivative(i + 1))
def get_state(self, t):
if self.type == "sinusoid":
p = self.amplitude * np.sin(2.0 * np.pi / self.period * t +
self.phase) + self.offset
p_dot = self.amplitude * (2.0 * np.pi / self.period) * np.cos(
2.0 * np.pi / self.period * t + self.phase)
p_ddot = -self.amplitude * (2.0 * np.pi / self.period)**2 * np.sin(
2.0 * np.pi / self.period * t + self.phase)
p_dddot = -self.amplitude * (
2.0 * np.pi / self.period
)**3 * np.cos(2.0 * np.pi / self.period * t + self.phase)
vx = p_dot.item(0)
vy = p_dot.item(1)
ax = p_ddot.item(0)
ay = p_ddot.item(1)
psi = np.arctan2(vy, vx)
psi_dot = (vx * ay + vy * ax) / (vx**2 + vy**2)
# psi = 10.*t
# psi_dot = 10.0
return DesiredState(p, p_dot, p_ddot, p_dddot, psi, psi_dot)
elif self.type == "bspline":
if t > self.knots[-1]:
# p = np.array([[self.x_spline(self.knots[-1]).item(0), 0.0, self.z_spline(self.knots[-1]).item(0)]]).T
p = np.array([[
0.0,
self.x_spline(self.knots[-1]).item(0),
self.z_spline(self.knots[-1]).item(0)
]]).T
derivs = []
for i in range(3):
derivs.append(np.zeros((3, 1)))
else:
# p = np.array([[self.x_spline(t).item(0), 0.0, self.z_spline(t).item(0)]]).T
p = np.array(
[[0.0,
self.x_spline(t).item(0),
self.z_spline(t).item(0)]]).T
derivs = []
for i in range(3):
# derivs.append(np.array([[self.x_derivs[i](t).item(0), 0.0, self.z_derivs[i](t).item(0)]]).T)
derivs.append(
np.array([[
0.0, self.x_derivs[i](t).item(0),
self.z_derivs[i](t).item(0)
]]).T)
vx = derivs[0].item(0)
vy = derivs[0].item(1)
ax = derivs[1].item(0)
ay = derivs[1].item(1)
# psi = np.arctan2(vy, vx)
# if (vx**2 + vy**2) == 0:
# psi_dot = 0.0
# else:
# psi_dot = (vx*ay + vy*ax)/(vx**2 + vy**2)
psi = 0.0
psi_dot = 0.0
return DesiredState(p, derivs[0], derivs[1], derivs[2], psi,
psi_dot)
def get_plot_points(self):
if self.type == "sinusoid":
t = np.linspace(0.0, np.max(self.period), 1000)
return self.amplitude * np.sin(2.0 * np.pi / self.period * t +
self.phase) + self.offset
elif self.type == "bspline":
t = np.linspace(self.knots[0], self.knots[-1], 1000)
plt.subplot(421)
plt.title("x")
plt.plot(t, self.x_spline(t))
plt.subplot(422)
plt.title("z")
plt.plot(t, self.z_spline(t))
plt.subplot(423)
plt.title("x1")
plt.plot(t, self.x_derivs[0](t))
plt.subplot(424)
plt.title("z1")
plt.plot(t, self.z_derivs[0](t))
plt.subplot(425)
plt.title("x2")
plt.plot(t, self.x_derivs[1](t))
plt.subplot(426)
plt.title("z2")
plt.plot(t, self.z_derivs[1](t))
plt.subplot(427)
plt.title("x3")
plt.plot(t, self.x_derivs[2](t))
plt.subplot(428)
plt.title("z3")
plt.plot(t, self.z_derivs[2](t))
plt.show()
input()
return np.vstack(
[np.zeros(t.shape),
self.x_spline(t),
self.z_spline(t)])
class bspline:
def __init__(self, ctrl_pts):
x = ctrl_pts[:, 0]
y = ctrl_pts[:, 1]
self.n_c = len(x)
self.t = range(-ORDER, ORDER + self.n_c + 1)
self.t_max = self.n_c
self.t_min = 0
self.c = np.array([x, y])
self.update()
def update(self):
self.basis = []
x = np.append(self.c[0], self.c[0, 0:ORDER])
y = np.append(self.c[1], self.c[1, 0:ORDER])
self.c_periodic = np.array([x, y])
self.n_c_periodic = len(x)
for i in range(self.n_c_periodic):
self.basis.append(
BSpline.basis_element(self.t[i:i + ORDER + 2], False))
self.bsplx = BSpline(self.t, x, ORDER, False)
self.bsply = BSpline(self.t, y, ORDER, False)
self.derx1 = self.bsplx.derivative(1)
self.dery1 = self.bsply.derivative(1)
self.derx2 = self.bsplx.derivative(2)
self.dery2 = self.bsply.derivative(2)
# Sample some values
self.sample_nb = NB_POINTS_BSPL
self.sample_t = np.linspace(self.t_min,
self.t_max,
self.sample_nb,
endpoint=True)
self.sample_values = np.zeros((self.sample_nb, 2))
for i in range(self.sample_nb):
self.sample_values[i] = self.estimate(self.sample_t[i])
#self.plot_curve(True)
#plt.show()
def estimate_with_basis(self, tk):
tk = self.modulo_tk(tk)
b = self.get_basis(tk)
pt = np.array([0.0, 0.0])
for i in range(self.n_c_periodic):
pt[0] += b[i] * self.c_periodic[0][i]
pt[1] += b[i] * self.c_periodic[1][i]
return pt
def get_basis(self, tk):
tk = self.modulo_tk(tk)
b_term = np.zeros(self.n_c_periodic)
for i in range(self.n_c_periodic):
tmp = self.basis[i](tk)
if not np.isnan(tmp):
b_term[i] = tmp
return b_term
def modulo_tk(self, tk):
return (tk - self.t_min) % (self.t_max - self.t_min) + self.t_min
def estimate(self, tk):
tk = self.modulo_tk(tk)
return np.array([self.bsplx(tk), self.bsply(tk)])
def derivative1(self, tk):
tk = self.modulo_tk(tk)
return np.array([self.derx1(tk), self.dery1(tk)])
def derivative2(self, tk):
tk = self.modulo_tk(tk)
return np.array([self.derx2(tk), self.dery2(tk)])
def plot_curve(self, plot_ctrl_pts=False):
plt.figure(utility.figMerge)
u = np.linspace(self.t_min, self.t_max, self.sample_nb)
pt = np.zeros((self.sample_nb, 2))
#pt2 = np.zeros((self.sample_nb,2))
for i in range(self.sample_nb):
pt[i] = self.estimate(u[i])
#pt2[i] = self.estimate_with_basis(u[i])
plt.plot(pt[:, 0], pt[:, 1], 'b', linewidth=2)
#plt.plot(pt2[:,0],pt2[:,1],'.r')
if plot_ctrl_pts:
plt.plot(np.append(self.c[0, :], self.c[0, 0]),
np.append(self.c[1, :], self.c[1, 0]),
'g',
linewidth=2)