def __init__(self, _wp, _alpha):
''' Initialize an instance of this class given a set of via points.
Such set must contain at least 3 points in R^n. The dimension of the
space where the curve q(t) lies as well as the number of intervals is
computed using the input.
Parameters:
----------
_wp: 2D float np.array
Array which contains the via points to get the total L2 norm of
the Jerk. The dimension of the space where the curve lies is
computed as _q.shape[1] and the number of intervals is
_q.shape[0]-1.
_alpha: real
_T: real'''
self.alpha_ = _alpha
self.dim_ = _wp.shape[1]
self.N_ = _wp.shape[0] - 1
self.wp_ = _wp.copy()
self.basis_ = cBasis1010(self.alpha_)
cFixedWaypointsFunctional.__init__(self, _wp, self.basis_)
self.Q1buff = np.zeros((6, 6))
self.Q2buff = np.zeros((6, 6))
self.Q3buff = np.zeros((6, 6))
self.P_ = np.eye(self.N_) - (1.0 / self.N_) * \
np.ones((self.N_, self.N_))
self.b_ = self.splcalc_.eval_b(self.wp_)
def testcontinuity(self):
print('Test continuity constraints with plot')
for i in range(3):
dim = np.random.randint(2, 3)
N = np.random.randint(3, 10)
a = np.random.rand()
wp = (np.random.rand(N + 1, dim) - 0.5) * 2 * np.pi
tauv = 0.5 + np.random.rand(N) * 3.0
tis = [np.sum(tauv[0:i]) for i in range(0, N + 1)]
T = np.sum(tauv)
splcalc = cSplineCalc(dim, N, cBasis1010(a))
spln = splcalc.getSpline(tauv, wp)
from matplotlib import pyplot as plt
t = np.arange(0, T, 0.005)
q_list = [spln.deriv(i)(t) for i in range(0, 6)]
fig, axs = plt.subplots(6, dim)
for i in range(0, 6):
for j in range(0, dim):
axs[i, j].plot(t, q_list[i][:, j])
axs[i, j].grid()
for ti in tis:
axs[i, j].axvline(x=ti, color='b', linestyle='--')
plt.show()
def approximate_optimal(_wp, _k):
from .cost1000 import optimal_taus
from ..interpolator import interpolate
assert _wp.ndim == 2
ni = _wp.shape[0] - 1
execution_time = 4 * ni / np.sqrt(2)
aux = 4 * ni * _k / np.sqrt(2)
alpha = 1.0 / (1.0 + np.power(execution_time / aux, 4))
basis = cBasis1010(alpha)
tauv = optimal_taus(_wp) * execution_time
result = interpolate(tauv, _wp, basis)
return result
def test_derivative_y(self):
''' Compare the numerical derivate of y w.r.t tau with the nominal one
'''
for i in range(40):
np.random.seed()
dim = np.random.randint(1, 3)
N = np.random.randint(2, 6)
a = np.random.rand()
wp = (np.random.rand(N + 1, dim) - 0.5) * 2 * np.pi
tauv = 1.0 + np.random.rand(N) * 2.0
splcalc = cSplineCalc(dim, N, cBasis1010(a))
dydtauNom, y = splcalc.eval_dydtau(tauv, wp)
y = y.copy()
dtau = 1.0e-8
err = 0.0
errp = 0.0
for iinter in range(0, N):
tauv_aux = tauv.copy()
tauv_aux[iinter] += -2 * dtau
y0 = splcalc.eval_y(tauv_aux, wp).copy() * (1.0 / 12.0)
tauv_aux[iinter] += dtau
y1 = splcalc.eval_y(tauv_aux, wp).copy() * (-2.0 / 3.0)
tauv_aux[iinter] += 2 * dtau
y2 = splcalc.eval_y(tauv_aux, wp).copy() * (2.0 / 3.0)
tauv_aux[iinter] += dtau
y3 = splcalc.eval_y(tauv_aux, wp).copy() * (-1.0 / 12.0)
dydtauiTest = (y0 + y1 + y2 + y3) / dtau
ev = np.abs(dydtauiTest - dydtauNom[:, iinter])
e = np.max(ev)
eidx = np.argmax(ev)
ep = e / abs(dydtauiTest[eidx])
if e > err:
err = e
if ep > errp:
errp = ep
assert ep < 5.0e-2, '''
error on dydtau = {:10.7e}
value of dydtau = {:10.7e}
relative error = {:10.7e}
'''.format(e, dydtauiTest[eidx], ep)
def test_eval_b(self):
import time
print('Test evaluation of b vector')
for i in range(20):
dim = np.random.randint(1, 8)
N = np.random.randint(3, 200)
a = np.random.rand()
wp = (np.random.rand(N + 1, dim) - 0.5) * 2 * np.pi
dwp0 = np.zeros((dim, ))
ddwp0 = np.zeros((dim, ))
dwpT = np.zeros((dim, ))
ddwpT = np.zeros((dim, ))
splcalc = cSplineCalc(dim, N, cBasis1010(a))
b1 = splcalc.eval_b(wp)
b2 = self.eval_b(wp, N, dim, dwp0, ddwp0, dwpT, ddwpT)
e = np.max(np.abs(b1 - b2))
assert e < 1.0e-8
def test_derivative_b(self):
'''
Here we rest the correctness of the numerical output of the basis
class comparing it with its analitical form optained using sympy
'''
print('Test derivative of b w.r.t. waypoint components')
np.random.seed()
dim = np.random.randint(1, 8)
N = np.random.randint(2, 60)
a = np.random.rand()
wp = (np.random.rand(N + 1, dim) - 0.5) * 2 * np.pi
splcalc = cSplineCalc(dim, N, cBasis1010(a))
dwp = 0.0005
for i in range(N + 1):
for j in range(dim):
wpidx = i
i = j
wp_aux = wp.copy()
wp_aux[wpidx, i] += -dwp
b1 = splcalc.eval_b(wp_aux).copy()
wp_aux[wpidx, i] += 2 * dwp
b2 = splcalc.eval_b(wp_aux).copy()
dbdwpij_num = 0.5 * (b2 - b1) / dwp
dbdwpij = splcalc.eval_dbdwpij(wpidx, i)
e = np.max(np.abs(dbdwpij_num - dbdwpij))
if e > 1.0e-8:
print('Erroe in db_dwpij:')
print('implementation:')
print(dbdwpij)
print('(b1-b2)/dwp:')
print(dbdwpij_num)
print('component', i)
print('waypoint ', wpidx)
print('dimension ', dim)
print('number of intervals ', N)
raise AssertionError('Error of {:14.7e}'.format(e))
def testInversion(self):
import time
print('Testinf inversion of matrix')
dim = 6 # np.random.randint(2, 6)
N = 50 # np.random.randint(3, 120)
a = np.random.rand()
splcalc = cSplineCalc(dim, N, cBasis1010(a))
for i in range(50):
tauv = np.random.rand(N)
A1 = splcalc.eval_A(tauv)
# A0 = self.eval_A(tauv, dim, N, cBasis1010(a))
# e = np.max(np.abs(A1 - A0.todense()))
# # print(A0)
# # print('----------------------------------------')
# # print(A1)
# # print(dim, N)
# assert e < 1.0e-8
splcalc.printPerformace()
pass
def test_derivative_A(self):
for i in range(5):
dim = 6 # np.random.randint(2, 6)
N = 20 # np.random.randint(3, 120)
a = np.random.rand()
splcalc = cSplineCalc(dim, N, cBasis1010(a))
tauv0 = 0.5 + np.random.rand(N)
dtau = 0.0001
z = np.ones((splcalc.linsys_shape_, ))
for iinter, taui in enumerate(tauv0):
tauv1 = tauv0.copy()
tauv1[iinter] += -dtau
A0 = splcalc.eval_A(tauv1).copy()
tauv1[iinter] += 2 * dtau
A1 = splcalc.eval_A(tauv1)
dAdtaui = 0.5 * (A1 - A0) / dtau
a = np.max(np.abs(dAdtaui))
testVal = dAdtaui.dot(z)
nomVal = np.ravel(
splcalc.eval_dAdtiy(tauv0, iinter, z).todense())
ev = np.abs(testVal - nomVal)
testValMax = np.max(testVal)
nomValMax = np.max(nomVal)
e = np.max(ev)
ep = e / (nomValMax)
assert ep < 5.0e-6, '''
error = {:10.3e}
max nominal value = {:10.3e}
max test value = {:10.3e}
tau compoenet = {:d}
interation = {:d}
'''.format(e, nomValMax, testValMax, iinter, i)
def test_derivative_wp(self):
''' Compare the numerical derivate of y w.r.t waypoints with the nominal one
'''
for _ in range(4):
np.random.seed()
dim = np.random.randint(1, 8)
N = np.random.randint(2, 20)
a = np.random.rand()
wp = (np.random.rand(N + 1, dim) - 0.5) * 2 * np.pi
tauv = 0.5 + np.random.rand(N) * 2.0
splcalc = cSplineCalc(dim, N, cBasis1010(a))
_, y = splcalc.eval_dydtau(tauv, wp)
y = y.copy()
err = 0.0
errp = 0.0
err = 0.0
errp = 0.0
dwp = 1.0e-5
wpidx = [(i, j) for i in range(N + 1) for j in range(dim)]
dydwpNom = np.zeros((y.shape[0], len(wpidx)))
dydwpNom, _ = splcalc.eval_dydu(tauv, wp, wpidx, dydwpNom)
for k, (i, j) in enumerate(wpidx):
wp_aux = wp.copy()
wpidx = i
wpcom = j
wp_aux[wpidx, wpcom] += -3 * dwp
y0 = splcalc.eval_y(tauv, wp_aux).copy() * (-1.0 / 60.0)
wp_aux[wpidx, wpcom] += dwp
y1 = splcalc.eval_y(tauv, wp_aux).copy() * (3.0 / 20.0)
wp_aux[wpidx, wpcom] += dwp
y2 = splcalc.eval_y(tauv, wp_aux).copy() * (-3.0 / 4.0)
wp_aux[wpidx, wpcom] += 2 * dwp
y3 = splcalc.eval_y(tauv, wp_aux).copy() * (3.0 / 4.0)
wp_aux[wpidx, wpcom] += dwp
y4 = splcalc.eval_y(tauv, wp_aux).copy() * (-3.0 / 20.0)
wp_aux[wpidx, wpcom] += dwp
y5 = splcalc.eval_y(tauv, wp_aux).copy() * (1.0 / 60.0)
dydwpTest = (y0 + y1 + y2 + y3 + y4 + y5) / dwp
ev = np.abs(dydwpNom[:, k] - dydwpTest)
e = np.max(ev)
eidx = np.argmax(ev)
# print('{:14.7e} {:14.7e} {:14.7e}'.format(
# e, dydwpNom[eidx, k], dydwpTest[eidx]))
ep = e / dydwpTest[eidx]
if e > err:
err = e
if ep > errp:
errp = ep
if e > 1.0e-4:
assert ep < 1.0e-8, '''
Relative Error = {:10.3e}
Absolute Error = {:10.3e}
'''.format(ep, e)