curves_python.cpp 52.2 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
  std::size_t dim() { return this->get_override("dim")(); }
  real min() { return this->get_override("min")(); }
  real max() { return this->get_override("max")(); }
};
25
26
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(curve_abc_t_isEquivalent_overloads, curve_abc_t::isEquivalent, 1, 3)

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
27
28
29
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")(); }
30
  curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
31
32
33
34
  std::size_t dim() { return this->get_override("dim")(); }
  real min() { return this->get_override("min")(); }
  real max() { return this->get_override("max")(); }
};
35
36
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(curve_3_t_isEquivalent_overloads, curve_3_t::isEquivalent, 1, 3)

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
37
38
39
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")(); }
40
  curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
41
42
43
44
  std::size_t dim() { return this->get_override("dim")(); }
  real min() { return this->get_override("min")(); }
  real max() { return this->get_override("max")(); }
};
45
46
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(curve_rotation_t_isEquivalent_overloads, curve_rotation_t::isEquivalent, 1, 3)

47
48
49
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")(); }
50
  curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
51
52
53
54
  std::size_t dim() { return this->get_override("dim")(); }
  real min() { return this->get_override("min")(); }
  real max() { return this->get_override("max")(); }
};
55
56
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(curve_SE3_t_isEquivalent_overloads, curve_SE3_t::isEquivalent, 1, 3)

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

179
static piecewise_t discretPointToPolynomialC0(const pointX_list_t& points,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
180
181
182
                                                               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);
183
  return piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,time_points_list);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
184
}
185
static piecewise_t discretPointToPolynomialC1(const pointX_list_t& points,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
186
187
188
189
190
                                                               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);
191
  return piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
192
193
      points_list, points_derivative_list, time_points_list);
}
194
static piecewise_t discretPointToPolynomialC2(const pointX_list_t& points,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
195
196
197
198
199
200
201
202
                                                               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);
203
  return piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
204
205
      points_list, points_derivative_list, points_second_derivative_list, time_points_list);
}
206
207

void addFinalPointC0(piecewise_t& self, const pointX_t& end, const real time) {
208
209
210
  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
211
212
213
    std::cout << "Warning: by adding this final point to the piecewise curve, you loose C1 continuity and only "
                 "guarantee C0 continuity."
              << std::endl;
214
215
  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
216
}
217
void addFinalPointC1(piecewise_t& self, const pointX_t& end, const pointX_t& d_end, const real time) {
218
219
220
  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
221
222
223
224
    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;
225
226
  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
227
}
228
void addFinalPointC2(piecewise_t& self, const pointX_t& end, const pointX_t& d_end,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
229
                     const pointX_t& dd_end, const real time) {
230
231
232
  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
233
234
235
236
    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;
237
238
239
  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
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
}

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

293
294
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
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
    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 */
313
314
315
316
317
318
319

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
320
321
322
323
324
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);
}

325
326
327
328
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
329
330
SE3Curve_t* wrapSE3CurveFromBezier3Translation(bezier3_t& translation_curve, const matrix3_t& init_rot,
                                               const matrix3_t& end_rot) {
331
332
  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
333
334
  return new SE3Curve_t(translation, init_rot, end_rot);
}
335

336
SE3Curve_t* wrapSE3CurveFromTranslation(const curve_ptr_t& translation_curve, const matrix3_t& init_rot,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
337
                                        const matrix3_t& end_rot) {
338
  return new SE3Curve_t(translation_curve, init_rot, end_rot);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
339
}
340

341
342
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
343
}
stevet's avatar
stevet committed
344

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
345
346
347
348
349
350
351
352
353
354
#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);
}

355
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
356

357
Motion_t se3ReturnDerivatePinocchio(const curve_SE3_t& curve, const real t, const std::size_t order) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
358
359
360
361
362
  return Motion_t(curve.derivate(t, order));
}
#endif  // CURVES_WITH_PINOCCHIO_SUPPORT
/* End wrap SE3Curves */

363
/* Wrap piecewiseSE3Curves */
364
365
366
367
#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
typedef pinocchio::SE3Tpl<real, 0> SE3_t;
typedef pinocchio::MotionTpl<real, 0> Motion_t;

368
void addFinalSE3(piecewise_SE3_t& self, const SE3_t& end, const real time) {
369
370
  if(self.num_curves() == 0)
    throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
371
372
373
374
375
376
377
378
  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);
}

