curves_python.cpp 50.4 KB
Newer Older
1
2
3
4
5
#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
#include <pinocchio/spatial/se3.hpp>
#include <pinocchio/spatial/motion.hpp>
#endif  // CURVES_WITH_PINOCCHIO_SUPPORT

stevet's avatar
stevet committed
6
#include "python_variables.h"
7
#include "archive_python_binding.h"
stevet's avatar
stevet committed
8
#include "optimization_python.h"
9
10
11

#include <boost/python.hpp>

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
12
13
14
15
16
17
18
19

namespace curves {
using namespace boost::python;

/* base wrap of curve_abc and others parent abstract class: must implement all pure virtual methods */
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")(); }
20
  curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
21
22
23
24
25
26
27
  std::size_t dim() { return this->get_override("dim")(); }
  real min() { return this->get_override("min")(); }
  real max() { return this->get_override("max")(); }
};
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")(); }
28
  curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
29
30
31
32
33
34
35
  std::size_t dim() { return this->get_override("dim")(); }
  real min() { return this->get_override("min")(); }
  real max() { return this->get_override("max")(); }
};
struct CurveRotationWrapper : curve_rotation_t, wrapper<curve_rotation_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")(); }
36
  curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
37
38
39
40
  std::size_t dim() { return this->get_override("dim")(); }
  real min() { return this->get_override("min")(); }
  real max() { return this->get_override("max")(); }
};
41
42
43
struct CurveSE3Wrapper : curve_SE3_t, wrapper<curve_SE3_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")(); }
44
  curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
  std::size_t dim() { return this->get_override("dim")(); }
  real min() { return this->get_override("min")(); }
  real max() { return this->get_override("max")(); }
};
/* 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 */
/* Helper converter constraintsX -> constraints 3 */
curve_constraints3_t convertToConstraints3(curve_constraints_t constraintsX) {
  curve_constraints3_t constraints3(3);
  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 */

/*3D constructors bezier */
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 */
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) {
  return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(
      array, constraints, T_min, T_max);
}
/*END constructors bezier */

/* 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;
}

cubic_hermite_spline_t* wrapCubicHermiteSplineConstructor(const pointX_list_t& points, const pointX_list_t& tangents,
                                                          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,
151
                                                                     const pointX_t& d_end, const pointX_t& dd_end,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
152
153
154
155
156
157
                                                                     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 */
158
159
piecewise_t* wrapPiecewiseCurveConstructor(const curve_ptr_t& curve) {
  return new piecewise_t(curve);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
160
}
161
162
piecewise_t* wrapPiecewisePolynomialCurveEmptyConstructor() {
  return new piecewise_t();
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
163
}
164
165
piecewise_SE3_t* wrapPiecewiseSE3Constructor(const curve_SE3_ptr_t& curve) {
  return new piecewise_SE3_t(curve);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
166
}
167
168
piecewise_SE3_t* wrapPiecewiseSE3EmptyConstructor() {
  return new piecewise_SE3_t();
169
170
}

171
static piecewise_t discretPointToPolynomialC0(const pointX_list_t& points,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
172
173
174
                                                               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);
175
  return piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,time_points_list);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
176
}
177
static piecewise_t discretPointToPolynomialC1(const pointX_list_t& points,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
178
179
180
181
182
                                                               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);
183
  return piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
184
185
      points_list, points_derivative_list, time_points_list);
}
186
static piecewise_t discretPointToPolynomialC2(const pointX_list_t& points,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
187
188
189
190
191
192
193
194
                                                               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);
195
  return piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
196
197
      points_list, points_derivative_list, points_second_derivative_list, time_points_list);
}
198
199

