Merge pull request #56 from pFernbach/topic/pickle

Add serialization of curve_constraints

include/curves/curve_constraint.h

@@ -12,13 +12,14 @@
 #define _CLASS_CURVE_CONSTRAINT
 
 #include "MathDefs.h"
-
+#include "serialization/archive.hpp"
+#include "serialization/eigen-matrix.hpp"
 #include <functional>
 #include <vector>
 
 namespace curves {
 template <typename Point>
-struct curve_constraints {
+struct curve_constraints : serialization::Serializable {
   typedef Point point_t;
   curve_constraints(const size_t dim = 3)
       : init_vel(point_t::Zero(dim)),
@@ -38,6 +39,25 @@ struct curve_constraints {
         end_jerk(other.end_jerk),
         dim_(other.dim_) {}
 
+  /// \brief Check if actual curve_constraints and other are equal.
+  /// \param other : the other curve_constraints to check.
+  /// \return true if the two curve_constraints are equals.
+  virtual bool operator==(const curve_constraints& other) const {
+    return  dim_ == other.dim_ &&
+        init_vel == other.init_vel &&
+        init_acc == other.init_acc &&
+        init_jerk == other.init_jerk &&
+        end_vel == other.end_vel &&
+        end_acc == other.end_acc &&
+        end_jerk == other.end_jerk;
+  }
+
+  /// \brief Check if actual curve_constraint and other are different.
+  /// \param other : the other curve_constraint to check.
+  /// \return true if the two curve_constraint are different.
+  virtual bool operator!=(const curve_constraints& other) const { return !(*this == other); }
+
+
   ~curve_constraints() {}
   point_t init_vel;
   point_t init_acc;
@@ -46,6 +66,22 @@ struct curve_constraints {
   point_t end_acc;
   point_t end_jerk;
   size_t dim_;
+
+   // Serialization of the class
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version) {
+    if (version) {
+      // Do something depending on version ?
+    }
+    ar& boost::serialization::make_nvp("init_vel", init_vel);
+    ar& boost::serialization::make_nvp("init_acc", init_acc);
+    ar& boost::serialization::make_nvp("init_jerk", init_jerk);
+    ar& boost::serialization::make_nvp("end_vel", end_vel);
+    ar& boost::serialization::make_nvp("end_acc", end_acc);
+    ar& boost::serialization::make_nvp("end_jerk", end_jerk);
+    ar& boost::serialization::make_nvp("dim", dim_);
+  }
 };
 }  // namespace curves
 #endif  //_CLASS_CUBICZEROVELACC

python/CMakeLists.txt

@@ -11,3 +11,4 @@ ADD_PYTHON_UNIT_TEST("python-minjerk" "python/test/test-minjerk.py" "python")
 ADD_PYTHON_UNIT_TEST("python-optimization" "python/test/optimization.py" "python")
 ADD_PYTHON_UNIT_TEST("python-notebook" "python/test/notebook.py" "python")
 ADD_PYTHON_UNIT_TEST("python-registration" "python/test/registration.py" "python")
+ADD_PYTHON_UNIT_TEST("python-curve-constraints" "python/test/test-curve-constraints.py" "python")

python/curves/curves_python.cpp

@@ -998,13 +998,30 @@ BOOST_PYTHON_MODULE(curves) {
 
   /** END cubic_hermite_spline **/
   /** BEGIN curve constraints**/
-  class_<curve_constraints_t>("curve_constraints", init<int>())
+  class_<curve_constraints_t>("curve_constraints", init<>())
+      .def(bp::init<int>(bp::arg("dimension"), "Init with a given dimension."))
       .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);
+      .add_property("end_jerk", &get_end_jerk, &set_end_jerk)
+      .def("__eq__", &curve_constraints_t::operator==)
+      .def("__ne__", &curve_constraints_t::operator!=)
+      .def("saveAsText", &curve_constraints_t::saveAsText<curve_constraints_t>, bp::args("filename"),
+           "Saves *this inside a text file.")
+      .def("loadFromText", &curve_constraints_t::loadFromText<curve_constraints_t>, bp::args("filename"),
+           "Loads *this from a text file.")
+      .def("saveAsXML", &curve_constraints_t::saveAsXML<curve_constraints_t>, bp::args("filename", "tag_name"),
+           "Saves *this inside a XML file.")
+      .def("loadFromXML", &curve_constraints_t::loadFromXML<curve_constraints_t>,
+           bp::args("filename", "tag_name"), "Loads *this from a XML file.")
+      .def("saveAsBinary", &curve_constraints_t::saveAsBinary<curve_constraints_t>, bp::args("filename"),
+           "Saves *this inside a binary file.")
+      .def("loadFromBinary", &curve_constraints_t::loadFromBinary<curve_constraints_t>, bp::args("filename"),
+           "Loads *this from a binary file.")
+      .def_pickle(curve_pickle_suite<curve_constraints_t>());
+;
   /** END curve constraints**/
   /** BEGIN bernstein polynomial**/
   class_<bernstein_t>("bernstein", init<const unsigned int, const unsigned int>())

python/test/test-curve-constraints.py

+# Copyright (c) 2020, CNRS
+# Authors: Pierre Fernbach <pfernbac@laas.fr>
+
+import unittest
+import curves
+from curves import curve_constraints
+import numpy as np
+import pickle
+from numpy import array, isclose, array_equal
+
+
+class CurveConstraintsTest(unittest.TestCase):
+    def test_operator_equal(self):
+        c = curve_constraints(3)
+        c.init_vel = array([[0., 1., 1.]]).transpose()
+        c.end_vel = array([[0., -1., 1.]]).transpose()
+        c.init_acc = array([[0., 1., -1.]]).transpose()
+        c.end_acc = array([[0., 100., 1.]]).transpose()
+        c.init_jerk = array([[2., 4., 1.]]).transpose()
+        c.end_jerk = array([[-1., 2., 7.]]).transpose()
+
+        c2 = curve_constraints(3)
+        c2.init_vel = array([[0., 1., 1.]]).transpose()
+        c2.end_vel = array([[0., -1., 1.]]).transpose()
+        c2.init_acc = array([[0., 1., -1.]]).transpose()
+        c2.end_acc = array([[0., 100., 1.]]).transpose()
+        c2.init_jerk = array([[2., 4., 1.]]).transpose()
+        c2.end_jerk = array([[-1., 2., 7.]]).transpose()
+        self.assertTrue(c == c2)
+        c2.init_vel = array([[1., 1., 1.]]).transpose()
+        self.assertTrue(c != c2)
+    
+    def test_serialization(self):
+        c = curve_constraints(3)
+        c.init_vel = array([[0., 1., 1.]]).transpose()
+        c.end_vel = array([[0., -1., 1.]]).transpose()
+        c.init_acc = array([[0., 1., -1.]]).transpose()
+        c.end_acc = array([[0., 100., 1.]]).transpose()
+        c.init_jerk = array([[2., 4., 1.]]).transpose()
+        c.end_jerk = array([[-1., 2., 7.]]).transpose()
+
+        c.saveAsText("curve_constraints.txt")
+        c.saveAsXML("curve_constraints.xml", "curve_constraints")
+        c.saveAsBinary("curve_constraints")
+
+        c_txt = curve_constraints()
+        c_txt.loadFromText("curve_constraints.txt")
+        self.assertEqual(c, c_txt)
+
+        c_xml = curve_constraints()
+        c_xml.loadFromXML("curve_constraints.xml", "curve_constraints")
+        self.assertEqual(c, c_xml)
+
+        c_bin = curve_constraints()
+        c_bin.loadFromBinary("curve_constraints")
+        self.assertEqual(c, c_bin)
+        
+        c_pickled = pickle.dumps(c)
+        c_from_pickle = pickle.loads(c_pickled)
+        self.assertEqual(c_from_pickle, c)
+
+
+if __name__ == '__main__':
+    unittest.main()

tests/CMakeLists.txt

@@ -7,6 +7,7 @@ SET(${PROJECT_NAME}_TESTS
   test-sinusoidal
   test-sinusoidal-serialization
   test-minjerk
+  test-curve-constraints
   )
 
 FOREACH(TEST ${${PROJECT_NAME}_TESTS})

tests/test-curve-constraints.cpp

+#define BOOST_TEST_MODULE test_curve_constraints
+
+#include "curves/fwd.h"
+#include "curves/bezier_curve.h"
+#include <boost/test/included/unit_test.hpp>
+
+using namespace curves;
+
+BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
+
+BOOST_AUTO_TEST_CASE(copy_constructor) {
+  bezier_t::curve_constraints_t constraints(3);
+  constraints.init_vel = point3_t(-1, -1, -1);
+  constraints.init_acc = point3_t(-2, -2, -2);
+  constraints.init_jerk = point3_t(1, 2, 3);
+  constraints.end_vel = point3_t(-10, -10, -10);
+  constraints.end_acc = point3_t(-20, -20, -20);
+  constraints.end_jerk = point3_t(-1, -2, -3);
+
+  bezier_t::curve_constraints_t constraints2(constraints);
+  BOOST_CHECK_EQUAL(constraints.dim_, constraints2.dim_);
+  BOOST_CHECK_EQUAL(constraints.init_vel, constraints2.init_vel);
+  BOOST_CHECK_EQUAL(constraints.init_acc, constraints2.init_acc);
+  BOOST_CHECK_EQUAL(constraints.init_jerk, constraints2.init_jerk);
+  BOOST_CHECK_EQUAL(constraints.end_vel, constraints2.end_vel);
+  BOOST_CHECK_EQUAL(constraints.end_vel, constraints2.end_vel);
+  BOOST_CHECK_EQUAL(constraints.end_jerk, constraints2.end_jerk);
+}
+
+BOOST_AUTO_TEST_CASE(operator_equal) {
+  bezier_t::curve_constraints_t constraints(3);
+  constraints.init_vel = point3_t(-1, -1, -1);
+  constraints.init_acc = point3_t(-2, -2, -2);
+  constraints.init_jerk = point3_t(1, 2, 3);
+  constraints.end_vel = point3_t(-10, -10, -10);
+  constraints.end_acc = point3_t(-20, -20, -20);
+  constraints.end_jerk = point3_t(-1, -2, -3);
+
+  bezier_t::curve_constraints_t constraints2(constraints);
+  BOOST_CHECK(constraints == constraints2);
+  constraints2.init_vel = point3_t(1, 1, 1);
+  BOOST_CHECK(constraints != constraints2);
+  constraints2.init_vel = constraints.init_vel;
+  constraints2.init_acc = point3_t(1, 1, 1);
+  BOOST_CHECK(constraints != constraints2);
+  constraints2.init_acc = constraints.init_acc;
+  constraints2.init_jerk = point3_t(1, 1, 1);
+  BOOST_CHECK(constraints != constraints2);
+  constraints2.init_jerk = constraints.init_jerk;
+  constraints2.end_vel = point3_t(1, 1, 1);
+  BOOST_CHECK(constraints != constraints2);
+  constraints2.end_vel = constraints.end_vel;
+  constraints2.end_acc = point3_t(1, 1, 1);
+  BOOST_CHECK(constraints != constraints2);
+  constraints2.end_acc = constraints.end_acc;
+  constraints2.end_jerk = point3_t(1, 1, 1);
+  BOOST_CHECK(constraints != constraints2);
+
+  bezier_t::curve_constraints_t constraints3(2);
+  constraints.init_vel = pointX_t(2);
+  constraints.init_vel << -1, -1;
+  constraints.init_acc = pointX_t(2);
+  constraints.init_acc <<-2, -2;
+  constraints.init_jerk = pointX_t(2);
+  constraints.init_jerk <<1, 2;
+  constraints.end_vel = pointX_t(2);
+  constraints.end_vel <<-10, -10;
+  constraints.end_acc = pointX_t(2);
+  constraints.end_acc <<-20, -20;
+  constraints.end_jerk = pointX_t(2);
+  constraints.end_jerk <<-1, -2;
+  BOOST_CHECK(constraints != constraints3);
+}
+
+
+
+BOOST_AUTO_TEST_CASE(serialization) {
+  bezier_t::curve_constraints_t constraints(3);
+  constraints.init_vel = point3_t(-1, -1, -1);
+  constraints.init_acc = point3_t(-2, -2, -2);
+  constraints.init_jerk = point3_t(1, 2, 3);
+  constraints.end_vel = point3_t(-10, -10, -10);
+  constraints.end_acc = point3_t(-20, -20, -20);
+  constraints.end_jerk = point3_t(-1, -2, -3);
+
+  std::string fileName("curve_constraints");
+
+  constraints.saveAsText<bezier_t::curve_constraints_t>(fileName + ".txt");
+  constraints.saveAsXML<bezier_t::curve_constraints_t>(fileName + ".xml", "curve_constraints");
+  constraints.saveAsBinary<bezier_t::curve_constraints_t>(fileName);
+
+  bezier_t::curve_constraints_t c_txt, c_xml, c_binary;
+  c_txt.loadFromText<bezier_t::curve_constraints_t>(fileName + ".txt");
+  c_xml.loadFromXML<bezier_t::curve_constraints_t>(fileName + ".xml", "curve_constraints");
+  c_binary.loadFromBinary<bezier_t::curve_constraints_t>(fileName);
+
+  BOOST_CHECK(constraints == c_txt);
+  BOOST_CHECK(constraints == c_xml);
+  BOOST_CHECK(constraints == c_binary);
+}
+
+BOOST_AUTO_TEST_SUITE_END()