curves_python.cpp 43.7 KB
Newer Older
1
#include "curves/bezier_curve.h"
2
#include "curves/polynomial.h"
3
4
#include "curves/exact_cubic.h"
#include "curves/curve_constraint.h"
5
#include "curves/curve_conversion.h"
6
#include "curves/bernstein.h"
7
#include "curves/cubic_hermite_spline.h"
8
#include "curves/piecewise_curve.h"
9
#include "curves/so3_linear.h"
10
#include "curves/se3_curve.h"
stevet's avatar
stevet committed
11
12
#include "python_definitions.h"
#include "python_variables.h"
13
#include "archive_python_binding.h"
14
15
16
17
18

#include <vector>

#include <eigenpy/memory.hpp>
#include <eigenpy/eigenpy.hpp>
19
#include <eigenpy/geometry.hpp>
20
#include <Eigen/Dense>
21
22
23
24

#include <boost/python.hpp>

/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
25
using namespace curves;
26
typedef double real;
Steve Tonneau's avatar
Steve Tonneau committed
27
typedef Eigen::VectorXd time_waypoints_t;
28

29
typedef Eigen::VectorXd pointX_t;
30
typedef Eigen::Matrix<double,3, 1> point3_t;
31
32
33
typedef Eigen::Matrix<double, Eigen::Dynamic, 1, 0, Eigen::Dynamic, 1> ret_pointX_t;
typedef std::pair<pointX_t, pointX_t> pair_pointX_tangent_t;
typedef Eigen::MatrixXd pointX_list_t;
34
typedef Eigen::Matrix<double,3, Eigen::Dynamic> point3_list_t;
35
typedef std::vector<pointX_t,Eigen::aligned_allocator<pointX_t> >  t_pointX_t;
36
typedef std::vector<pointX_t,Eigen::aligned_allocator<point3_t> >  t_point3_t;
37
typedef std::vector<pair_pointX_tangent_t,Eigen::aligned_allocator<pair_pointX_tangent_t> > t_pair_pointX_tangent_t;
38
typedef curves::curve_constraints<pointX_t> curve_constraints_t;
39
40
typedef curves::curve_constraints<point3_t> curve_constraints3_t;

41
42
typedef std::pair<real, pointX_t> waypoint_t;
typedef std::vector<waypoint_t> t_waypoint_t;
43
typedef Eigen::Matrix<real,3, 3> matrix3_t;
44
45
typedef Eigen::Matrix<real,4, 4> matrix4_t;
typedef Eigen::Transform<double,3,Eigen::Affine> transform_t;
46
typedef Eigen::Quaternion<real> quaternion_t;
47

48
// Curves
49
50
typedef curve_abc<real, real, true, pointX_t> curve_abc_t; // generic class of curve of dynamic size
typedef curve_abc<real, real, true, point3_t> curve_3_t; // generic class of curve of size 3
Pierre Fernbach's avatar
Pierre Fernbach committed
51
typedef curve_abc<real, real, true, matrix3_t,point3_t> curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
52
53
typedef curves::cubic_hermite_spline <real, real, true, pointX_t> cubic_hermite_spline_t;
typedef curves::bezier_curve  <real, real, true, pointX_t> bezier_t;
54
typedef curves::bezier_curve  <real, real, true, point3_t> bezier3_t;
55
typedef curves::polynomial  <real, real, true, pointX_t, t_pointX_t> polynomial_t;
56
typedef polynomial_t::coeff_t coeff_t;
57
58
59
60
typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, polynomial_t> piecewise_polynomial_curve_t;
typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, bezier_t> piecewise_bezier_curve_t;
typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
typedef curves::exact_cubic  <real, real, true, pointX_t, t_pointX_t> exact_cubic_t;
61
62
// Bezier 3

63
typedef curves::Bern<double> bernstein_t;
64
typedef SO3Linear  <double, double, true> SO3Linear_t;
65
typedef SE3Curve  <double, double, true> SE3Curve_t;
Steve Tonneau's avatar
Steve Tonneau committed
66

67
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
Steve Tonneau's avatar
Steve Tonneau committed
68
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bernstein_t)
69
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(cubic_hermite_spline_t)
70
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
71
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier3_t)
72
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynomial_t)
73
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
74
75
76
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_polynomial_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_bezier_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_cubic_hermite_curve_t)
77
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic_t)
78
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(SO3Linear_t)
79
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(SE3Curve_t)
80

81
82
83
84
85
86
87
88
89
90
91
92
93
94
namespace curves
{
  using namespace boost::python;

  /* base wrap of curve_abc : must implement all pure virtual method of curve_abc */
  struct CurveWrapper : curve_abc_t, wrapper<curve_abc_t>
  {
      point_t operator()(const real) { return this->get_override("operator()")();}
      point_t derivate(const real, const std::size_t) { return this->get_override("derivate")();}
      std::size_t dim() { return this->get_override("dim")();}
      real min() { return this->get_override("min")();}
      real max() { return this->get_override("max")();}

  };
95
96
97
98
99
100
101
102
103
104
105

  struct Curve3Wrapper : curve_3_t, wrapper<curve_3_t>
  {
      point_t operator()(const real) { return this->get_override("operator()")();}
      point_t derivate(const real, const std::size_t) { return this->get_override("derivate")();}
      std::size_t dim() { return this->get_override("dim")();}
      real min() { return this->get_override("min")();}
      real max() { return this->get_override("max")();}

  };

106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
  /* end base wrap of curve_abc */

