diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 747cd7c..816a444 100644 --- a/src/CMakeLists.txt +++ b/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) diff --git a/src/pyhpp/constraints/by-substitution.cc b/src/pyhpp/constraints/by-substitution.cc index 277b332..1db7f11 100644 --- a/src/pyhpp/constraints/by-substitution.cc +++ b/src/pyhpp/constraints/by-substitution.cc @@ -28,9 +28,13 @@ // OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include +#include #include #include +#include + // 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 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(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_("SolverStatus") .value("ERROR_INCREASED", HierarchicalIterative::ERROR_INCREASED) @@ -80,7 +125,13 @@ void exposeBySubstitution() { &HierarchicalIterative::rightHandSide)) .def("rightHandSide", static_cast( - &HierarchicalIterative::rightHandSide)); + &HierarchicalIterative::rightHandSide)) + .add_property("errorThreshold", + static_cast( + &BySubstitution::errorThreshold), + static_cast( + &BySubstitution::errorThreshold)) + .def("describeError", &BySubstitution_describeError); } } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/constraints/differentiable-function.cc b/src/pyhpp/constraints/differentiable-function.cc index 0d9dd6e..27eecfd 100644 --- a/src/pyhpp/constraints/differentiable-function.cc +++ b/src/pyhpp/constraints/differentiable-function.cc @@ -107,6 +107,8 @@ void exposeDifferentiableFunction() { .def("__str__", &to_str) .def("__call__", &DFWrapper::py_value) .def("J", &DFWrapper::py_jacobian) + .def("name", &DFWrapper::name, + return_value_policy(), 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()) //; // class_("ExplicitConstraintSet", init()) .def("__str__", &to_str) - .def("add", &ExplicitConstraintSet::add, DocClassMethod(add)); + .def("add", &ExplicitConstraintSet::add, DocClassMethod(add)) + .def("errorSize", &ExplicitConstraintSet::errorSize); } } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/constraints/iterative-solver.cc b/src/pyhpp/constraints/iterative-solver.cc index d90e4af..a733141 100644 --- a/src/pyhpp/constraints/iterative-solver.cc +++ b/src/pyhpp/constraints/iterative-solver.cc @@ -29,6 +29,7 @@ // OF THE POSSIBILITY OF SUCH DAMAGE. // cland-format off +#include #include // 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") .def(vector_indexing_suite()); @@ -98,7 +107,11 @@ void exposeHierarchicalIterativeSolver() { static_cast( &HierarchicalIterative::solveLevelByLevel), static_cast( - &HierarchicalIterative::solveLevelByLevel)); + &HierarchicalIterative::solveLevelByLevel)) + .def("numberStacks", &HierarchicalIterative::numberStacks) + .def("constraintsForPriority", &getConstraintsForPriority) + .def("dimension", &HierarchicalIterative::dimension, + return_value_policy()); } } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/core/constraint.cc b/src/pyhpp/core/constraint.cc index 187fee0..b9c3b3a 100644 --- a/src/pyhpp/core/constraint.cc +++ b/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", no_init) @@ -146,7 +152,8 @@ void exposeConstraint() { .def("setRightHandSideFromConfig", &rightHandSideFromConfig) .def("setRightHandSideOfConstraint", &setRightHandSideOfConstraint) .def("sigma", &ConfigProjector::sigma, - return_value_policy(), DocClassMethod(sigma)); + return_value_policy(), DocClassMethod(sigma)) + .def("numericalConstraints", &getNumConstraints); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index bb682de..35d4908 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/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> 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(index) > waypointEdge->nbWaypoints()) { + throw std::logic_error("Waypoint index out of range"); + } + return (PyWEdge(waypointEdge->waypoint(static_cast(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_("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_("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)); diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index 6840a03..02e7523 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -58,7 +58,10 @@ 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 PyWStatePtr_t; @@ -66,7 +69,12 @@ typedef std::shared_ptr PyWStatePtr_t; 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 PyWEdgePtr_t; diff --git a/src/pyhpp/tools/__init__.py b/src/pyhpp/tools/__init__.py index ce53a5e..a776cf5 100644 --- a/src/pyhpp/tools/__init__.py +++ b/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 diff --git a/src/pyhpp/tools/constraint_error.py b/src/pyhpp/tools/constraint_error.py new file mode 100644 index 0000000..c4e5078 --- /dev/null +++ b/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}" + )