8000 Merge pull request #763 from murrayrm/bspline-04Aug2022 · python-control/python-control@58b1bc5 · GitHub
[go: up one dir, main page]
Skip to content

Commit 58b1bc5

Browse files
murrayrmmurrayrm
authored
Merge pull request #763 from murrayrm/bspline-04Aug2022
Add B-splines and solve_flat_ocp to flatsys
2 parents 59aeceb + 47262f5 commit 58b1
10000
bc5

File tree

15 files changed

+1400
-143
lines changed

15 files changed

+1400
-143
lines changed

‎control/flatsys/__init__.py

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -46,19 +46,22 @@
4646
defined using the :class:`~control.flatsys.BasisFamily` class. The resulting
4747
trajectory is return as a :class:`~control.flatsys.SystemTrajectory` object
4848
and can be evaluated using the :func:`~control.flatsys.SystemTrajectory.eval`
49-
member function.
49+
member function. Alternatively, the :func:`~control.flatsys.solve_flat_ocp`
50+
function can be used to solve an optimal control problem with trajectory and
51+
final costs or constraints.
5052
5153
"""
5254

5355
# Basis function families
5456
from .basis import BasisFamily
5557
from .poly import PolyFamily
5658
from .bezier import BezierFamily
59+
from .bspline import BSplineFamily
5760

5861
# Classes
5962
from .systraj import SystemTrajectory
6063
from .flatsys import FlatSystem
6164
from .linflat import LinearFlatSystem
6265

6366
# Package functions
64-
from .flatsys import point_to_point
67+
from .flatsys import point_to_point, solve_flat_ocp

‎control/flatsys/basis.py

Lines changed: 52 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@
3636
# OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
3737
# SUCH DAMAGE.
3838

39+
import numpy as np
40+
3941

4042
# Basis family class (for use as a base class)
4143
class BasisFamily:
@@ -47,7 +49,11 @@ class BasisFamily:
4749
4850
:math:`z_i^{(q)}(t)` = basis.eval_deriv(self, i, j, t)
4951
50-
Parameters
52+
A basis set can either consist of a single variable that is used for
53+
each flat output (nvars = None) or a different variable for different
54+
flat outputs (nvars > 0).
55+
56+
Attributes
5157
----------
5258
N : int
5359
Order of the basis set.
@@ -56,10 +62,52 @@ class BasisFamily:
5662
def __init__(self, N):
5763
"""Create a basis family of order N."""
5864
self.N = N # save number of basis functions
65+
self.nvars = None # default number of variables
66+
self.coef_offset = [0] # coefficient offset for each variable
67+
self.coef_length = [N] # coefficient length for each variable
5968

60-
def __call__(self, i, t):
69+
def __repr__(self):
70+
return f'<{self.__class__.__name__}: nvars={self.nvars}, ' + \
71+
f'N={self.N}>'
72+
73+
def __call__(self, i, t, var=None):
6174
"""Evaluate the ith basis function at a point in time"""
62-
return self.eval_deriv(i, 0, t)
75+
return self.eval_deriv(i, 0, t, var=var)
76+
77+
def var_ncoefs(self, var):
78+
"""Get the number of coefficients for a variable"""
79+
return self.N if self.nvars is None else self.coef_length[var]
80+
81+
def eval(self, coeffs, tlist, var=None):
82+
"""Compute function values given the coefficients and time points."""
83+
if self.nvars is None and var != None:
84+
raise SystemError("multi-variable call to a scalar basis")
85+
86+
elif self.nvars is None:
87+
# Single variable basis
88+
return [
89+
sum([coeffs[i] * self(i, t) for i in range(self.N)])
90+
for t in tlist]
91+
92+
elif var is None:
93+
# Multi-variable basis with single list of coefficients
94+
values = np.empty((self.nvars, tlist.size))
95+
offset = 0
96+
for j in range(self.nvars):
97+
coef_len = self.var_ncoefs(j)
98+
values[j] = np.array([
99+
sum([coeffs[offset + i] * self(i, t, var=j)
100+
for i in range(coef_len)])
101+
for t in tlist])
102+
offset += coef_len
103+
return values
104+
105+
else:
106+
return np.array([
107+
sum([coeffs[i] * self(i, t, var=var)
108+
for i in range(self.var_ncoefs(var))])
109+
for t in tlist])
63110

64-
def eval_deriv(self, i, j, t):
111+
def eval_deriv(self, i, j, t, var=None):
112+
"""Evaluate the kth derivative of the ith basis function at time t."""
65113
raise NotImplementedError("Internal error; improper basis functions")

‎control/flatsys/bezier.py

Lines changed: 15 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -48,18 +48,26 @@ class BezierFamily(BasisFamily):
4848
This class represents the family of polynomials of the form
4949
5050
.. math::
51-
\phi_i(t) = \sum_{i=0}^n {n \choose i}
52-
\left( \frac{t}{T_\text{f}} - t \right)^{n-i}
53-
\left( \frac{t}{T_f} \right)^i
51+
\phi_i(t) = \sum_{i=0}^N {N \choose i}
52+
\left( \frac{t}{T} - t \right)^{N-i}
53+
\left( \frac{t}{T} \right)^i
54+
55+
Parameters
56+
----------
57+
N : int
58+
Degree of the Bezier curve.
59+
60+
T : float
61+
Final time (used for rescaling).
5462
5563
"""
5664
def __init__(self, N, T=1):
5765
"""Create a polynomial basis of order N."""
5866
super(BezierFamily, self).__init__(N)
59-
self.T = T # save end of time interval
67+
self.T = float(T) # save end of time interval
6068

