Commit 879bb820 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'export_plot' into 'devel'

Export plot

See merge request loco-3d/curves!42
parents 05880e99 d2f18b68
......@@ -18,48 +18,6 @@ To do so, tools are provided to:
The library is template-based, thus generic: the curves can be of any dimension, and can be implemented in double, float ...
----------
Example of use for and end-effector trajectory
-------------
The library comes with an helper class to automatically generate end-effector trajectories.
For instance, to create a 2 second long trajectory from the point (0,0,0) to (1,1,0), with a waypoint
at (0.5,0.5,0.5), one can use the following code:
```cpp
typedef std::pair<double, Eigen::Vector3d> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
// loading helper class namespace
using namespace spline::helpers;
// Create waypoints
waypoints.push_back(std::make_pair(0., Eigen::Vector3d(0,0,0)));
waypoints.push_back(std::make_pair(1., Eigen::Vector3d(0.5,0.5,0.5)));
waypoints.push_back(std::make_pair(2., Eigen::Vector3d(1,1,0)));
exact_cubic_t* eff_traj = effector_spline(waypoints.begin(),waypoints.end());
// evaluate spline
(*eff_traj)(0.); // (0,0,0)
(*eff_traj)(2.); // (1,1,0)
```
If rotation of the effector must be considered, the code is almost the same:
```cpp
// initial rotation is 0, end rotation is a rotation by Pi around x axis
quat_t init_rot(0,0,0,1), end_rot(1,0,0,0);
effector_spline_rotation eff_traj_rot(waypoints.begin(),waypoints.end(), init_quat, end_quat);
// evaluate spline
eff_traj_rot(0.); // (0,0,0,0,0,0,1)
eff_traj_rot(1.); // (0.5,0.5,0.5,0.707107,0,0,0.707107) // Pi/2 around x axis
eff_traj_rot(2.); // (0,0,0,1,0,0,0)
```
Additional parameters for the same methods an be used to specify parameters for the take off and
landing phases: height and duration of the phase, and along which normal.
Please refer to the Main.cpp files to see all the unit tests and possibilities offered by the library
Installation
-------------
......@@ -75,20 +33,18 @@ This package is available as binary in [robotpkg/wip](http://robotpkg.openrobots
To handle this with cmake, use the recursive option to clone the repository.
For instance, using http:
```
git clone --recursive https://github.com/stonneau/spline.git $SPLINE_DIR
git clone --recursive https://github.com/loco-3d/curves $CURVES_DIR
```
The library is header only, so the build only serves to build the tests and python bindings:
```sh
cd $SPLINE_DIR && mkdir build && cd build
cmake .. && make
../bin/tests
cd $CURVES_DIR && mkdir build && cd build
cmake .. && make && make test
```
If everything went fine you should obtain the following output:
```sh
performing tests...
no errors found
100% tests passed, 0 tests failed out of 3
```
### Optional: Python bindings installation
To install the Python bindings, in the CMakeLists.txt file, first enable the BUILD_PYTHON_INTERFACE option:
......@@ -98,13 +54,18 @@ OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
Then rebuild the library:
```sh
cd $SPLINE_DIR/build
cd $CURVES_DIR/build
cmake -DCMAKE_INSTALL_PREFIX=${DEVEL_DIR}/install ..
make install
```
The python bindings should then be accessible through the package centroidal_dynamics.
To see example of use, you can refer to the [test file](https://github.com/stonneau/spline/blob/master/python/test/test.py)
which is rather self explanatory:
In spite of an exhaustive documentation, please refer to the C++ documentation, which mostly applies
to python. For the moment, only bezier curves are binded.
Documentation and tutorial
-------------
For a python tutorial, you can refer to the [jupyter notebook](https://github.com/loco-3d/curves/blob/devel/python/test/sandbox/test.ipynb) . The [test file](https://github.com/loco-3d/curves/blob/master/python/test/test.py)
is more exhaustive and rather self explanatory.
Please refer to the C++ manual, which mostly applies
to python.
File added
This diff is collapsed.
......@@ -40,7 +40,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
typedef typename t_point_t::const_iterator cit_point_t;
typedef bezier_curve<Time, Numeric, Safe, Point> bezier_curve_t;
typedef boost::shared_ptr<bezier_curve_t> bezier_curve_ptr_t;
typedef piecewise_curve<Time, Numeric, Safe, point_t> piecewise_curve_t;
typedef piecewise_curve<Time, Numeric, Safe, point_t, point_t, bezier_curve_t> piecewise_curve_t;
typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class
typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
......
......@@ -29,7 +29,7 @@ template <typename Time, typename Numeric, bool Safe, typename Point,
struct exact_cubic;
template <typename Time, typename Numeric, bool Safe, typename Point,
typename Point_derivate>
typename Point_derivate, typename CurveType>
struct piecewise_curve;
template <typename Time, typename Numeric, bool Safe,typename Point, typename T_Point>
......@@ -82,19 +82,19 @@ typedef polynomial<double, double, true, pointX_t, t_pointX_t> polynomial_t;
typedef exact_cubic<double, double, true, pointX_t,t_pointX_t, polynomial_t> exact_cubic_t;
typedef bezier_curve<double, double, true, pointX_t> bezier_t;
typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
typedef piecewise_curve <double, double, true, pointX_t,pointX_t> piecewise_t;
typedef piecewise_curve <double, double, true, pointX_t,pointX_t, curve_abc_t> piecewise_t;
// definition of all curves class with point3 as return type:
typedef polynomial<double, double, true, point3_t, t_point3_t> polynomial3_t;
typedef exact_cubic<double, double, true, point3_t,t_point3_t, polynomial_t> exact_cubic3_t;
typedef bezier_curve<double, double, true, point3_t> bezier3_t;
typedef cubic_hermite_spline<double, double, true, point3_t> cubic_hermite_spline3_t;
typedef piecewise_curve <double, double, true, point3_t,point3_t> piecewise3_t;
typedef piecewise_curve <double, double, true, point3_t,point3_t, curve_3_t> piecewise3_t;
// special curves with return type fixed:
typedef SO3Linear<double, double, true> SO3Linear_t;
typedef SE3Curve<double, double, true> SE3Curve_t;
typedef piecewise_curve <double, double, true, transform_t, point6_t> piecewise_SE3_t;
typedef piecewise_curve <double, double, true, transform_t, point6_t, curve_SE3_t> piecewise_SE3_t;
}
......
......@@ -25,7 +25,7 @@ namespace curves {
///
template <typename Time = double, typename Numeric = Time, bool Safe = false,
typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename Point_derivate = Point >
typename Point_derivate = Point, typename CurveType = curve_abc<Time, Numeric, Safe, Point,Point_derivate> >
struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_derivate> {
typedef Point point_t;
typedef Point_derivate point_derivate_t;
......@@ -33,11 +33,12 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
typedef std::vector<point_derivate_t, Eigen::aligned_allocator<point_derivate_t> > t_point_derivate_t;
typedef Time time_t;
typedef Numeric num_t;
typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_t; // parent class
typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> base_curve_t; // parent class
typedef CurveType curve_t; // contained curves base class
typedef boost::shared_ptr<curve_t> curve_ptr_t;
typedef typename std::vector<curve_ptr_t> t_curve_ptr_t;
typedef typename std::vector<Time> t_time_t;
typedef piecewise_curve<Time, Numeric, Safe, Point,Point_derivate> piecewise_curve_t;
typedef piecewise_curve<Time, Numeric, Safe, Point,Point_derivate,CurveType> piecewise_curve_t;
public:
/// \brief Empty constructor. Add at least one curve to call other class functions.
///
......@@ -97,7 +98,7 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
return true;
}
virtual bool isApprox(const curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
virtual bool isApprox(const base_curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
const piecewise_curve_t* other_cast = dynamic_cast<const piecewise_curve_t*>(other);
if(other_cast)
return isApprox(*other_cast,prec);
......@@ -435,7 +436,7 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
if (version) {
// Do something depending on version ?
}
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_t);
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(base_curve_t);
ar& boost::serialization::make_nvp("dim", dim_);
ar& boost::serialization::make_nvp("curves", curves_);
ar& boost::serialization::make_nvp("time_curves", time_curves_);
......@@ -446,8 +447,8 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
}; // End struct piecewise curve
template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate>
const double piecewise_curve<Time, Numeric, Safe, Point,Point_derivate >::MARGIN(0.001);
template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate, typename CurveType>
const double piecewise_curve<Time, Numeric, Safe, Point,Point_derivate, CurveType >::MARGIN(0.001);
} // namespace curves
......
......@@ -24,7 +24,16 @@ IF(APPLE)
SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES SUFFIX ".so")
ENDIF(APPLE)
INSTALL(TARGETS ${PY_NAME} DESTINATION ${PYTHON_SITELIB})
INSTALL(TARGETS ${PY_NAME} DESTINATION ${PYTHON_SITELIB}/${PY_NAME})
install (FILES plot/plot.py
deploy/__init__.py
DESTINATION ${PYTHON_SITELIB}/${PY_NAME})
install (FILES deploy/optimization/__init__.py
DESTINATION ${PYTHON_SITELIB}/${PY_NAME}/optimization)
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")
......@@ -176,6 +176,21 @@ piecewise_SE3_t* wrapPiecewiseSE3EmptyConstructor() {
return new piecewise_SE3_t();
}
typedef bezier_t::piecewise_curve_t piecewise_bezier_t;
piecewise_bezier_t* wrapPiecewiseBezierConstructor(const bezier_t::bezier_curve_ptr_t& curve) {
return new piecewise_bezier_t(curve);
}
piecewise_bezier_t* wrapPiecewiseBezierEmptyConstructor() {
return new piecewise_bezier_t();
}
typedef bezier_linear_variable_t::piecewise_curve_t piecewise_linear_bezier_t;
piecewise_linear_bezier_t* wrapPiecewiseBezierLinearConstructor(const bezier_linear_variable_t::bezier_curve_ptr_t& curve) {
return new piecewise_linear_bezier_t(curve);
}
piecewise_linear_bezier_t* wrapPiecewiseBezierLinearEmptyConstructor() {
return new piecewise_linear_bezier_t();
}
static piecewise_t discretPointToPolynomialC0(const pointX_list_t& points,
const time_waypoints_t& time_points) {
t_pointX_t points_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points);
......@@ -494,7 +509,7 @@ BOOST_PYTHON_MODULE(curves) {
/** BEGIN bezier3 curve**/
class_<bezier3_t, bases<curve_3_t> >("bezier3", init<>())
class_<bezier3_t, bases<curve_3_t>, boost::shared_ptr<bezier3_t> >("bezier3", init<>())
.def("__init__", make_constructor(&wrapBezier3Constructor))
.def("__init__", make_constructor(&wrapBezier3ConstructorBounds))
.def("__init__", make_constructor(&wrapBezier3ConstructorConstraints))
......@@ -519,7 +534,7 @@ BOOST_PYTHON_MODULE(curves) {
;
/** END bezier3 curve**/
/** BEGIN bezier curve**/
class_<bezier_t, bases<curve_abc_t> >("bezier", init<>())
class_<bezier_t, bases<curve_abc_t>, boost::shared_ptr<bezier_t> >("bezier", init<>())
.def("__init__", make_constructor(&wrapBezierConstructor))
.def("__init__", make_constructor(&wrapBezierConstructorBounds))
.def("__init__", make_constructor(&wrapBezierConstructorConstraints))
......@@ -567,7 +582,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("isZero", &linear_variable_t::isZero)
.def("norm", &linear_variable_t::norm);
class_<bezier_linear_variable_t>("bezier_linear_variable", no_init)
class_<bezier_linear_variable_t, bases<curve_abc_t>, boost::shared_ptr<bezier_linear_variable_t> >("bezier_linear_variable", no_init)
.def("__init__", make_constructor(&wrapBezierLinearConstructor))
.def("__init__", make_constructor(&wrapBezierLinearConstructorBounds))
.def("min", &bezier_linear_variable_t::min)
......@@ -708,6 +723,68 @@ BOOST_PYTHON_MODULE(curves) {
.def(bp::self != bp::self)
;
class_<piecewise_bezier_t, bases<curve_abc_t> >("piecewise_bezier", init<>())
.def("__init__",
make_constructor(&wrapPiecewiseBezierConstructor, default_call_policies(), arg("curve")),
"Create a peicewise Bezier curve containing the given curve.")
.def("__init__", make_constructor(&wrapPiecewiseBezierEmptyConstructor),
"Create an empty piecewise-Beziercurve.")
.def("append", &piecewise_bezier_t::add_curve_ptr,
"Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
"where T_{min} is equal toT_{max} of the actual piecewise curve.")
.def("is_continuous", &piecewise_bezier_t::is_continuous,
"Check if the curve is continuous at the given order.",args("self,order"))
.def("curve_at_index", &piecewise_bezier_t::curve_at_index,
return_value_policy<copy_const_reference>())
.def("curve_at_time", &piecewise_bezier_t::curve_at_time, return_value_policy<copy_const_reference>())
.def("num_curves", &piecewise_bezier_t::num_curves)
.def("saveAsText", &piecewise_bezier_t::saveAsText<piecewise_bezier_t>, bp::args("filename"),
"Saves *this inside a text file.")
.def("loadFromText", &piecewise_bezier_t::loadFromText<piecewise_bezier_t>,
bp::args("filename"), "Loads *this from a text file.")
.def("saveAsXML", &piecewise_bezier_t::saveAsXML<piecewise_bezier_t>,
bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
.def("loadFromXML", &piecewise_bezier_t::loadFromXML<piecewise_bezier_t>,
bp::args("filename", "tag_name"), "Loads *this from a XML file.")
.def("saveAsBinary", &piecewise_bezier_t::saveAsBinary<piecewise_bezier_t>,
bp::args("filename"), "Saves *this inside a binary file.")
.def("loadFromBinary", &piecewise_bezier_t::loadFromBinary<piecewise_bezier_t>,
bp::args("filename"), "Loads *this from a binary file.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
;
class_<piecewise_linear_bezier_t, bases<curve_abc_t> >("piecewise_bezier_linear", init<>())
.def("__init__",
make_constructor(&wrapPiecewiseBezierLinearConstructor, default_call_policies(), arg("curve")),
"Create a peicewise Bezier curve containing the given curve.")
.def("__init__", make_constructor(&wrapPiecewiseBezierLinearEmptyConstructor),
"Create an empty piecewise-Beziercurve.")
.def("append", &piecewise_linear_bezier_t::add_curve_ptr,
"Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
"where T_{min} is equal toT_{max} of the actual piecewise curve.")
.def("is_continuous", &piecewise_linear_bezier_t::is_continuous,
"Check if the curve is continuous at the given order.",args("self,order"))
.def("curve_at_index", &piecewise_linear_bezier_t::curve_at_index,
return_value_policy<copy_const_reference>())
.def("curve_at_time", &piecewise_linear_bezier_t::curve_at_time, return_value_policy<copy_const_reference>())
.def("num_curves", &piecewise_linear_bezier_t::num_curves)
.def("saveAsText", &piecewise_linear_bezier_t::saveAsText<piecewise_linear_bezier_t>, bp::args("filename"),
"Saves *this inside a text file.")
.def("loadFromText", &piecewise_linear_bezier_t::loadFromText<piecewise_linear_bezier_t>,
bp::args("filename"), "Loads *this from a text file.")
.def("saveAsXML", &piecewise_linear_bezier_t::saveAsXML<piecewise_linear_bezier_t>,
bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
.def("loadFromXML", &piecewise_linear_bezier_t::loadFromXML<piecewise_linear_bezier_t>,
bp::args("filename", "tag_name"), "Loads *this from a XML file.")
.def("saveAsBinary", &piecewise_linear_bezier_t::saveAsBinary<piecewise_linear_bezier_t>,
bp::args("filename"), "Saves *this inside a binary file.")
.def("loadFromBinary", &piecewise_linear_bezier_t::loadFromBinary<piecewise_linear_bezier_t>,
bp::args("filename"), "Loads *this from a binary file.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
;
class_<piecewise_SE3_t, bases<curve_SE3_t> >("piecewise_SE3", init<>())
.def("__init__", make_constructor(&wrapPiecewiseSE3Constructor, default_call_policies(), arg("curve")),
"Create a piecewise-se3 curve containing the given se3 curve.")
......
#!/usr/bin/env python
# Copyright (c) 2019 CNRS
# Author : Steve Tonneau
from curves import *
import plot
#!/usr/bin/env python
# Copyright (c) 2019 CNRS
# Author : Steve Tonneau
from curves.curves.optimization import *
import eigenpy
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
from numpy import array
......@@ -63,7 +64,6 @@ if __name__ == '__main__':
a = bezier(waypoints)
fig = plt.figure()
# ax = fig.add_subplot(131, projection="3d")
ax = fig.add_subplot(121)
plotBezier2D(a, axes=[1, 0], ax=ax)
plotControlPoints2D(a, axes=[1, 0], ax=ax)
......
......@@ -56,12 +56,13 @@ linear_variable_t* wayPointsToLists(const bezier_linear_variable_t& self) {
const t_point& wps = self.waypoints();
// retrieve num variables.
std::size_t dim = wps[0].B().cols();
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrices(dim, wps.size() * 3);
Eigen::Matrix<real, Eigen::Dynamic, 1> vectors = Eigen::Matrix<real, Eigen::Dynamic, 1>::Zero(3 * wps.size());
std::size_t dimRows = wps[0].c().rows();
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrices(dim, wps.size() * dimRows);
Eigen::Matrix<real, Eigen::Dynamic, 1> vectors = Eigen::Matrix<real, Eigen::Dynamic, 1>::Zero(dimRows * wps.size());
int i = 0;
for (cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++i) {
matrices.block(0, i * 3, dim, 3) = cit->B().transpose();
vectors.segment<3>(i * 3, i * 3 + 2) = cit->c();
matrices.block(0, i * dimRows, dim, dimRows) = cit->B().transpose();
vectors.segment(i * dimRows, dimRows) = cit->c();
}
return new linear_variable_t(matrices.transpose(), vectors.transpose());
}
......
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:
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()
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):