  /* Template constructor bezier */
  template <typename Bezier, typename PointList, typename T_Point>
  Bezier* wrapBezierConstructorTemplate(const PointList& array, const real T_min =0., const real T_max =1.)
  {
    T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
    return new Bezier(asVector.begin(), asVector.end(), T_min, T_max);
  }

  template <typename Bezier, typename PointList, typename T_Point, typename CurveConstraints>
  Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints,
                                                   const real T_min =0., const real T_max =1.)
  {
    T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
    return new Bezier(asVector.begin(), asVector.end(), constraints, T_min, T_max);
  }
  /* End Template constructor bezier */
124
125
126
127
128
129
130
131
132
133
  /* Helper converter constraintsX -> constraints 3 */
  curve_constraints3_t convertToConstraints3(curve_constraints_t constraintsX){
    curve_constraints3_t constraints3;
    constraints3.init_vel =point3_t(constraintsX.init_vel);
    constraints3.init_acc = point3_t(constraintsX.init_acc);
    constraints3.end_vel = point3_t(constraintsX.end_vel);
    constraints3.end_acc = point3_t(constraintsX.end_acc);
    return constraints3;
  }
  /* END helper converter constraintsX -> constraints 3 */
134
135

  /*3D constructors bezier */
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
  bezier3_t* wrapBezier3Constructor(const pointX_list_t& array)
  {
    return wrapBezierConstructorTemplate<bezier3_t, pointX_list_t, t_point3_t>(array) ;
  }
  bezier3_t* wrapBezier3ConstructorBounds(const pointX_list_t& array, const real T_min, const real T_max)
  {
    return wrapBezierConstructorTemplate<bezier3_t, pointX_list_t, t_point3_t>(array, T_min, T_max) ;
  }
  bezier3_t* wrapBezier3ConstructorConstraints(const pointX_list_t& array, const curve_constraints_t& constraints)
  {
    return wrapBezierConstructorConstraintsTemplate<bezier3_t, pointX_list_t, t_point3_t, curve_constraints3_t>(array, convertToConstraints3(constraints)) ;
  }
  bezier3_t* wrapBezier3ConstructorBoundsConstraints(const pointX_list_t& array, const curve_constraints_t& constraints,
                                                   const real T_min, const real T_max)
  {
    return wrapBezierConstructorConstraintsTemplate<bezier3_t, pointX_list_t, t_point3_t, curve_constraints3_t>(array, convertToConstraints3(constraints),T_min, T_max) ;
  }
  /*END 3D constructors bezier */

  /*constructors bezier */
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
  bezier_t* wrapBezierConstructor(const pointX_list_t& array)
  {
    return wrapBezierConstructorTemplate<bezier_t, pointX_list_t, t_pointX_t>(array) ;
  }
  bezier_t* wrapBezierConstructorBounds(const pointX_list_t& array, const real T_min, const real T_max)
  {
    return wrapBezierConstructorTemplate<bezier_t, pointX_list_t, t_pointX_t>(array, T_min, T_max) ;
  }
  bezier_t* wrapBezierConstructorConstraints(const pointX_list_t& array, const curve_constraints_t& constraints)
  {
    return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(array, constraints) ;
  }
  bezier_t* wrapBezierConstructorBoundsConstraints(const pointX_list_t& array, const curve_constraints_t& constraints,
                                                   const real T_min, const real T_max)
  {
171
    return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(array, constraints,
172
173
                                                                                                            T_min, T_max) ;
  }
174
  /*END constructors bezier */
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190

  /* Wrap Cubic hermite spline */
  t_pair_pointX_tangent_t getPairsPointTangent(const pointX_list_t& points, const pointX_list_t& tangents)
  {
    t_pair_pointX_tangent_t res;
    if (points.size() != tangents.size())
    {
      throw std::length_error("size of points and tangents must be the same");
    }
    for(int i =0;i<points.cols();++i)
    {
      res.push_back(pair_pointX_tangent_t(points.col(i), tangents.col(i)));
    }
    return res;
  }