void addFinalPointC0(piecewise_t& self, const pointX_t& end, const real time) {
200
201
202
  if(self.num_curves() == 0)
    throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
  if (self.is_continuous(1) && self.num_curves()>1 )
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
203
204
205
    std::cout << "Warning: by adding this final point to the piecewise curve, you loose C1 continuity and only "
                 "guarantee C0 continuity."
              << std::endl;
206
207
  curve_ptr_t pol(new polynomial_t(self(self.max()), end, self.max(), time));
  self.add_curve_ptr(pol);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
208
}
209
void addFinalPointC1(piecewise_t& self, const pointX_t& end, const pointX_t& d_end, const real time) {
210
211
212
  if(self.num_curves() == 0)
    throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
  if (self.is_continuous(2) && self.num_curves()>1 )
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
213
214
215
216
    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;
217
218
  curve_ptr_t pol(new polynomial_t(self(self.max()), self.derivate(self.max(), 1), end, d_end, self.max(), time));
  self.add_curve_ptr(pol);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
219
}
220
void addFinalPointC2(piecewise_t& self, const pointX_t& end, const pointX_t& d_end,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
221
                     const pointX_t& dd_end, const real time) {
222
223
224
  if(self.num_curves() == 0)
    throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
  if (self.is_continuous(3) && self.num_curves()>1 )
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
225
226
227
228
    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;
229
230
231
  curve_ptr_t pol(new polynomial_t(self(self.max()), self.derivate(self.max(), 1), self.derivate(self.max(), 2), end, d_end, dd_end,
                   self.max(), time));
  self.add_curve_ptr(pol);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
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
}

/* 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());
}

exact_cubic_t* wrapExactCubicConstructorConstraint(const coeff_t& array, const time_waypoints_t& time_wp,
                                                   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
pointX_t get_init_vel(const curve_constraints_t& c) { return c.init_vel; }

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

pointX_t get_init_jerk(const curve_constraints_t& c) { return c.init_jerk; }

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

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

pointX_t get_end_jerk(const curve_constraints_t& c) { return c.end_jerk; }

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

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

void set_init_jerk(curve_constraints_t& c, const pointX_t& val) { c.init_jerk = val; }

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

void set_end_acc(curve_constraints_t& c, const pointX_t& val) { c.end_acc = val; }

void set_end_jerk(curve_constraints_t& c, const pointX_t& val) { c.end_jerk = val; }

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));
}

285
286
bezier_t::piecewise_curve_t (bezier_t::*splitspe)(const bezier_t::vector_x_t&) const = &bezier_t::split;
bezier_linear_variable_t::piecewise_curve_t (bezier_linear_variable_t::*split_py)(
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
    const bezier_linear_variable_t::vector_x_t&) const = &bezier_linear_variable_t::split;

/* End wrap exact cubic spline */

/* 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);
}

/* End wrap SO3Linear */

/* Wrap SE3Curves */
305
306
307
308
309
310
311

matrix4_t se3Return(const curve_SE3_t& curve, const real t) { return curve(t).matrix(); }

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

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

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
312
313
314
315
316
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);
}

317
318
319
320
SE3Curve_t* wrapSE3CurveFromPosAndRotation(const pointX_t& init_pos, const pointX_t& end_pos, const matrix3_t& init_rot, const matrix3_t& end_rot,const real& t_min, const real& t_max) {
  return new SE3Curve_t(init_pos,end_pos,init_rot,end_rot, t_min, t_max);
}

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
321
322
SE3Curve_t* wrapSE3CurveFromBezier3Translation(bezier3_t& translation_curve, const matrix3_t& init_rot,
                                               const matrix3_t& end_rot) {
323
324
  boost::shared_ptr<bezier_t> translation(new bezier_t(translation_curve.waypoints().begin(), translation_curve.waypoints().end(),
                                       translation_curve.min(), translation_curve.max()));
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
325
326
  return new SE3Curve_t(translation, init_rot, end_rot);
}
327

328
SE3Curve_t* wrapSE3CurveFromTranslation(const curve_ptr_t& translation_curve, const matrix3_t& init_rot,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
329
                                        const matrix3_t& end_rot) {
330
  return new SE3Curve_t(translation_curve, init_rot, end_rot);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
331
}
332

333
334
SE3Curve_t* wrapSE3CurveFromTwoCurves(const curve_ptr_t& translation_curve,const curve_rotation_ptr_t& rotation_curve) {
  return new SE3Curve_t(translation_curve,rotation_curve);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
335
}
stevet's avatar
stevet committed
336

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
337
338
339
340
341
342
343
344
345
346
#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
typedef pinocchio::SE3Tpl<real, 0> SE3_t;
typedef pinocchio::MotionTpl<real, 0> Motion_t;

