Add readable constraint error reporting

src/CMakeLists.txt

@@ -201,3 +201,4 @@ add_python_library(
 # Install tool submodule
 python_install_on_site(pyhpp/tools __init__.py)
 python_install_on_site(pyhpp/tools xacro.py)
+python_install_on_site(pyhpp/tools constraint_error.py)

src/pyhpp/constraints/by-substitution.cc

@@ -28,9 +28,13 @@
 // OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <boost/python.hpp>
+#include <eigenpy/eigenpy.hpp>
+#include <hpp/constraints/implicit-constraint-set.hh>
 #include <hpp/constraints/solver/by-substitution.hh>
 #include <pyhpp/constraints/fwd.hh>
 
+#include <set>
+
 // DocNamespace(hpp::constraints::solver)
 
 using namespace boost::python;
@@ -46,6 +50,47 @@ tuple BySubstitution_solve(const BySubstitution& hs, const vector_t& q) {
   return make_tuple(qout, s);
 }
 
+boost::python::list BySubstitution_describeError(BySubstitution& solver,
+                                                  vectorIn_t arg) {
+  size_type implicitDim = solver.dimension();
+  size_type explicitDim = solver.explicitConstraintSet().errorSize();
+  vector_t error(implicitDim + explicitDim);
+  bool satisfied = solver.isSatisfied(arg, error);
+  (void)satisfied;
+
+  boost::python::list result;
+  size_type offset = 0;
+
+  // Implicit constraints by priority level
+  std::set<ImplicitPtr_t> implicitSet;
+  for (std::size_t p = 0; p < solver.numberStacks(); ++p) {
+    const auto& stack = solver.constraints(p);
+    for (const auto& c : stack.constraints()) {
+      implicitSet.insert(c);
+      const DifferentiableFunction& f = c->function();
+      size_type nv = f.outputDerivativeSize();
+      vector_t errSlice = error.segment(offset, nv);
+      result.append(boost::python::make_tuple(
+          f.name(), errSlice, std::string("implicit"), static_cast<int>(p)));
+      offset += nv;
+    }
+  }
+
+  // Explicit constraints: those in numericalConstraints() not in any stack
+  for (const auto& c : solver.numericalConstraints()) {
+    if (implicitSet.count(c) == 0) {
+      const DifferentiableFunction& f = c->function();
+      size_type nv = f.outputDerivativeSize();
+      vector_t errSlice = error.segment(offset, nv);
+      result.append(boost::python::make_tuple(
+          f.name(), errSlice, std::string("explicit"), -1));
+      offset += nv;
+    }
+  }
+
+  return result;
+}
+
 void exposeBySubstitution() {
   enum_<HierarchicalIterative::Status>("SolverStatus")
       .value("ERROR_INCREASED", HierarchicalIterative::ERROR_INCREASED)
@@ -80,7 +125,13 @@ void exposeBySubstitution() {
                &HierarchicalIterative::rightHandSide))
       .def("rightHandSide",
            static_cast<vector_t (HierarchicalIterative::*)() const>(
-               &HierarchicalIterative::rightHandSide));
+               &HierarchicalIterative::rightHandSide))
+      .add_property("errorThreshold",
+                    static_cast<value_type (BySubstitution::*)() const>(
+                        &BySubstitution::errorThreshold),
+                    static_cast<void (BySubstitution::*)(const value_type&)>(
+                        &BySubstitution::errorThreshold))
+      .def("describeError", &BySubstitution_describeError);
 }
 }  // namespace constraints
 }  // namespace pyhpp

src/pyhpp/constraints/differentiable-function.cc

@@ -107,6 +107,8 @@ void exposeDifferentiableFunction() {
       .def("__str__", &to_str<DifferentiableFunction>)
       .def("__call__", &DFWrapper::py_value)
       .def("J", &DFWrapper::py_jacobian)
+      .def("name", &DFWrapper::name,
+           return_value_policy<copy_const_reference>(), DocClassMethod(name))
 
       .add_property("ni", &DifferentiableFunction::inputSize)
       .add_property("no", &DifferentiableFunction::outputSize)
@@ -129,6 +131,9 @@ void exposeDifferentiableFunction() {
       .def("outputDerivativeSize",
            &DifferentiableFunction::outputDerivativeSize,
            DocClassMethod(outputDerivativeSize))
+
+      .def("name", &DifferentiableFunction::name,
+           return_value_policy<copy_const_reference>())
       //;
 
       // class_<DFWrapper, DFWrapper::Ptr_t, boost::noncopyable,

src/pyhpp/constraints/explicit-constraint-set.cc

@@ -45,7 +45,8 @@ void exposeExplicitConstraintSet() {
   class_<ExplicitConstraintSet>("ExplicitConstraintSet",
                                 init<LiegroupSpacePtr_t>())
       .def("__str__", &to_str<ExplicitConstraintSet>)
-      .def("add", &ExplicitConstraintSet::add, DocClassMethod(add));
+      .def("add", &ExplicitConstraintSet::add, DocClassMethod(add))
+      .def("errorSize", &ExplicitConstraintSet::errorSize);
 }
 }  // namespace constraints
 }  // namespace pyhpp