191
  cubic_hermite_spline_t* wrapCubicHermiteSplineConstructor(const pointX_list_t& points, const pointX_list_t& tangents,
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
                                                            const time_waypoints_t& time_pts)
  {
    t_pair_pointX_tangent_t ppt = getPairsPointTangent(points, tangents);
    std::vector<real> time_control_pts;
    for( int i =0; i<time_pts.size(); ++i)
    {
      time_control_pts.push_back(time_pts[i]);
    }
    return new cubic_hermite_spline_t(ppt.begin(), ppt.end(), time_control_pts);
  }
  /* End wrap Cubic hermite spline */

  /* Wrap polynomial */
  polynomial_t* wrapPolynomialConstructor1(const coeff_t& array, const real min, const real max)
  {
    return new polynomial_t(array, min, max);
  }
  polynomial_t* wrapPolynomialConstructor2(const coeff_t& array)
  {
    return new polynomial_t(array, 0., 1.);
  }
  polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree1(const pointX_t& init,const pointX_t& end,const real min, const real max)
  {
    return new polynomial_t(init,end,min,max);
  }
  polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree3(const pointX_t& init,const pointX_t& d_init,const pointX_t& end,const pointX_t& d_end,const real min, const real max)
  {
    return new polynomial_t(init,d_init,end,d_end,min,max);
  }
  polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree5(const pointX_t& init,const pointX_t& d_init,const pointX_t& dd_init,const pointX_t& end,const point_t& d_end,const point_t& dd_end,const real min, const real max)
  {
    return new polynomial_t(init,d_init,dd_init,end,d_end,dd_end,min,max);
  }
  /* End wrap polynomial */

  /* Wrap piecewise curve */
  piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveConstructor(const polynomial_t& pol)
  {
    return new piecewise_polynomial_curve_t(pol);
  }
  piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveEmptyConstructor()
  {
    return new piecewise_polynomial_curve_t();
  }
  piecewise_bezier_curve_t* wrapPiecewiseBezierCurveConstructor(const bezier_t& bc)
  {
    return new piecewise_bezier_curve_t(bc);
  }
  piecewise_bezier_curve_t* wrapPiecewiseBezierCurveEmptyConstructor()
  {
    return new piecewise_bezier_curve_t();
  }
  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);
    return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,time_points_list);
  }
  static piecewise_polynomial_curve_t discretPointToPolynomialC1(const pointX_list_t& points,const pointX_list_t& points_derivative, const time_waypoints_t& time_points){
    t_pointX_t points_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points);
    t_pointX_t points_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_derivative);
    t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
    return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,points_derivative_list,time_points_list);
  }
  static piecewise_polynomial_curve_t discretPointToPolynomialC2(const pointX_list_t& points,const pointX_list_t& points_derivative,const pointX_list_t& points_second_derivative, const time_waypoints_t& time_points){
    t_pointX_t points_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points);
    t_pointX_t points_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_derivative);
    t_pointX_t points_second_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_second_derivative);

    t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
    return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,points_derivative_list,points_second_derivative_list,time_points_list);
  }
  void addFinalPointC0(piecewise_polynomial_curve_t self,const pointX_t& end,const real time){
    if(self.is_continuous(1))
      std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C1 continuity and only guarantee C0 continuity."<<std::endl;
    polynomial_t pol(self(self.max()),end,self.max(),time);
    self.add_curve(pol);
  }
  void addFinalPointC1(piecewise_polynomial_curve_t self,const pointX_t& end,const pointX_t& d_end,const real time){
    if(self.is_continuous(2))
      std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C2 continuity and only guarantee C1 continuity."<<std::endl;
    if(!self.is_continuous(1))
      std::cout<<"Warning: the current piecewise curve is not C1 continuous."<<std::endl;
    polynomial_t pol(self(self.max()),self.derivate(self.max(),1),end,d_end,self.max(),time);
    self.add_curve(pol);
  }
  void addFinalPointC2(piecewise_polynomial_curve_t self,const pointX_t& end,const pointX_t& d_end,const pointX_t& dd_end,const real time){
    if(self.is_continuous(3))
      std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C3 continuity and only guarantee C2 continuity."<<std::endl;
    if(!self.is_continuous(2))
      std::cout<<"Warning: the current piecewise curve is not C2 continuous."<<std::endl;
    polynomial_t pol(self(self.max()),self.derivate(self.max(),1),self.derivate(self.max(),2),end,d_end,dd_end,self.max(),time);
    self.add_curve(pol);
  }


  /* end wrap piecewise polynomial curve */

  /* Wrap exact cubic spline */
  t_waypoint_t getWayPoints(const coeff_t& array, const time_waypoints_t& time_wp)
  {
    t_waypoint_t res;
    for(int i =0;i<array.cols();++i)
    {
      res.push_back(std::make_pair(time_wp(i), array.col(i)));
    }
    return res;
  }

  exact_cubic_t* wrapExactCubicConstructor(const coeff_t& array, const time_waypoints_t& time_wp)
  {
    t_waypoint_t wps = getWayPoints(array, time_wp);
    return new exact_cubic_t(wps.begin(), wps.end());
  }

314
  exact_cubic_t* wrapExactCubicConstructorConstraint(const coeff_t& array, const time_waypoints_t& time_wp,
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
                                                     const curve_constraints_t& constraints)
  {
    t_waypoint_t wps = getWayPoints(array, time_wp);
    return new exact_cubic_t(wps.begin(), wps.end(), constraints);
  }

  /// For constraints XD
  point_t get_init_vel(const curve_constraints_t& c)
  {
    return c.init_vel;
  }

  point_t get_init_acc(const curve_constraints_t& c)
  {
    return c.init_acc;
  }

  point_t get_end_vel(const curve_constraints_t& c)
  {
    return c.end_vel;
  }

  point_t get_end_acc(const curve_constraints_t& c)
  {
    return c.end_acc;
  }

  void set_init_vel(curve_constraints_t& c, const point_t& val)
  {
    c.init_vel = val;
  }

  void set_init_acc(curve_constraints_t& c, const point_t& val)
  {
    c.init_acc = val;
  }

  void set_end_vel(curve_constraints_t& c, const point_t& val)
  {
    c.end_vel = val;
  }

  void set_end_acc(curve_constraints_t& c, const point_t& val)
  {
    c.end_acc = val;
  }
  /* End wrap exact cubic spline */

363
364
365
366
367
368
369
370
371
372
373
  /* Wrap SO3Linear */
  SO3Linear_t* wrapSO3LinearConstructorFromQuaternion(const quaternion_t& init_rot, const quaternion_t& end_rot, const real min, const real max)
  {
    return new SO3Linear_t(init_rot,end_rot, min, max);
  }

  SO3Linear_t* wrapSO3LinearConstructorFromMatrix(const matrix3_t& init_rot, const matrix3_t& end_rot, const real min, const real max)
  {
    return new SO3Linear_t(init_rot,end_rot, min, max);
  }