SE3Curve_t* wrapSE3CurveFromSE3Pinocchio(const SE3_t& init_pose, const SE3_t& end_pose, const real min,
                                         const real max) {
  return new SE3Curve_t(transform_t(init_pose.toHomogeneousMatrix()), transform_t(end_pose.toHomogeneousMatrix()), min,
                        max);
}

347
SE3_t se3ReturnPinocchio(const curve_SE3_t& curve, const real t) { return SE3_t(curve(t).matrix()); }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
348

349
Motion_t se3ReturnDerivatePinocchio(const curve_SE3_t& curve, const real t, const std::size_t order) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
350
351
352
353
354
  return Motion_t(curve.derivate(t, order));
}
#endif  // CURVES_WITH_PINOCCHIO_SUPPORT
/* End wrap SE3Curves */

355
/* Wrap piecewiseSE3Curves */
356
357
358
359
#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
typedef pinocchio::SE3Tpl<real, 0> SE3_t;
typedef pinocchio::MotionTpl<real, 0> Motion_t;

360
void addFinalSE3(piecewise_SE3_t& self, const SE3_t& end, const real time) {
361
362
  if(self.num_curves() == 0)
    throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
363
364
365
366
367
368
369
370
  if (self.is_continuous(1) && self.num_curves()>1 )
    std::cout << "Warning: by adding this final transform to the piecewise curve, you loose C1 continuity and only "
                 "guarantee C0 continuity."
              << std::endl;
  SE3Curve_t curve(self(self.max()), transform_t(end.toHomogeneousMatrix()), self.max(), time);
  self.add_curve(curve);
}

371
#endif  // CURVES_WITH_PINOCCHIO_SUPPORT
372

373
void addFinalTransform(piecewise_SE3_t& self, const matrix4_t& end, const real time) {
374
375
  if(self.num_curves() == 0)
    throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
376
377
378
379
380
381
382
383
  if (self.is_continuous(1) && self.num_curves()>1 )
    std::cout << "Warning: by adding this final transform to the piecewise curve, you loose C1 continuity and only "
                 "guarantee C0 continuity."
              << std::endl;
  SE3Curve_t curve(self(self.max()), transform_t(end), self.max(), time);
  self.add_curve(curve);
}

