8000 initial bspline implementation · forgi86/python-control@3dcad07 · GitHub
[go: up one dir, main page]
Skip to content

Commit 3dcad07

Browse files
murrayrmmurrayrm
committed
initial bspline implementation
1 parent d4c1f14 commit 3dcad07

File tree

6 files changed

+398
-6
lines changed

6 files changed

+398
-6
lines changed

‎control/flatsys/__init__.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,7 @@
5454
from .basis import BasisFamily
5555
from .poly import PolyFamily
5656
from .bezier import BezierFamily
57+
from .bspline import BSplineFamily
5758

5859
# Classes
5960
from .systraj import SystemTrajectory

‎control/flatsys/basis.py

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,5 +61,10 @@ def __call__(self, i, t):
6161
"""Evaluate the ith basis function at a point in time"""
6262
return self.eval_deriv(i, 0, t)
6363

64+
def eval(self, coeffs, tlist):
65+
return [
66+
sum([coeffs[i] * self(i, t) for i in range(self.N)])
67+
for t in tlist]
68+
6469
def eval_deriv(self, i, j, t):
6570
raise NotImplementedError("Internal error; improper basis functions")

‎control/flatsys/bspline.py

Lines changed: 198 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,198 @@
1+ 8000
# bspline.py - B-spline basis functions
2+
# RMM, 2 Aug 2022
3+
#
4+
# This class implements a set of B-spline basis functions that implement a
5+
# piecewise polynomial at a set of breakpoints t0, ..., tn with given orders
6+
# and smoothness.
7+
#
8+
9+
import numpy as np
10+
from .basis import BasisFamily
11+
from scipy.interpolate import BSpline, splev
12+
13+
class BSplineFamily(BasisFamily):
14+
"""B-spline basis functions.
15+
16+
This class represents a B-spline basis for piecewise polynomials defined
17+
across a set of breakpoints with given order and smoothness.
18+
19+
"""
20+
def __init__(self, breakpoints, degree, smoothness=None, vars=1):
21+
"""Create a B-spline basis for piecewise smooth polynomials
22+
23+
Define B-spline polynomials for a set of one or more variables.
24+
B-splines are characterized by a set of intervals separated by break
25+
points. On each interval we have a polynomial of a certain order
26+
and the spline is continuous up to a given smoothness at interior
27+
break points.
28+
29+
Parameters
30+
----------
31+
breakpoints : 1D array or 2D array of float
32+
The breakpoints for the spline(s).
33+
34+
degree : int or list of ints
35+
For each spline variable, the degree of the polynomial between
36+
break points. If a single number is given and more than one
37+
spline variable is specified, the same order is used for each
38+
spline variable.
39+
40+
smoothness : int or list of ints
41+
For each spline variable, the smoothness at breakpoints (number
42+
of derivatives that should match).
43+
44+
vars : int or list of str, option
45+
The number of spline variables or a list of spline variable names.
46+
47+
"""
48+
# Process the breakpoints for the spline */
49+
breakpoints = np.array(breakpoints, dtype=float)
50+
if breakpoints.ndim == 2:
51+
raise NotImplementedError(
52+
"breakpoints for each spline variable not yet supported")
53+
elif breakpoints.ndim != 1:
54+
raise ValueError("breakpoints must be convertable to a 1D array")
55+
elif breakpoints.size < 2:
56+
raise ValueError("break point vector must have at least 2 values")
57+
elif np.any(np.diff(breakpoints) <= 0):
58+
raise ValueError("break points must be strictly increasing values")
59+
60+
# Decide on the number of spline variables
61+
if isinstance(vars, list) and all([isinstance(v, str) for v in vars]):
62+
raise NotImplemented("list of variable names not yet supported")
63+
elif not isinstance(vars, int):
64+
raise TypeError("vars must be an integer or list of strings")
65+
else:
66+
nvars = vars
67+
68+
#
69+
# Process B-spline parameters (order, smoothness)
70+
#
71+
# B-splines are characterized by a set of intervals separated by
72+
# breakpoints. On each interval we have a polynomial of a certain
73+
# order and the spline is continuous up to a given smoothness at
74+
# breakpoints. The code in this section allows some flexibility in
75+
# the way that all of this information is supplied, including using
76+
# scalar values for parameters (which are then broadcast to each
77+
# output) and inferring values and dimensions from other
78+
# information, when possible.
79+
#
80+
81+
# Utility function for broadcasting spline params (order, smoothness)
82+
def process_spline_parameters(
83+
values, length, allowed_types, minimum=0,
84+
default=None, name='unknown'):
85+
86+
# Preprocessing
87+
if values is None and default is None:
88+
return None
89+
elif values is None:
90+
values = default
91+
elif isinstance(values, np.ndarray):
92+
# Convert ndarray to list
93+
values = values.tolist()
94+
95+
# Figure out what type of object we were passed
96+
if isinstance(values, allowed_types):
97+
# Single number of an allowed type => broadcast to list
98+
values = [values for i in range(length)]
99+
elif all([isinstance(v, allowed_types) for v in values]):
100+
# List of values => make sure it is the right size
101+
if len(values) != length:
102+
raise ValueError(f"length of '{name}' does not match n")
103+
else:
104+
raise ValueError(f"could not parse '{name}' keyword")
105+
106+
# Check to make sure the values are OK
107+
if values is not None and any([val < minimum for val in values]):
108+
raise ValueError(
109+
f"invalid value for {name}; must be at least {minimum}")
110+
111+
return values
112+
113+
# Degree of polynomial
114+
degree = process_spline_parameters(
115+
degree, nvars, (int), name='degree', minimum=1)
116+
117+
# Smoothness at breakpoints; set default to degree - 1 (max possible)
118+
smoothness = process_spline_parameters(
119+
smoothness, nvars, (int), name='smoothness', minimum=0,
120+
default=[d - 1 for d in degree])
121+
122+
# Make sure degree is sufficent for the level of smoothness
123+
if any([degree[i] - smoothness[i] < 1 for i in range(nvars)]):
124+
raise ValueError("degree must be greater than smoothness")
125+
126+
# Store the parameters and process them in call_ntg()
127+
self.nvars = nvars
128+
self.breakpoints = breakpoints
129+
self.degree = degree
130+
self.smoothness = smoothness
131+
self.nintervals = breakpoints.size - 1
132+
133+
#
134+
# Compute parameters for a SciPy BSpline object
135+
#
136+
# To create a B-spline, we need to compute the knot points, keeping
137+
# track of the use of repeated knot points at the initial knot and
138+
# final knot as well as repeated knots at intermediate points
139+
# depending on the desired smoothness.
140+
#
141+
142+
# Store the coefficients for each output (useful later)
143+
self.coef_offset, self.coef_length, offset = [], [], 0
144+
for i in range(self.nvars):
145+
# Compute number of coefficients for the piecewise polynomial
146+
ncoefs = (self.degree[i] + 1) * (len(self.breakpoints) - 1) - \
147+
(self.smoothness[i] + 1) * (len(self.breakpoints) - 2)
148+
149+
self.coef_offset.append(offset)
150+
self.coef_length.append(ncoefs)
151+
offset += ncoefs
152+
self.N = offset # save the total number of coefficients
153+
154+
# Create knot points for each spline variable
155+
# TODO: extend to multi-dimensional breakpoints
156+
self.knotpoints = []
157+
for i in range(self.nvars):
158+
# Allocate space for the knotpoints
159+
self.knotpoints.append(np.empty(
160+
(self.degree[i] + 1) + (len(self.breakpoints) - 2) * \
161+
(self.degree[i] - self.smoothness[i]) + (self.degree[i] + 1)))
162+
163+
# Initial knot points
164+
self.knotpoints[i][0:self.degree[i] + 1] = self.breakpoints[0]
165+
offset = self.degree[i] + 1
166+
167+
# Interior knot points
168+
nknots = self.degree[i] - self.smoothness[i]
169+
assert nknots > 0 # just in case
170+
for j in range(1, self.breakpoints.size - 1):
171+
self.knotpoints[i][offset:offset+nknots] = self.breakpoints[j]
172+
offset += nknots
173+
174+
# Final knot point
175+
self.knotpoints[i][offset:offset + self.degree[i] + 1] = \
176+
self.breakpoints[-1]
177+
178+
def eval(self, coefs, tlist):
179+
return np.array([
180+
BSpline(self.knotpoints[i],
181+
coefs[self.coef_offset[i]:
182+
self.coef_offset[i] + self.coef_length[i]],
183+
self.degree[i])(tlist)
184+
for i in range(self.nvars)])
185+
186+
# Compute the kth derivative of the ith basis function at time t
187+
def eval_deriv(self, i, k, t, squeeze=True):
188+
"""Evaluate the kth derivative of the ith basis function at time t."""
189+
if self.nvars > 1 or not squeeze:
190+
raise NotImplementedError(
191+
"derivatives of multi-variable splines not yet supported")
192+
193+
# Create a coefficient vector for this spline
194+
coefs = np.zeros(self.coef_length[0]); coefs[i] = 1
195+
196+
# Evaluate the derivative of the spline at the desired point in time
197+
return BSpline(self.knotpoints[0], coefs,
198+
self.degree[0]).derivative(k)(t)

‎control/flatsys/flatsys.py

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -413,7 +413,10 @@ def point_to_point(
413413
#
414414

415415
# Start by solving the least squares problem
416+
# TODO: add warning if rank is too small
416417
alpha, residuals, rank, s = np.linalg.lstsq(M, Z, rcond=None)
418+
if rank < Z.size:
419+
warnings.warn("basis too small; solution may not exist")
417420

418421
if cost is not None or trajectory_constraints is not None:
419422
# Search over the null space to minimize cost/satisfy constraints
@@ -425,6 +428,7 @@ def traj_cost(null_coeffs):
425428
coeffs = alpha + N @ null_coeffs
426429

427430
# Evaluate the costs at the listed time points
431+
# TODO: store Mt ahead of time, since it doesn't change
428432
costval = 0
429433
for t in timepts:
430434
M_t = _basis_flag_matrix(sys, basis, zflag_T0, t)

0 commit comments

Comments
 (0)
0