"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[0]\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[0]\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<double, double, true, curves::linear_variable<double, true>, curves::linear_variable<double, true> >",

"\u001b[0;31mTypeError\u001b[0m: No to_python (by-value) converter found for C++ type: curves::piecewise_curve<double, double, true, curves::linear_variable<double, true>, curves::linear_variable<double, true> >"

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.

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:

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

defto_least_square(A,b):

defto_least_square(A,b):

returndot(A.T,A),-dot(A.T,b)

returndot(A.T,A),-dot(A.T,b)

defgenCost(variableBezier,ptsTime):

defgenCost(variableBezier,ptsTime):

#first evaluate variableBezier for each time sampled

#first evaluate variableBezier for each time sampled

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[1])*0.001

reg=identity(A.shape[1])*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.