384
385
/* End wrap piecewiseSE3Curves */

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
386
387
388
389
390
// TO DO : Replace all load and save function for serialization in class by using
//         SerializableVisitor in archive_python_binding.
BOOST_PYTHON_MODULE(curves) {
  /** BEGIN eigenpy init**/
  eigenpy::enableEigenPy();
Wolfgang Merkt's avatar
Wolfgang Merkt committed
391
  ENABLE_SPECIFIC_MATRIX_TYPE(pointX_t);
392
393
  ENABLE_SPECIFIC_MATRIX_TYPE(point3_t);
  ENABLE_SPECIFIC_MATRIX_TYPE(point6_t);
Wolfgang Merkt's avatar
Wolfgang Merkt committed
394
395
396
397
398
  ENABLE_SPECIFIC_MATRIX_TYPE(pointX_list_t);
  ENABLE_SPECIFIC_MATRIX_TYPE(coeff_t);
  ENABLE_SPECIFIC_MATRIX_TYPE(matrix3_t);
  ENABLE_SPECIFIC_MATRIX_TYPE(matrix4_t);
  // ENABLE_SPECIFIC_MATRIX_TYPE(quaternion_t);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
399
400
401
402
403
404
405
406
407
  eigenpy::exposeQuaternion();
  /*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"))
408
      .def("compute_derivate", pure_virtual(&curve_abc_t::compute_derivate_ptr),return_value_policy<manage_new_object>(), "Return the derivative of *this at the order N.",  args("self", "N"))
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
      .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.");

  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"))
430
      .def("compute_derivate", pure_virtual(&curve_3_t::compute_derivate_ptr),return_value_policy<manage_new_object>(), "Return the derivative of *this at the order N.",  args("self", "N"))
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
431
432
433
434
435
436
437
438
439
      .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.");

  class_<CurveRotationWrapper, boost::noncopyable, bases<curve_abc_t> >("curve_rotation", no_init)
      .def("__call__", pure_virtual(&curve_rotation_t::operator()), "Evaluate the curve at the given time.",
           args("self", "t"))
      .def("derivate", pure_virtual(&curve_rotation_t::derivate),
           "Evaluate the derivative of order N of curve at time t.", args("self", "t", "N"))
440
      .def("compute_derivate", pure_virtual(&curve_rotation_t::compute_derivate_ptr),return_value_policy<manage_new_object>(), "Return the derivative of *this at the order N.",  args("self", "N"))
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
441
442
443
444
      .def("min", pure_virtual(&curve_rotation_t::min), "Get the LOWER bound on interval definition of the curve.")
      .def("max", pure_virtual(&curve_rotation_t::max), "Get the HIGHER bound on interval definition of the curve.")
      .def("dim", pure_virtual(&curve_rotation_t::dim), "Get the dimension of the curve.");

445
  class_<CurveSE3Wrapper, boost::noncopyable, bases<curve_abc_t> >("curve_SE3", no_init)
446
      .def("__call__", &se3Return, "Evaluate the curve at the given time. Return as an homogeneous matrix.",
447
448
           args("self", "t"))
      .def("derivate", pure_virtual(&curve_SE3_t::derivate),
449
           "Evaluate the derivative of order N of curve at time t. Return as a vector 6.", args("self", "t", "N"))
450
      .def("compute_derivate", pure_virtual(&curve_SE3_t::compute_derivate_ptr),return_value_policy<manage_new_object>(), "Return the derivative of *this at the order N.",  args("self", "N"))
451
452
      .def("min", pure_virtual(&curve_SE3_t::min), "Get the LOWER bound on interval definition of the curve.")
      .def("max", pure_virtual(&curve_SE3_t::max), "Get the HIGHER bound on interval definition of the curve.")
453
454
455
456
457
458
459
460
461
462
463
464
465
      .def("dim", pure_virtual(&curve_SE3_t::dim), "Get the dimension of the curve.")
      .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"))
    #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
      .def("evaluateAsSE3", &se3ReturnPinocchio, "Evaluate the curve at the given time. Return as a pinocchio.SE3 object",
           args("self", "t"))
      .def("derivateAsMotion", &se3ReturnDerivatePinocchio,
           "Evaluate the derivative of order N of curve at time t. Return as a pinocchio.Motion",
           args("self", "t", "N"))
    #endif  // CURVES_WITH_PINOCCHIO_SUPPORT
      ;
466
467


Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
468
469
  /** BEGIN bezier3 curve**/
  class_<bezier3_t, bases<curve_3_t> >("bezier3", init<>())
470
471
472
473
      .def("__init__", make_constructor(&wrapBezier3Constructor))
      .def("__init__", make_constructor(&wrapBezier3ConstructorBounds))
      .def("__init__", make_constructor(&wrapBezier3ConstructorConstraints))
      .def("__init__", make_constructor(&wrapBezier3ConstructorBoundsConstraints))
474
      .def("compute_primitive", &bezier3_t::compute_primitive)
475
      .def("waypointAtIndex", &bezier3_t::waypointAtIndex)
476
477
      .def_readonly("degree", &bezier3_t::degree_)
      .def_readonly("nbWaypoints", &bezier3_t::size_)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
478
479
480
481
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
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
491
492
493
494
      ;
  /** END bezier3 curve**/
  /** BEGIN bezier curve**/
  class_<bezier_t, bases<curve_abc_t> >("bezier", init<>())
495
496
497
498
499
      .def("__init__", make_constructor(&wrapBezierConstructor))
      .def("__init__", make_constructor(&wrapBezierConstructorBounds))
      .def("__init__", make_constructor(&wrapBezierConstructorConstraints))
      .def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints))
      .def("compute_primitive", &bezier_t::compute_primitive)
500
      .def("waypointAtIndex", &bezier_t::waypointAtIndex)
