Add more in depth testing to hpp-python

src/pyhpp/constraints/by-substitution.cc

@@ -45,6 +45,13 @@ tuple BySubstitution_solve(const BySubstitution& hs, const vector_t& q) {
 }
 
 void exposeBySubstitution() {
+  enum_<HierarchicalIterative::Status>("SolverStatus")
+      .value("ERROR_INCREASED", HierarchicalIterative::ERROR_INCREASED)
+      .value("MAX_ITERATION_REACHED",
+             HierarchicalIterative::MAX_ITERATION_REACHED)
+      .value("INFEASIBLE", HierarchicalIterative::INFEASIBLE)
+      .value("SUCCESS", HierarchicalIterative::SUCCESS);
+
   class_<BySubstitution, bases<HierarchicalIterative> >(
       "BySubstitution", init<LiegroupSpacePtr_t>())
       .def("explicitConstraintSetHasChanged",

src/pyhpp/core/static_stability_constraint_factory.py

@@ -28,6 +28,7 @@
 # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 # DAMAGE.
 
+import numpy as np
 from hpp import Transform
 from pinocchio import SE3
 
@@ -39,12 +40,10 @@ def __init__(self, problem, robot):
         self.robot = robot
 
     def _getCOM(self, com):
-        from numpy import array
-
         if com == "":
-            return array(self.robot.getCenterOfMass())
+            return np.array(self.robot.getCenterOfMass())
         else:
-            return array(self.problem.getPartialCom(com))
+            return np.array(self.problem.getPartialCom(com))
 
     # # Create static stability constraints where the robot slides on the ground,
     # # \param prefix prefix of the names of the constraint
@@ -171,14 +170,13 @@ def createAlignedCOMStabilityConstraint(
 
         # COM between feet
         result.append(prefix + "com-between-feet")
-        # TODO: verify createComBetweenFeet parameters conversion
         constraint = self.problem.createComBetweenFeet(
             result[-1],
             comName,
             leftAnkle,
             rightAnkle,
-            [0, 0, 0],
-            [0, 0, 0],
+            np.zeros(3),
+            np.zeros(3),
             "",
             x,
             [True] * 4,

src/pyhpp/manipulation/graph.cc

@@ -234,16 +234,16 @@ namespace manipulation {
 // Utility functions
 // =============================================================================
 
-std::vector<std::vector<int>> matrixToVectorVector(
+std::vector<std::vector<double>> matrixToVectorVector(
     const Eigen::Ref<const Eigen::Matrix<double, -1, -1>>& input) {
-  std::vector<std::vector<int>> result;
+  std::vector<std::vector<double>> result;
   result.reserve(input.rows());
 
   for (int i = 0; i < input.rows(); ++i) {
-    std::vector<int> row;
+    std::vector<double> row;
     row.reserve(input.cols());
     for (int j = 0; j < input.cols(); ++j) {
-      row.push_back(static_cast<int>(input(i, j)));
+      row.push_back(input(i, j));
     }
     result.push_back(std::move(row));
   }
@@ -817,7 +817,7 @@ void PyWGraph::addLevelSetFoliation(PyWEdgePtr_t edge,
 boost::python::list PyWGraph::getSecurityMarginMatrixForTransition(
     PyWEdgePtr_t edge) {
   try {
-    std::vector<std::vector<int>> matrix =
+    std::vector<std::vector<double>> matrix =
         matrixToVectorVector(edge->obj->securityMargins());
 
     boost::python::list result;

tests/CMakeLists.txt

@@ -12,16 +12,9 @@
 # details.  You should have received a copy of the GNU Lesser General Public
 # License along with hpp-python  If not, see <http://www.gnu.org/licenses/>.
 
-set(PYTHON_UNIT_TESTS
-    imports
-    liegroup
-    load_ur3
-    differentiable-function
-    path-planner
-    rrt
-    path
-    constraint_graph
-    graph_factory2)
+# =============================================================================
+# Existing Python unit tests (run as simple scripts)
+# =============================================================================
 
 foreach(UNIT_TEST ${PYTHON_UNIT_TESTS})
   add_python_unit_test(${UNIT_TEST} tests/${UNIT_TEST}.py src)
@@ -33,3 +26,30 @@ set_tests_properties(
     ENVIRONMENT_MODIFICATION
     "ROS_PACKAGE_PATH=path_list_prepend:${example-robot-data_INCLUDE_DIRS}/../share;ROS_PACKAGE_PATH=path_list_prepend:${hpp-environments_INCLUDE_DIRS}/../share;"
 )
+
+# =============================================================================
+# unittest-based unit tests (uses Python's built-in unittest framework)
+# =============================================================================
+
+# Compute Python path for unit tests
+compute_pythonpath(UNITTEST_ENV_VARIABLES src)
+
+# Discover and add individual test files
+file(GLOB UNITTEST_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/unit/test_*.py")
+foreach(TEST_FILE ${UNITTEST_TEST_FILES})
+  get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE)
+  add_test(
+    NAME unittest_${TEST_NAME}
+    COMMAND ${PYTHON_EXECUTABLE} -m unittest discover -s
+            ${CMAKE_CURRENT_SOURCE_DIR} -p "${TEST_NAME}.py" -v
+    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR})
+  set_tests_properties(
+    unittest_${TEST_NAME}
+    PROPERTIES
+      ENVIRONMENT
+      "${UNITTEST_ENV_VARIABLES}"
+      ENVIRONMENT_MODIFICATION
+      "ROS_PACKAGE_PATH=path_list_prepend:${example-robot-data_INCLUDE_DIRS}/../share;ROS_PACKAGE_PATH=path_list_prepend:${hpp-environments_INCLUDE_DIRS}/../share;"
+      LABELS
+      "python;unit")
+endforeach()