Movatterモバイル変換

[0]ホーム
URL:

Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

 Sign up
Appearance settings

Commit58b1bc5

Browse files
authored
Merge pull request#763 from murrayrm/bspline-04Aug2022
Add B-splines and solve_flat_ocp to flatsys
2 parents59aeceb +47262f5 commit58b1bc5

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 .basisimportBasisFamily
5557
from .polyimportPolyFamily
5658
from .bezierimportBezierFamily
59+
from .bsplineimportBSplineFamily
5760

5861
# Classes
5962
from .systrajimportSystemTrajectory
6063
from .flatsysimportFlatSystem
6164
from .linflatimportLinearFlatSystem
6265

6366
# Package functions
64-
from .flatsysimportpoint_to_point
67+
from .flatsysimportpoint_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+
importnumpyasnp
40+
3941

4042
# Basis family class (for use as a base class)
4143
classBasisFamily:
@@ -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+
returnf'<{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-
returnself.eval_deriv(i,0,t)
75+
returnself.eval_deriv(i,0,t,var=var)
76+
77+
defvar_ncoefs(self,var):
78+
"""Get the number of coefficients for a variable"""
79+
returnself.Nifself.nvarsisNoneelseself.coef_length[var]
80+
81+
defeval(self,coeffs,tlist,var=None):
82+
"""Compute function values given the coefficients and time points."""
83+
ifself.nvarsisNoneandvar!=None:
84+
raiseSystemError("multi-variable call to a scalar basis")
85+
86+
elifself.nvarsisNone:
87+
# Single variable basis
88+
return [
89+
sum([coeffs[i]*self(i,t)foriinrange(self.N)])
90+
fortintlist]
91+
92+
elifvarisNone:
93+
# Multi-variable basis with single list of coefficients
94+
values=np.empty((self.nvars,tlist.size))
95+
offset=0
96+
forjinrange(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+
foriinrange(coef_len)])
101+
fortintlist])
102+
offset+=coef_len
103+
returnvalues
104+
105+
else:
106+
returnnp.array([
107+
sum([coeffs[i]*self(i,t,var=var)
108+
foriinrange(self.var_ncoefs(var))])
109+
fortintlist])
63110

64-
defeval_deriv(self,i,j,t):
111+
defeval_deriv(self,i,j,t,var=None):
112+
"""Evaluate the kth derivative of the ith basis function at time t."""
65113
raiseNotImplementedError("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-
defeval_deriv(self,i,k,t):
70+
defeval_deriv(self,i,k,t,var=None):
6371
"""Evaluate the kth derivative of the ith basis function at time t."""
6472
ifi>=self.N:
6573
raiseValueError("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
returnbinom(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
forjinrange(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+
importnumpyasnp
10+
from .basisimportBasisFamily
11+
fromscipy.interpolateimportBSpline,splev
12+
13+
classBSplineFamily(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+
ifbreakpoints.ndim==2:
49+
raiseNotImplementedError(
50+
"breakpoints for each spline variable not yet supported")
51+
elifbreakpoints.ndim!=1:
52+
raiseValueError("breakpoints must be convertable to a 1D array")
53+
elifbreakpoints.size<2:
54+
raiseValueError("break point vector must have at least 2 values")
55+
elifnp.any(np.diff(breakpoints)<=0):
56+
raiseValueError("break points must be strictly increasing values")
57+
58+
# Decide on the number of spline variables
59+
ifvarsisNone:
60+
nvars=1
61+
self.nvars=None# track as single variable
62+
elifnotisinstance(vars,int):
63+
raiseTypeError("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+
defprocess_spline_parameters(
83+
values,length,allowed_types,minimum=0,
84+
default=None,name='unknown'):
85+
86+
# Preprocessing
87+
ifvaluesisNoneanddefaultisNone:
88+
returnNone
89+
elifvaluesisNone:
90+
values=default
91+
elifisinstance(values,np.ndarray):
92+
# Convert ndarray to list
93+
values=values.tolist()
94+
95+
# Figure out what type of object we were passed
96+
ifisinstance(values,allowed_types):
97+
# Single number of an allowed type => broadcast to list
98+
values= [valuesforiinrange(length)]
99+
elifall([isinstance(v,allowed_types)forvinvalues]):
100+
# List of values => make sure it is the right size
101+
iflen(values)!=length:
102+
raiseValueError(f"length of '{name}' does not match"
103+
f" number of variables")
104+
else:
105+
raiseValueError(f"could not parse '{name}' keyword")
106+
107+
# Check to make sure the values are OK
108+
ifvaluesisnotNoneandany([val<minimumforvalinvalues]):
109+
raiseValueError(
110+
f"invalid value for '{name}'; must be at least{minimum}")
111+
112+
returnvalues
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-1fordindegree])
122+
123+
# Make sure degree is sufficent for the level of smoothness
124+
ifany([degree[i]-smoothness[i]<1foriinrange(nvars)]):
125+
raiseValueError("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+
foriinrange(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+
foriinrange(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+
assertnknots>0# just in case
169+
forjinrange(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+
returnf'<{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+
defeval_deriv(self,i,k,t,var=None):
183+
"""Evaluate the kth derivative of the ith basis function at time t."""
184+
ifself.nvarsisNoneor (self.nvars==1andvarisNone):
185+
# Use same variable for all requests
186+
var=0
187+
elifself.nvars>1andvarisNone:
188+
raiseSystemError(
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+
returnBSpline(self.knotpoints[var],coefs,
196+
self.degree[var]).derivative(k)(t)

0 commit comments

Comments
 (0)
[8]ページ先頭
©2009-2025 Movatter.jp