### removed qp dependency

parent fbe8cb60
 from __future__ import print_function import quadprog from numpy import array, dot, hstack, vstack from scipy.optimize import linprog def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None, verbose=False): """ min (1/2)x' P x + q' x subject to G x <= h subject to C x = d """ # qp_G = .5 * (P + P.T) # make sure P is symmetric qp_G = .5 * (P + P.T) # make sure P is symmetric qp_a = -q qp_C = None qp_b = None meq = 0 if C is not None: if G is not None: qp_C = -vstack([C, G]).T qp_b = -hstack([d, h]) else: qp_C = -C.transpose() qp_b = -d meq = C.shape elif G is not None: # no equality constraint qp_C = -G.T qp_b = -h res = quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq) if verbose: return res # print('qp status ', res) return res def to_least_square(A, b): "least square form of ||Ax-b||**2" return dot(A.T, A), -dot(A.T, b) def solve_least_square(A, b, G=None, h=None, C=None, d=None): """ min ||Ax-b||**2 subject to G x <= h subject to C x = d """ P, q = to_least_square(A, b) # P = dot(A.T, A) # q = -dot(A.T, b).reshape(b.shape) return quadprog_solve_qp(P, q, G, h, C, d) def solve_lp(q, G=None, h=None, C=None, d=None): """ min q' x subject to G x <= h subject to C x = d """ res = linprog(q, A_ub=G, b_ub=h, A_eq=C, b_eq=d, bounds=[(-100000., 10000.) for _ in range(q.shape)], method='interior-point', callback=None, options={'presolve': True}) return res if __name__ == '__main__': A = array([[1., 2., 0.], [-8., 3., 2.], [0., 1., 1.]]) b = array([3., 2., 3.]) P = dot(A.T, A) q = 2 * dot(b, A).reshape((3, )) G = array([[1., 2., 1.], [2., 0., 1.], [-1., 2., -1.]]) h = array([3., 2., -2.]).reshape((3, )) res2 = solve_least_square(A, b, G, h) res1 = quadprog_solve_qp(P, q, G, h) print(res1) print(res2)
 ... @@ -238,7 +238,37 @@ ... @@ -238,7 +238,37 @@ "metadata": {}, "metadata": {}, "outputs": [], "outputs": [], "source": [ "source": [ "from qp import quadprog_solve_qp\n", "import quadprog\n", "from numpy import array, hstack, vstack\n", "\n", "def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None, verbose=False):\n", " \"\"\"\n", " min (1/2)x' P x + q' x\n", " subject to G x <= h\n", " subject to C x = d\n", " \"\"\"\n", " # qp_G = .5 * (P + P.T) # make sure P is symmetric\n", " qp_G = .5 * (P + P.T) # make sure P is symmetric\n", " qp_a = -q\n", " qp_C = None\n", " qp_b = None\n", " meq = 0\n", " if C is not None:\n", " if G is not None:\n", " qp_C = -vstack([C, G]).T\n", " qp_b = -hstack([d, h])\n", " else:\n", " qp_C = -C.transpose()\n", " qp_b = -d\n", " meq = C.shape\n", " elif G is not None: # no equality constraint\n", " qp_C = -G.T\n", " qp_b = -h\n", " res = quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq)\n", " if verbose:\n", " return res\n", " # print('qp status ', res)\n", " return res\n", "\n", "\n", "res = quadprog_solve_qp(A, b)" "res = quadprog_solve_qp(A, b)" ] ] ... @@ -683,9 +713,21 @@ ... @@ -683,9 +713,21 @@ }, }, { { "cell_type": "code", "cell_type": "code", "execution_count": null, "execution_count": 21, "metadata": {}, "metadata": {}, "outputs": [], "outputs": [ { "ename": "TypeError", "evalue": "No to_python (by-value) converter found for C++ type: curves::piecewise_curve, curves::linear_variable >", "output_type": "error", "traceback": [ "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", "\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)", "\u001b[0;32m\u001b[0m in \u001b[0;36m\u001b[0;34m()\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[0;31m#first, split the variable curve\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 2\u001b[0;31m \u001b[0mpiecewiseCurve\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mvariableBezier\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msplit\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0marray\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0.4\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0.8\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mT\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 3\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 4\u001b[0m \u001b[0mconstrainedCurve\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mpiecewiseCurve\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcurve_at_index\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 5\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n", "\u001b[0;31mTypeError\u001b[0m: No to_python (by-value) converter found for C++ type: curves::piecewise_curve, curves::linear_variable >" ] } ], "source": [ "source": [ "#first, split the variable curve\n", "#first, split the variable curve\n", "piecewiseCurve = variableBezier.split(array([[0.4, 0.8]]).T)\n", "piecewiseCurve = variableBezier.split(array([[0.4, 0.8]]).T)\n", ... ...
 %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: # Curve optimization with the curves library # Curve optimization with the curves library %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: The [curve library](https://github.com/loco-3d/curves) is a header-only C++ library (also binded in python) that allows you The [curve library](https://github.com/loco-3d/curves) is a header-only C++ library (also binded in python) that allows you to create curves, in arbitrary dimensions (2, 3, n). to create curves, in arbitrary dimensions (2, 3, n). Originally, the library focused on spline curves, but it has now been extended to generic polynomials, cubic hermite splines, Bezier curves and more. Originally, the library focused on spline curves, but it has now been extended to generic polynomials, cubic hermite splines, Bezier curves and more. A nice upcoming extension is the ability to design curves in the Special Euclidian group SE3. A nice upcoming extension is the ability to design curves in the Special Euclidian group SE3. However in this tutorial we are going to focus on a rather unique trait of the library, which is the ability to work with variable control points. Rather than being given a constant value, the control points can be expressed as the linear combination of one or several variables. The main advantage of this representation is that variable curves However in this tutorial we are going to focus on a rather unique trait of the library, which is the ability to work with variable control points. Rather than being given a constant value, the control points can be expressed as the linear combination of one or several variables. The main advantage of this representation is that variable curves can be automatically derivated or integrated with any effort. can be automatically derivated or integrated with any effort. The other interest of variable curves is the ability to easily formulate optimization problems, which will be the focus of this tutorial. We will use the python bindings of the curve library to go step-by-step to formulating and solving an optimization problem. The other interest of variable curves is the ability to easily formulate optimization problems, which will be the focus of this tutorial. We will use the python bindings of the curve library to go step-by-step to formulating and solving an optimization problem. ## The problem: trajectory fitting ## The problem: trajectory fitting We start with a simple, unconstrained problem. We start with a simple, unconstrained problem. Let us first consider a 3D curve: Let us first consider a 3D curve: %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python # importing classical numpy objects # importing classical numpy objects from numpy import zeros, array, identity, dot from numpy import zeros, array, identity, dot from numpy.linalg import norm from numpy.linalg import norm import numpy as np import numpy as np np.set_printoptions(formatter={'float': lambda x: "{0:0.1f}".format(x)}) np.set_printoptions(formatter={'float': lambda x: "{0:0.1f}".format(x)}) #use array representation for binding eigen objects to python #use array representation for binding eigen objects to python import eigenpy import eigenpy eigenpy.switchToNumpyArray() eigenpy.switchToNumpyArray() #importing the bezier curve class #importing the bezier curve class from curves import (bezier) from curves import (bezier) #importing tools to plot bezier curves #importing tools to plot bezier curves from curves.plot import (plotBezier) from curves.plot import (plotBezier) from mpl_toolkits.mplot3d import Axes3D from mpl_toolkits.mplot3d import Axes3D import matplotlib.pyplot as plt import matplotlib.pyplot as plt import numpy as np import numpy as np #We describe a degree 3 curve as a Bezier curve with 4 control points #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() waypoints = array([[1., 2., 3.], [-4., -5., -6.], [4., 5., 6.], [7., 8., 9.]]).transpose() ref = bezier(waypoints) ref = bezier(waypoints) #plotting the curve with its control points #plotting the curve with its control points plotBezier(ref,showControlPoints = True, color="g") plotBezier(ref,showControlPoints = True, color="g") ``` ``` %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: We now assume that we only have partial information about this curve, and that we want to reconstruct it. We now assume that we only have partial information about this curve, and that we want to reconstruct it. We will first generate a discretization of the curve to represent a temporal sampling: We will first generate a discretization of the curve to represent a temporal sampling: %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python numSamples = 10; fNumSamples = float(numSamples) numSamples = 10; fNumSamples = float(numSamples) ptsTime = [ (ref(float(t) / fNumSamples), float(t) / fNumSamples) for t in range(numSamples+1)] ptsTime = [ (ref(float(t) / fNumSamples), float(t) / fNumSamples) for t in range(numSamples+1)] for el in ptsTime: for el in ptsTime: print el print el ``` ``` %%%% Output: stream %%%% Output: stream (array([1.0, 2.0, 3.0]), 0.0) (array([1.0, 2.0, 3.0]), 0.0) (array([-0.1, 0.4, 0.9]), 0.1) (array([-0.1, 0.4, 0.9]), 0.1) (array([-0.6, -0.4, -0.1]), 0.2) (array([-0.6, -0.4, -0.1]), 0.2) (array([-0.5, -0.4, -0.2]), 0.3) (array([-0.5, -0.4, -0.2]), 0.3) (array([0.1, 0.2, 0.4]), 0.4) (array([0.1, 0.2, 0.4]), 0.4) (array([1.0, 1.2, 1.5]), 0.5) (array([1.0, 1.2, 1.5]), 0.5) (array([2.2, 2.6, 3.0]), 0.6) (array([2.2, 2.6, 3.0]), 0.6) (array([3.4, 4.1, 4.7]), 0.7) (array([3.4, 4.1, 4.7]), 0.7) (array([4.7, 5.6, 6.4]), 0.8) (array([4.7, 5.6, 6.4]), 0.8) (array([6.0, 6.9, 7.9]), 0.9) (array([6.0, 6.9, 7.9]), 0.9) (array([7.0, 8.0, 9.0]), 1.0) (array([7.0, 8.0, 9.0]), 1.0) %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: Each entry of ptsTime is a couple (position, time) that describes our input data. Each entry of ptsTime is a couple (position, time) that describes our input data. ### Sanity check ### Sanity check Let's first solve a trivial problem, to see if we can reconstruct the curve with a polynomial Let's first solve a trivial problem, to see if we can reconstruct the curve with a polynomial of same degree. of same degree. To achieve this we will use the problemDefinition class, which will automatically generate the variable expression of the curve To achieve this we will use the problemDefinition class, which will automatically generate the variable expression of the curve %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python from curves.optimization import (problem_definition, setup_control_points) from curves.optimization import (problem_definition, setup_control_points) #dimension of our problem (here 3 as our curve is 3D) #dimension of our problem (here 3 as our curve is 3D) dim = 3 dim = 3 refDegree = 3 refDegree = 3 pD = problem_definition(dim) 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 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 #generates the variable bezier curve with the parameters of problemDefinition problem = setup_control_points(pD) problem = setup_control_points(pD) #for now we only care about the curve itself #for now we only care about the curve itself variableBezier = problem.bezier() variableBezier = problem.bezier() ``` ``` %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: The evaluation of a variable Bezier returns a matrix B and a vector c, such The evaluation of a variable Bezier returns a matrix B and a vector c, such that B x + c , with x a vector variable, defines the value of the curve that B x + c , with x a vector variable, defines the value of the curve %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python linearVariable = variableBezier(0.) linearVariable = variableBezier(0.) print "B: \n", linearVariable.B() print "B: \n", linearVariable.B() print "c:\n",linearVariable.c() print "c:\n",linearVariable.c() print "Shape of B: ", linearVariable.B().shape print "Shape of B: ", linearVariable.B().shape ``` ``` %%%% Output: stream %%%% Output: stream B: B: [[1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0] [[1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0] [0.0 1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0] [0.0 1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0] [0.0 0.0 1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0]] [0.0 0.0 1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0]] c: c: [0.0 0.0 0.0] [0.0 0.0 0.0] Shape of B: (3, 12) Shape of B: (3, 12) %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: B has 3 rows and 12 columns. Because the fitting curve is of degree 3, it has 4 control points of dimension 3, which gives a variable of size 12. The row number also matches the dimension of the problem. B has 3 rows and 12 columns. Because the fitting curve is of degree 3, it has 4 control points of dimension 3, which gives a variable of size 12. The row number also matches the dimension of the problem. Then A is zero everywhere, expect for the first 3 columns that contain the identity. This is expected as the start of a Bezier curve is equal to the first control point. Then A is zero everywhere, expect for the first 3 columns that contain the identity. This is expected as the start of a Bezier curve is equal to the first control point. If we evaluate variableBezier at t = 0.2 for instance, we get a more complex expression: If we evaluate variableBezier at t = 0.2 for instance, we get a more complex expression: %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python print "B: \n", variableBezier(0.2).B() print "B: \n", variableBezier(0.2).B() ``` ``` %%%% Output: stream %%%% Output: stream B: B: [[0.5 0.0 0.0 0.4 0.0 0.0 0.1 0.0 0.0 0.0 0.0 0.0] [[0.5 0.0 0.0 0.4 0.0 0.0 0.1 0.0 0.0 0.0 0.0 0.0] [0.0 0.5 0.0 0.0 0.4 0.0 0.0 0.1 0.0 0.0 0.0 0.0] [0.0 0.5 0.0 0.0 0.4 0.0 0.0 0.1 0.0 0.0 0.0 0.0] [0.0 0.0 0.5 0.0 0.0 0.4 0.0 0.0 0.1 0.0 0.0 0.0]] [0.0 0.0 0.5 0.0 0.0 0.4 0.0 0.0 0.1 0.0 0.0 0.0]] %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: With variableBezier, we can easily define a least square problem to reconstruct the original curve. With variableBezier, we can easily define a least square problem to reconstruct the original curve. We just have to formulate a cost function that, for each sample in ptsTime minimizes the distance between the evaluation of variableBezier and the sampled point. We define it as follows: We just have to formulate a cost function that, for each sample in ptsTime minimizes the distance between the evaluation of variableBezier and the sampled point. We define it as follows: %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python #least square form of ||Ax-b||**2 #least square form of ||Ax-b||**2 def to_least_square(A, b): def to_least_square(A, b): return dot(A.T, A), - dot(A.T, b) return dot(A.T, A), - dot(A.T, b) def genCost(variableBezier, ptsTime): def genCost(variableBezier, ptsTime): #first evaluate variableBezier for each time sampled #first evaluate variableBezier for each time sampled allsEvals = [(variableBezier(time), pt) for (pt,time) in ptsTime] allsEvals = [(variableBezier(time), pt) for (pt,time) in ptsTime] #then compute the least square form of the cost for each points #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] allLeastSquares = [to_least_square(el.B(), el.c() + pt) for (el, pt) in allsEvals] #and finally sum the costs #and finally sum the costs Ab = [sum(x) for x in zip(*allLeastSquares)] Ab = [sum(x) for x in zip(*allLeastSquares)] return Ab, Ab return Ab, Ab A, b = genCost(variableBezier, ptsTime) A, b = genCost(variableBezier, ptsTime) ``` ``` %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: Here we use quadprog to solve the least square. Because there are no constraint this might seem overkill, however we will introduce them soon enough. Here we use quadprog to solve the least square. Because there are no constraint this might seem overkill, however we will introduce them soon enough. %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python from qp import quadprog_solve_qp import quadprog from numpy import array, hstack, vstack def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None, verbose=False): """ min (1/2)x' P x + q' x subject to G x <= h subject to C x = d """ # qp_G = .5 * (P + P.T) # make sure P is symmetric qp_G = .5 * (P + P.T) # make sure P is symmetric qp_a = -q qp_C = None qp_b = None meq = 0 if C is not None: if G is not None: qp_C = -vstack([C, G]).T qp_b = -hstack([d, h]) else: qp_C = -C.transpose() qp_b = -d meq = C.shape elif G is not None: # no equality constraint qp_C = -G.T qp_b = -h res = quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq) if verbose: return res # print('qp status ', res) return res res = quadprog_solve_qp(A, b) res = quadprog_solve_qp(A, b) ``` ``` %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: Let's check whether our optimization worked ! Let's check whether our optimization worked ! We can transform the variable Bezier as a regular Bezier curve as follows, and plot the result to verify that the curves match. We can transform the variable Bezier as a regular Bezier curve as follows, and plot the result to verify that the curves match. %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python def evalAndPlot(variableBezier, res): def evalAndPlot(variableBezier, res): fitBezier = variableBezier.evaluate(res.reshape((-1,1)) ) fitBezier = variableBezier.evaluate(res.reshape((-1,1)) ) #plot reference curve in blue, fitted curve in green #plot reference curve in blue, fitted curve in green fig = plt.figure() fig = plt.figure() ax = fig.add_subplot(111, projection="3d") ax = fig.add_subplot(111, projection="3d") plotBezier(ref, ax = ax, linewidth=4.) #thicker line to visualize overlap plotBezier(ref, ax = ax, linewidth=4.) #thicker line to visualize overlap plotBezier(fitBezier, ax = ax, color ="g", linewidth=3.) plotBezier(fitBezier, ax = ax, color ="g", linewidth=3.) plt.show() plt.show() return fitBezier return fitBezier fitBezier = evalAndPlot(variableBezier, res) fitBezier = evalAndPlot(variableBezier, res) ``` ``` %%%% Output: display_data %%%% Output: display_data  %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: ### initial and terminal constraints ### initial and terminal constraints Let's try to fit the reference curve with a curve of lesser degree Let's try to fit the reference curve with a curve of lesser degree %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python pD.degree = refDegree - 1 pD.degree = refDegree - 1 problem = setup_control_points(pD) problem = setup_control_points(pD) variableBezier = problem.bezier() variableBezier = problem.bezier() A, b = genCost(variableBezier, ptsTime) A, b = genCost(variableBezier, ptsTime) res = quadprog_solve_qp(A, b) res = quadprog_solve_qp(A, b) fitBezier = evalAndPlot(variableBezier, res) fitBezier = evalAndPlot(variableBezier, res) ``` ``` %%%% Output: display_data %%%% Output: display_data  %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: We can see that the initial and goal positions are not reached. We can see that the initial and goal positions are not reached. A constraint_flag can be used to impose constraints on the initial/goal positions A constraint_flag can be used to impose constraints on the initial/goal positions and derivatives if required. and derivatives if required. Let's rewrite simplefit to handle such case Let's rewrite simplefit to handle such case %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python from curves.optimization import constraint_flag from curves.optimization import constraint_flag pD.flag = constraint_flag.INIT_POS | constraint_flag.END_POS pD.flag = constraint_flag.INIT_POS | constraint_flag.END_POS #set initial position #set initial position pD.init_pos = array([ptsTime[ 0]]).T pD.init_pos = array([ptsTime[ 0]]).T #set end position #set end position pD.end_pos = array([ptsTime[-1]]).T pD.end_pos = array([ptsTime[-1]]).T problem = setup_control_points(pD) problem = setup_control_points(pD) variableBezier = problem.bezier() variableBezier = problem.bezier() ``` ``` %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: By imposing the initial and final position, we effectively reduce the number of variables by 6: By imposing the initial and final position, we effectively reduce the number of variables by 6: %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python print "Shape of B: ", variableBezier(0).B().shape print "Shape of B: ", variableBezier(0).B().shape ``` ``` %%%% Output: stream %%%% Output: stream Shape of B: (3, 3) Shape of B: (3, 3) %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: The least squares problem then has the following solution The least squares problem then has the following solution %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python prob = setup_control_points(pD) prob = setup_control_points(pD) variableBezier = prob.bezier() variableBezier = prob.bezier() A, b = genCost(variableBezier, ptsTime) A, b = genCost(variableBezier, ptsTime) res = quadprog_solve_qp(A, b) res = quadprog_solve_qp(A, b) _ = evalAndPlot(variableBezier, res) _ = evalAndPlot(variableBezier, res) ``` ``` %%%% Output: display_data %%%% Output: display_data  %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: To impose constraints on the derivatives, we can activate the appropriate constraint flags as follows. To impose constraints on the derivatives, we can activate the appropriate constraint flags as follows. Note that derivatives constraints on velocities will only be considered if the constraints on position are also active. Note that derivatives constraints on velocities will only be considered if the constraints on position are also active. For instance to impose a 0 velocity and acceleration at the initial and goal states we can proceed as follows: For instance to impose a 0 velocity and acceleration at the initial and goal states we can proceed as follows: %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python #values are 0 by default, so if the constraint is zero this can be skipped #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_vel = array([[0., 0., 0.]]).T pD.init_acc = array([[0., 0., 0.]]).T pD.init_acc = array([[0., 0., 0.]]).T pD.end_vel = array([[0., 0., 0.]]).T pD.end_vel = array([[0., 0., 0.]]).T pD.end_acc = 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 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 ``` ``` %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: However, the definition of the variable problem will result in an error. Do you know why ? However, the definition of the variable problem will result in an error. Do you know why ? %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python try: try: prob = setup_control_points(pD) prob = setup_control_points(pD) except RuntimeError,e: except RuntimeError,e: print e print e ``` ``` %%%% Output: stream %%%% Output: stream In setup_control_points; too many constraints for the considered degree In setup_control_points; too many constraints for the considered degree %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: Indeed, there are not enough variables left in the problem to satisfy the constraints. We need to increase the degree of the curve: Indeed, there are not enough variables left in the problem to satisfy the constraints. We need to increase the degree of the curve: %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python pD.degree = refDegree + 4 pD.degree = refDegree + 4 prob = setup_control_points(pD) prob = setup_control_points(pD) variableBezier = prob.bezier() variableBezier = prob.bezier() A, b = genCost(variableBezier, ptsTime) A, b = genCost(variableBezier, ptsTime) res = quadprog_solve_qp(A, b) res = quadprog_solve_qp(A, b) fitBezier = evalAndPlot(variableBezier, res) fitBezier = evalAndPlot(variableBezier, res) ``` ``` %%%% Output: display_data %%%% Output: display_data  %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: We can check that the derivatives of the curve are 0 at start and end We can check that the derivatives of the curve are 0 at start and end %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python print "initial velocity", fitBezier.derivate(fitBezier.min(),1) print "initial velocity", fitBezier.derivate(fitBezier.min(),1) print "initial acceleration", fitBezier.derivate(fitBezier.min(),2) print "initial acceleration", fitBezier.derivate(fitBezier.min(),2) print "end velocity", fitBezier.derivate(fitBezier.max(),1) print "end velocity", fitBezier.derivate(fitBezier.max(),1) print "end acceleration", fitBezier.derivate(fitBezier.max(),2) print "end acceleration", fitBezier.derivate(fitBezier.max(),2) ``` ``` %%%% Output: stream %%%% Output: stream initial velocity [0.0 0.0 0.0] initial velocity [0.0 0.0 0.0] initial acceleration [0.0 0.0 0.0] initial acceleration [0.0 0.0 0.0] end velocity [0.0 0.0 0.0] end velocity [0.0 0.0 0.0] end acceleration [0.0 0.0 0.0] end acceleration [0.0 0.0 0.0] %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: Of course, with such constraints the curve does not really look like the original one anymore. Of course, with such constraints the curve does not really look like the original one anymore. Although it is not recommended, the library is robust enough to allow for adding an arbitrary number of control points. Although it is not recommended, the library is robust enough to allow for adding an arbitrary number of control points. Just for fun, let's add 60 more control points and check that the curve is matched better Just for fun, let's add 60 more control points and check that the curve is matched better %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python pD.degree = refDegree + 60 pD.degree = refDegree + 60 prob = setup_control_points(pD) prob = setup_control_points(pD) variableBezier = prob.bezier() variableBezier = prob.bezier() A, b = genCost(variableBezier, ptsTime) A, b = genCost(variableBezier, ptsTime) #regularization matrix #regularization matrix reg = identity(A.shape) * 0.001 reg = identity(A.shape) * 0.001 res = quadprog_solve_qp(A + reg, b) res = quadprog_solve_qp(A + reg, b) fitBezier = evalAndPlot(variableBezier, res) fitBezier = evalAndPlot(variableBezier, res) ``` ``` %%%% Output: display_data %%%% Output: display_data  %% Cell type:markdown id: tags: %% Cell type:markdown id: tags: ## Adding equality and inequality constraints ## Adding equality and inequality constraints Suppose we want to add specific constraint. Suppose we want to add specific constraint. For instance, we want that the velocity be exactly 0 at t = 0.8, additionally to the start and goal positions being satisfied. This can be done easily by obtaining the variable equation for the variable curve at that time. For instance, we want that the velocity be exactly 0 at t = 0.8, additionally to the start and goal positions being satisfied. This can be done easily by obtaining the variable equation for the variable curve at that time. %% Cell type:code id: tags: %% Cell type:code id: tags: ``` python ``` python #set initial / terminal constraints #set initial / terminal constraints pD.flag = constraint_flag.END_POS | constraint_flag.INIT_POS pD.flag = constraint_flag.END_POS | constraint_flag.INIT_POS pD.degree = refDegree pD.degree = refDegree prob = setup_control_points(pD) prob = setup_control_points(pD) variableBezier = prob.bezier() variableBezier = prob.bezier() #get value of the curve first order derivative at t = 0.8 #get value of the curve first order derivative at t = 0.8 t08Constraint = variableBezier.derivate(0.8,1) t08Constraint = variableBezier.derivate(0.8,1) target = zeros(3) target = zeros(3) A, b = genCost(variableBezier, ptsTime) A, b = genCost(variableBezier, ptsTime) #solve optimization problem with quadprog #solve optimization problem with quadprog res = quadprog_solve_qp(A, b, C=t08Constraint.B(), d=target - t08Constraint.c()) res = quadprog_solve_qp(A, b, C=t08Constraint.B(), d=target - t08Constraint.c()) fitBezier = evalAndPlot(variableBezier, res) fitBezier = evalAndPlot(variableBezier, res) assert norm(fitBezier.derivate(0.8,1) - target) <= 0.001 assert norm(fitBezier.derivate(0.8,1) - target) <= 0.001 ``` ``` %%%% Output: display_data %%%% Output: display_data