374
  /* End wrap SO3Linear */
375

376
377
378
379
380
381
  /* Wrap SE3Curves */
  SE3Curve_t* wrapSE3CurveFromTransform(const matrix4_t& init_pose, const matrix4_t& end_pose, const real min, const real max)
  {
    return new SE3Curve_t(transform_t(init_pose),transform_t(end_pose), min, max);
  }

Pierre Fernbach's avatar
Pierre Fernbach committed
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409

  SE3Curve_t* wrapSE3CurveFromBezier3Translation(bezier3_t& translation_curve,const matrix3_t& init_rot, const matrix3_t& end_rot )
  {
    bezier_t* translation = new bezier_t(translation_curve.waypoints().begin(),translation_curve.waypoints().end(),translation_curve.min(),translation_curve.max());
    return new SE3Curve_t(translation,init_rot,end_rot);
  }

  SE3Curve_t* wrapSE3CurveFromBezierTranslation(bezier_t& translation_curve,const matrix3_t& init_rot, const matrix3_t& end_rot )
  {
    return new SE3Curve_t(&translation_curve,init_rot,end_rot);
  }

  SE3Curve_t* wrapSE3CurveFromPolynomialTranslation(polynomial_t& translation_curve,const matrix3_t& init_rot, const matrix3_t& end_rot )
  {
    return new SE3Curve_t(&translation_curve,init_rot,end_rot);
  }

  SE3Curve_t* wrapSE3CurveFromPiecewisePolynomialTranslation(piecewise_polynomial_curve_t& translation_curve,const matrix3_t& init_rot, const matrix3_t& end_rot )
  {
    return new SE3Curve_t(&translation_curve,init_rot,end_rot);
  }

  SE3Curve_t* wrapSE3CurveFromTwoCurves(curve_abc_t& translation_curve, curve_rotation_t& rotation_curve)
  {
    return new SE3Curve_t(&translation_curve,&rotation_curve);
  }


410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
  matrix4_t se3Return(const SE3Curve_t& curve, const real t)
  {
    return curve(t).matrix();
  }

  pointX_t se3ReturnDerivate(const SE3Curve_t& curve, const real t, const std::size_t order)
  {
    return curve.derivate(t,order);
  }

  matrix3_t se3returnRotation(const SE3Curve_t& curve, const real t)
  {
    return curve(t).rotation();
  }

  pointX_t se3returnTranslation(const SE3Curve_t& curve, const real t)
  {
    return pointX_t(curve(t).translation());
  }

  /* End wrap SE3Curves */
431

432
  // TO DO : Replace all load and save function for serialization in class by using