6169
# Compute the kth derivative of the ith basis function at time t
62-
def eval_deriv(self, i, k, t):
70+
def eval_deriv(self, i, k, t, var=None):
6371
"""Evaluate the kth derivative of the ith basis function at time t."""
6472
if i >= self.N:
6573
raise ValueError("Basis function index too high")
@@ -78,6 +86,7 @@ def eval_deriv(self, i, k, t):
7886
# Return the kth derivative of the ith Bezier basis function
7987
return binom(n, i) * sum([
8088
(-1)**(j-i) *
81-
binom(n-i, j-i) * factorial(j)/factorial(j-k) * np.power(u, j-k)
89+
binom(n-i, j-i) * factorial(j)/factorial(j-k) * \
90+
np.power(u, j-k) / np.power(self.T, k)
8291
for j in range(max(i, k), n+1)
8392
])

‎control/flatsys/bspline.py

Lines changed: 196 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,196 @@
1+
# 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 degree and smoothness. On each
18+
interval between two breakpoints, we have a polynomial of a given degree
19+
and the spline is continuous up to a given smoothness at interior
20+
breakpoints.
21+
22+
Parameters
23+
----------
24+
breakpoints : 1D array or 2D array of float
25+
The breakpoints for the spline(s).
26+
27+
degree : int or list of ints
28+
For each spline variable, the degree of the polynomial between
29+
breakpoints. If a single number is given and more than one spline
30+
variable is specified, the same degree is used for each spline
31+
variable.
32+
33+
smoothness : int or list of ints
34+
For each spline variable, the smoothness at breakpoints (number of
35+
derivatives that should match).
36+
37+
vars : None or int, optional
38+
The number of spline variables. If specified as None (default),
39+
then the spline basis describes a single variable, with no indexing.
40+
If the number of spine variables is > 0, then the spline basis is
41+
index using the `var` keyword.
42+
43+
"""
44+
def __init__(self, breakpoints, degree, smoothness=None, vars=None):
45+
"""Create a B-spline basis for piecewise smooth polynomials."""
46+
# Process the breakpoints for the spline */
47+
breakpoints = np.array(breakpoints, dtype=float)
48+
if breakpoints.ndim == 2:
49+
raise NotImplementedError(
50+
"breakpoints for each spline variable not yet supported")
51+
elif breakpoints.ndim != 1:
52+
raise ValueError("breakpoints must be convertable to a 1D array")
53+
elif breakpoints.size < 2:
54+
raise ValueError("break point vector must have at least 2 values")
55+
elif np.any(np.diff(breakpoints) <= 0):
56+
raise ValueError("break points must be strictly increasing values")
57+
58+
# Decide on the number of spline variables
59+
if vars is None:
60+
nvars = 1
61+
self.nvars = None # track as single variable
62+
elif not isinstance(vars, int):
63+
raise TypeError("vars must be an integer")
64+
else:
65+
nvars = vars
66+
self.nvars = nvars
67+
68+
#
69+
# Process B-spline parameters (degree, smoothness)
70+
#
71+
# B-splines are defined on a set of intervals separated by
72+
# breakpoints. On each interval we have a polynomial of a certain
73+
# degree 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 (degree, 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"
103+
f" number of variables")
104+
else:
105+
raise ValueError(f"could not parse '{name}' keyword")
106+
107+
# Check to make sure the values are OK
108+
if values is not None and any([val < minimum for val in values]):
109+
raise ValueError(
110+
f"invalid value for '{name}'; must be at least {minimum}")
111+
112+
return values
113+
114+
# Degree of polynomial
115+
degree = process_spline_parameters(
116+
degree, nvars, (int), name='degree', minimum=1)
117+
118+
# Smoothness at breakpoints; set default to degree - 1 (max possible)
119+
smoothness = process_spline_parameters(
120+
smoothness, nvars, (int), name='smoothness', minimum=0,
121+
default=[d - 1 for d in degree])
122+
123+
# Make sure degree is sufficent for the level of smoothness
124+
if any([degree[i] - smoothness[i] < 1 for i in range(nvars)]):
125+
raise ValueError("degree must be greater than smoothness")
126+
127+
# Store the parameters for the spline (self.nvars already stored)
128+
self.breakpoints = breakpoints
129+
self.degree = degree
130+
self.smoothness = smoothness
131+
132+
#
133+
# Compute parameters for a SciPy BSpline object
134+
#
135+
# To create a B-spline, we need to compute the knotpoints, keeping
136+
# track of the use of repeated knotpoints at the initial knot and
137+
# final knot as well as repeated knots at intermediate points
138+
# depending on the desired smoothness.
139+
#
140+
141+
# Store the coefficients for each output (useful later)
142+
self.coef_offset, self.coef_length, offset = [], [], 0
143+
for i in range(nvars):
144+
# Compute number of coefficients for the piecewise polynomial
145+
ncoefs = (self.degree[i] + 1) * (len(self.breakpoints) - 1) - \
146+
(self.smoothness[i] + 1) * (len(self.breakpoints) - 2)
147+
148+
self.coef_offset.append(offset)
149+
self.coef_length.append(ncoefs)
150+
offset += ncoefs
151+
self.N = offset # save the total number of coefficients
152+
153+
# Create knotpoints for each spline variable
154+
# TODO: extend to multi-dimensional breakpoints
155+
self.knotpoints = []
156+
for i in range(nvars):
157+
# Allocate space for the knotpoints
158+
self.knotpoints.append(np.empty(
159+
(self.degree[i] + 1) + (len(self.breakpoints) - 2) * \
160+
(self.degree[i] - self.smoothness[i]) + (self.degree[i] + 1)))
161+
162+
# Initial knotpoints (multiplicity = order)
163+
self.knotpoints[i][0:self.degree[i] + 1] = self.breakpoints[0]
164+
offset = self.degree[i] + 1
165+
166+
# Interior knotpoints (multiplicity = degree - smoothness)
167+
nknots = self.degree[i] - self.smoothness[i]
168+
assert nknots > 0 # just in case
169+
for j in range(1, self.breakpoints.size - 1):
170+
self.knotpoints[i][offset:offset+nknots] = self.breakpoints[j]
171+
offset += nknots
172+
173+
# Final knotpoint (multiplicity = order)
174+
self.knotpoints[i][offset:offset + self.degree[i] + 1] = \
175+
self.breakpoints[-1]
176+
177+
def __repr__(self):
178+
return f'<{self.__class__.__name__}: nvars={self.nvars}, ' + \
179+
f'degree={self.degree}, smoothness={self.smoothness}>'
180+
181+
# Compute the kth derivative of the ith basis function at time t
182+
def eval_deriv(self, i, k, t, var=None):
183+
"""Evaluate the kth derivative of the ith basis function at time t."""
184+
if self.nvars is None or (self.nvars == 1 and var is None):
185+
# Use same variable for all requests
186+
var = 0
187+
elif self.nvars > 1 and var is None:
188+
raise SystemError(
189+
"scalar variable call to multi-variable splines")
190+
191+
# Create a coefficient vector for this spline
192+
coefs = np.zeros(self.coef_length[var]); coefs[i] = 1
193+
194+
# Evaluate the derivative of the spline at the desired point in time
195+
return BSpline(self.knotpoints[var], coefs,
196+
self.degree[var]).derivative(k)(t)

0 commit comments

Comments
 (0)
0