Skip to content

Commit

Permalink
cleaning bindings
Browse files Browse the repository at this point in the history
  • Loading branch information
stevet committed Oct 10, 2019
1 parent 1d7d632 commit 771006b
Show file tree
Hide file tree
Showing 15 changed files with 717 additions and 183 deletions.
1 change: 1 addition & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,3 +6,4 @@ build-rel/
.*~
*.user
*.pyc
*.ipynb_checkpoints
23 changes: 22 additions & 1 deletion include/curves/linear_variable.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@

#include "curve_abc.h"
#include "bezier_curve.h"
#include "serialization/archive.hpp"
#include "serialization/eigen-matrix.hpp"

#include "MathDefs.h"

Expand All @@ -23,7 +25,7 @@
namespace curves
{
template <typename Numeric=double, bool Safe=true>
struct linear_variable
struct linear_variable : public serialization::Serializable
{
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
Expand Down Expand Up @@ -94,10 +96,29 @@ struct linear_variable

std::size_t size() const {return zero ? 0 : std::max(B_.cols(), c_.size()) ;}

Numeric norm() const
{
return isZero() ? 0 : (B_.norm() + c_.norm());
}

const matrix_x_t& B() const {return B_;}
const vector_x_t& c () const {return c_;}
bool isZero () const {return zero;}


// Serialization of the class
friend class boost::serialization::access;

template <class Archive>
void serialize(Archive& ar, const unsigned int version) {
if (version) {
// Do something depending on version ?
}
ar& boost::serialization::make_nvp("B_", B_);
ar& boost::serialization::make_nvp("c_", c_);
ar& boost::serialization::make_nvp("zero", zero);
}

private:
matrix_x_t B_;
vector_x_t c_;
Expand Down
25 changes: 17 additions & 8 deletions include/curves/optimization/definitions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ struct quadratic_problem


template<typename Point, typename Numeric>
struct problem_definition
struct problem_definition : public curve_constraints<Point>
{
typedef Point point_t;
typedef Numeric num_t;
Expand All @@ -60,20 +60,29 @@ struct problem_definition
typedef typename T_vector_x_t::const_iterator CIT_vector_x_t;

problem_definition(const int dim)
: flag(NONE)
, start(point_t::Zero(dim))
, end(point_t::Zero(dim))
, curveConstraints(dim)
: curve_constraints_t(dim)
, flag(NONE)
, init_pos(point_t::Zero(dim))
, end_pos(point_t::Zero(dim))
, degree(5)
, totalTime(1.)
, splitTimes_(vector_x_t::Zero(0))
, dim_(dim){}

problem_definition(const curve_constraints_t& parent)
: curve_constraints_t(parent)
, flag(NONE)
, init_pos(point_t::Zero(parent.dim_))
, end_pos(point_t::Zero(parent.dim_))
, degree(5)
, totalTime(1.)
, splitTimes_(vector_x_t::Zero(0))
, dim_(parent.dim_){}


constraint_flag flag;
point_t start;
point_t end;
curve_constraints_t curveConstraints;
point_t init_pos;
point_t end_pos;
std::size_t degree;
num_t totalTime;
vector_x_t splitTimes_;
Expand Down
25 changes: 12 additions & 13 deletions include/curves/optimization/details.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,6 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
typedef linear_variable<Numeric> var_t;
typedef problem_data<Point, Numeric> problem_data_t;

const curve_constraints<Point>& constraints = pDef.curveConstraints;
const std::size_t& degree = pDef.degree;
const constraint_flag& flag = pDef.flag;

Expand All @@ -120,26 +119,26 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
std::size_t i =0;
if(flag & INIT_POS)
{
variables_.push_back(var_t(pDef.start));
variables_.push_back(var_t(pDef.init_pos));
++numConstants;
++i;
if(flag & INIT_VEL)
{
point_t vel = pDef.start + (constraints.init_vel / (num_t)degree) / pDef.totalTime;
point_t vel = pDef.init_pos + (pDef.init_vel / (num_t)degree) / pDef.totalTime;
variables_.push_back(var_t(vel));
++numConstants;
++i;
if(flag & INIT_ACC)
{
point_t acc = (constraints.init_acc / (num_t)(degree * (degree-1)))
point_t acc = (pDef.init_acc / (num_t)(degree * (degree-1)))
/ (pDef.totalTime * pDef.totalTime)
+ 2* vel- pDef.start;;
+ 2* vel- pDef.init_pos;;
variables_.push_back(var_t(acc));
++numConstants;
++i;
if(flag & INIT_JERK){
point_t jerk = constraints.init_jerk*pDef.totalTime*pDef.totalTime*pDef.totalTime/(num_t)(degree*(degree-1)*(degree-2))
+ 3*acc -3*vel +pDef.start;
point_t jerk = pDef.init_jerk*pDef.totalTime*pDef.totalTime*pDef.totalTime/(num_t)(degree*(degree-1)*(degree-2))
+ 3*acc -3*vel +pDef.init_pos;
variables_.push_back(var_t(jerk));
++numConstants;
++i;
Expand All @@ -156,15 +155,15 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
{
if(flag & END_VEL)
{
point_t vel = pDef.end - (constraints.end_vel / (num_t)degree) / pDef.totalTime;
point_t vel = pDef.end_pos - (pDef.end_vel / (num_t)degree) / pDef.totalTime;
if(flag & END_ACC)
{
point_t acc = (constraints.end_acc / (num_t)(degree * (degree-1)))
point_t acc = (pDef.end_acc / (num_t)(degree * (degree-1)))
/ (pDef.totalTime) * (pDef.totalTime)
+ 2* vel - pDef.end;
+ 2* vel - pDef.end_pos;
if(flag & END_JERK){
point_t jerk = -constraints.end_jerk*pDef.totalTime*pDef.totalTime*pDef.totalTime/(num_t)(degree*(degree-1)*(degree-2))
+ 3*acc -3*vel + pDef.end;
point_t jerk = -pDef.end_jerk*pDef.totalTime*pDef.totalTime*pDef.totalTime/(num_t)(degree*(degree-1)*(degree-2))
+ 3*acc -3*vel + pDef.end_pos;
variables_.push_back(var_t(jerk));
++numConstants;
++i;
Expand All @@ -191,7 +190,7 @@ problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition
++i;
}
}
variables_.push_back(var_t(pDef.end));
variables_.push_back(var_t(pDef.end_pos));
++numConstants; ++i;
}
// add remaining variables (only if no end_pos constraints)
Expand Down
19 changes: 19 additions & 0 deletions include/curves/piecewise_curve.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -157,6 +157,25 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point> {
return isContinuous;
}

std::size_t num_curves() const
{
return curves_.size();
}

const curve_t& curve_at_time(const time_t t) const
{
return curves_[find_interval(t)];
}

const curve_t& curve_at_index(const std::size_t idx) const
{
if (Safe && idx >= num_curves())
{
throw std::length_error("curve_at_index: requested index greater than number of curves in piecewise_curve instance");
}
return curves_[idx];
}

template <typename Bezier>
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Bezier> convert_piecewise_curve_to_bezier() {
check_if_not_empty();
Expand Down
74 changes: 51 additions & 23 deletions python/curves_python.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,18 +154,14 @@ namespace curves
{
return new piecewise_bezier_curve_t(bc);
}
piecewise_bezier_curve_t* wrapPiecewiseBezierCurveEmptyConstructor()
piecewise_bezier_linear_curve_t* wrapPiecewiseLinearBezierCurveConstructor(const bezier_linear_variable_t& bc)
{
return new piecewise_bezier_curve_t();
return new piecewise_bezier_linear_curve_t(bc);
}
piecewise_cubic_hermite_curve_t* wrapPiecewiseCubicHermiteCurveConstructor(const cubic_hermite_spline_t& ch)
{
return new piecewise_cubic_hermite_curve_t(ch);
}
piecewise_cubic_hermite_curve_t* wrapPiecewiseCubicHermiteCurveEmptyConstructor()
{
return new piecewise_cubic_hermite_curve_t();
}
static piecewise_polynomial_curve_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);
t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
Expand Down Expand Up @@ -296,13 +292,6 @@ namespace curves
c.end_jerk = val;
}

matrix_vector* bezier_linear_variable_t_operator_call(const bezier_linear_variable_t* b, const double t)
{
bezier_linear_variable_t::point_t p = b->operator ()(t);
matrix_vector* res = new matrix_vector(p.B(), p.c());
return res;
}

bezier_t* bezier_linear_variable_t_evaluate(const bezier_linear_variable_t* b, const pointX_t& x)
{
return new bezier_t(evaluateLinear<bezier_t, bezier_linear_variable_t>(*b, x));
Expand Down Expand Up @@ -370,6 +359,7 @@ namespace curves
.def("waypointAtIndex", &bezier_t::waypointAtIndex)
.def_readonly("degree", &bezier_t::degree_)
.def_readonly("nbWaypoints", &bezier_t::size_)
.def("split", &split_bezier, return_value_policy<manage_new_object>())
.def("saveAsText", &bezier_t::saveAsText<bezier_t>,bp::args("filename"),"Saves *this inside a text file.")
.def("loadFromText",&bezier_t::loadFromText<bezier_t>,bp::args("filename"),"Loads *this from a text file.")
.def("saveAsXML",&bezier_t::saveAsXML<bezier_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
Expand All @@ -386,31 +376,44 @@ namespace curves
.def_readonly("b", &matrix_pair::b)
;

class_<matrix_vector>
("matrix_vector", no_init)
.def_readonly("A", &matrix_vector::A)
.def_readonly("b", &matrix_vector::b)
;

class_<LinearBezierVector>
("bezierVarVector", no_init)
.def_readonly("size", &LinearBezierVector::size)
.def("at", &LinearBezierVector::at, return_value_policy<manage_new_object>())
;

class_<linear_variable_t>
("linear_variable", init<>())
.def(init<linear_variable_t::vector_x_t>())
.def(init<linear_variable_t::matrix_x_t>())
.def(init<linear_variable_t::matrix_x_t, linear_variable_t::vector_x_t>())
.def(init<linear_variable_t::matrix_x_t, linear_variable_t::vector_x_t>())
.def("__call__", &linear_variable_t::operator())
.def(self += linear_variable_t())
.def(self -= linear_variable_t())
.def(self *= double())
.def(self /= double())
.def("B", &linear_variable_t::B, return_value_policy<copy_const_reference>())
.def("c", &linear_variable_t::c, return_value_policy<copy_const_reference>())
.def("size", &linear_variable_t::size)
.def("isZero", &linear_variable_t::isZero)
.def("norm", &linear_variable_t::norm)
;

class_<bezier_linear_variable_t>
("bezierVar", no_init)
("bezier_linear_variable", no_init)
.def("__init__", make_constructor(&wrapBezierLinearConstructor))
.def("__init__", make_constructor(&wrapBezierLinearConstructorBounds))
.def("min", &bezier_linear_variable_t::min)
.def("max", &bezier_linear_variable_t::max)
//.def("__call__", &bezier_linear_control_t::operator())
.def("__call__", &bezier_linear_variable_t_operator_call, bp::return_value_policy<bp::manage_new_object>())
.def("__call__", &bezier_linear_variable_t::operator())
.def("evaluate", &bezier_linear_variable_t_evaluate, bp::return_value_policy<bp::manage_new_object>())
.def("derivate", &bezier_linear_variable_t::derivate)
.def("compute_derivate", &bezier_linear_variable_t::compute_derivate)
.def("compute_primitive", &bezier_linear_variable_t::compute_primitive)
.def("split", &split_py, return_value_policy<manage_new_object>())
.def("waypoints", &wayPointsToLists, return_value_policy<manage_new_object>())
.def("waypointAtIndex", &bezier_linear_variable_t::waypointAtIndex)
.def_readonly("degree", &bezier_linear_variable_t::degree_)
.def_readonly("nbWaypoints", &bezier_linear_variable_t::size_)
;
Expand Down Expand Up @@ -485,6 +488,9 @@ namespace curves
"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_polynomial_curve_t::is_continuous,"Check if the curve is continuous at the given order.")
.def("curve_at_index",&piecewise_polynomial_curve_t::curve_at_index, return_value_policy<copy_const_reference>())
.def("curve_at_time" ,&piecewise_polynomial_curve_t::curve_at_time , return_value_policy<copy_const_reference>())
.def("num_curves" ,&piecewise_polynomial_curve_t::num_curves)
.def("saveAsText", &piecewise_polynomial_curve_t::saveAsText<piecewise_polynomial_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
.def("loadFromText",&piecewise_polynomial_curve_t::loadFromText<piecewise_polynomial_curve_t>,bp::args("filename"),"Loads *this from a text file.")
.def("saveAsXML",&piecewise_polynomial_curve_t::saveAsXML<piecewise_polynomial_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
Expand All @@ -499,6 +505,9 @@ namespace curves
.def("compute_derivate",&piecewise_polynomial_curve_t::compute_derivate,"Return a piecewise_polynomial curve which is the derivate of this.",args("self","order"))
.def("add_curve", &piecewise_bezier_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier_curve_t::is_continuous)
.def("curve_at_index",&piecewise_bezier_curve_t::curve_at_index, return_value_policy<copy_const_reference>())
.def("curve_at_time" ,&piecewise_bezier_curve_t::curve_at_time , return_value_policy<copy_const_reference>())
.def("num_curves" ,&piecewise_bezier_curve_t::num_curves)
.def("saveAsText", &piecewise_bezier_curve_t::saveAsText<piecewise_bezier_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
.def("loadFromText",&piecewise_bezier_curve_t::loadFromText<piecewise_bezier_curve_t>,bp::args("filename"),"Loads *this from a text file.")
.def("saveAsXML",&piecewise_bezier_curve_t::saveAsXML<piecewise_bezier_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
Expand All @@ -511,7 +520,10 @@ namespace curves
("piecewise_cubic_hermite_curve", init<>())
.def("__init__", make_constructor(&wrapPiecewiseCubicHermiteCurveConstructor))
.def("add_curve", &piecewise_cubic_hermite_curve_t::add_curve)
.def("is_continuous", &piecewise_cubic_hermite_curve_t::is_continuous)
.def("is_continuous", &piecewise_cubic_hermite_curve_t::is_continuous,"Check if the curve is continuous at the given order.")
.def("curve_at_index",&piecewise_cubic_hermite_curve_t::curve_at_index, return_value_policy<copy_const_reference>())
.def("curve_at_time" ,&piecewise_cubic_hermite_curve_t::curve_at_time , return_value_policy<copy_const_reference>())
.def("num_curves" ,&piecewise_cubic_hermite_curve_t::num_curves)
.def("saveAsText", &piecewise_cubic_hermite_curve_t::saveAsText<piecewise_cubic_hermite_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
.def("loadFromText",&piecewise_cubic_hermite_curve_t::loadFromText<piecewise_cubic_hermite_curve_t>,bp::args("filename"),"Loads *this from a text file.")
.def("saveAsXML",&piecewise_cubic_hermite_curve_t::saveAsXML<piecewise_cubic_hermite_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
Expand All @@ -520,6 +532,22 @@ namespace curves
.def("loadFromBinary",&piecewise_cubic_hermite_curve_t::loadFromBinary<piecewise_cubic_hermite_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
;

class_<piecewise_bezier_linear_curve_t, bases<curve_abc_t> >
("piecewise_bezier_linear_curve_t", init<>())
.def("__init__", make_constructor(&wrapPiecewiseLinearBezierCurveConstructor))
.def("add_curve", &piecewise_bezier_linear_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier_linear_curve_t::is_continuous,"Check if the curve is continuous at the given order.")
.def("curve_at_index",&piecewise_bezier_linear_curve_t::curve_at_index, return_value_policy<copy_const_reference>())
.def("curve_at_time" ,&piecewise_bezier_linear_curve_t::curve_at_time , return_value_policy<copy_const_reference>())
.def("num_curves" ,&piecewise_bezier_linear_curve_t::num_curves)
.def("saveAsText", &piecewise_bezier_linear_curve_t::saveAsText<piecewise_bezier_linear_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
.def("loadFromText",&piecewise_bezier_linear_curve_t::loadFromText<piecewise_bezier_linear_curve_t>,bp::args("filename"),"Loads *this from a text file.")
.def("saveAsXML",&piecewise_bezier_linear_curve_t::saveAsXML<piecewise_bezier_linear_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
.def("loadFromXML",&piecewise_bezier_linear_curve_t::loadFromXML<piecewise_bezier_linear_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
.def("saveAsBinary",&piecewise_bezier_linear_curve_t::saveAsBinary<piecewise_bezier_linear_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
.def("loadFromBinary",&piecewise_bezier_linear_curve_t::loadFromBinary<piecewise_bezier_linear_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
;

/** END piecewise curve function **/
/** BEGIN exact_cubic curve**/
class_<exact_cubic_t, bases<curve_abc_t> >
Expand Down
Loading

0 comments on commit 771006b

Please sign in to comment.