379
#endif  // CURVES_WITH_PINOCCHIO_SUPPORT
380

381
void addFinalTransform(piecewise_SE3_t& self, const matrix4_t& end, const real time) {
382
383
  if(self.num_curves() == 0)
    throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
384
385
386
387
388
389
390
391
  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);
}

392
393
/* End wrap piecewiseSE3Curves */

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
394
395
396
397
398
// 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
399
  ENABLE_SPECIFIC_MATRIX_TYPE(pointX_t);
400
401
  ENABLE_SPECIFIC_MATRIX_TYPE(point3_t);
  ENABLE_SPECIFIC_MATRIX_TYPE(point6_t);
Wolfgang Merkt's avatar
Wolfgang Merkt committed
402
403
404
405
406
  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
407
408
409
410
411
412
413
414
415
  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"))
416
417
418
419
420
      .def("isEquivalent",&curve_abc_t::isEquivalent,curve_abc_t_isEquivalent_overloads(
             (bp::arg("other"),
             bp::arg("prec") = Eigen::NumTraits<double>::dummy_precision(),
             bp::arg("order") = 5),
           "isEquivalent check if self and other are approximately equal by values, given a precision treshold."))
421
      .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
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
      .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"))
443
444
445
446
447
      .def("isEquivalent",&curve_3_t::isEquivalent,curve_3_t_isEquivalent_overloads(
             (bp::arg("other"),
             bp::arg("prec") = Eigen::NumTraits<double>::dummy_precision(),
             bp::arg("order") = 5),
           "isEquivalent check if self and other are approximately equal by values, given a precision treshold."))
448
      .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
449
450
451
452
453
454
455
456
457
      .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"))
458
459
460
461
462
      .def("isEquivalent",&curve_rotation_t::isEquivalent,curve_rotation_t_isEquivalent_overloads(
             (bp::arg("other"),
             bp::arg("prec") = Eigen::NumTraits<double>::dummy_precision(),
             bp::arg("order") = 5),
           "isEquivalent check if self and other are approximately equal by values, given a precision treshold."))
463
      .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
464
465
466
467
      .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.");

468
  class_<CurveSE3Wrapper, boost::noncopyable, bases<curve_abc_t> >("curve_SE3", no_init)
469
      .def("__call__", &se3Return, "Evaluate the curve at the given time. Return as an homogeneous matrix.",
470
471
           args("self", "t"))
      .def("derivate", pure_virtual(&curve_SE3_t::derivate),
472
           "Evaluate the derivative of order N of curve at time t. Return as a vector 6.", args("self", "t", "N"))
473
474
475
476
477
      .def("isEquivalent",&curve_SE3_t::isEquivalent,curve_SE3_t_isEquivalent_overloads(
             (bp::arg("other"),
             bp::arg("prec") = Eigen::NumTraits<double>::dummy_precision(),
             bp::arg("order") = 5),
           "isEquivalent check if self and other are approximately equal by values, given a precision treshold."))
478
      .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"))
479
480
      .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.")
481
482
483
484
485
486
487
488
489
490
491
492
493
      .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
      ;
494
495


Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
496
497
  /** BEGIN bezier3 curve**/
  class_<bezier3_t, bases<curve_3_t> >("bezier3", init<>())
498
499
500
501
      .def("__init__", make_constructor(&wrapBezier3Constructor))
      .def("__init__", make_constructor(&wrapBezier3ConstructorBounds))
      .def("__init__", make_constructor(&wrapBezier3ConstructorConstraints))
      .def("__init__", make_constructor(&wrapBezier3ConstructorBoundsConstraints))
502
      .def("compute_primitive", &bezier3_t::compute_primitive)
503
      .def("waypointAtIndex", &bezier3_t::waypointAtIndex)
504
505
      .def_readonly("degree", &bezier3_t::degree_)
      .def_readonly("nbWaypoints", &bezier3_t::size_)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
506
507
508
509
510
511
512
513
514
515
      .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.")
516
      //.def(SerializableVisitor<bezier_t>())
517
518
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
519
520
521
522
      ;
  /** END bezier3 curve**/
  /** BEGIN bezier curve**/
  class_<bezier_t, bases<curve_abc_t> >("bezier", init<>())
