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)),