src/pyhpp/constraints/iterative-solver.cc

@@ -29,6 +29,7 @@
 // OF THE POSSIBILITY OF SUCH DAMAGE.
 
 // cland-format off
+#include <hpp/constraints/implicit-constraint-set.hh>
 #include <hpp/constraints/solver/hierarchical-iterative.hh>
 // cland-format on
 
@@ -46,6 +47,14 @@ namespace constraints {
 using namespace hpp::constraints;
 using namespace hpp::constraints::solver;
 
+static boost::python::list getConstraintsForPriority(
+    HierarchicalIterative& hi, std::size_t priority) {
+  boost::python::list result;
+  for (const auto& c : hi.constraints(priority).constraints())
+    result.append(c);
+  return result;
+}
+
 void exposeHierarchicalIterativeSolver() {
   class_<ComparisonTypes_t>("ComparisonTypes")
       .def(vector_indexing_suite<ComparisonTypes_t>());
@@ -98,7 +107,11 @@ void exposeHierarchicalIterativeSolver() {
                     static_cast<bool (HierarchicalIterative::*)() const>(
                         &HierarchicalIterative::solveLevelByLevel),
                     static_cast<void (HierarchicalIterative::*)(bool)>(
-                        &HierarchicalIterative::solveLevelByLevel));
+                        &HierarchicalIterative::solveLevelByLevel))
+      .def("numberStacks", &HierarchicalIterative::numberStacks)
+      .def("constraintsForPriority", &getConstraintsForPriority)
+      .def("dimension", &HierarchicalIterative::dimension,
+           return_value_policy<copy_const_reference>());
 }
 }  // namespace constraints
 }  // namespace pyhpp

src/pyhpp/core/constraint.cc

@@ -86,6 +86,12 @@ static void setRightHandSideOfConstraint(
   configProj->rightHandSide(constraint, config);
 }
 
+static boost::python::list getNumConstraints(ConfigProjector& cp) {
+  boost::python::list result;
+  for (const auto& c : cp.numericalConstraints()) result.append(c);
+  return result;
+}
+
 void exposeConstraint() {
   // DocClass(Constraint)
   class_<Constraint, ConstraintPtr_t, boost::noncopyable>("Constraint", no_init)
@@ -146,7 +152,8 @@ void exposeConstraint() {
       .def("setRightHandSideFromConfig", &rightHandSideFromConfig)
       .def("setRightHandSideOfConstraint", &setRightHandSideOfConstraint)
       .def("sigma", &ConfigProjector::sigma,
-           return_value_policy<return_by_value>(), DocClassMethod(sigma));
+           return_value_policy<return_by_value>(), DocClassMethod(sigma))
+      .def("numericalConstraints", &getNumConstraints);
 }
 }  // namespace core
 }  // namespace pyhpp

src/pyhpp/manipulation/graph.cc

@@ -227,6 +227,8 @@ const char* DOC_CREATEPREPLACEMENTCONSTRAINT =
     "Create pre-placement constraint with specified width margin. "
     "Used for approaching placement configurations before final placement.";
 