523
524
525
526
527
      .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)
528
      .def("waypointAtIndex", &bezier_t::waypointAtIndex)
529
530
      .def_readonly("degree", &bezier_t::degree_)
      .def_readonly("nbWaypoints", &bezier_t::size_)
stevet's avatar
stevet committed
531
      .def("split", splitspe)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
532
533
534
535
536
537
538
539
540
541
      .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.")
542
543
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
544
      //.def(SerializableVisitor<bezier_t>())
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
545
546
547
548
549
550
      ;
  /** 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)
551
      .def_readonly("size", &LinearBezierVector::size)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
      .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)
578
      .def("compute_derivate", &bezier_linear_variable_t::compute_derivate_ptr, return_value_policy<manage_new_object>())
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
579
580
581
582
583
      .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_)
584
585
586
587
      .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
588
589
590
591
592
593
594
595
596
597
598

  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
599
           "Create polynomial spline from an Eigen matrix of coefficient defined for t in [min,max]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
600
601
           " The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest "
           "order."
602
           " Spline order is given by the number of the columns -1.")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
603
      .def("__init__", make_constructor(&wrapPolynomialConstructor2, default_call_policies(), arg("coeffs")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
604
           "Create polynomial spline from an Eigen matrix of coefficient defined for t in [0,1]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
605
606
           " The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest "
           "order."
607
           " Spline order is given by the number of the columns -1.")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
608
609
610
      .def("__init__",
           make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree1, default_call_policies(),
                            args("init", "end", "min", "max")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
611
           "Create a polynomial of degree 1 defined for t in [min,max], "
612
           "such that c(min) == init and c(max) == end.")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
613
614
615
      .def("__init__",
           make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree3, default_call_policies(),
                            args("init", "d_init", "end", "d_end", "min", "max")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
616
           "Create a polynomial of degree 3 defined for t in [min,max], "
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
617
618
619
620
621
           "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
622
           "Create a polynomial of degree 5 defined for t in [min,max], "
623
624
625
           "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")
626
627
      .def("coeffAtDegree", &polynomial_t::coeffAtDegree)
      .def("coeff", &polynomial_t::coeff)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
628
629
630
631
632
633
634
635
636
637
638
      .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"),
639
640
641
642
           "Loads *this from a binary file.")
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
      ;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
643
644
645

  /** END polynomial function**/
  /** BEGIN piecewise curve function **/
646
  class_<piecewise_t, bases<curve_abc_t> >("piecewise", init<>())
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
647
      .def("__init__",
648
649
           make_constructor(&wrapPiecewiseCurveConstructor, default_call_policies(), arg("curve")),
           "Create a peicewise curve containing the given curve.")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
650
651
652
653
654
655
656
657
658
659
660
661
      .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"))
662
      .staticmethod("FromPointsList")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
663
664
665
666
667
668
669
670
671
672
673
674
675
676
      .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"))
677
      .def("append", &piecewise_t::add_curve_ptr,
678
679
           "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.")
680
      .def("is_continuous", &piecewise_t::is_continuous,
681
           "Check if the curve is continuous at the given order.",args("self,order"))
682
      .def("convert_piecewise_curve_to_polynomial",
683
           &piecewise_t::convert_piecewise_curve_to_polynomial<polynomial_t>,
684
           "Convert a piecewise curve to to a piecewise polynomial curve")
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
685
      .def("convert_piecewise_curve_to_bezier",
686
687
           &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
688
      .def("convert_piecewise_curve_to_cubic_hermite",
689
690
691
           &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
692
           return_value_policy<copy_const_reference>())
693
694
695
      .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
696
           "Saves *this inside a text file.")
697
      .def("loadFromText", &piecewise_t::loadFromText<piecewise_t>,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
698
           bp::args("filename"), "Loads *this from a text file.")
699
      .def("saveAsXML", &piecewise_t::saveAsXML<piecewise_t>,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
700
           bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
701
      .def("loadFromXML", &piecewise_t::loadFromXML<piecewise_t>,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
702
           bp::args("filename", "tag_name"), "Loads *this from a XML file.")
703
      .def("saveAsBinary", &piecewise_t::saveAsBinary<piecewise_t>,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
704
           bp::args("filename"), "Saves *this inside a binary file.")
705
      .def("loadFromBinary", &piecewise_t::loadFromBinary<piecewise_t>,
706
707
708
709
           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
710

711
  class_<piecewise_SE3_t, bases<curve_SE3_t> >("piecewise_SE3", init<>())
712
713
714
715
      .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.")
716
      .def("append", &piecewise_SE3_t::add_curve_ptr,
717
718
719
           "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"))
720
721
722
723
      .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)
