def get_variables(self, child=None, name=None, **kwargs):
if child is None:
return self._var_result
elif name is None:
return self._var_result.prefix(child.label)
else:
if name in child._substitutes:
if name in child._splines_prim and not ('spline' in kwargs and
not kwargs['spline']):
basis = child._splines_prim[name]['basis']
if 'symbolic' in kwargs and kwargs['symbolic']:
if 'substitute' in kwargs and not kwargs['substitute']:
coeffs = child._substitutes[name][1]
else:
coeffs = child._substitutes[name][0]
else:
fun = self.substitutes[child][name]
coeffs = np.array(
fun(self._var_result, self._par_result))
return [
BSpline(basis, coeffs[:, k])
for k in range(coeffs.shape[1])
]
else:
if 'symbolic' in kwargs and kwargs['symbolic']:
if 'substitute' in kwargs and not kwargs['substitute']:
return child._substitutes[name][1]
else:
return child._substitutes[name][0]
else:
fun = self.substitutes[child][name]
return np.array(fun(self._var_result,
self._par_result))
if name in child._splines_prim and not ('spline' in kwargs
and not kwargs['spline']):
basis = child._splines_prim[name]['basis']
if 'symbolic' in kwargs and kwargs['symbolic']:
coeffs = child._variables[name]
else:
coeffs = np.array(self._var_result[child.label, name])
return [
BSpline(basis, coeffs[:, k])
for k in range(coeffs.shape[1])
]
else:
if 'symbolic' in kwargs and kwargs['symbolic']:
return child._variables[name]
else:
return np.array(self._var_result[child.label, name])
def define_substitute(self, name, expr):
if isinstance(expr, list):
return [
self.define_substitute(name + str(l), e)
for l, e in enumerate(expr)
]
else:
if name in self._substitutes:
raise ValueError('Name %s already used for substitutes!' %
(name))
symbol_name = self._add_label(name)
if isinstance(expr, BSpline):
self._splines_prim[name] = {'basis': expr.basis}
coeffs = MX.sym(symbol_name, expr.coeffs.shape[0], 1)
subst = BSpline(expr.basis, coeffs)
self._substitutes[name] = [expr.coeffs, subst.coeffs]
inp_sym, inp_num = [], []
for sym in symvar(expr.coeffs):
inp_sym.append(sym)
inp_num.append(
DM(self._values[self.symbol_dict[sym.name()][1]]))
fun = Function('eval', inp_sym, [expr.coeffs])
self._values[name] = fun(*inp_num)
self.add_to_dict(coeffs, name)
else:
subst = MX.sym(symbol_name, expr.shape[0], expr.shape[1])
self._substitutes[name] = [expr, subst]
self.add_to_dict(subst, name)
return subst
def crop_spline(spline, min_value, max_value):
T, knots2 = get_interval_T(spline.basis, min_value, max_value)
if isinstance(spline.coeffs, (SX, MX)):
coeffs2 = mtimes(T, spline.coeffs)
else:
coeffs2 = T.dot(spline.coeffs)
basis2 = BSplineBasis(knots2, spline.basis.degree)
return BSpline(basis2, coeffs2)
def _define_mx_spline(self, name, size0, size1, dictionary, basis, value=None):
if size1 > 1:
return [self._define_mx_spline(name+str(l), size0,
1, dictionary, basis, value)
for l in range(size1)]
else:
coeffs = self._define_mx(
name, len(basis), size0, dictionary, value)
self._splines_prim[name] = {'basis': basis}
return [BSpline(basis, coeffs[:, k]) for k in range(size0)]
def concat_splines(segments, segment_times):
spl0 = segments[0]
knots = [s.basis.knots*segment_times[0] for s in spl0]
degree = [s.basis.degree for s in spl0]
coeffs = [s.coeffs for s in spl0]
for k in range(1, len(segments)):
for l, s in enumerate(segments[k]):
if s.basis.degree != degree[l]:
raise ValueError(
'Splines at index ' + l + 'should have same degree.')
knots[l] = np.r_[
knots[l], s.basis.knots[degree[l]+1:]*segment_times[k] + knots[l][-1]]
coeffs[l] = np.r_[coeffs[l], s.coeffs]
bases = [BSplineBasis(knots[l], degree[l])
for l in range(len(segments[0]))]
return [BSpline(bases[l], coeffs[l]) for l in range(len(segments[0]))]
def get_parameters(self, child=None, name=None, **kwargs):
if child is None:
return self._par_result
elif name is None:
return self._par_result.prefix(child.label)
else:
if name in child._splines_prim and not ('spline' in kwargs and not kwargs['spline']):
basis = child._splines_prim[name]['basis']
if 'symbolic' in kwargs and kwargs['symbolic']:
coeffs = child._parameters[name]
else:
coeffs = np.array(self._par_result[child.label, name])
return [BSpline(basis, coeffs[:, k]) for k in range(coeffs.shape[1])]
else:
if 'symbolic' in kwargs and kwargs['symbolic']:
return child._parameters[name]
else:
return np.array(self._par_result[child.label, name])
def running_integral(spline):
# Compute running integral from spline
basis = spline.basis
coeffs = spline.coeffs
knots = basis.knots
degree = basis.degree
knots_int = np.r_[knots[0], knots, knots[-1]]
degree_int = degree + 1
basis_int = BSplineBasis(knots_int, degree_int)
coeffs_int = [0.]
for i in range(len(basis_int)-1):
coeffs_int.append(coeffs_int[i]+(knots[degree+i+1]-knots[i])/float(degree_int)*coeffs[i])
if isinstance(coeffs, (MX, SX)):
coeffs_int = vertcat(*coeffs_int)
else:
coeffs_int = np.array(coeffs_int)
spline_int = BSpline(basis_int, coeffs_int)
return spline_int
def concat_splines(segments, segment_times, n_insert=None):
# While concatenating check continuity of segments, this determines
# the required amount of knots to insert. If segments are continuous
# up to degree, no extra knots are required. If they are not continuous
# at all, degree+1 knots are inserted in between the splines.
spl0 = segments[0]
knots = [s.basis.knots * segment_times[0] for s in spl0] # give dimensions
degree = [s.basis.degree for s in spl0]
coeffs = [s.coeffs for s in spl0]
prev_segment = [s.scale(segment_times[0], shift=0) for s in spl0
] # save the combined segment until now (with dimenions)
prev_time = segment_times[0] # save the motion time of combined segment
for k in range(1, len(segments)):
for l, s in enumerate(segments[k]):
if s.basis.degree != degree[l]:
# all concatenated splines should be of the same degree
raise ValueError('Splines at index ' + l +
'should have same degree.')
if n_insert is None:
# check continuity, n_insert can be different for each l
n_insert = degree[l] + 1 # starts at max value
for d in range(degree[l] + 1):
# use ipopt default tolerance as a treshold for check (1e-3)
# give dimensions, to compare using the same time scale
# use scale function to give segments[k][l] dimensions
# prev_segment already has dimensions
# Todo: sometimes this check fails, or you get 1e-10 and 1e-11 values that should actually be equal
# this seems due to the finite tolerance of ipopt? and due to the fact that these are floating point numbers
val1 = segments[k][l].scale(
segment_times[k],
shift=prev_time).derivative(d)(prev_time)
val2 = prev_segment[l].derivative(d)(prev_time)
if (abs(val1 - val2) * 0.5 / (val1 + val2) <= 1e-3):
# more continuity = insert less knots
n_insert -= 1
else:
# spline values were not equal, stop comparing and use latest n_insert value
break
# else: # keep n_insert from input
if n_insert != degree[l] + 1:
# concatenation requires re-computing the coefficients and/or adding extra knots
# here we make the knot vector and coeffs of the total spline (the final output),
# but we also compute the knot vector and coeffs for concatenating just two splines,
# to limit the size of the system that we need to solve
# i.e.: when concatenating 3 splines, we first concatenate segments 1 and 2,
# and afterwards segments 2 and 3, this happens due to the main for loop
# make total knot vector
end_idx = len(knots[l]) - (degree[l] + 1) + n_insert
knots[l] = np.r_[knots[l][:end_idx],
s.basis.knots[degree[l] + 1:] *
segment_times[k] +
knots[l][-1]] # last term = time shift
# make knot vector for two segments to concatenate
end_idx_concat = len(
prev_segment[l].basis.knots) - (degree[l] + 1) + n_insert
knots1 = prev_segment[l].basis.knots[:end_idx_concat]
knots2 = s.basis.knots[degree[l] +
1:] * segment_times[k] + knots1[
-1] # last term = time shift
knots_concat = np.r_[knots1, knots2]
# make union basis for two segments to concatenate
basis_concat = BSplineBasis(knots_concat, degree[l])
grev_bc = basis_concat.greville()
# shift first and last greville point inwards, to avoid numerical problems,
# in which the spline evaluates to 0 because the evaluation lies just outside the domain
grev_bc[0] = grev_bc[0] + (grev_bc[1] - grev_bc[0]) * 0.01
grev_bc[-1] = grev_bc[-1] - (grev_bc[-1] - grev_bc[-2]) * 0.01
# evaluate basis on its greville points
eval_bc = basis_concat(grev_bc).toarray()
# only the last degree coeffs of segment1 and the first degree coeffs
# of segment2 will change --> we can reduce the size of the system to solve
# Todo: implement this reduction
# first give the segment dimensions, by scaling it and shifting it appropriatly
# this is necessary, since by default the domain is [0,1]
# then evaluate the splines that you want to concatenate on greville points
# of union basis, will give 0 outside their domain
s_1 = prev_segment[l](grev_bc)
s_2 = segments[k][l].scale(segment_times[k],
shift=prev_time)(grev_bc)
# sum to get total evaluation
eval_sc = s_1 + s_2
# solve system to find new coeffs
coeffs_concat = la.solve(eval_bc, eval_sc)
# save new coefficients
coeffs[l] = coeffs_concat
else:
#there was no continuity, just compute new knot vector and stack coefficients
knots[l] = np.r_[knots[l], s.basis.knots[degree[l] + 1:] *
segment_times[k] + knots[l][-1]]
coeffs[l] = np.r_[coeffs[l], s.coeffs]
# going to next segment, update time shift
new_bases = [
BSplineBasis(knots[l], degree[l]) for l in range(len(segments[0]))
]
prev_segment = [
BSpline(new_bases[l], coeffs[l]) for l in range(len(segments[0]))
]
prev_time += segment_times[k]
bases = [
BSplineBasis(knots[l], degree[l]) for l in range(len(segments[0]))
]
return [BSpline(bases[l], coeffs[l]) for l in range(len(segments[0]))]