def test_integral_non_uniform(self):
for order in range(2, 8, 2):
# Create a spline with three segments
aspl = bsplines.EuclideanBSpline(order, 1)
kr = aspl.numKnotsRequired(4)
kc = aspl.numCoefficientsRequired(4)
# Choose a non-uniform knot sequence.
knots = numpy.linspace(0.0, (kr - 1), kr)
knots = knots * knots
cp = numpy.random.random(kc)
cpa = numpy.array([cp])
aspl = bsplines.EuclideanBSpline(order, 1)
aspl.initWithKnotsAndControlVertices(knots, cpa)
fspl = (knots, cp, order - 1)
for a in numpy.arange(aspl.getMinTime(),
aspl.getMaxTime() - 1e-15, 0.4):
for i in numpy.arange(aspl.getMinTime(),
aspl.getMaxTime() - 1e-15, 0.4):
#print "Eval at %f\n" % (i)
f = fp.splint(a, float(i), fspl)
b = aspl.evalI(a, i)
self.assertAlmostEqual(
b,
f,
msg=
"order %d spline integral evaluated on [%f,%f] (%f != %f) was not right"
% (order, a, i, float(b), f))
def test_init(self):
numpy.random.seed(5)
# Test the initialization from two times and two positions.
p_0 = numpy.array([1, 2, 3])
p_1 = numpy.array([2, 4, 6])
t_0 = 0.0
t_1 = 0.1
dt = t_1 - t_0
v = (p_1 - p_0) / dt
for order in range(2, 10):
aspl = bsplines.EuclideanBSpline(order, 3)
self.assertEqual(order, aspl.splineOrder())
#print "order: %d" % order
#print "p_0: %s" % p_0
#print "p_1: %s" % p_1
# Initialize the spline with these two times
aspl.initUniformSpline(numpy.array([t_0, t_1]),
numpy.array([p_0, p_1]).transpose(), 1, 0.1)
b_0 = aspl.eval(t_0)
b_1 = aspl.eval(t_1)
v_0 = aspl.evalD(t_0, 1)
v_1 = aspl.evalD(t_1, 1)
#print "b_0: %s" % b_0
#print "b_1: %s" % b_1
for j in range(0, p_0.size):
# Keep the threshold low for even power cases.
self.assertAlmostEqual(p_0[j], b_0[j], places=2)
self.assertAlmostEqual(p_1[j], b_1[j], places=2)
self.assertAlmostEqual(v_0[j], v[j], places=2)
self.assertAlmostEqual(v_1[j], v[j], places=2)
def Continualization(t_xyz_xyzw, lamb=1e-5, max_time=3000):
order = 5
pos_spl = bsplines.EuclideanBSpline(order, 3)
pos_spl.initUniformSpline(t_xyz_xyzw[0], t_xyz_xyzw[1:4], max_time, lamb)
quat_spl = bsplines.UnitQuaternionBSpline(order)
quat_spl.initUniformSpline(t_xyz_xyzw[0], t_xyz_xyzw[4:], max_time, lamb)
return BTrajectory(pos_spl, quat_spl)
def createUniformKnotBSpline(order, segments, dim, knotSpacing=1.0):
aspl = bsplines.EuclideanBSpline(order, dim)
# Choose a uniform knot sequence.
aspl.initConstantUniformSpline(0, segments * knotSpacing, segments,
numpy.zeros((dim, 1)))
kc = aspl.getNumControlVertices()
cp = numpy.random.random([dim, kc])
aspl.setControlVertices(cp)
return (aspl, (aspl.getKnotsVector(), cp, order - 1))
def createRandomKnotBSpline(order, segments, dim):
aspl = bsplines.EuclideanBSpline(order, dim)
kr = aspl.numKnotsRequired(segments)
kc = aspl.numCoefficientsRequired(segments)
# Choose a uniform knot sequence.
knots = numpy.random.random(kr) * 10
knots.sort()
cp = numpy.random.random([dim, kc])
aspl.setKnotVectorAndCoefficients(knots, cp)
return (aspl, (knots, cp, order - 1))
def createExponentialKnotBSpline(order, segments, dim, knotSpacing=1.0):
aspl = bsplines.EuclideanBSpline(order, dim)
kr = aspl.numKnotsRequired(segments)
kc = aspl.numCoefficientsRequired(segments)
# Choose a uniform knot sequence.
knots = numpy.zeros(kr)
for i in range(0, kr):
knots[i] = knotSpacing * 2**i
cp = numpy.random.random([dim, kc])
aspl.setKnotVectorAndCoefficients(knots, cp)
return (aspl, (knots, cp, order - 1))
def test_time_interval2(self):
numpy.random.seed(6)
# Test two functions:
for order in range(2, 10):
nSegments = 3
aspl = bsplines.EuclideanBSpline(order, 3)
kr = aspl.numKnotsRequired(nSegments)
kc = aspl.numCoefficientsRequired(nSegments)
# Choose a uniform knot sequence at 0.0, 1.0, ...
knots = numpy.linspace(0.0, kr - 1, kr)
cp = numpy.linspace(1.0, kc, kc)
# build a vector-valued spline
cpa = numpy.array([cp, cp * cp, cp * cp * cp])
aspl.initWithKnotsAndControlVertices(knots, cpa)
# Check that the time interval function works.
ti = aspl.timeInterval()
self.assertEqual(ti[0], aspl.getMinTime())
self.assertEqual(ti[1], aspl.getMaxTime())
def test_constant_init(self):
tmin = 0.0
tmax = 5.0
for order in range(2, 6):
for dim in range(1, 4):
for segs in range(1, 4):
c = numpy.random.random([dim])
# Initialize a constant spline
aspl = bsplines.EuclideanBSpline(order, dim)
aspl.initConstantSpline(tmin, tmax, segs, c)
# Test the time boundaries
self.assertAlmostEqual(tmin, aspl.getMinTime())
self.assertAlmostEqual(tmax, aspl.getMaxTime())
# Test the value.
for t in numpy.arange(aspl.getMinTime(), aspl.getMaxTime(),
0.1):
self.assertMatricesEqual(
aspl.evalD(t, 0), c, 1e-15,
"Error getting back the constant value")
def test_random(self):
numpy.random.seed(3)
for order in range(2, 10):
aspl = bsplines.EuclideanBSpline(order, 1)
kr = aspl.numKnotsRequired(3)
kc = aspl.numCoefficientsRequired(3)
knots = numpy.random.random([kr]) * 10
knots.sort()
cp = numpy.random.random([kc])
cpa = numpy.array([cp])
aspl.initWithKnotsAndControlVertices(knots, cpa)
fspl = (knots, cp, order - 1)
for i in numpy.linspace(aspl.getMinTime(), aspl.getMaxTime(), 10):
f = fp.spalde(float(i), fspl)
a = aspl.eval(i)
for j in range(0, f.shape[0]):
a = aspl.evalD(i, j)
self.assertAlmostEqual(a, f[j])
def aslam_spl(traj):
import bsplines
lamb = 0.00001
maxTime = 5000
resample_t = np.linspace(traj.t[6], traj.t[-6], 20000)
s = bsplines.EuclideanBSpline(4, 3)
s.initUniformSpline(traj.t, traj.xyz, maxTime, lamb)
new_xyz = np.array([s.eval(t) for t in resample_t]).transpose()
v_xyz = np.array([s.evalD(t, 1) for t in resample_t]).transpose()
s = bsplines.UnitQuaternionBSpline(4)
s.initUniformSpline(traj.t, traj.xyzw, maxTime, lamb)
new_xyzw = np.array([s.eval(t) for t in resample_t]).transpose()
dq_dt = np.array([s.evalD(t, 1) for t in resample_t]).transpose()
new_traj = Trajectory3d.from_t_xyz_xyzw(resample_t,
xyz=new_xyz,
xyzw=new_xyzw)
vel = Trajectory3d.from_t_xyz_xyzw(resample_t, xyz=v_xyz, xyzw=dq_dt)
return new_traj, vel
from __future__ import print_function
from scipy import integrate
from pylab import arange, array, show, plot, axis
import bsplines
splineOrder = 4
s = bsplines.EuclideanBSpline(splineOrder, 1)
maxTime = 9
minTime = 0
pos = maxTime
lamb = 0.00001
goal = 1.0
knotGenerator = s.initConstantUniformSplineWithKnotDelta(
minTime, maxTime, 1, array([0]))
print("Knots:" + str(s.getKnotsVector()))
def plotSpline(s, showIt=False):
t = arange(s.getMinTime(), s.getMaxTime(), 0.01)
v = array([s.eval(tt) for tt in t])
plot(t, v)
if showIt: show()
def getAcceleration(s):
return integrate.quad(lambda t: s.evalD(t, 2) * (s.evalD(t, 2)),