724
725
726
727
      .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"))
728
      .def("saveAsText", &piecewise_SE3_t::saveAsText<piecewise_SE3_t>, bp::args("filename"),
729
           "Saves *this inside a text file.")
730
      .def("loadFromText", &piecewise_SE3_t::loadFromText<piecewise_SE3_t>, bp::args("filename"),
731
           "Loads *this from a text file.")
732
      .def("saveAsXML", &piecewise_SE3_t::saveAsXML<piecewise_SE3_t>,
733
           bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
734
      .def("loadFromXML", &piecewise_SE3_t::loadFromXML<piecewise_SE3_t>,
735
           bp::args("filename", "tag_name"), "Loads *this from a XML file.")
736
      .def("saveAsBinary", &piecewise_SE3_t::saveAsBinary<piecewise_SE3_t>, bp::args("filename"),
737
           "Saves *this inside a binary file.")
738
      .def("loadFromBinary", &piecewise_SE3_t::loadFromBinary<piecewise_SE3_t>, bp::args("filename"),
739
           "Loads *this from a binary file.")
740
741
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
742
        #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
743
744
745
746
          .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"))
747
        #endif  // CURVES_WITH_PINOCCHIO_SUPPORT
748
749
        ;

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

  /** END exact_cubic curve**/
  /** BEGIN cubic_hermite_spline **/
  class_<cubic_hermite_spline_t, bases<curve_abc_t> >("cubic_hermite_spline", init<>())
776
      .def("__init__", make_constructor(&wrapCubicHermiteSplineConstructor))
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
777
778
779
780
781
782
783
784
785
786
787
      .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"),
788
789
790
791
           "Loads *this from a binary file.")
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
      ;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
792
793
794
795
796
797
798
799
800
801
802
803
804

  /** 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>())
805
806
807
808
      .def("__call__", &bernstein_t::operator())
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
      ;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
809
810
811
812
813
814
815
  /** 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
816
           "Create a SO3 Linear curve between two rotations, defined for t in [min,max]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
817
818
819
820
           " 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
821
           "Create a SO3 Linear curve between two rotations, defined for t in [min,max]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
822
823
824
825
           " 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.")
826
827
828
829
830
831
832
833
834
835
836
837
      .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.")
838
839
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
Guilhem Saurel's avatar
Guilhem Saurel committed
840
      ;
841

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
842
843
  /** END  SO3 Linear**/
  /** BEGIN SE3 Curve**/
844
  class_<SE3Curve_t, bases<curve_SE3_t> >("SE3Curve", init<>())
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
845
846
847
      .def("__init__",
           make_constructor(&wrapSE3CurveFromTransform, default_call_policies(),
                            args("init_transform", "end_transform", "min", "max")),
Wolfgang Merkt's avatar
Wolfgang Merkt committed
848
           "Create a SE3 curve between two transform, defined for t in [min,max]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
849
850
           " Using linear interpolation for translation and slerp for rotation between init and end."
           " The input transform are expressed as 4x4 matrix.")
851
852
853
      .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
854
           "Create a SE3 curve between two transform, defined for t in [min,max]."
855
856
           " 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
857
858
859
860
861
862
      .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
863
      .def("__init__",
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
864
865
866
867
868
869
870
           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
871
      .def("__init__",
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
872
873
874
875
876
877
878
           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")
879
880
881
882
883
884
885
886
887
888
889
890
      .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.")
891
892
      .def(bp::self == bp::self)
      .def(bp::self != bp::self)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
893
894
895
896
#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
897
           "Create a SE3 curve between two SE3 objects from Pinocchio, defined for t in [min,max]."
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
898
899
900
901
902
903
           " Using linear interpolation for translation and slerp for rotation between init and end.")
#endif  // CURVES_WITH_PINOCCHIO_SUPPORT
      ;

  /** END SE3 Curve**/
  /** BEGIN curves conversion**/
904
905
906
  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
907
908
909
910
911
912
913
914
915
916
917
918
  /** 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