GCC Code Coverage Report

Directory: ./
File: python/spline_python.cpp
Date: 2025-04-07 13:04:42
Exec Total Coverage
Lines: 58 108 53.7%
Branches: 50 176 28.4%
Line Branch Exec Source
1 // #include "parametriccurves/bezier_curve.h"
2 #include <parametric-curves/polynomial.hpp>
3 #include <parametric-curves/spatial/force-curve.hpp>
4 #include <parametric-curves/spline.hpp>
5
6 // #include "parametriccurves/splines/spline_deriv_constraint.h"
7 #include <boost/python.hpp>
8 #include <eigenpy/eigenpy.hpp>
9 #include <eigenpy/memory.hpp>
10 #include <parametric-curves/curve-constraint.hpp>
11 #include <vector>
12
13 /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
14 namespace bp = boost::python;
15
16 typedef double real;
17 typedef Eigen::Vector3d point_t;
18 typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> point6_t;
19 typedef Eigen::Matrix<double, 3, 1, 0, 3, 1> ret_point_t;
20 typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> ret_point6_t;
21 typedef Eigen::VectorXd time_waypoints_t;
22 typedef Eigen::VectorXd time_vector_t;
23 typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
24 typedef Eigen::Matrix<real, 6, Eigen::Dynamic> point_list6_t;
25 typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
26 typedef std::vector<point6_t, Eigen::aligned_allocator<point6_t> > t_point6_t;
27 typedef std::pair<real, point_t> Waypoint;
28 typedef std::vector<Waypoint> T_Waypoint;
29 typedef std::pair<real, point6_t> Waypoint6;
30 typedef std::vector<Waypoint6> T_Waypoint6;
31 typedef std::vector<t_point_t, Eigen::aligned_allocator<t_point_t> >
32 t3d_poly_coeffs_vector_t;
33 typedef typename t3d_poly_coeffs_vector_t::iterator it3d_poly_coeffs_vector_t;
34 typedef typename t3d_poly_coeffs_vector_t::const_iterator
35 cit3d_poly_coeffs_vector_t;
36
37 // typedef spline::bezier_curve <real, real, 3, true, point_t> bezier_t;
38 // typedef spline::bezier_curve <real, real, 6, true, point6_t> bezier6_t;
39 typedef parametriccurves::Polynomial<real, 3, point_t> polynom_t;
40 typedef typename std::vector<polynom_t, Eigen::aligned_allocator<polynom_t> >
41 t_spline_t;
42 typedef parametriccurves::Spline<real, 3, point_t> spline_t;
43 typedef parametriccurves::spatial::ForceCurve<real> force_t;
44 typedef polynom_t::coeff_t coeff_t;
45 typedef std::pair<real, point_t> waypoint_t;
46 typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> >
47 t_waypoint_t;
48
49 // typedef spline::spline_deriv_constraint <real, real, 3, true, point_t,
50 // t_point_t> spline_deriv_constraint_t;
51 typedef parametriccurves::curve_constraints<point_t> curve_constraints_t;
52 typedef parametriccurves::curve_constraints<point6_t> curve_constraints6_t;
53 /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
54
55 // EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
56 // EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier6_t)
57 EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynom_t)
58 EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_t)
59
60 EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
61 // EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t)
62
63 namespace parametriccurves {
64 using namespace boost::python;
65 template <typename PointList, typename T_Point>
66 T_Point vectorFromEigenArray(const PointList& array) {
67 T_Point res;
68 for (int i = 0; i < array.cols(); ++i) res.push_back(array.col(i));
69 return res;
70 }
71 /*
72 template <typename Bezier, typename PointList, typename T_Point>
73 Bezier* wrapBezierConstructorTemplate(const PointList& array, const real lb =
74 0., const real ub =1.)
75 {
76 T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
77 return new Bezier(asVector.begin(), asVector.end(), lb, ub);
78 }
79
80 template <typename Bezier, typename PointList, typename T_Point, typename
81 CurveConstraints> Bezier* wrapBezierConstructorConstraintsTemplate(const
82 PointList& array, const CurveConstraints& constraints, const real lb = 0., const
83 real ub =1.)
84 {
85 T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
86 return new Bezier(asVector.begin(), asVector.end(), constraints, lb, ub);
87 }
88 */
89 /*3D constructors */
90 /*
91 bezier_t* wrapBezierConstructor(const point_list_t& array)
92 {
93 return wrapBezierConstructorTemplate<bezier_t, point_list_t,
94 t_point_t>(array) ;
95 }
96 bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real lb,
97 const real ub)
98 {
99 return wrapBezierConstructorTemplate<bezier_t, point_list_t,
100 t_point_t>(array, lb, ub) ;
101 }
102 bezier_t* wrapBezierConstructorConstraints(const point_list_t& array, const
103 curve_constraints_t& constraints)
104 {
105 return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t,
106 t_point_t, curve_constraints_t>(array, constraints) ;
107 }
108 bezier_t* wrapBezierConstructorBoundsConstraints(const point_list_t& array,
109 const curve_constraints_t& constraints, const real lb, const real ub)
110 {
111 return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t,
112 t_point_t, curve_constraints_t>(array, constraints, lb, ub) ;
113 }
114 */
115 /*END 3D constructors */
116 /*6D constructors */
117 /*
118 bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
119 {
120 return wrapBezierConstructorTemplate<bezier6_t, point_list6_t,
121 t_point6_t>(array) ;
122 }
123 bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real
124 lb, const real ub)
125 {
126 return wrapBezierConstructorTemplate<bezier6_t, point_list6_t,
127 t_point6_t>(array, lb, ub) ;
128 }
129 bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const
130 curve_constraints6_t& constraints)
131 {
132 return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t,
133 t_point6_t, curve_constraints6_t>(array, constraints) ;
134 }
135 bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array,
136 const curve_constraints6_t& constraints, const real lb, const real ub)
137 {
138 return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t,
139 t_point6_t, curve_constraints6_t>(array, constraints, lb, ub) ;
140 }
141 */
142 /*END 6D constructors */
143
144 1 polynom_t* wrapSplineConstructor(const coeff_t& array) {
145
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 return new polynom_t(array, 0., 1.);
146 }
147
148 ✗ t_waypoint_t getWayPoints(const coeff_t& array,
149 const time_waypoints_t& time_wp) {
150 ✗ t_waypoint_t res;
151 ✗ for (int i = 0; i < array.cols(); ++i)
152 ✗ res.push_back(std::make_pair(time_wp(i), array.col(i)));
153 ✗ return res;
154 }
155
156 template <typename BezierType, int dim>
157 Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> wayPointsToList(
158 const BezierType& self) {
159 typedef typename BezierType::t_point_t t_point;
160 typedef typename BezierType::t_point_t::const_iterator cit_point;
161 const t_point& wps = self.waypoints();
162 Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> res(dim, wps.size());
163 int col = 0;
164 for (cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++col)
165 res.block<dim, 1>(0, col) = *cit;
166 return res;
167 }
168 ✗ void spline_from_waypoints(spline_t& self, const coeff_t& array,
169 const time_waypoints_t& time_wp) {
170 ✗ t_waypoint_t wps = getWayPoints(array, time_wp);
171 ✗ self.createSplineFromWayPoints(wps.begin(), wps.end());
172 ✗ return;
173 }
174
175 ✗ spline_t* spline_by_concatenation_constructor(const bp::list& list_splines) {
176 ✗ t_spline_t subSplines;
177 ✗ subSplines.clear();
178 ✗ for (int i = 0; i < len(list_splines); ++i) {
179 ✗ spline_t _sp = bp::extract<spline_t>(list_splines[i]);
180 ✗ const t_spline_t& _vec_subspline = _sp.getSubsplines();
181 ✗ subSplines.insert(subSplines.end(), _vec_subspline.begin(),
182 _vec_subspline.end());
183 }
184 ✗ return new spline_t(subSplines);
185 }
186
187
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 spline_t* wrapExactCubicConstructorvoid() { return new spline_t(); }
188
189 ✗ spline_t* wrapExactCubicConstructorPolySequence(
190 const bp::list& list_polynomials, const time_vector_t& time_vector) {
191 typedef std::vector<t_point_t, Eigen::aligned_allocator<t_point_t> >
192 t3d_poly_coeffs_vector_t;
193 ✗ t3d_poly_coeffs_vector_t poly_coeffs_vector;
194 ✗ t_spline_t subSplines;
195 ✗ subSplines.clear();
196
197 ✗ assert(time_vector.size() == len(list_polynomials) + 1);
198 ✗ for (int i = 0; i < len(list_polynomials); ++i) {
199 ✗ subSplines.push_back(polynom_t(bp::extract<coeff_t>(list_polynomials[i]),
200 ✗ time_vector[i], time_vector[i + 1]));
201 // time_vector[i], time_vector[i+1]));
202 }
203 ✗ return new spline_t(subSplines);
204 }
205
206
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 force_t* wrapForceCurveConstructorvoid() { return new force_t(); }
207
208 ✗ force_t* wrapForceCurveConstructorSplines(const spline_t& linear_part,
209 const spline_t& ang_part) {
210 ✗ return new force_t(linear_part, ang_part);
211 }
212
213 ✗ void spline_from_waypoints_constr(spline_t& self, const coeff_t& array,
214 const time_waypoints_t& time_wp,
215 const curve_constraints_t& constraints) {
216 ✗ t_waypoint_t wps = getWayPoints(array, time_wp);
217 ✗ self.createSplineFromWayPointsConstr(wps.begin(), wps.end(), constraints);
218 ✗ return;
219 }
220
221 ✗ point_t get_init_vel(const curve_constraints_t& c) { return c.init_vel; }
222
223 ✗ point_t get_init_acc(const curve_constraints_t& c) { return c.init_acc; }
224
225 ✗ point_t get_end_vel(const curve_constraints_t& c) { return c.end_vel; }
226
227 ✗ point_t get_end_acc(const curve_constraints_t& c) { return c.end_acc; }
228
229 1 void set_init_vel(curve_constraints_t& c, const point_t& val) {
230 1 c.init_vel = val;
231 1 }
232
233 ✗ void set_init_acc(curve_constraints_t& c, const point_t& val) {
234 ✗ c.init_acc = val;
235 }
236
237 ✗ void set_end_vel(curve_constraints_t& c, const point_t& val) {
238 ✗ c.end_vel = val;
239 }
240
241 ✗ void set_end_acc(curve_constraints_t& c, const point_t& val) {
242 ✗ c.end_acc = val;
243 }
244
245
2/4
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 4 BOOST_PYTHON_MODULE(libparametric_curves_pywrap) {
246 /** BEGIN eigenpy init**/
247
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 bp::import("eigenpy");
248
249 2 eigenpy::enableEigenPySpecific<point_t>();
250 2 eigenpy::enableEigenPySpecific<ret_point_t>();
251 2 eigenpy::enableEigenPySpecific<point_list_t>();
252 2 eigenpy::enableEigenPySpecific<point6_t>();
253 2 eigenpy::enableEigenPySpecific<ret_point6_t>();
254 2 eigenpy::enableEigenPySpecific<point_list6_t>();
255 2 eigenpy::enableEigenPySpecific<coeff_t>();
256 /*eigenpy::exposeAngleAxis();
257 eigenpy::exposeQuaternion();*/
258 /** END eigenpy init**/
259
260 /** BEGIN bezier curve 6**/
261 /*
262 class_<bezier6_t>
263 ("bezier6", no_init)
264 .def("__init__", make_constructor(&wrapBezierConstructor6))
265 .def("__init__", make_constructor(&wrapBezierConstructorBounds6))
266 //.def("__init__",
267 make_constructor(&wrapBezierConstructor6Constraints))
268 //.def("__init__",
269 make_constructor(&wrapBezierConstructorBounds6Constraints)) .def("min",
270 &bezier6_t::min) .def("max", &bezier6_t::max) .def("__call__",
271 &bezier6_t::operator()) .def("derivate", &bezier6_t::derivate)
272 .def("compute_derivate", &bezier6_t::compute_derivate)
273 .def("compute_primitive", &bezier6_t::compute_primitive)
274 .def("waypoints", &wayPointsToList<bezier6_t,6>)
275 .def_readonly("degree", &bezier6_t::degree_)
276 .def_readonly("nbWaypoints", &bezier6_t::size_)
277 ;
278 */
279 /** END bezier curve**/
280
281 /** BEGIN bezier curve**/
282 /*
283 class_<bezier_t>
284 ("bezier", no_init)
285 .def("__init__", make_constructor(&wrapBezierConstructor))
286 .def("__init__", make_constructor(&wrapBezierConstructorBounds))
287 .def("__init__", make_constructor(&wrapBezierConstructorConstraints))
288 .def("__init__",
289 make_constructor(&wrapBezierConstructorBoundsConstraints)) .def("min",
290 &bezier_t::min) .def("max", &bezier_t::max) .def("__call__",
291 &bezier_t::operator()) .def("derivate", &bezier_t::derivate)
292 .def("compute_derivate", &bezier_t::compute_derivate)
293 .def("compute_primitive", &bezier_t::compute_primitive)
294 .def("waypoints", &wayPointsToList<bezier_t,3>)
295 .def_readonly("degree", &bezier_t::degree_)
296 .def_readonly("nbWaypoints", &bezier_t::size_)
297 ;
298 */
299 /** END bezier curve**/
300
301 /** BEGIN spline curve function**/
302
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 class_<polynom_t>("polynomial",
303 2 init<const polynom_t::coeff_t, const real, const real>())
304
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 .def("__init__", make_constructor(&wrapSplineConstructor))
305
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("min", &polynom_t::tmin)
306
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("max", &polynom_t::tmax)
307
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("__call__", &polynom_t::operator())
308
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("derivate", &polynom_t::derivate);
309 /** END cubic function**/
310
311 /** BEGIN spline curve**/
312 2 class_<spline_t>("spline", no_init)
313
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 .def("__init__", make_constructor(&wrapExactCubicConstructorvoid))
314
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 .def("__init__", make_constructor(&wrapExactCubicConstructorPolySequence))
315
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 .def("__init__", make_constructor(&spline_by_concatenation_constructor))
316
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("min", &spline_t::tmin,
317 ✗ bp::return_value_policy<bp::return_by_value>())
318
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("max", &spline_t::tmax,
319 ✗ bp::return_value_policy<bp::return_by_value>())
320
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("__call__", &spline_t::operator(),
321 ✗ bp::return_value_policy<bp::return_by_value>())
322
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("derivate", &spline_t::derivate,
323 ✗ bp::return_value_policy<bp::return_by_value>())
324
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("create_spline_from_waypoints", &spline_from_waypoints,
325
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 boost::python::args("waypoints", "time vector"),
326 "Creates a cubic spline from a set of way points")
327
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("create_spline_from_waypoints_constr", &spline_from_waypoints_constr,
328
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 boost::python::args("waypoints", "time vector", "constraints"),
329 "Creates a spline from a set of way points and constraints")
330
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("load_from_file", &spline_t::loadFromFile,
331
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 boost::python::args("filename"), "Loads *this")
332
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("save_to_file", &spline_t::saveToFile,
333
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 boost::python::args("filename"), "Saves *this");
334 /** BEGIN force curve**/
335
336 2 class_<force_t>("forcecurve", no_init)
337
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 .def("__init__", make_constructor(&wrapForceCurveConstructorvoid))
338
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 .def("__init__", make_constructor(&wrapForceCurveConstructorSplines))
339
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("min", &force_t::tmin,
340 ✗ bp::return_value_policy<bp::return_by_value>())
341
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("max", &force_t::tmax,
342 ✗ bp::return_value_policy<bp::return_by_value>())
343
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("__call__", &force_t::operator(),
344 ✗ bp::return_value_policy<bp::return_by_value>())
345
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("derivate", &force_t::derivate,
346 ✗ bp::return_value_policy<bp::return_by_value>())
347
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("set_motion_vector", &force_t::setMotionVector,
348
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 boost::python::args("motionvector"),
349 "sets motion vector for derivate")
350
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("load_from_file", &force_t::loadFromFile,
351
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 boost::python::args("filename"), "Loads *this")
352
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .def("save_to_file", &force_t::saveToFile,
353
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 boost::python::args("filename"), "Saves *this");
354
355 /** END force curve**/
356
357 /** END bezier curve**/
358
359 /** BEGIN curve constraints**/
360
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 4 class_<curve_constraints_t>("curve_constraints", init<>())
361
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .add_property("init_vel", &get_init_vel, &set_init_vel)
362
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .add_property("init_acc", &get_init_acc, &set_init_acc)
363
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .add_property("end_vel", &get_end_vel, &set_end_vel)
364
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 .add_property("end_acc", &get_end_acc, &set_end_acc);
365 /** END curve constraints**/
366 2 }
367 } // namespace parametriccurves
368