501
502
      .def_readonly("degree", &bezier_t::degree_)
      .def_readonly("nbWaypoints", &bezier_t::size_)
stevet's avatar
stevet committed
503
      .def("split", splitspe)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
504
505
506
507
508
509
510
511
512
513
      .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.")
514
515
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
516
      //.def(SerializableVisitor<bezier_t>())
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
517
518
519
520
521
522
      ;
  /** END bezier curve**/
  /** BEGIN variable points bezier curve**/
  class_<matrix_pair>("matrix_pair", no_init).def_readonly("A", &matrix_pair::A).def_readonly("b", &matrix_pair::b);

  class_<LinearBezierVector>("bezierVarVector", no_init)
523
      .def_readonly("size", &LinearBezierVector::size)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
      .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>("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_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)
550
      .def("compute_derivate", &bezier_linear_variable_t::compute_derivate_ptr, return_value_policy<manage_new_object>())
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
551
552
553
554
555
      .def("compute_primitive", &bezier_linear_variable_t::compute_primitive)
      .def("split", split_py)
      .def("waypoints", &wayPointsToLists, return_value_policy<manage_new_object>())
      .def("waypointAtIndex", &bezier_linear_variable_t::waypointAtIndex)
      .def_readonly("degree", &bezier_linear_variable_t::degree_)
556
557
558
559
      .def_readonly("nbWaypoints", &bezier_linear_variable_t::size_)
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
      ;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
560
561
562
563
564
565
566
567
568
569
570

  class_<quadratic_variable_t>("cost", no_init)
      .add_property("A", &cost_t_quad)
      .add_property("b", &cost_t_linear)
      .add_property("c", &cost_t_constant);

  /** 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")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
571
           "Create polynomial spline from an Eigen matrix of coefficient defined for t in [min,max]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
572
573
           " The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest "
           "order."
574
           " Spline order is given by the number of the columns -1.")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
575
      .def("__init__", make_constructor(&wrapPolynomialConstructor2, default_call_policies(), arg("coeffs")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
576
           "Create polynomial spline from an Eigen matrix of coefficient defined for t in [0,1]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
577
578
           " The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest "
           "order."
579
           " Spline order is given by the number of the columns -1.")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
580
581
582
      .def("__init__",
           make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree1, default_call_policies(),
                            args("init", "end", "min", "max")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
583
           "Create a polynomial of degree 1 defined for t in [min,max], "
584
           "such that c(min) == init and c(max) == end.")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
585
586
587
      .def("__init__",
           make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree3, default_call_policies(),
                            args("init", "d_init", "end", "d_end", "min", "max")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
588
           "Create a polynomial of degree 3 defined for t in [min,max], "
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
589
590
591
592
593
           "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")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
594
           "Create a polynomial of degree 5 defined for t in [min,max], "
595
596
597
           "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")
598
599
      .def("coeffAtDegree", &polynomial_t::coeffAtDegree)
      .def("coeff", &polynomial_t::coeff)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
600
601
602
603
604
605
606
607
608
609
610
      .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"),
611
612
613
614
           "Loads *this from a binary file.")
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
      ;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
615
616
617

  /** END polynomial function**/
  /** BEGIN piecewise curve function **/
618
  class_<piecewise_t, bases<curve_abc_t> >("piecewise", init<>())
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
619
      .def("__init__",
620
621
           make_constructor(&wrapPiecewiseCurveConstructor, default_call_policies(), arg("curve")),
           "Create a peicewise curve containing the given curve.")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
622
623
624
625
626
627
628
629
630
631
632
633
      .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"))
634
      .staticmethod("FromPointsList")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
635
636
637
638
639
640
641
642
643
644
645
646
647
648
      .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"))
649
      .def("append", &piecewise_t::add_curve_ptr,
650
651
           "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.")
652
      .def("is_continuous", &piecewise_t::is_continuous,
653
           "Check if the curve is continuous at the given order.",args("self,order"))
654
      .def("convert_piecewise_curve_to_polynomial",
655
           &piecewise_t::convert_piecewise_curve_to_polynomial<polynomial_t>,
656
           "Convert a piecewise curve to to a piecewise polynomial curve")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
657
      .def("convert_piecewise_curve_to_bezier",
658
659
           &piecewise_t::convert_piecewise_curve_to_bezier<bezier_t>,
           "Convert a piecewise curve to to a piecewise bezier curve")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
660
      .def("convert_piecewise_curve_to_cubic_hermite",
661
662
663
           &piecewise_t::convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>,
           "Convert a piecewise curve to to a piecewise cubic hermite spline")
      .def("curve_at_index", &piecewise_t::curve_at_index,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
664
           return_value_policy<copy_const_reference>())
665
666
667
      .def("curve_at_time", &piecewise_t::curve_at_time, return_value_policy<copy_const_reference>())
      .def("num_curves", &piecewise_t::num_curves)
      .def("saveAsText", &piecewise_t::saveAsText<piecewise_t>, bp::args("filename"),
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
668
           "Saves *this inside a text file.")
669
      .def("loadFromText", &piecewise_t::loadFromText<piecewise_t>,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
670
           bp::args("filename"), "Loads *this from a text file.")
671
      .def("saveAsXML", &piecewise_t::saveAsXML<piecewise_t>,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
672
           bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
673
      .def("loadFromXML", &piecewise_t::loadFromXML<piecewise_t>,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
674
           bp::args("filename", "tag_name"), "Loads *this from a XML file.")
675
      .def("saveAsBinary", &piecewise_t::saveAsBinary<piecewise_t>,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
676
           bp::args("filename"), "Saves *this inside a binary file.")
677
      .def("loadFromBinary", &piecewise_t::loadFromBinary<piecewise_t>,
678
679
680
681
           bp::args("filename"), "Loads *this from a binary file.")
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
      ;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
682

683
  class_<piecewise_SE3_t, bases<curve_SE3_t> >("piecewise_SE3", init<>())
684
685
686
687
      .def("__init__", make_constructor(&wrapPiecewiseSE3Constructor, default_call_policies(), arg("curve")),
      "Create a piecewise-se3 curve containing the given se3 curve.")
      .def("__init__", make_constructor(&wrapPiecewiseSE3EmptyConstructor),
      "Create an empty piecewise-se3 curve.")
688
      .def("append", &piecewise_SE3_t::add_curve_ptr,
689
690
691
           "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.",
           args("self,curve"))
692
693
694
695
      .def("is_continuous", &piecewise_SE3_t::is_continuous, "Check if the curve is continuous at the given order.",args("self,order"))
      .def("curve_at_index", &piecewise_SE3_t::curve_at_index, return_value_policy<copy_const_reference>())
      .def("curve_at_time", &piecewise_SE3_t::curve_at_time, return_value_policy<copy_const_reference>())
      .def("num_curves", &piecewise_SE3_t::num_curves)
696
697
698
699
      .def("append", &addFinalTransform,
       "Append a new linear SE3 curve 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"))
700
      .def("saveAsText", &piecewise_SE3_t::saveAsText<piecewise_SE3_t>, bp::args("filename"),
701
           "Saves *this inside a text file.")
702
      .def("loadFromText", &piecewise_SE3_t::loadFromText<piecewise_SE3_t>, bp::args("filename"),
703
           "Loads *this from a text file.")
704
      .def("saveAsXML", &piecewise_SE3_t::saveAsXML<piecewise_SE3_t>,
705
           bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
706
      .def("loadFromXML", &piecewise_SE3_t::loadFromXML<piecewise_SE3_t>,
707
           bp::args("filename", "tag_name"), "Loads *this from a XML file.")
708
      .def("saveAsBinary", &piecewise_SE3_t::saveAsBinary<piecewise_SE3_t>, bp::args("filename"),
709
           "Saves *this inside a binary file.")
710
      .def("loadFromBinary", &piecewise_SE3_t::loadFromBinary<piecewise_SE3_t>, bp::args("filename"),
711
           "Loads *this from a binary file.")
712
713
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
714
        #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
715
716
717
718
          .def("append", &addFinalSE3,
           "Append a new linear SE3 curve 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"))
719
        #endif  // CURVES_WITH_PINOCCHIO_SUPPORT
720
721
        ;

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
722
723
724
  /** END piecewise curve function **/
  /** BEGIN exact_cubic curve**/
  class_<exact_cubic_t, bases<curve_abc_t> >("exact_cubic", init<>())
725
726
727
728
      .def("__init__", make_constructor(&wrapExactCubicConstructor))
      .def("__init__", make_constructor(&wrapExactCubicConstructorConstraint))
      .def("getNumberSplines", &exact_cubic_t::getNumberSplines)
      .def("getSplineAt", &exact_cubic_t::getSplineAt)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
729
730
731
732
733
734
735
736
737
738
739
      .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"),
740
741
742
743
           "Loads *this from a binary file.")
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
      ;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
