added evaluation of variable curve

include/curves/linear_variable.h

@@ -11,6 +11,7 @@
 #define _CLASS_LINEAR_VARIABLE
 
 #include "curve_abc.h"
+#include "bezier_curve.h"
 
 #include "MathDefs.h"
 
@@ -21,7 +22,7 @@
 
 namespace curves
 {
-template <int Dim, typename Numeric=double>
+template <int Dim, typename Numeric=double, bool Safe=true>
 struct linear_variable
 {
     typedef Eigen::Matrix<Numeric, Dim, Dim> matrix_dim_t;
@@ -40,6 +41,8 @@ struct linear_variable
     {
         if(isZero())
             return c();
+        if(Safe && B().cols() != val.rows() )
+            throw std::length_error("Cannot evaluate linear variable, variable value does not have the correct dimension");
         return B() * val + c();
     }
 
@@ -137,6 +140,16 @@ linear_variable<D,N> operator/(const linear_variable<D,N>& w,const double k)
     linear_variable<D,N> res(w.B(), w.c());
     return res/=k;
 }
+
+template <typename BezierFixed, typename BezierLinear, typename X >
+BezierFixed evaluateLinear(const BezierLinear& bIn, const X x)
+{
+    typename BezierFixed::t_point_t fixed_wps;
+    for (typename BezierLinear::cit_point_t cit = bIn.waypoints().begin(); cit != bIn.waypoints().end(); ++cit)
+        fixed_wps.push_back(cit->operator()(x));
+    return BezierFixed(fixed_wps.begin(),fixed_wps.end(), bIn.T_min_,bIn.T_max_);
+}
+
 } // namespace curves
 #endif //_CLASS_LINEAR_VARIABLE
 

include/curves/optimization/definitions.h

@@ -71,7 +71,6 @@ struct problem_definition
 
 
     constraint_flag flag;
-    //derivative_flag costFlag;
     point_t start;
     point_t end;
     curve_constraints_t curveConstraints;

python/curves_python.cpp

@@ -296,6 +296,20 @@ namespace curves
       c.end_jerk = val;
   }
 
+  matrix_pair* bezier_linear_variable_t_operator_call(const bezier_linear_variable_t* b, const double t)
+  {
+      bezier_linear_variable_t::point_t p = b->operator ()(t);
+      matrix_pair* res = new matrix_pair;
+      res->res = std::make_pair(p.B(),p.c());
+      return res;
+  }
+
+  bezier_t* bezier_linear_variable_t_evaluate(const bezier_linear_variable_t* b, const pointX_t& x)
+  {
+     return new bezier_t(evaluateLinear<bezier_t, bezier_linear_variable_t>(*b, x));
+  }
+
+
   /* End wrap exact cubic spline */
 
 
@@ -384,6 +398,8 @@ namespace curves
         .def("min", &bezier_linear_variable_t::min)
         .def("max", &bezier_linear_variable_t::max)
         //.def("__call__", &bezier_linear_control_t::operator())
+        .def("__call__", &bezier_linear_variable_t_operator_call, bp::return_value_policy<bp::manage_new_object>())
+        .def("evaluate", &bezier_linear_variable_t_evaluate, bp::return_value_policy<bp::manage_new_object>())
         .def("derivate", &bezier_linear_variable_t::derivate)
         .def("compute_derivate", &bezier_linear_variable_t::compute_derivate)
         .def("compute_primitive", &bezier_linear_variable_t::compute_primitive)

python/test/optimization.py

 import os
 import unittest
+import eigenpy
+eigenpy.switchToNumpyArray()
 
-from numpy import array_equal, isclose, matrix, random, array
+from numpy import array_equal, isclose, matrix, random, array, zeros
 from numpy.linalg import norm
 
 from curves import (bezier, bezier_from_hermite, bezier_from_polynomial, cubic_hermite_spline, curve_constraints,
@@ -11,8 +13,6 @@ from curves import (bezier, bezier_from_hermite, bezier_from_polynomial, cubic_h
 
 from curves.optimization import(constraint_flag, integral_cost_flag, quadratic_problem, setup_control_points, generate_problem, generate_integral_problem, problemData, problemDefinition)
 
-import eigenpy
-eigenpy.switchToNumpyArray()
 
 #generate problem data
 
@@ -23,6 +23,15 @@ c.init_acc = matrix([0., 1., -1.]).transpose()
 c.end_acc = matrix([0., 0., 0]).transpose()
 
 pD = problemDefinition()
+pD.start
 pD.curveConstraints = c
 pD.start = array([[0.,0.,0.]]).T
 pD.end = array([[1.,1.,1.]]).T
+pD.flag = constraint_flag.INIT_VEL | constraint_flag.INIT_POS
+
+#~ generate_integral_problem(pD,integral_cost_flag.ACCELERATION)
+
+#generate problem
+problem = setup_control_points(pD)
+bezier = problem.bezier()
+res = bezier.evaluate(array([zeros(12)]).T)(0.)