+const char* DOC_NEIGHBOREDGES =
+    "Get the list of edges connected to this state.";
 }  // namespace
 
 namespace pyhpp {
@@ -258,9 +260,51 @@ std::vector<std::vector<double>> matrixToVectorVector(
 
 PyWState::PyWState(const StatePtr_t& state) : obj(state) {}
 std::string PyWState::name() const { return obj->name(); }
+std::size_t PyWState::id() const { return obj->id(); }
+
+boost::python::list PyWState::neighborEdges() {
+  try {
+    boost::python::list result;
+    for (const auto& edge : obj->neighborEdges()) {
+      if (edge) {
+        result.append(PyWEdgePtr_t(new PyWEdge(edge)));
+      }
+    }
+    return result;
+  } catch (const std::exception& exc) {
+    throw std::logic_error(exc.what());
+  }
+}
+
+hpp::core::ConstraintSetPtr_t PyWState::configConstraint() const {
+  return obj->configConstraint();
+}
 
 PyWEdge::PyWEdge(const EdgePtr_t& edge) : obj(edge) {}
+std::size_t PyWEdge::id() const { return obj->id(); }
 std::string PyWEdge::name() const { return obj->name(); }
+std::size_t PyWEdge::nbWaypoints() const {
+  using hpp::manipulation::graph::WaypointEdge;
+  auto waypointEdge = HPP_DYNAMIC_PTR_CAST(WaypointEdge, obj);
+  return waypointEdge ? waypointEdge->nbWaypoints() : 0;
+}
+bool PyWEdge::isWaypointEdge() const {
+  using hpp::manipulation::graph::WaypointEdge;
+  return HPP_DYNAMIC_PTR_CAST(WaypointEdge, obj) != nullptr;
+}
+
+PyWEdge PyWEdge::waypoint(int index) const {
+  using hpp::manipulation::graph::WaypointEdge;
+  auto waypointEdge = HPP_DYNAMIC_PTR_CAST(WaypointEdge, obj);
+  if (!waypointEdge) {
+    throw std::logic_error("Edge is not a WaypointEdge");
+  }
+  if (index < 0 ||
+      static_cast<std::size_t>(index) > waypointEdge->nbWaypoints()) {
+    throw std::logic_error("Waypoint index out of range");
+  }
+  return (PyWEdge(waypointEdge->waypoint(static_cast<std::size_t>(index))));
+}
 
 PyWGraph::PyWGraph(const hpp::manipulation::graph::GraphPtr_t& object)
     : obj(object) {}
@@ -1240,11 +1284,19 @@ using namespace boost::python;
 void exposeGraph() {
   // DocClass(State)
   class_<PyWState, PyWStatePtr_t>("State", no_init)
-      .def("name", &PyWState::name, DocClassMethod(name));
+      .def("name", &PyWState::name, DocClassMethod(name))
+      .def("id", &PyWState::id, DocClassMethod(id))
+      .def("configConstraint", &PyWState::configConstraint)
+      .PYHPP_DEFINE_METHOD1(PyWState, neighborEdges, DOC_NEIGHBOREDGES);
 
   // DocClass(Edge)
   class_<PyWEdge, PyWEdgePtr_t>("Transition", no_init)
+      .def("id", &PyWEdge::id, DocClassMethod(id))
       .def("name", &PyWEdge::name, DocClassMethod(name))
+      .def("isWaypointEdge", &PyWEdge::isWaypointEdge,
+           DocClassMethod(isWaypointEdge))
+      .def("nbWaypoints", &PyWEdge::nbWaypoints, DocClassMethod(nbWaypoints))
+      .def("waypoint", &PyWEdge::waypoint, DocClassMethod(waypoint))
       .def("pathValidation", &PyWEdge::pathValidation,
            DocClassMethod(pathValidation));
 

src/pyhpp/manipulation/graph.hh

@@ -58,15 +58,23 @@ typedef hpp::manipulation::ConstraintAndComplement_t ConstraintAndComplement_t;
 struct PyWState {
   StatePtr_t obj;
   PyWState(const StatePtr_t& object);
+  std::size_t id() const;
   std::string name() const;
+  boost::python::list neighborEdges();
+  hpp::core::ConstraintSetPtr_t configConstraint() const;
 };
 typedef std::shared_ptr<PyWState> PyWStatePtr_t;
 
 /// Python wrapper for Edge
 struct PyWEdge {
   EdgePtr_t obj;
   PyWEdge(const EdgePtr_t& object);
+  std::size_t id() const;
   std::string name() const;
+  bool isWaypointEdge() const;
+  std::size_t nbWaypoints() const;
+  std::size_t weight() const;
+  PyWEdge waypoint(int index) const;
   PathValidationPtr_t pathValidation() const;
 };
 typedef std::shared_ptr<PyWEdge> PyWEdgePtr_t;

src/pyhpp/tools/__init__.py

@@ -1 +1,2 @@
+from .constraint_error import describe_error, print_error  # noqa: F401
 from .xacro import process_xacro, retrieve_resource  # noqa: F401

src/pyhpp/tools/constraint_error.py

@@ -0,0 +1,60 @@
+import numpy as np
+
+
+def describe_error(config_projector, q):
+    """Compute constraint errors and map each component to its constraint.
+
+    Args:
+        config_projector: A ConfigProjector instance.
+        q: Configuration vector to check.
+
+    Returns:
+        Tuple of (entries, satisfied) where entries is a list of dicts with:
+            name: constraint function name
+            error: numpy array of error values
+            norm: L2 norm of the error
+            kind: "implicit" or "explicit"
+            priority: priority level (implicit) or None (explicit)
+            satisfied: whether norm < threshold
+    """
+    solver = config_projector.solver()
+    threshold = config_projector.errorThreshold()
+
+    raw = solver.describeError(q)
+
+    entries = []
+    for name, error, kind, priority in raw:
+        error = np.array(error)
+        norm = float(np.linalg.norm(error))
+        entries.append(
+            {
+                "name": name,
+                "error": error,
+                "norm": norm,
+                "kind": kind,
+                "priority": priority if priority >= 0 else None,
+                "satisfied": norm < threshold,
+            }
+        )
+
+    satisfied = all(e["satisfied"] for e in entries)
+    return entries, satisfied
+
+
+def print_error(config_projector, q):
+    """Print a human-readable breakdown of constraint errors."""
+    entries, satisfied = describe_error(config_projector, q)
+    threshold = config_projector.errorThreshold()
+
+    print(f"Overall satisfied: {satisfied}  (threshold: {threshold:.0e})")
+    print(
+        f"{'Constraint':<50} {'Kind':<10} {'Pri':<5} {'Norm':>12} {'OK?':>5}"
+    )
+    print("-" * 85)
+    for e in entries:
+        pri = str(e["priority"]) if e["priority"] is not None else "-"
+        ok = "yes" if e["satisfied"] else "NO"
+        print(
+            f"{e['name']:<50} {e['kind']:<10} {pri:<5} "
+            f"{e['norm']:>12.6e} {ok:>5}"
+        )