744
745
746
747

  /** END exact_cubic curve**/
  /** BEGIN cubic_hermite_spline **/
  class_<cubic_hermite_spline_t, bases<curve_abc_t> >("cubic_hermite_spline", init<>())
748
      .def("__init__", make_constructor(&wrapCubicHermiteSplineConstructor))
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
749
750
751
752
753
754
755
756
757
758
759
      .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"),
760
761
762
763
           "Loads *this from a binary file.")
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
      ;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
764
765
766
767
768
769
770
771
772
773
774
775
776

  /** END cubic_hermite_spline **/
  /** BEGIN curve constraints**/
  class_<curve_constraints_t>("curve_constraints", init<int>())
      .add_property("init_vel", &get_init_vel, &set_init_vel)
      .add_property("init_acc", &get_init_acc, &set_init_acc)
      .add_property("init_jerk", &get_init_jerk, &set_init_jerk)
      .add_property("end_vel", &get_end_vel, &set_end_vel)
      .add_property("end_acc", &get_end_acc, &set_end_acc)
      .add_property("end_jerk", &get_end_jerk, &set_end_jerk);
  /** END curve constraints**/
  /** BEGIN bernstein polynomial**/
  class_<bernstein_t>("bernstein", init<const unsigned int, const unsigned int>())