433
434
435
436
437
438
439
440
  //         SerializableVisitor in archive_python_binding.
  BOOST_PYTHON_MODULE(curves)
  {
    /** BEGIN eigenpy init**/
    eigenpy::enableEigenPy();
    eigenpy::enableEigenPySpecific<pointX_t,pointX_t>();
    eigenpy::enableEigenPySpecific<pointX_list_t,pointX_list_t>();
    eigenpy::enableEigenPySpecific<coeff_t,coeff_t>();
441
    eigenpy::enableEigenPySpecific<matrix3_t,matrix3_t>();
442
    eigenpy::enableEigenPySpecific<matrix4_t,matrix4_t>();
443
444
    //eigenpy::enableEigenPySpecific<quaternion_t,quaternion_t>();
    eigenpy::exposeQuaternion();
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
    /*eigenpy::exposeAngleAxis();
    eigenpy::exposeQuaternion();*/
    /** END eigenpy init**/
    class_<CurveWrapper,boost::noncopyable>("curve",no_init)
        .def("__call__", pure_virtual(&curve_abc_t::operator()),"Evaluate the curve at the given time.",args("self","t"))
        .def("derivate", pure_virtual(&curve_abc_t::derivate),"Evaluate the derivative of order N of curve at time t.",args("self","t","N"))
        .def("min", pure_virtual(&curve_abc_t::min), "Get the LOWER bound on interval definition of the curve.")
        .def("max", pure_virtual(&curve_abc_t::max),"Get the HIGHER bound on interval definition of the curve.")
        .def("dim", pure_virtual(&curve_abc_t::dim),"Get the dimension of the curve.")
        .def("saveAsText", pure_virtual(&curve_abc_t::saveAsText<curve_abc_t>),bp::args("filename"),"Saves *this inside a text file.")
        .def("loadFromText",pure_virtual(&curve_abc_t::loadFromText<curve_abc_t>),bp::args("filename"),"Loads *this from a text file.")
        .def("saveAsXML",pure_virtual(&curve_abc_t::saveAsXML<curve_abc_t>),bp::args("filename","tag_name"),"Saves *this inside a XML file.")
        .def("loadFromXML",pure_virtual(&curve_abc_t::loadFromXML<curve_abc_t>),bp::args("filename","tag_name"),"Loads *this from a XML file.")
        .def("saveAsBinary",pure_virtual(&curve_abc_t::saveAsBinary<curve_abc_t>),bp::args("filename"),"Saves *this inside a binary file.")
        .def("loadFromBinary",pure_virtual(&curve_abc_t::loadFromBinary<curve_abc_t>),bp::args("filename"),"Loads *this from a binary file.")
        ;

462
463
464
465
466
467
468
469
470
    class_<Curve3Wrapper,boost::noncopyable, bases<curve_abc_t> >("curve3",no_init)
        .def("__call__", pure_virtual(&curve_3_t::operator()),"Evaluate the curve at the given time.",args("self","t"))
        .def("derivate", pure_virtual(&curve_3_t::derivate),"Evaluate the derivative of order N of curve at time t.",args("self","t","N"))
        .def("min", pure_virtual(&curve_3_t::min), "Get the LOWER bound on interval definition of the curve.")
        .def("max", pure_virtual(&curve_3_t::max),"Get the HIGHER bound on interval definition of the curve.")
        .def("dim", pure_virtual(&curve_3_t::dim),"Get the dimension of the curve.")
        ;


471
    /** BEGIN bezier3 curve**/
472
    class_<bezier3_t, bases<curve_3_t> >("bezier3", init<>())
473
474
475
476
      .def("__init__", make_constructor(&wrapBezier3Constructor))
      .def("__init__", make_constructor(&wrapBezier3ConstructorBounds))
      .def("__init__", make_constructor(&wrapBezier3ConstructorConstraints))
      .def("__init__", make_constructor(&wrapBezier3ConstructorBoundsConstraints))
477
478
      .def("compute_derivate", &bezier3_t::compute_derivate)
      .def("compute_primitive", &bezier3_t::compute_primitive)
479
      .def("waypointAtIndex", &bezier3_t::waypointAtIndex)
480
481
      .def_readonly("degree", &bezier3_t::degree_)
      .def_readonly("nbWaypoints", &bezier3_t::size_)
482
483
484
485
486
487
      .def("saveAsText", &bezier3_t::saveAsText<bezier3_t>,bp::args("filename"),"Saves *this inside a text file.")
      .def("loadFromText",&bezier3_t::loadFromText<bezier3_t>,bp::args("filename"),"Loads *this from a text file.")
      .def("saveAsXML",&bezier3_t::saveAsXML<bezier3_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
      .def("loadFromXML",&bezier3_t::loadFromXML<bezier3_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
      .def("saveAsBinary",&bezier3_t::saveAsBinary<bezier3_t>,bp::args("filename"),"Saves *this inside a binary file.")
      .def("loadFromBinary",&bezier3_t::loadFromBinary<bezier3_t>,bp::args("filename"),"Loads *this from a binary file.")
488
      //.def(SerializableVisitor<bezier_t>())
489
490
491
492
    ;
    /** END bezier3 curve**/
    /** BEGIN bezier curve**/
    class_<bezier_t, bases<curve_abc_t> >("bezier", init<>())
493
494
495
496
497
498
      .def("__init__", make_constructor(&wrapBezierConstructor))
      .def("__init__", make_constructor(&wrapBezierConstructorBounds))
      .def("__init__", make_constructor(&wrapBezierConstructorConstraints))
      .def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints))
      .def("compute_derivate", &bezier_t::compute_derivate)
      .def("compute_primitive", &bezier_t::compute_primitive)
499
      .def("waypointAtIndex", &bezier_t::waypointAtIndex)
500
501
      .def_readonly("degree", &bezier_t::degree_)
      .def_readonly("nbWaypoints", &bezier_t::size_)
502
503
504
505
506
507
      .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.")
      .def("loadFromXML",&bezier_t::loadFromXML<bezier_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
      .def("saveAsBinary",&bezier_t::saveAsBinary<bezier_t>,bp::args("filename"),"Saves *this inside a binary file.")
      .def("loadFromBinary",&bezier_t::loadFromBinary<bezier_t>,bp::args("filename"),"Loads *this from a binary file.")
508
      //.def(SerializableVisitor<bezier_t>())
509
510
511
512
513
    ;
    /** END bezier curve**/
    /** BEGIN variable points bezier curve**/
    class_<LinearControlPointsHolder>
    ("LinearWaypoint", no_init)
514
      .def_readonly("A", &LinearControlPointsHolder::A)
515
516
517
518
      .def_readonly("b", &LinearControlPointsHolder::b)
    ;
    class_<LinearBezierVector>
    ("bezierVarVector", no_init)
519
      .def_readonly("size", &LinearBezierVector::size)
520
521
522
      .def("at", &LinearBezierVector::at, return_value_policy<manage_new_object>())
    ;
    class_<bezier_linear_variable_t>("bezierVar", no_init)
523
524
525
526
      .def("__init__", make_constructor(&wrapBezierLinearConstructor))
      .def("__init__", make_constructor(&wrapBezierLinearConstructorBounds))
      .def("min", &bezier_linear_variable_t::min)
      .def("max", &bezier_linear_variable_t::max)
527
      .def("dim", &bezier_linear_variable_t::dim)
528
529
530
531
532
533
534
      //.def("__call__", &bezier_linear_control_t::operator())
      .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, return_value_policy<manage_new_object>())
      .def("waypoints", &wayPointsToLists, return_value_policy<manage_new_object>())
      .def_readonly("degree", &bezier_linear_variable_t::degree_)
535
536
537
538
539
540
      .def_readonly("nbWaypoints", &bezier_linear_variable_t::size_)
    ;
    /** END variable points bezier curve**/
    /** BEGIN polynomial curve function**/
    class_<polynomial_t , bases<curve_abc_t> >("polynomial",  init<>())
      .def("__init__", make_constructor(&wrapPolynomialConstructor1,default_call_policies(),args("coeffs","min","max")),
541
           "Create polynomial spline from an Eigen matrix of coefficient defined for t \in [min,max]."
542
           " The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest order."
543
           " Spline order is given by the number of the columns -1.")
544
      .def("__init__", make_constructor(&wrapPolynomialConstructor2,default_call_policies(),arg("coeffs")),
545
           "Create polynomial spline from an Eigen matrix of coefficient defined for t \in [0,1]."
546
           " The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest order."
547
           " Spline order is given by the number of the columns -1.")
548
549
      .def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree1,
                                        default_call_policies(),args("init","end","min","max")),
