added notebook as test

python/CMakeLists.txt

@@ -36,3 +36,4 @@ install (FILES deploy/optimization/__init__.py
 
 ADD_PYTHON_UNIT_TEST("python-curves" "python/test/test.py" "python")
 ADD_PYTHON_UNIT_TEST("python-optimization" "python/test/optimization.py" "python")
+ADD_PYTHON_UNIT_TEST("python-notebook" "python/test/notebook.py" "python")

python/test/notebook.py

+import os
+import unittest
+from math import sqrt
+
+import eigenpy
+import numpy as np
+from numpy import array, array_equal, isclose, random, zeros, identity, dot, hstack, vstack
+from numpy.linalg import norm
+
+from curves import (CURVES_WITH_PINOCCHIO_SUPPORT, Quaternion, SE3Curve, SO3Linear, bezier, bezier3,
+                    cubic_hermite_spline, curve_constraints, exact_cubic,polynomial,
+                    piecewise, piecewise_SE3, convert_to_polynomial,convert_to_bezier,convert_to_hermite)
+
+eigenpy.switchToNumpyArray()
+
+
+
+#importing the bezier curve class
+from curves import (bezier)
+
+#dummy methods
+def plot(*karrgs):
+        pass
+
+
+
+class TestNotebook(unittest.TestCase):
+    # def print_str(self, inStr):
+    #   print inStr
+    #   return
+
+    def test_notebook(self):
+        print("test_notebook")
+            
+        #We describe a degree 3 curve as a Bezier curve with 4 control points
+        waypoints = array([[1., 2., 3.], [-4., -5., -6.], [4., 5., 6.], [7., 8., 9.]]).transpose()
+        ref = bezier(waypoints)
+
+
+        numSamples = 10; fNumSamples = float(numSamples)
+        ptsTime = [ (ref(float(t) / fNumSamples), float(t) / fNumSamples) for t in range(numSamples+1)]
+
+
+        from curves.optimization import (problem_definition, setup_control_points)
+
+        #dimension of our problem (here 3 as our curve is 3D)
+        dim = 3
+        refDegree = 3
+        pD = problem_definition(dim)
+        pD.degree = refDegree #we want to fit a curve of the same degree as the reference curve for the sanity check
+
+        #generates the variable bezier curve with the parameters of problemDefinition
+        problem = setup_control_points(pD)
+        #for now we only care about the curve itself
+        variableBezier = problem.bezier()
+
+        linearVariable = variableBezier(0.)
+
+        #least square form of ||Ax-b||**2 
+        def to_least_square(A, b):
+            return dot(A.T, A), - dot(A.T, b)
+
+        def genCost(variableBezier, ptsTime):
+            #first evaluate variableBezier for each time sampled
+            allsEvals = [(variableBezier(time), pt) for (pt,time) in ptsTime]
+            #then compute the least square form of the cost for each points
+            allLeastSquares = [to_least_square(el.B(), el.c() + pt) for (el, pt) in  allsEvals]
+            #and finally sum the costs
+            Ab = [sum(x) for x in zip(*allLeastSquares)]
+            return Ab[0], Ab[1]
+        A, b = genCost(variableBezier, ptsTime)
+
+
+        def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None, verbose=False):
+            return  zeros(P.shape[0])
+
+        res = quadprog_solve_qp(A, b)
+
+
+
+
+        def evalAndPlot(variableBezier, res):
+            fitBezier = variableBezier.evaluate(res.reshape((-1,1)) ) 
+            return fitBezier
+            
+        fitBezier = evalAndPlot(variableBezier, res)
+
+
+
+        pD.degree = refDegree - 1
+
+
+        problem = setup_control_points(pD)
+        variableBezier = problem.bezier()
+
+        A, b = genCost(variableBezier, ptsTime)
+        res = quadprog_solve_qp(A, b)
+        fitBezier = evalAndPlot(variableBezier, res)
+            
+
+        from curves.optimization import constraint_flag
+
+        pD.flag = constraint_flag.INIT_POS | constraint_flag.END_POS
+        #set initial position
+        pD.init_pos = array([ptsTime[ 0][0]]).T
+        #set end position
+        pD.end_pos   = array([ptsTime[-1][0]]).T
+        problem = setup_control_points(pD)
+        variableBezier = problem.bezier()
+
+
+        prob = setup_control_points(pD)
+        variableBezier = prob.bezier()
+        A, b = genCost(variableBezier, ptsTime)
+        res = quadprog_solve_qp(A, b)
+        _ = evalAndPlot(variableBezier, res)
+            
+
+
+        #values are 0 by default, so if the constraint is zero this can be skipped
+        pD.init_vel = array([[0., 0., 0.]]).T
+        pD.init_acc = array([[0., 0., 0.]]).T
+        pD.end_vel = array([[0., 0., 0.]]).T
+        pD.end_acc = array([[0., 0., 0.]]).T
+        pD.flag = constraint_flag.END_POS | constraint_flag.INIT_POS | constraint_flag.INIT_VEL  | constraint_flag.END_VEL  | constraint_flag.INIT_ACC  | constraint_flag.END_ACC
+
+        err = False
+        try:
+            prob = setup_control_points(pD)
+        except RuntimeError,e:
+            err = True
+        assert err
+
+
+        pD.degree = refDegree + 4
+        prob = setup_control_points(pD)
+        variableBezier = prob.bezier()
+        A, b = genCost(variableBezier, ptsTime)
+        res = quadprog_solve_qp(A, b)
+        fitBezier = evalAndPlot(variableBezier, res)
+
+
+
+        pD.degree = refDegree + 60
+        prob = setup_control_points(pD)
+        variableBezier = prob.bezier()
+        A, b = genCost(variableBezier, ptsTime)
+        #regularization matrix 
+        reg = identity(A.shape[1]) * 0.001
+        res = quadprog_solve_qp(A + reg, b)
+        fitBezier = evalAndPlot(variableBezier, res)
+
+
+        #set initial / terminal constraints
+        pD.flag = constraint_flag.END_POS | constraint_flag.INIT_POS
+        pD.degree = refDegree
+        prob = setup_control_points(pD)
+        variableBezier = prob.bezier()
+
+        #get value of the curve first order derivative at t = 0.8
+        t08Constraint = variableBezier.derivate(0.8,1)
+        target = zeros(3) 
+
+        A, b = genCost(variableBezier, ptsTime)
+        #solve optimization problem with quadprog
+
+        res = quadprog_solve_qp(A, b, C=t08Constraint.B(), d=target - t08Constraint.c())
+        fitBezier = evalAndPlot(variableBezier, res)
+
+        #returns a curve composed of the split curves, 2 in our case
+        piecewiseCurve = ref.split(array([[0.6]]).T)
+
+        #displaying the obtained curves
+
+        #first, split the variable curve
+        piecewiseCurve = variableBezier.split(array([[0.4, 0.8]]).T)
+
+        constrainedCurve = piecewiseCurve.curve_at_index(1)
+
+        #find the number of variables
+        problemSize = prob.numVariables * dim
+        #find the number of constraints, as many as waypoints
+        nConstraints = constrainedCurve.nbWaypoints
+
+        waypoints = constrainedCurve.waypoints()
+
+        ineqMatrix = zeros((nConstraints, problemSize))
+        ineqVector = zeros(nConstraints)
+
+
+        #finding the z equation of each control point
+        for i in range(nConstraints):
+            wayPoint = constrainedCurve.waypointAtIndex(i)
+            ineqMatrix[i,:] = wayPoint.B()[2,:]
+            ineqVector[i] =  -wayPoint.c()[2]
+
+            
+        res = quadprog_solve_qp(A, b, G=ineqMatrix, h = ineqVector)
+        fitBezier = variableBezier.evaluate(res.reshape((-1,1)) ) 
+
+
+
+if __name__ == '__main__':
+    unittest.main()