777
778
779
780
      .def("__call__", &bernstein_t::operator())
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
      ;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
781
782
783
784
785
786
787
  /** END bernstein polynomial**/

  /** BEGIN SO3 Linear**/
  class_<SO3Linear_t, bases<curve_rotation_t> >("SO3Linear", init<>())
      .def("__init__",
           make_constructor(&wrapSO3LinearConstructorFromMatrix, default_call_policies(),
                            args("init_rotation", "end_rotation", "min", "max")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
788
           "Create a SO3 Linear curve between two rotations, defined for t in [min,max]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
789
790
791
792
           " The input rotations are expressed as 3x3 matrix.")
      .def("__init__",
           make_constructor(&wrapSO3LinearConstructorFromQuaternion, default_call_policies(),
                            args("init_rotation", "end_rotation", "min", "max")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
793
           "Create a SO3 Linear curve between two rotations, defined for t in [min,max]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
794
795
796
797
           " The input rotations are expressed as Quaternions.")
      .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.")
798
799
800
801
802
803
804
805
806
807
808
809
      .def("saveAsText", &SO3Linear_t::saveAsText<SO3Linear_t>,bp::args("filename"),
      "Saves *this inside a text file.")
      .def("loadFromText",&SO3Linear_t::loadFromText<SO3Linear_t>,bp::args("filename"),
      "Loads *this from a text file.")
      .def("saveAsXML",&SO3Linear_t::saveAsXML<SO3Linear_t>,bp::args("filename","tag_name"),
      "Saves *this inside a XML file.")
      .def("loadFromXML",&SO3Linear_t::loadFromXML<SO3Linear_t>,bp::args("filename","tag_name"),
      "Loads *this from a XML file.")
      .def("saveAsBinary",&SO3Linear_t::saveAsBinary<SO3Linear_t>,bp::args("filename"),
      "Saves *this inside a binary file.")
      .def("loadFromBinary",&SO3Linear_t::loadFromBinary<SO3Linear_t>,bp::args("filename"),
      "Loads *this from a binary file.")
810
811
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
Guilhem Saurel's avatar
Guilhem Saurel committed
812
      ;
813

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
814
815
  /** END  SO3 Linear**/
  /** BEGIN SE3 Curve**/
816
  class_<SE3Curve_t, bases<curve_SE3_t> >("SE3Curve", init<>())
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
817
818
819
      .def("__init__",
           make_constructor(&wrapSE3CurveFromTransform, default_call_policies(),
                            args("init_transform", "end_transform", "min", "max")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
820
           "Create a SE3 curve between two transform, defined for t in [min,max]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
821
822
           " Using linear interpolation for translation and slerp for rotation between init and end."
           " The input transform are expressed as 4x4 matrix.")
823
824
825
      .def("__init__",
           make_constructor(&wrapSE3CurveFromPosAndRotation, default_call_policies(),
                            args("init_translation", "end_translation","init_rotation","end_rotation", "min", "max")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
826
           "Create a SE3 curve between two transform, defined for t in [min,max]."
827
828
           " Using linear interpolation for translation and slerp for rotation between init and end."
           " The input translations are expressed as 3d vector and the rotations as 3x3 matrix.")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
829
830
831
832
833
834
      .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.")
Pierre Fernbach's avatar
Pierre Fernbach committed
835
      .def("__init__",
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
836
837
838
839
840
841
842
           make_constructor(&wrapSE3CurveFromTranslation, default_call_policies(),
                            args("translation_curve", "init_rotation", "end_rotation")),
           "Create a SE3 curve from a translation curve and 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")
Pierre Fernbach's avatar
Pierre Fernbach committed
843
      .def("__init__",
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
844
845
846
847
848
849
850
           make_constructor(&wrapSE3CurveFromBezier3Translation, default_call_policies(),
                            args("translation_curve", "init_rotation", "end_rotation")),
           "Create a SE3 curve from a translation curve and 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")
851
852
853
854
855
856
857
858
859
860
861
862
      .def("saveAsText", &SE3Curve_t::saveAsText<SE3Curve_t>,bp::args("filename"),
      "Saves *this inside a text file.")
      .def("loadFromText",&SE3Curve_t::loadFromText<SE3Curve_t>,bp::args("filename"),
      "Loads *this from a text file.")
      .def("saveAsXML",&SE3Curve_t::saveAsXML<SE3Curve_t>,bp::args("filename","tag_name"),
      "Saves *this inside a XML file.")
      .def("loadFromXML",&SE3Curve_t::loadFromXML<SE3Curve_t>,bp::args("filename","tag_name"),
      "Loads *this from a XML file.")
      .def("saveAsBinary",&SE3Curve_t::saveAsBinary<SE3Curve_t>,bp::args("filename"),
      "Saves *this inside a binary file.")
      .def("loadFromBinary",&SE3Curve_t::loadFromBinary<SE3Curve_t>,bp::args("filename"),
      "Loads *this from a binary file.")
863
864
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
865
866
867
868
#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
      .def("__init__",
           make_constructor(&wrapSE3CurveFromSE3Pinocchio, default_call_policies(),
                            args("init_SE3", "end_SE3", "min", "max")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
869
           "Create a SE3 curve between two SE3 objects from Pinocchio, defined for t in [min,max]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
870
871
872
873
874
875
           " Using linear interpolation for translation and slerp for rotation between init and end.")
#endif  // CURVES_WITH_PINOCCHIO_SUPPORT
      ;

  /** END SE3 Curve**/
  /** BEGIN curves conversion**/
876
877
878
  def("convert_to_polynomial", polynomial_from_curve<polynomial_t>);
  def("convert_to_bezier", bezier_from_curve<bezier_t>);
  def("convert_to_hermite", hermite_from_curve<cubic_hermite_spline_t>);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
879
880
881
882
883
884
885
886
887
888
889
890
  /** END curves conversion**/

  optimization::python::exposeOptimization();

#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
  scope().attr("CURVES_WITH_PINOCCHIO_SUPPORT") = true;
#else
  scope().attr("CURVES_WITH_PINOCCHIO_SUPPORT") = false;
#endif

}  // End BOOST_PYTHON_MODULE
}  // namespace curves