550
551
           "Create a polynomial of degree 1 defined for t \in [min,max], "
           "such that c(min) == init and c(max) == end.")
552
553
554
555
556
557
558
559
      .def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree3,
                                        default_call_policies(),args("init","d_init","end","d_end","min","max")),
          "Create a polynomial of degree 3 defined for t \in [min,max], "
          "such that c(min) == init and c(max) == end"
          " dc(min) == d_init and dc(max) == d_end")
      .def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree5,
                                        default_call_policies(),
                                        args("init","d_init","dd_init","end","d_end","dd_end","min","max")),
560
561
562
563
           "Create a polynomial of degree 5 defined for t \in [min,max], "
           "such that c(min) == init and c(max) == end"
           " dc(min) == d_init and dc(max) == d_end"
           " ddc(min) == dd_init and ddc(max) == dd_end")
564
565
      .def("coeffAtDegree", &polynomial_t::coeffAtDegree)
      .def("coeff", &polynomial_t::coeff)
566
567
568
569
570
571
572
      .def("compute_derivate", &polynomial_t::compute_derivate,"Compute derivative of order N of curve at time t.")
      .def("saveAsText", &polynomial_t::saveAsText<polynomial_t>,bp::args("filename"),"Saves *this inside a text file.")
      .def("loadFromText",&polynomial_t::loadFromText<polynomial_t>,bp::args("filename"),"Loads *this from a text file.")
      .def("saveAsXML",&polynomial_t::saveAsXML<polynomial_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
      .def("loadFromXML",&polynomial_t::loadFromXML<polynomial_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
      .def("saveAsBinary",&polynomial_t::saveAsBinary<polynomial_t>,bp::args("filename"),"Saves *this inside a binary file.")
      .def("loadFromBinary",&polynomial_t::loadFromBinary<polynomial_t>,bp::args("filename"),"Loads *this from a binary file.")
Guilhem Saurel's avatar
Guilhem Saurel committed
573
      ;
574

575
576
577
578
579
    /** END polynomial function**/
    /** BEGIN piecewise curve function **/
    class_<piecewise_polynomial_curve_t , bases<curve_abc_t> >
    ("piecewise_polynomial_curve", init<>())
      .def("__init__", make_constructor(&wrapPiecewisePolynomialCurveConstructor,default_call_policies(),arg("curve")),
580
           "Create a peicewise-polynomial curve containing the given polynomial curve.")
581
582
583
584
585
586
      .def("FromPointsList",&discretPointToPolynomialC0,
           "Create a piecewise-polynomial connecting exactly all the given points at the given time. The created piecewise is C0 continuous.",args("points","time_points"))
       .def("FromPointsList",&discretPointToPolynomialC1,
             "Create a piecewise-polynomial connecting exactly all the given points at the given time and respect the given points derivative values. The created piecewise is C1 continuous.",args("points","points_derivative","time_points"))
       .def("FromPointsList",&discretPointToPolynomialC2,
             "Create a piecewise-polynomial connecting exactly all the given points at the given time and respect the given points derivative and second derivative values. The created piecewise is C2 continuous.",args("points","points_derivative","points_second_derivative","time_points"))
587
      .staticmethod("FromPointsList")
588
589
590
591
592
593
594
      .def("append",&addFinalPointC0,
           "Append a new polynomial curve of degree 1 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end",args("self","end","time"))
       .def("append",&addFinalPointC1,
             "Append a new polynomial curve of degree 3 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end. It guarantee C1 continuity and guarantee that self.derivate(time,1) == d_end",args("self","end","d_end","time"))
       .def("append",&addFinalPointC2,
              "Append a new polynomial curve of degree 5 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end. It guarantee C2 continuity and guarantee that self.derivate(time,1) == d_end and self.derivate(time,2) == dd_end",args("self","end","d_end","d_end","time"))
      .def("compute_derivate",&piecewise_polynomial_curve_t::compute_derivate,"Return a piecewise_polynomial curve which is the derivate of this.",args("self","order"))
595
      .def("append", &piecewise_polynomial_curve_t::add_curve,
596
597
           "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.")
598
599
600
601
602
603
604
      .def("is_continuous", &piecewise_polynomial_curve_t::is_continuous,"Check if the curve is continuous at the given order.")
      .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.")
      .def("loadFromXML",&piecewise_polynomial_curve_t::loadFromXML<piecewise_polynomial_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
      .def("saveAsBinary",&piecewise_polynomial_curve_t::saveAsBinary<piecewise_polynomial_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
      .def("loadFromBinary",&piecewise_polynomial_curve_t::loadFromBinary<piecewise_polynomial_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
Guilhem Saurel's avatar
Guilhem Saurel committed
605
      ;
606
607
608

    class_<piecewise_bezier_curve_t , bases<curve_abc_t> >
    ("piecewise_bezier_curve", init<>())
609
      .def("__init__", make_constructor(&wrapPiecewiseBezierCurveConstructor))
610
      .def("compute_derivate",&piecewise_polynomial_curve_t::compute_derivate,"Return a piecewise_polynomial curve which is the derivate of this.",args("self","order"))
611
612
      .def("add_curve", &piecewise_bezier_curve_t::add_curve)
      .def("is_continuous", &piecewise_bezier_curve_t::is_continuous)
613
614
615
616
617
618
      .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.")
      .def("loadFromXML",&piecewise_bezier_curve_t::loadFromXML<piecewise_bezier_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
      .def("saveAsBinary",&piecewise_bezier_curve_t::saveAsBinary<piecewise_bezier_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
      .def("loadFromBinary",&piecewise_bezier_curve_t::loadFromBinary<piecewise_bezier_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
Guilhem Saurel's avatar
Guilhem Saurel committed
619
      ;
620
621
622

    class_<piecewise_cubic_hermite_curve_t, bases<curve_abc_t> >
    ("piecewise_cubic_hermite_curve", init<>())
623
624
625
      .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)
626
627
628
629
630
631
      .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.")
      .def("loadFromXML",&piecewise_cubic_hermite_curve_t::loadFromXML<piecewise_cubic_hermite_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
      .def("saveAsBinary",&piecewise_cubic_hermite_curve_t::saveAsBinary<piecewise_cubic_hermite_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
      .def("loadFromBinary",&piecewise_cubic_hermite_curve_t::loadFromBinary<piecewise_cubic_hermite_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
Guilhem Saurel's avatar
Guilhem Saurel committed
632
      ;
633
634
635
636
637

    /** END piecewise curve function **/
    /** BEGIN exact_cubic curve**/
    class_<exact_cubic_t, bases<curve_abc_t> >
    ("exact_cubic", init<>())
638
639
640
641
      .def("__init__", make_constructor(&wrapExactCubicConstructor))
      .def("__init__", make_constructor(&wrapExactCubicConstructorConstraint))
      .def("getNumberSplines", &exact_cubic_t::getNumberSplines)
      .def("getSplineAt", &exact_cubic_t::getSplineAt)
642
643
644
645
646
647
      .def("saveAsText", &exact_cubic_t::saveAsText<exact_cubic_t>,bp::args("filename"),"Saves *this inside a text file.")
      .def("loadFromText",&exact_cubic_t::loadFromText<exact_cubic_t>,bp::args("filename"),"Loads *this from a text file.")
      .def("saveAsXML",&exact_cubic_t::saveAsXML<exact_cubic_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
      .def("loadFromXML",&exact_cubic_t::loadFromXML<exact_cubic_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
      .def("saveAsBinary",&exact_cubic_t::saveAsBinary<exact_cubic_t>,bp::args("filename"),"Saves *this inside a binary file.")
      .def("loadFromBinary",&exact_cubic_t::loadFromBinary<exact_cubic_t>,bp::args("filename"),"Loads *this from a binary file.")
Guilhem Saurel's avatar
Guilhem Saurel committed
648
      ;
649
650
651
652
653

    /** END exact_cubic curve**/
    /** BEGIN cubic_hermite_spline **/
    class_<cubic_hermite_spline_t, bases<curve_abc_t> >
    ("cubic_hermite_spline", init<>())
654
      .def("__init__", make_constructor(&wrapCubicHermiteSplineConstructor))
655
656
657
658
659
660
      .def("saveAsText", &cubic_hermite_spline_t::saveAsText<cubic_hermite_spline_t>,bp::args("filename"),"Saves *this inside a text file.")
      .def("loadFromText",&cubic_hermite_spline_t::loadFromText<cubic_hermite_spline_t>,bp::args("filename"),"Loads *this from a text file.")
      .def("saveAsXML",&cubic_hermite_spline_t::saveAsXML<cubic_hermite_spline_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
      .def("loadFromXML",&cubic_hermite_spline_t::loadFromXML<cubic_hermite_spline_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
      .def("saveAsBinary",&cubic_hermite_spline_t::saveAsBinary<cubic_hermite_spline_t>,bp::args("filename"),"Saves *this inside a binary file.")
      .def("loadFromBinary",&cubic_hermite_spline_t::loadFromBinary<cubic_hermite_spline_t>,bp::args("filename"),"Loads *this from a binary file.")
Guilhem Saurel's avatar
Guilhem Saurel committed
661
      ;
662
663
664
665
666

    /** END cubic_hermite_spline **/
    /** BEGIN curve constraints**/
    class_<curve_constraints_t>
    ("curve_constraints", init<>())
667
668
669
      .add_property("init_vel", &get_init_vel, &set_init_vel)
      .add_property("init_acc", &get_init_acc, &set_init_acc)
      .add_property("end_vel", &get_end_vel, &set_end_vel)
670
671
672
673
674
675
676
677
678
      .add_property("end_acc", &get_end_acc, &set_end_acc)
    ;
    /** END curve constraints**/
    /** BEGIN bernstein polynomial**/
    class_<bernstein_t>
    ("bernstein", init<const unsigned int, const unsigned int>())
      .def("__call__", &bernstein_t::operator())
    ;
    /** END bernstein polynomial**/
Pierre Fernbach's avatar
Pierre Fernbach committed
679

680
681
682
683
684
685
686
687
688
    /** BEGIN SO3 Linear**/
    class_<SO3Linear_t>("SO3Linear",  init<>())
      .def("__init__", make_constructor(&wrapSO3LinearConstructorFromMatrix,default_call_policies(),args("init_rotation","end_rotation","min","max")),"Create a SO3 Linear curve between two rotations, defined for t \in [min,max]."
     " The input rotations are expressed as 3x3 matrix.")
      .def("__init__", make_constructor(&wrapSO3LinearConstructorFromQuaternion,default_call_policies(),args("init_rotation","end_rotation","min","max")),"Create a SO3 Linear curve between two rotations, defined for t \in [min,max]."
         " The input rotations are expressed as Quaternions.")
      .def("__call__", &SO3Linear_t::operator(),"Output the rotation (as a 3x3 matrix) at the given time. This rotation is obtained by a Spherical Linear Interpolation between the initial and final rotation.")
      .def("computeAsQuaternion",&SO3Linear_t::computeAsQuaternion,"Output the quaternion of the rotation at the given time. This rotation is obtained by a Spherical Linear Interpolation between the initial and final rotation.")
      .def("derivate",&SO3Linear_t::derivate,"Output the derivate of the curve at the given time and order",args("self","time","order"))
689
690
        .def("min", &SO3Linear_t::min, "Get the LOWER bound on interval definition of the curve.")
        .def("max", &SO3Linear_t::max,"Get the HIGHER bound on interval definition of the curve.")
691
692
693
        ;

    /** END  SO3 Linear**/
694
695
    /** BEGIN SE3 Curve**/
    class_<SE3Curve_t>("SE3Curve",  init<>())
Pierre Fernbach's avatar
Pierre Fernbach committed
696
697
698
699
      .def("__init__",
       make_constructor(&wrapSE3CurveFromTransform,default_call_policies(),
       args("init_transform","end_transform","min","max")),
     "Create a SE3 curve between two transform, defined for t \in [min,max]."
700
701
     " Using linear interpolation for translation and slerp for rotation between init and end."
     " The input transform are expressed as 4x4 matrix.")
Pierre Fernbach's avatar
Pierre Fernbach committed
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
      .def("__init__",
       make_constructor(&wrapSE3CurveFromTwoCurves,
       default_call_policies(),
       args("translation_curve","rotation_curve")),
       "Create a SE3 curve from a translation curve and a rotation one."
        "The translation curve should be of dimension 3 and the rotation one should output 3x3 matrix"
        "Both curves should have the same time bounds.")
        .def("__init__",
         make_constructor(&wrapSE3CurveFromBezier3Translation,
         default_call_policies(),
         args("translation_curve","init_rotation","end_rotation")),
         "Create a SE3 curve from a translation curve two rotation"
          "The translation curve should be of dimension 3, the time definition of the SE3curve will the same as the translation curve."
          "The orientation along the SE3Curve will be a slerp between the two given rotations."
          "The orientations should be represented as 3x3 rotation matrix")
        .def("__init__",
         make_constructor(&wrapSE3CurveFromBezierTranslation,
         default_call_policies(),
         args("translation_curve","init_rotation","end_rotation")),
         "Create a SE3 curve from a translation curve two rotation"
          "The translation curve should be of dimension 3, the time definition of the SE3curve will the same as the translation curve."
          "The orientation along the SE3Curve will be a slerp between the two given rotations."
          "The orientations should be represented as 3x3 rotation matrix")
        .def("__init__",
         make_constructor(&wrapSE3CurveFromPolynomialTranslation,
         default_call_policies(),
         args("translation_curve","init_rotation","end_rotation")),
         "Create a SE3 curve from a translation curve two rotation"
          "The translation curve should be of dimension 3, the time definition of the SE3curve will the same as the translation curve."
          "The orientation along the SE3Curve will be a slerp between the two given rotations."
          "The orientations should be represented as 3x3 rotation matrix")
        .def("__init__",
         make_constructor(&wrapSE3CurveFromPiecewisePolynomialTranslation,
         default_call_policies(),
         args("translation_curve","init_rotation","end_rotation")),
         "Create a SE3 curve from a translation curve two rotation"
          "The translation curve should be of dimension 3, the time definition of the SE3curve will the same as the translation curve."
          "The orientation along the SE3Curve will be a slerp between the two given rotations."
          "The orientations should be represented as 3x3 rotation matrix")
741
742
743
744
745
746
747
748
        .def("__call__", &se3Return,"Output the transform (as a 4x4 matrix) at the given time.")
        .def("rotation", &se3returnRotation,"Output the rotation (as a 3x3 matrix) at the given time.",args("self","time"))
        .def("translation", &se3returnTranslation,"Output the rotation (as a vector 3) at the given time.",args("self","time"))
        .def("derivate",&se3ReturnDerivate,"Output the derivate of the curve at the given time and order",args("self","time","order"))
        .def("min", &SE3Curve_t::min, "Get the LOWER bound on interval definition of the curve.")
        .def("max", &SE3Curve_t::max,"Get the HIGHER bound on interval definition of the curve.")
        ;
    /** END SE3 Curve**/
749
750
751
752
753
754
755
756
757
758
    /** BEGIN curves conversion**/
    def("polynomial_from_bezier", polynomial_from_curve<polynomial_t,bezier_t>);
    def("polynomial_from_hermite", polynomial_from_curve<polynomial_t,cubic_hermite_spline_t>);
    def("bezier_from_hermite", bezier_from_curve<bezier_t,cubic_hermite_spline_t>);
    def("bezier_from_polynomial", bezier_from_curve<bezier_t,polynomial_t>);
    def("hermite_from_bezier", hermite_from_curve<cubic_hermite_spline_t, bezier_t>);
    def("hermite_from_polynomial", hermite_from_curve<cubic_hermite_spline_t, polynomial_t>);
    /** END curves conversion**/
  } // End BOOST_PYTHON_MODULE
} // namespace curves