Skip to content
GitLab
Commit aefa6394 authored by Steve T's avatar Steve T
Browse files

ndcurves is the final name

parent b1e1168a
Hide whitespace changes
Inline Side-by-side
python/nd_curves/python_variables.cpp → python/ndcurves/python_variables.cpp
View file @ aefa6394
#include "python_variables.h"
#include "nd_curves/python/python_definitions.h"
#include "ndcurves/python/python_definitions.h"
#include <Eigen/Core>
namespace nd_curves {
namespace ndcurves {
std::vector<linear_variable_t> matrix3DFromEigenArray(const point_list3_t& matrices, const point_list3_t& vectors) {
if (vectors.cols() * 3 != matrices.cols()) {
throw std::invalid_argument("vectors.cols() * 3 != matrices.cols()");
......@@ -71,4 +71,4 @@ linear_variable_t* wayPointsToLists(const bezier_linear_variable_t& self) {
return new linear_variable_t(matrices.transpose(), vectors.transpose());
}
} // namespace nd_curves
} // namespace ndcurves
python/nd_curves/python_variables.h → python/ndcurves/python_variables.h
View file @ aefa6394
#include "nd_curves/fwd.h"
#include "nd_curves/linear_variable.h"
#include "nd_curves/bezier_curve.h"
#include "nd_curves/constant_curve.h"
#include "nd_curves/polynomial.h"
#include "nd_curves/exact_cubic.h"
#include "nd_curves/curve_constraint.h"
#include "nd_curves/curve_conversion.h"
#include "nd_curves/bernstein.h"
#include "nd_curves/cubic_hermite_spline.h"
#include "nd_curves/piecewise_curve.h"
#include "nd_curves/so3_linear.h"
#include "nd_curves/se3_curve.h"
#include "nd_curves/sinusoidal.h"
#include "nd_curves/python/python_definitions.h"
#include "ndcurves/fwd.h"
#include "ndcurves/linear_variable.h"
#include "ndcurves/bezier_curve.h"
#include "ndcurves/constant_curve.h"
#include "ndcurves/polynomial.h"
#include "ndcurves/exact_cubic.h"
#include "ndcurves/curve_constraint.h"
#include "ndcurves/curve_conversion.h"
#include "ndcurves/bernstein.h"
#include "ndcurves/cubic_hermite_spline.h"
#include "ndcurves/piecewise_curve.h"
#include "ndcurves/so3_linear.h"
#include "ndcurves/se3_curve.h"
#include "ndcurves/sinusoidal.h"
#include "ndcurves/python/python_definitions.h"
#include <eigenpy/memory.hpp>
#include <eigenpy/eigenpy.hpp>
#include <eigenpy/geometry.hpp>
......@@ -23,7 +23,7 @@
#ifndef _VARIABLES_PYTHON_BINDINGS
#define _VARIABLES_PYTHON_BINDINGS
namespace nd_curves {
namespace ndcurves {
static const int dim = 3;
typedef linear_variable<real, true> linear_variable_t;
typedef quadratic_variable<real> quadratic_variable_t;
......@@ -62,29 +62,29 @@ struct LinearBezierVector {
};
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
} // namespace nd_curves
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::bernstein_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::curve_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::matrix_x_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::pointX_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::linear_variable_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::bezier_linear_variable_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::matrix_pair)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::polynomial_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::exact_cubic_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::bezier_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::constant_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::cubic_hermite_spline_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::piecewise_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::polynomial3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::exact_cubic3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::bezier3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::constant3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::cubic_hermite_spline3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::piecewise3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::SO3Linear_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::SE3Curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::sinusoidal_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(nd_curves::piecewise_SE3_t)
} // namespace ndcurves
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::bernstein_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::curve_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::matrix_x_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::pointX_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::linear_variable_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::bezier_linear_variable_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::matrix_pair)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::polynomial_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::exact_cubic_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::bezier_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::constant_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::cubic_hermite_spline_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::piecewise_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::polynomial3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::exact_cubic3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::bezier3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::constant3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::cubic_hermite_spline3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::piecewise3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::SO3Linear_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::SE3Curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::sinusoidal_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(ndcurves::piecewise_SE3_t)
#endif //_VARIABLES_PYTHON_BINDINGS
python/test/notebook.py
View file @ aefa6394
......@@ -4,7 +4,7 @@ import eigenpy
from numpy import array, dot, identity, zeros
# importing the bezier curve class
from nd_curves import bezier
from ndcurves import bezier
eigenpy.switchToNumpyArray()
......@@ -30,7 +30,7 @@ class TestNotebook(unittest.TestCase):
fNumSamples = float(numSamples)
ptsTime = [(ref(float(t) / fNumSamples), float(t) / fNumSamples) for t in range(numSamples + 1)]
from nd_curves.optimization import (problem_definition, setup_control_points)
from ndcurves.optimization import (problem_definition, setup_control_points)
# dimension of our problem (here 3 as our curve is 3D)
dim = 3
......
python/test/optimization.py
View file @ aefa6394
......@@ -4,7 +4,7 @@ import eigenpy
from numpy import array, matrix, zeros
from numpy.linalg import norm
from nd_curves.optimization import (constraint_flag, generate_integral_problem, integral_cost_flag, problem_definition,
from ndcurves.optimization import (constraint_flag, generate_integral_problem, integral_cost_flag, problem_definition,
setup_control_points)
eigenpy.switchToNumpyArray()
......
python/test/registration.py
View file @ aefa6394
......@@ -9,15 +9,15 @@ class TestRegistration(unittest.TestCase):
"""
def test_pinocchio_then_curves(self):
import pinocchio
import nd_curves
import ndcurves
self.assertTrue(hasattr(pinocchio, 'Quaternion'))
self.assertTrue(hasattr(nd_curves, 'Quaternion'))
self.assertTrue(hasattr(ndcurves, 'Quaternion'))
def test_curves_then_pinocchio(self):
import nd_curves
import ndcurves
import pinocchio
self.assertTrue(hasattr(pinocchio, 'Quaternion'))
self.assertTrue(hasattr(nd_curves, 'Quaternion'))
self.assertTrue(hasattr(ndcurves, 'Quaternion'))
if __name__ == '__main__':
......
python/test/sandbox/test.ipynb
View file @ aefa6394
......@@ -30,7 +30,7 @@
},
{
"cell_type": "code",
"execution_count": 21,
"execution_count": 33,
"metadata": {},
"outputs": [
{
......@@ -58,10 +58,10 @@
"eigenpy.switchToNumpyArray()\n",
"\n",
"#importing the bezier curve class\n",
"from nd_curves import (bezier)\n",
"from ndcurves import (bezier)\n",
"\n",
"#importing tools to plot bezier curves\n",
"from nd_curves.plot import (plotBezier)\n",
"from ndcurves.plot import (plotBezier)\n",
"from mpl_toolkits.mplot3d import Axes3D\n",
"import matplotlib.pyplot as plt\n",
"import numpy as np\n",
......@@ -85,7 +85,7 @@
},
{
"cell_type": "code",
"execution_count": 2,
"execution_count": 34,
"metadata": {},
"outputs": [
{
......@@ -127,11 +127,11 @@
},
{
"cell_type": "code",
"execution_count": 3,
"execution_count": 35,
"metadata": {},
"outputs": [],
"source": [
"from nd_curves.optimization import (problem_definition, setup_control_points)\n",
"from ndcurves.optimization import (problem_definition, setup_control_points)\n",
"\n",
"#dimension of our problem (here 3 as our curve is 3D)\n",
"dim = 3\n",
......@@ -155,7 +155,7 @@
},
{
"cell_type": "code",
"execution_count": 4,
"execution_count": 36,
"metadata": {},
"outputs": [
{
......@@ -191,7 +191,7 @@
},
{
"cell_type": "code",
"execution_count": 5,
"execution_count": 37,
"metadata": {},
"outputs": [
{
......@@ -219,7 +219,7 @@
},
{
"cell_type": "code",
"execution_count": 6,
"execution_count": 38,
"metadata": {},
"outputs": [],
"source": [
......@@ -247,7 +247,7 @@
},
{
"cell_type": "code",
"execution_count": 7,
"execution_count": 39,
"metadata": {},
"outputs": [],
"source": [
......@@ -295,7 +295,7 @@
},
{
"cell_type": "code",
"execution_count": 8,
"execution_count": 40,
"metadata": {},
"outputs": [
{
......@@ -335,7 +335,7 @@
},
{
"cell_type": "code",
"execution_count": 9,
"execution_count": 41,
"metadata": {},
"outputs": [
{
......@@ -376,7 +376,7 @@
},
{
"cell_type": "code",
"execution_count": 10,
"execution_count": 42,
"metadata": {},
"outputs": [],
"source": [
......@@ -400,7 +400,7 @@
},
{
"cell_type": "code",
"execution_count": 11,
"execution_count": 43,
"metadata": {},
"outputs": [
{
......@@ -424,7 +424,7 @@
},
{
"cell_type": "code",
"execution_count": 12,
"execution_count": 44,
"metadata": {},
"outputs": [
{
......@@ -461,7 +461,7 @@
},
{
"cell_type": "code",
"execution_count": 13,
"execution_count": 45,
"metadata": {},
"outputs": [],
"source": [
......@@ -482,7 +482,7 @@
},
{
"cell_type": "code",
"execution_count": 14,
"execution_count": 46,
"metadata": {},
"outputs": [
{
......@@ -509,7 +509,7 @@
},
{
"cell_type": "code",
"execution_count": 15,
"execution_count": 47,
"metadata": {},
"outputs": [
{
......@@ -543,7 +543,7 @@
},
{
"cell_type": "code",
"execution_count": 16,
"execution_count": 48,
"metadata": {},
"outputs": [
{
......@@ -575,7 +575,7 @@
},
{
"cell_type": "code",
"execution_count": 17,
"execution_count": 49,
"metadata": {},
"outputs": [
{
......@@ -614,7 +614,7 @@
},
{
"cell_type": "code",
"execution_count": 18,
"execution_count": 50,
"metadata": {},
"outputs": [
{
......@@ -669,7 +669,7 @@
},
{
"cell_type": "code",
"execution_count": 19,
"execution_count": 51,
"metadata": {},
"outputs": [
{
......@@ -714,7 +714,7 @@
},
{
"cell_type": "code",
"execution_count": 20,
"execution_count": 52,
"metadata": {},
"outputs": [
{
......@@ -778,27 +778,6 @@
"ax.plot_surface(xx, yy, z, alpha=0.2)\n",
"plt.show()"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
......
%% Cell type:markdown id: tags:
# Curve optimization with the curves library
%% 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
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.
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
can be automatically derivated or integrated without 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 through the steps of formulating and solving an optimization problem.
## The problem: trajectory fitting
We start with a simple, unconstrained problem.
Let us first consider a 3D curve:
%% Cell type:code id: tags:
``` python
# importing classical numpy objects
from numpy import zeros, array, identity, dot
from numpy.linalg import norm
import numpy as np
np.set_printoptions(formatter={'float': lambda x: "{0:0.1f}".format(x)})
#use array representation for binding eigen objects to python
import eigenpy
eigenpy.switchToNumpyArray()
#importing the bezier curve class
from nd_curves import (bezier)
from ndcurves import (bezier)
#importing tools to plot bezier curves
from nd_curves.plot import (plotBezier)
from ndcurves.plot import (plotBezier)
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import numpy as np
#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)
#plotting the curve with its control points
plotBezier(ref,showControlPoints = True, color="g")
```
%%%% Output: display_data
%% 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 will first generate a discretization of the curve to represent a temporal sampling:
%% Cell type:code id: tags:
``` python
numSamples = 10; fNumSamples = float(numSamples)
ptsTime = [ (ref(float(t) / fNumSamples), float(t) / fNumSamples) for t in range(numSamples+1)]
for el in ptsTime:
print (el)
```
%%%% Output: stream
(array([1.0, 2.0, 3.0]), 0.0)
(array([-0.1, 0.4, 0.9]), 0.1)
(array([-0.6, -0.4, -0.1]), 0.2)
(array([-0.5, -0.4, -0.2]), 0.3)
(array([0.1, 0.2, 0.4]), 0.4)
(array([1.0, 1.2, 1.5]), 0.5)
(array([2.2, 2.6, 3.0]), 0.6)
(array([3.4, 4.1, 4.7]), 0.7)
(array([4.7, 5.6, 6.4]), 0.8)
(array([6.0, 6.9, 7.9]), 0.9)
(array([7.0, 8.0, 9.0]), 1.0)
%% Cell type:markdown id: tags:
Each entry of ptsTime is a couple (position, time) that describes our input data.
### Sanity check
Let's first solve a trivial problem, to see if we can reconstruct the curve with a polynomial
of same degree.
To achieve this we will use the problemDefinition class, which will automatically generate the variable expression of the curve
%% Cell type:code id: tags:
``` python
from nd_curves.optimization import (problem_definition, setup_control_points)
from ndcurves.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()
```
%% Cell type:markdown id: tags:
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
%% Cell type:code id: tags:
``` python
linearVariable = variableBezier(0.)
print ("B: \n", linearVariable.B())
print ("c:\n",linearVariable.c())
print ("Shape of B: ", linearVariable.B().shape)
```
%%%% Output: stream
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]
[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]]
c:
[0.0 0.0 0.0]
Shape of B: (3, 12)
%% 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.
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:
%% Cell type:code id: tags:
``` python
print ("B: \n", variableBezier(0.2).B())
```
%%%% Output: stream
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.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]]
%% Cell type:markdown id: tags:
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:
%% Cell type:code id: tags:
``` python
#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)
```
%% 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.
%% Cell type:code id: tags:
``` python
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_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[0]
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[0]
res = quadprog_solve_qp(A, b)
```
%% Cell type:markdown id: tags:
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.
%% Cell type:code id: tags:
``` python
def evalAndPlot(variableBezier, res):
fitBezier = variableBezier.evaluate(res.reshape((-1,1)) )
#plot reference curve in blue, fitted curve in green
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
plotBezier(ref, ax = ax, linewidth=4.) #thicker line to visualize overlap
plotBezier(fitBezier, ax = ax, color ="g", linewidth=3.)
plt.show()
return fitBezier
fitBezier = evalAndPlot(variableBezier, res)
```
%%%% Output: display_data
%% Cell type:markdown id: tags:
### initial and terminal constraints
Let's try to fit the reference curve with a curve of lesser degree
%% Cell type:code id: tags:
``` python
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)
```
%%%% Output: display_data
%% Cell type:markdown id: tags:
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
and derivatives if required.
Let's rewrite simplefit to handle such case
%% Cell type:code id: tags:
``` python
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()
```
%% Cell type:markdown id: tags:
By imposing the initial and final position, we effectively reduce the number of variables by 6:
%% Cell type:code id: tags:
``` python
print ("Shape of B: ", variableBezier(0).B().shape)
```
%%%% Output: stream
Shape of B: (3, 3)
%% Cell type:markdown id: tags:
The least squares problem then has the following solution
%% Cell type:code id: tags:
``` python
prob = setup_control_points(pD)
variableBezier = prob.bezier()
A, b = genCost(variableBezier, ptsTime)
res = quadprog_solve_qp(A, b)
_ = evalAndPlot(variableBezier, res)
```
%%%% Output: display_data
%% Cell type:markdown id: tags:
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.
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:
``` python
#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
```
%% Cell type:markdown id: tags:
However, the definition of the variable problem will result in an error. Do you know why ?
%% Cell type:code id: tags:
``` python
try:
prob = setup_control_points(pD)
except RuntimeError as e:
print (e)
```
%%%% Output: stream
In setup_control_points; too many constraints for the considered degree
%% 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:
%% Cell type:code id: tags:
``` python
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)
```
%%%% Output: display_data