Merge pull request #41 from pFernbach/topic/fix_10

Add piecewise::load_from_text_file

include/curves/piecewise_curve.h

@@ -12,6 +12,8 @@
 #include "curve_conversion.h"
 #include <boost/smart_ptr/shared_ptr.hpp>
 #include <boost/serialization/vector.hpp>
+#include <fstream>
+#include <sstream>
 
 namespace curves {
 /// \class PiecewiseCurve.
@@ -362,6 +364,125 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deri
     return piecewise_res;
   }
 
+  /**
+   * @brief load_piecewise_from_text_file build a piecewise polynomial from a list of discrete points read from a file.
+   * The file should contains one points per line, optionally with it's derivative and second derivatives.
+   * Each lines should then contains dim, 2*dim or 3*dim values
+   * @param filename the (absolute) name of the file to load
+   * @param dt the time step between each points in the file
+   * @param dim the dimension of the curve
+   * @return a piecewise curves containing polynomial connectiong all the points in the file
+   */
+  template <typename Polynomial>
+  static piecewise_curve_t load_piecewise_from_text_file(const std::string& filename, const time_t dt, const size_t dim){
+    if(dim <= 0)
+      throw std::invalid_argument("The dimension should be strictly positive.");
+    if(dt <= 0.)
+      throw std::invalid_argument("The time step should be strictly positive.");
+
+    piecewise_curve_t piecewise_res;
+    std::ifstream file;
+    file.open(filename.c_str());
+    point_t last_pos = point_t::Zero(dim),
+        last_vel = point_t::Zero(dim),
+        last_acc = point_t::Zero(dim),
+        new_pos = point_t::Zero(dim),
+        new_vel = point_t::Zero(dim),
+        new_acc = point_t::Zero(dim);
+    bool use_vel, use_acc;
+    std::string line;
+    // read first line to found out if we use velocity / acceleration :
+    std::getline(file, line);
+    std::istringstream iss_length(line);
+    const size_t length = std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
+    if(length == dim){
+      use_vel = false;
+      use_acc = false;
+    }else if(length == dim * 2){
+      use_vel = true;
+      use_acc = false;
+    }else if(length == dim * 3){
+      use_vel = true;
+      use_acc = true;
+    }else{
+      std::stringstream error;
+      error<<"The first line of the file shold contains either " << dim << ", " << dim * 2 << " or " << dim * 3 <<
+             "values, got : " << length;
+      throw std::invalid_argument(error.str());
+    }
+    // initialize the first points of the trajectory:
+    num_t val;
+    std::istringstream iss(line);
+    for(size_t i = 0 ; i < dim ; ++i){
+      iss >> val;
+      last_pos[i] = val;
+    }
+    if(use_vel){
+      for(size_t i = 0 ; i < dim ; ++i){
+        iss >> val;
+        last_vel[i] = val;
+      }
+    }
+    if(use_acc){
+      for(size_t i = 0 ; i < dim ; ++i){
+        iss >> val;
+        last_acc[i] = val;
+      }
+    }
+
+    size_t current_length;
+    size_t line_id = 0;
+    // parse all lines of the file:
+    while (std::getline(file, line))
+    {
+        ++line_id;
+        std::istringstream iss_length(line);
+        current_length = std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
+        if(current_length != length){
+          std::stringstream error;
+          error<<"Cannot parse line " << line_id << " got " << current_length << " values instead of " << length;
+          throw std::invalid_argument(error.str());
+        }
+        std::istringstream iss(line);
+        // parse the points values from the file:
+        for(size_t i = 0 ; i < dim ; ++i){
+          iss >> val;
+          new_pos[i] = val;
+        }
+        if(use_vel){
+          for(size_t i = 0 ; i < dim ; ++i){
+            iss >> val;
+            new_vel[i] = val;
+          }
+        }
+        if(use_acc){
+          for(size_t i = 0 ; i < dim ; ++i){
+            iss >> val;
+            new_acc[i] = val;
+          }
+        }
+        // append a new curves connectiong this points
+        if(use_acc){
+          piecewise_res.add_curve(Polynomial(last_pos, last_vel, last_acc,
+                                             new_pos, new_vel, new_acc,
+                                             dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
+        }else if(use_vel){
+          piecewise_res.add_curve(Polynomial(last_pos, last_vel,
+                                             new_pos, new_vel,
+                                             dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
+        }else{
+          piecewise_res.add_curve(Polynomial(last_pos, new_pos,
+                                             dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
+        }
+        last_pos = new_pos;
+        last_vel = new_vel;
+        last_acc = new_acc;
+    }
+
+    file.close();
+    return piecewise_res;
+  }
+
  private:
   /// \brief Get index of the interval corresponding to time t for the interpolation.
   /// \param t : time where to look for interval.

python/curves/curves_python.cpp

@@ -272,6 +272,9 @@ static piecewise_t discretPointToPolynomialC2(const pointX_list_t& points, const
   return piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(
       points_list, points_derivative_list, points_second_derivative_list, time_points_list);
 }
+static piecewise_t load_piecewise_from_text_file(const std::string& filename, const real dt, const std::size_t dim){
+  return piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename, dt, dim);
+}
 
 
 void addFinalPointC0(piecewise_t& self, const pointX_t& end, const real time) {
@@ -804,6 +807,11 @@ BOOST_PYTHON_MODULE(curves) {
            "given points derivative and second derivative values. The created piecewise is C2 continuous.",
            args("points", "points_derivative", "points_second_derivative", "time_points"))
       .staticmethod("FromPointsList")
+      .def("FromPointsFile", &load_piecewise_from_text_file, args("filename", "dt", "dimension"),
+           "Create a piecewise-polynomial connecting exactly all the points in the given text file."
+           "The file should contains one points per line, optionally with it's derivative and second derivatives."
+           "Each lines should thus contains dim, 2*dim or 3*dim values")
+      .staticmethod("FromPointsFile")
       .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",

tests/Main.cpp

@@ -1448,6 +1448,113 @@ void PiecewisePolynomialCurveFromDiscretePoints(bool& error) {
   }
 }
 
+void PiecewisePolynomialCurveFromFile(bool& error){
+  std::string filename_pos(TEST_DATA_PATH "discrete_points_pos.txt");
+  std::string filename_vel(TEST_DATA_PATH "discrete_points_vel.txt");
+  std::string filename_acc(TEST_DATA_PATH "discrete_points_acc.txt");
+  std::string filename_error(TEST_DATA_PATH "discrete_points_error.txt");
+
+  piecewise_t c_pos = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_pos, 0.01, 3);
+  if(c_pos.min() != 0.){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0" << std::endl;
+    error = true;
+  }
+  if(c_pos.max() != 0.03){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 0.03" << std::endl;
+    error = true;
+  }
+  pointX_t p0(3), p2(3);
+  p0<<-0.003860389372941039, 0.0012353625242474164, 0.009005041639999767;
+  p2<<-0.0028803627898293283, 0.0011918668401150736, 0.009005041639999767;
+  if(! c_pos(0.).isApprox(p0)){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
+    error = true;
+  }
+  if(! c_pos(0.02).isApprox(p2)){
+   std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
+   error = true;
+  }
+
+  piecewise_t c_vel = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_vel, 0.05, 3);
+  if(c_pos.min() != 0.){
+   std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0" << std::endl;
+   error = true;
+  }
+  if(! QuasiEqual(c_vel.max(), 0.15)){
+   std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 0.15" << std::endl;
+   error = true;
+  }
+  pointX_t p3(3);
+  p3<<0.2968141884672718, 0.0012916907964522569, 0.00951023474821927;
+  if(! c_vel(0.).isApprox(p0)){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
+    error = true;
+  }
+  if(! c_vel(0.15).isApprox(p3)){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
+    error = true;
+  }
+  if(! c_vel.derivate(0., 1).isZero()){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, c_vel derivative at 0. should be null" << std::endl;
+    error = true;
+  }
+  if(! c_vel.derivate(0.1, 1).isZero()){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, c_vel derivative at 0.1 should be null" << std::endl;
+    error = true;
+  }
+
+  piecewise_t c_acc = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_acc, 0.001, 3);
+  if(c_acc.min() != 0.){
+  std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0" << std::endl;
+  error = true;
+  }
+  if(! QuasiEqual(c_acc.max(), 7.85)){
+  std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 7.85" << std::endl;
+  error = true;
+  }
+  if(! c_acc(0.).isApprox(p0)){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
+    error = true;
+  }
+  pointX_t p5200(3);
+  p5200<<0.30273356072723845, -0.07619420199174821, 0.010015348526727433;
+  if(! c_acc(5.2).isApprox(p5200)){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match" << std::endl;
+    error = true;
+  }
+  if(! c_acc.derivate(0., 1).isZero()){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc derivative at 0 should be null" << std::endl;
+    error = true;
+  }
+  if(! c_acc.derivate(0.5, 1).isZero()){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc derivative should at 0.5 be null" << std::endl;
+    error = true;
+  }
+  if(! c_acc.derivate(0., 2).isZero()){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc second derivative at 0 should be null" << std::endl;
+    error = true;
+  }
+  if(! c_acc.derivate(5., 2).isZero()){
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc second derivative at 5 should be null" << std::endl;
+    error = true;
+  }
+
+  try {
+    piecewise_t c_error = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_acc, 0.01, 4);
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, dimension do not match, an error should be raised" << std::endl;
+    error = true;
+  } catch (std::invalid_argument e) {
+  }
+  try {
+    piecewise_t c_error = piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_error, 0.01, 3);
+    std::cout << "PiecewisePolynomialCurveFromFile, Error, discrete_points_error should not be parsed correctly" << std::endl;
+    error = true;
+  } catch (std::invalid_argument e) {
+  }
+
+}
+
+
 void serializationCurvesTest(bool& error) {
   try {
     std::string errMsg1("in serializationCurveTest, Error While serializing Polynomial : ");
@@ -2769,6 +2876,7 @@ int main(int /*argc*/, char** /*argv[]*/) {
   CubicHermitePairsPositionDerivativeTest(error);
   piecewiseCurveTest(error);
   PiecewisePolynomialCurveFromDiscretePoints(error);
+  PiecewisePolynomialCurveFromFile(error);
   toPolynomialConversionTest(error);
   cubicConversionTest(error);
   curveAbcDimDynamicTest(error);

tests/data/discrete_points_error.txt

+-0.003860389372941039 0.0012353625242474164 0.009005041639999767
+-0.003367975609860228 0.0012135142149832263 0.009005041639999767
+-0.0028803627898293283 0.0011918668401150736
+-0.0023975258947521072 0.0011704190312931094 0.009005041639999767

tests/data/discrete_points_pos.txt

+-0.003860389372941039 0.0012353625242474164 0.009005041639999767
+-0.003367975609860228 0.0012135142149832263 0.009005041639999767
+-0.0028803627898293283 0.0011918668401150736 0.009005041639999767
+-0.0023975258947521072 0.0011704190312931094 0.009005041639999767

tests/data/discrete_points_vel.txt

+-0.003860389372941039 0.0012353625242474164 0.009005041639999767 0 0 0
+-0.003367975609860228 0.0012135142149832263 0.009005041639999767 0 0 0
+0.2968095500861058 0.0013266763658670213 0.00951023474821927 0 0 0 
+0.2968141884672718 0.0012916907964522569 0.00951023474821927 0 0 0