From 1fe6f35369fdcc40ab190794e5736de5ce85ad16 Mon Sep 17 00:00:00 2001 From: Diego Pintat-Gil Date: Tue, 21 Apr 2026 16:07:35 +0200 Subject: [PATCH 1/6] Expose more Python binding metadata (id, name, nbWaypoints) for DifferentiableFunct ion, Edge, and State --- src/pyhpp/constraints/differentiable-function.cc | 2 ++ src/pyhpp/manipulation/graph.cc | 14 ++++++++++++-- src/pyhpp/manipulation/graph.hh | 12 ++++++++---- 3 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/pyhpp/constraints/differentiable-function.cc b/src/pyhpp/constraints/differentiable-function.cc index f0ec356c..509b4b8c 100644 --- a/src/pyhpp/constraints/differentiable-function.cc +++ b/src/pyhpp/constraints/differentiable-function.cc @@ -104,6 +104,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) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index bb682de2..b4e4fddb 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -258,9 +258,16 @@ 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(); } 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; +} PyWGraph::PyWGraph(const hpp::manipulation::graph::GraphPtr_t& object) : obj(object) {} @@ -1240,13 +1247,16 @@ 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)); // DocClass(Edge) class_("Transition", no_init) + .def("id", &PyWEdge::id, DocClassMethod(id)) .def("name", &PyWEdge::name, DocClassMethod(name)) + .def("nbWaypoints", &PyWEdge::nbWaypoints, DocClassMethod(nbWaypoints)) .def("pathValidation", &PyWEdge::pathValidation, - DocClassMethod(pathValidation)); + DocClassMethod(pathValidation)), // DocClass(Graph) class_( diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index 6840a038..88395af3 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -58,16 +58,20 @@ 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; }; typedef std::shared_ptr PyWStatePtr_t; /// Python wrapper for Edge struct PyWEdge { - EdgePtr_t obj; - PyWEdge(const EdgePtr_t& object); - std::string name() const; - PathValidationPtr_t pathValidation() const; + EdgePtr_t obj; + PyWEdge(const EdgePtr_t& object); + std::size_t id() const; + std::string name() const; + std::size_t nbWaypoints() const; + std::size_t weight() const; + PathValidationPtr_t pathValidation() const; }; typedef std::shared_ptr PyWEdgePtr_t; From 873b23065d361d1b264479d40a5f9be5f19c6f09 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 22 Apr 2026 13:53:34 +0000 Subject: [PATCH 2/6] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .../constraints/differentiable-function.cc | 4 +- src/pyhpp/manipulation/graph.cc | 250 +++++++++--------- src/pyhpp/manipulation/graph.hh | 14 +- 3 files changed, 138 insertions(+), 130 deletions(-) diff --git a/src/pyhpp/constraints/differentiable-function.cc b/src/pyhpp/constraints/differentiable-function.cc index e35fe3b6..cf8e3be3 100644 --- a/src/pyhpp/constraints/differentiable-function.cc +++ b/src/pyhpp/constraints/differentiable-function.cc @@ -107,8 +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)) - + .def("name", &DFWrapper::name, + return_value_policy() DocClassMethod(name)) .add_property("ni", &DifferentiableFunction::inputSize) .add_property("no", &DifferentiableFunction::outputSize) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index b4e4fddb..93955d84 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -1258,127 +1258,135 @@ void exposeGraph() { .def("pathValidation", &PyWEdge::pathValidation, DocClassMethod(pathValidation)), - // DocClass(Graph) - class_( - "Graph", - init()) - - .def("_get_native_graph", &getGraphCapsule) - .def_readwrite("robot", &PyWGraph::robot) - // Configuration methods - .PYHPP_DEFINE_GETTER_SETTER(PyWGraph, maxIterations, - hpp::manipulation::size_type) - .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(PyWGraph, errorThreshold, - hpp::manipulation::value_type) - - // Graph construction - .PYHPP_DEFINE_METHOD1(PyWGraph, createState, DOC_CREATESTATE) - .PYHPP_DEFINE_METHOD1(PyWGraph, createTransition, DOC_CREATETRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, createWaypointTransition, - DOC_CREATEWAYPOINTTRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, createLevelSetTransition, - DOC_CREATELEVELSETTRANSITION) - - // Transition/State management - .PYHPP_DEFINE_METHOD1(PyWGraph, setContainingNode, DOC_SETCONTAININGNODE) - .PYHPP_DEFINE_METHOD1(PyWGraph, getContainingNode, DOC_GETCONTAININGNODE) - .PYHPP_DEFINE_METHOD1(PyWGraph, setShort, DOC_SETSHORT) - .PYHPP_DEFINE_METHOD1(PyWGraph, isShort, DOC_ISSHORT) - .PYHPP_DEFINE_METHOD1(PyWGraph, getNodesConnectedByTransition, - DOC_GETNODESCONNECTEDBYTRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, setWeight, DOC_SETWEIGHT) - .PYHPP_DEFINE_METHOD1(PyWGraph, getWeight, DOC_GETWEIGHT) - .PYHPP_DEFINE_METHOD1(PyWGraph, setWaypoint, DOC_SETWAYPOINT) - - .PYHPP_DEFINE_METHOD(PyWGraph, getState) - .PYHPP_DEFINE_METHOD(PyWGraph, getTransition) - .PYHPP_DEFINE_METHOD(PyWGraph, getStates) - .PYHPP_DEFINE_METHOD(PyWGraph, getTransitions) - .PYHPP_DEFINE_METHOD(PyWGraph, getStateNames) - .PYHPP_DEFINE_METHOD(PyWGraph, getTransitionNames) - - // State queries - .PYHPP_DEFINE_METHOD1(PyWGraph, getStateFromConfiguration, DOC_GETSTATE) - - // Constraint management - .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraint, - DOC_ADDNUMERICALCONSTRAINT) - .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsToState, - DOC_ADDNUMERICALCONSTRAINTSTOSTATE) - .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsToTransition, - DOC_ADDNUMERICALCONSTRAINTSTOTRANSITION) - .PYHPP_DEFINE_METHOD(PyWGraph, addNumericalConstraintsToGraph) - .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsForPath, - DOC_ADDNUMERICALCONSTRAINTSFORPATH) - .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForState, - DOC_GETNUMERICALCONSTRAINTSFORSTATE) - .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForEdge, - DOC_GETNUMERICALCONSTRAINTSFOREDGE) - .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForGraph, - DOC_GETNUMERICALCONSTRAINTSFORGRAPH) - .PYHPP_DEFINE_METHOD1(PyWGraph, resetConstraints, DOC_RESETCONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, registerConstraints, - DOC_REGISTERCONSTRAINTS) - - .def("createPlacementConstraint", &PyWGraph::createPlacementConstraint1, - DOC_CREATEPLACEMENTCONSTRAINT) - .def("createPlacementConstraint", &PyWGraph::createPlacementConstraint2, - DOC_CREATEPLACEMENTCONSTRAINT) - .def("createPrePlacementConstraint", - &PyWGraph::createPrePlacementConstraint1, - DOC_CREATEPREPLACEMENTCONSTRAINT) - .def("createPrePlacementConstraint", - &PyWGraph::createPrePlacementConstraint2, - DOC_CREATEPREPLACEMENTCONSTRAINT) - .def("createGraspConstraint", &PyWGraph::createGraspConstraint) - .def("createPreGraspConstraint", &PyWGraph::createPreGraspConstraint) - // Configuration error checking - .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForState, - DOC_GETCONFIGERRORFORSTATE) - .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransition, - DOC_GETCONFIGERRORFORTRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransitionLeaf, - DOC_GETCONFIGERRORFORTRANSITIONLEAF) - .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransitionTarget, - DOC_GETCONFIGERRORFORTRANSITIONTARGET) - - // Constraint application - .PYHPP_DEFINE_METHOD1(PyWGraph, applyStateConstraints, - DOC_APPLYSTATECONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, applyLeafConstraints, - DOC_APPLYLEAFCONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, generateTargetConfig, - DOC_GENERATETARGETCONFIG) - - // Level set transitions - .PYHPP_DEFINE_METHOD1(PyWGraph, addLevelSetFoliation, - DOC_ADDLEVELSETFOLIATION) - - // Security margins and collision - .PYHPP_DEFINE_METHOD1(PyWGraph, getSecurityMarginMatrixForTransition, - DOC_GETSECURITYMARGINMATRIXFORTRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, setSecurityMarginForTransition, - DOC_SETSECURITYMARGINFORTRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, getRelativeMotionMatrix, - DOC_GETRELATIVEMOTIONMATRIX) - .PYHPP_DEFINE_METHOD1(PyWGraph, removeCollisionPairFromTransition, - DOC_REMOVECOLLISIONPAIRFROMTRANSITION) - - // Subgraph management - .PYHPP_DEFINE_METHOD1(PyWGraph, createSubGraph, DOC_CREATESUBGRAPH) - .PYHPP_DEFINE_METHOD1(PyWGraph, setTargetNodeList, DOC_SETTARGETNODELIST) - - // Display and debugging - .PYHPP_DEFINE_METHOD1(PyWGraph, displayStateConstraints, - DOC_DISPLAYSTATECONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, displayTransitionConstraints, - DOC_DISPLAYTRANSITIONCONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, displayTransitionTargetConstraints, - DOC_DISPLAYTRANSITIONTARGETCONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, display, DOC_DISPLAY) - - // Initialization - .PYHPP_DEFINE_METHOD1(PyWGraph, initialize, DOC_INITIALIZE); + // DocClass(Graph) + class_( + "Graph", init()) + + .def("_get_native_graph", &getGraphCapsule) + .def_readwrite("robot", &PyWGraph::robot) + // Configuration methods + .PYHPP_DEFINE_GETTER_SETTER(PyWGraph, maxIterations, + hpp::manipulation::size_type) + .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(PyWGraph, errorThreshold, + hpp::manipulation::value_type) + + // Graph construction + .PYHPP_DEFINE_METHOD1(PyWGraph, createState, DOC_CREATESTATE) + .PYHPP_DEFINE_METHOD1(PyWGraph, createTransition, + DOC_CREATETRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, createWaypointTransition, + DOC_CREATEWAYPOINTTRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, createLevelSetTransition, + DOC_CREATELEVELSETTRANSITION) + + // Transition/State management + .PYHPP_DEFINE_METHOD1(PyWGraph, setContainingNode, + DOC_SETCONTAININGNODE) + .PYHPP_DEFINE_METHOD1(PyWGraph, getContainingNode, + DOC_GETCONTAININGNODE) + .PYHPP_DEFINE_METHOD1(PyWGraph, setShort, DOC_SETSHORT) + .PYHPP_DEFINE_METHOD1(PyWGraph, isShort, DOC_ISSHORT) + .PYHPP_DEFINE_METHOD1(PyWGraph, getNodesConnectedByTransition, + DOC_GETNODESCONNECTEDBYTRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, setWeight, DOC_SETWEIGHT) + .PYHPP_DEFINE_METHOD1(PyWGraph, getWeight, DOC_GETWEIGHT) + .PYHPP_DEFINE_METHOD1(PyWGraph, setWaypoint, DOC_SETWAYPOINT) + + .PYHPP_DEFINE_METHOD(PyWGraph, getState) + .PYHPP_DEFINE_METHOD(PyWGraph, getTransition) + .PYHPP_DEFINE_METHOD(PyWGraph, getStates) + .PYHPP_DEFINE_METHOD(PyWGraph, getTransitions) + .PYHPP_DEFINE_METHOD(PyWGraph, getStateNames) + .PYHPP_DEFINE_METHOD(PyWGraph, getTransitionNames) + + // State queries + .PYHPP_DEFINE_METHOD1(PyWGraph, getStateFromConfiguration, + DOC_GETSTATE) + + // Constraint management + .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraint, + DOC_ADDNUMERICALCONSTRAINT) + .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsToState, + DOC_ADDNUMERICALCONSTRAINTSTOSTATE) + .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsToTransition, + DOC_ADDNUMERICALCONSTRAINTSTOTRANSITION) + .PYHPP_DEFINE_METHOD(PyWGraph, addNumericalConstraintsToGraph) + .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsForPath, + DOC_ADDNUMERICALCONSTRAINTSFORPATH) + .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForState, + DOC_GETNUMERICALCONSTRAINTSFORSTATE) + .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForEdge, + DOC_GETNUMERICALCONSTRAINTSFOREDGE) + .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForGraph, + DOC_GETNUMERICALCONSTRAINTSFORGRAPH) + .PYHPP_DEFINE_METHOD1(PyWGraph, resetConstraints, + DOC_RESETCONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, registerConstraints, + DOC_REGISTERCONSTRAINTS) + + .def("createPlacementConstraint", + &PyWGraph::createPlacementConstraint1, + DOC_CREATEPLACEMENTCONSTRAINT) + .def("createPlacementConstraint", + &PyWGraph::createPlacementConstraint2, + DOC_CREATEPLACEMENTCONSTRAINT) + .def("createPrePlacementConstraint", + &PyWGraph::createPrePlacementConstraint1, + DOC_CREATEPREPLACEMENTCONSTRAINT) + .def("createPrePlacementConstraint", + &PyWGraph::createPrePlacementConstraint2, + DOC_CREATEPREPLACEMENTCONSTRAINT) + .def("createGraspConstraint", &PyWGraph::createGraspConstraint) + .def("createPreGraspConstraint", &PyWGraph::createPreGraspConstraint) + // Configuration error checking + .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForState, + DOC_GETCONFIGERRORFORSTATE) + .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransition, + DOC_GETCONFIGERRORFORTRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransitionLeaf, + DOC_GETCONFIGERRORFORTRANSITIONLEAF) + .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransitionTarget, + DOC_GETCONFIGERRORFORTRANSITIONTARGET) + + // Constraint application + .PYHPP_DEFINE_METHOD1(PyWGraph, applyStateConstraints, + DOC_APPLYSTATECONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, applyLeafConstraints, + DOC_APPLYLEAFCONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, generateTargetConfig, + DOC_GENERATETARGETCONFIG) + + // Level set transitions + .PYHPP_DEFINE_METHOD1(PyWGraph, addLevelSetFoliation, + DOC_ADDLEVELSETFOLIATION) + + // Security margins and collision + .PYHPP_DEFINE_METHOD1(PyWGraph, getSecurityMarginMatrixForTransition, + DOC_GETSECURITYMARGINMATRIXFORTRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, setSecurityMarginForTransition, + DOC_SETSECURITYMARGINFORTRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, getRelativeMotionMatrix, + DOC_GETRELATIVEMOTIONMATRIX) + .PYHPP_DEFINE_METHOD1(PyWGraph, removeCollisionPairFromTransition, + DOC_REMOVECOLLISIONPAIRFROMTRANSITION) + + // Subgraph management + .PYHPP_DEFINE_METHOD1(PyWGraph, createSubGraph, DOC_CREATESUBGRAPH) + .PYHPP_DEFINE_METHOD1(PyWGraph, setTargetNodeList, + DOC_SETTARGETNODELIST) + + // Display and debugging + .PYHPP_DEFINE_METHOD1(PyWGraph, displayStateConstraints, + DOC_DISPLAYSTATECONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, displayTransitionConstraints, + DOC_DISPLAYTRANSITIONCONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, displayTransitionTargetConstraints, + DOC_DISPLAYTRANSITIONTARGETCONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, display, DOC_DISPLAY) + + // Initialization + .PYHPP_DEFINE_METHOD1(PyWGraph, initialize, DOC_INITIALIZE); } } // namespace manipulation diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index 88395af3..553cfc91 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -65,13 +65,13 @@ typedef std::shared_ptr 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; - std::size_t nbWaypoints() const; - std::size_t weight() const; - PathValidationPtr_t pathValidation() const; + EdgePtr_t obj; + PyWEdge(const EdgePtr_t& object); + std::size_t id() const; + std::string name() const; + std::size_t nbWaypoints() const; + std::size_t weight() const; + PathValidationPtr_t pathValidation() const; }; typedef std::shared_ptr PyWEdgePtr_t; From 3f4bff150110c3018b503c0957507abfe44ff2ad Mon Sep 17 00:00:00 2001 From: Diego Pintat-Gil Date: Wed, 22 Apr 2026 17:40:05 +0200 Subject: [PATCH 3/6] fix syntax error' --- src/pyhpp/constraints/differentiable-function.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pyhpp/constraints/differentiable-function.cc b/src/pyhpp/constraints/differentiable-function.cc index cf8e3be3..6816441e 100644 --- a/src/pyhpp/constraints/differentiable-function.cc +++ b/src/pyhpp/constraints/differentiable-function.cc @@ -107,8 +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)) + .def("name", &DFWrapper::name, return_value_policy(), DocClassMethod(name)) + .add_property("ni", &DifferentiableFunction::inputSize) .add_property("no", &DifferentiableFunction::outputSize) From e4a93b2672cc359b6e0ba2e09f324fabdd43e3db Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 22 Apr 2026 15:43:20 +0000 Subject: [PATCH 4/6] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/constraints/differentiable-function.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pyhpp/constraints/differentiable-function.cc b/src/pyhpp/constraints/differentiable-function.cc index 6816441e..683e5532 100644 --- a/src/pyhpp/constraints/differentiable-function.cc +++ b/src/pyhpp/constraints/differentiable-function.cc @@ -107,8 +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)) - + .def("name", &DFWrapper::name, + return_value_policy(), DocClassMethod(name)) .add_property("ni", &DifferentiableFunction::inputSize) .add_property("no", &DifferentiableFunction::outputSize) From 9c93f82cdb307313bb97df6cbbc19fd8b7d5c149 Mon Sep 17 00:00:00 2001 From: Diego Pintat-Gil Date: Wed, 29 Apr 2026 16:36:25 +0200 Subject: [PATCH 5/6] add bindings on edges and nodes --- src/pyhpp/manipulation/graph.cc | 293 ++++++++++++++++++-------------- src/pyhpp/manipulation/graph.hh | 4 + 2 files changed, 165 insertions(+), 132 deletions(-) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index 93955d84..b02d7447 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 { @@ -260,6 +262,20 @@ 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()); + } +} + 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(); } @@ -268,6 +284,23 @@ std::size_t PyWEdge::nbWaypoints() const { 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) {} @@ -1248,145 +1281,141 @@ void exposeGraph() { // DocClass(State) class_("State", no_init) .def("name", &PyWState::name, DocClassMethod(name)) - .def("id", &PyWState::id, DocClassMethod(id)); + .def("id", &PyWState::id, DocClassMethod(id)) + .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)), - - // DocClass(Graph) - class_( - "Graph", init()) - - .def("_get_native_graph", &getGraphCapsule) - .def_readwrite("robot", &PyWGraph::robot) - // Configuration methods - .PYHPP_DEFINE_GETTER_SETTER(PyWGraph, maxIterations, - hpp::manipulation::size_type) - .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(PyWGraph, errorThreshold, - hpp::manipulation::value_type) - - // Graph construction - .PYHPP_DEFINE_METHOD1(PyWGraph, createState, DOC_CREATESTATE) - .PYHPP_DEFINE_METHOD1(PyWGraph, createTransition, - DOC_CREATETRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, createWaypointTransition, - DOC_CREATEWAYPOINTTRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, createLevelSetTransition, - DOC_CREATELEVELSETTRANSITION) - - // Transition/State management - .PYHPP_DEFINE_METHOD1(PyWGraph, setContainingNode, - DOC_SETCONTAININGNODE) - .PYHPP_DEFINE_METHOD1(PyWGraph, getContainingNode, - DOC_GETCONTAININGNODE) - .PYHPP_DEFINE_METHOD1(PyWGraph, setShort, DOC_SETSHORT) - .PYHPP_DEFINE_METHOD1(PyWGraph, isShort, DOC_ISSHORT) - .PYHPP_DEFINE_METHOD1(PyWGraph, getNodesConnectedByTransition, - DOC_GETNODESCONNECTEDBYTRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, setWeight, DOC_SETWEIGHT) - .PYHPP_DEFINE_METHOD1(PyWGraph, getWeight, DOC_GETWEIGHT) - .PYHPP_DEFINE_METHOD1(PyWGraph, setWaypoint, DOC_SETWAYPOINT) - - .PYHPP_DEFINE_METHOD(PyWGraph, getState) - .PYHPP_DEFINE_METHOD(PyWGraph, getTransition) - .PYHPP_DEFINE_METHOD(PyWGraph, getStates) - .PYHPP_DEFINE_METHOD(PyWGraph, getTransitions) - .PYHPP_DEFINE_METHOD(PyWGraph, getStateNames) - .PYHPP_DEFINE_METHOD(PyWGraph, getTransitionNames) - - // State queries - .PYHPP_DEFINE_METHOD1(PyWGraph, getStateFromConfiguration, - DOC_GETSTATE) - - // Constraint management - .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraint, - DOC_ADDNUMERICALCONSTRAINT) - .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsToState, - DOC_ADDNUMERICALCONSTRAINTSTOSTATE) - .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsToTransition, - DOC_ADDNUMERICALCONSTRAINTSTOTRANSITION) - .PYHPP_DEFINE_METHOD(PyWGraph, addNumericalConstraintsToGraph) - .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsForPath, - DOC_ADDNUMERICALCONSTRAINTSFORPATH) - .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForState, - DOC_GETNUMERICALCONSTRAINTSFORSTATE) - .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForEdge, - DOC_GETNUMERICALCONSTRAINTSFOREDGE) - .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForGraph, - DOC_GETNUMERICALCONSTRAINTSFORGRAPH) - .PYHPP_DEFINE_METHOD1(PyWGraph, resetConstraints, - DOC_RESETCONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, registerConstraints, - DOC_REGISTERCONSTRAINTS) - - .def("createPlacementConstraint", - &PyWGraph::createPlacementConstraint1, - DOC_CREATEPLACEMENTCONSTRAINT) - .def("createPlacementConstraint", - &PyWGraph::createPlacementConstraint2, - DOC_CREATEPLACEMENTCONSTRAINT) - .def("createPrePlacementConstraint", - &PyWGraph::createPrePlacementConstraint1, - DOC_CREATEPREPLACEMENTCONSTRAINT) - .def("createPrePlacementConstraint", - &PyWGraph::createPrePlacementConstraint2, - DOC_CREATEPREPLACEMENTCONSTRAINT) - .def("createGraspConstraint", &PyWGraph::createGraspConstraint) - .def("createPreGraspConstraint", &PyWGraph::createPreGraspConstraint) - // Configuration error checking - .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForState, - DOC_GETCONFIGERRORFORSTATE) - .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransition, - DOC_GETCONFIGERRORFORTRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransitionLeaf, - DOC_GETCONFIGERRORFORTRANSITIONLEAF) - .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransitionTarget, - DOC_GETCONFIGERRORFORTRANSITIONTARGET) - - // Constraint application - .PYHPP_DEFINE_METHOD1(PyWGraph, applyStateConstraints, - DOC_APPLYSTATECONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, applyLeafConstraints, - DOC_APPLYLEAFCONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, generateTargetConfig, - DOC_GENERATETARGETCONFIG) - - // Level set transitions - .PYHPP_DEFINE_METHOD1(PyWGraph, addLevelSetFoliation, - DOC_ADDLEVELSETFOLIATION) - - // Security margins and collision - .PYHPP_DEFINE_METHOD1(PyWGraph, getSecurityMarginMatrixForTransition, - DOC_GETSECURITYMARGINMATRIXFORTRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, setSecurityMarginForTransition, - DOC_SETSECURITYMARGINFORTRANSITION) - .PYHPP_DEFINE_METHOD1(PyWGraph, getRelativeMotionMatrix, - DOC_GETRELATIVEMOTIONMATRIX) - .PYHPP_DEFINE_METHOD1(PyWGraph, removeCollisionPairFromTransition, - DOC_REMOVECOLLISIONPAIRFROMTRANSITION) - - // Subgraph management - .PYHPP_DEFINE_METHOD1(PyWGraph, createSubGraph, DOC_CREATESUBGRAPH) - .PYHPP_DEFINE_METHOD1(PyWGraph, setTargetNodeList, - DOC_SETTARGETNODELIST) - - // Display and debugging - .PYHPP_DEFINE_METHOD1(PyWGraph, displayStateConstraints, - DOC_DISPLAYSTATECONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, displayTransitionConstraints, - DOC_DISPLAYTRANSITIONCONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, displayTransitionTargetConstraints, - DOC_DISPLAYTRANSITIONTARGETCONSTRAINTS) - .PYHPP_DEFINE_METHOD1(PyWGraph, display, DOC_DISPLAY) - - // Initialization - .PYHPP_DEFINE_METHOD1(PyWGraph, initialize, DOC_INITIALIZE); + DocClassMethod(pathValidation)); + + // DocClass(Graph) + class_( + "Graph", + init()) + + .def("_get_native_graph", &getGraphCapsule) + .def_readwrite("robot", &PyWGraph::robot) + // Configuration methods + .PYHPP_DEFINE_GETTER_SETTER(PyWGraph, maxIterations, + hpp::manipulation::size_type) + .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(PyWGraph, errorThreshold, + hpp::manipulation::value_type) + + // Graph construction + .PYHPP_DEFINE_METHOD1(PyWGraph, createState, DOC_CREATESTATE) + .PYHPP_DEFINE_METHOD1(PyWGraph, createTransition, DOC_CREATETRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, createWaypointTransition, + DOC_CREATEWAYPOINTTRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, createLevelSetTransition, + DOC_CREATELEVELSETTRANSITION) + + // Transition/State management + .PYHPP_DEFINE_METHOD1(PyWGraph, setContainingNode, DOC_SETCONTAININGNODE) + .PYHPP_DEFINE_METHOD1(PyWGraph, getContainingNode, DOC_GETCONTAININGNODE) + .PYHPP_DEFINE_METHOD1(PyWGraph, setShort, DOC_SETSHORT) + .PYHPP_DEFINE_METHOD1(PyWGraph, isShort, DOC_ISSHORT) + .PYHPP_DEFINE_METHOD1(PyWGraph, getNodesConnectedByTransition, + DOC_GETNODESCONNECTEDBYTRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, setWeight, DOC_SETWEIGHT) + .PYHPP_DEFINE_METHOD1(PyWGraph, getWeight, DOC_GETWEIGHT) + .PYHPP_DEFINE_METHOD1(PyWGraph, setWaypoint, DOC_SETWAYPOINT) + + .PYHPP_DEFINE_METHOD(PyWGraph, getState) + .PYHPP_DEFINE_METHOD(PyWGraph, getTransition) + .PYHPP_DEFINE_METHOD(PyWGraph, getStates) + .PYHPP_DEFINE_METHOD(PyWGraph, getTransitions) + .PYHPP_DEFINE_METHOD(PyWGraph, getStateNames) + .PYHPP_DEFINE_METHOD(PyWGraph, getTransitionNames) + + // State queries + .PYHPP_DEFINE_METHOD1(PyWGraph, getStateFromConfiguration, DOC_GETSTATE) + + // Constraint management + .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraint, + DOC_ADDNUMERICALCONSTRAINT) + .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsToState, + DOC_ADDNUMERICALCONSTRAINTSTOSTATE) + .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsToTransition, + DOC_ADDNUMERICALCONSTRAINTSTOTRANSITION) + .PYHPP_DEFINE_METHOD(PyWGraph, addNumericalConstraintsToGraph) + .PYHPP_DEFINE_METHOD1(PyWGraph, addNumericalConstraintsForPath, + DOC_ADDNUMERICALCONSTRAINTSFORPATH) + .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForState, + DOC_GETNUMERICALCONSTRAINTSFORSTATE) + .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForEdge, + DOC_GETNUMERICALCONSTRAINTSFOREDGE) + .PYHPP_DEFINE_METHOD1(PyWGraph, getNumericalConstraintsForGraph, + DOC_GETNUMERICALCONSTRAINTSFORGRAPH) + .PYHPP_DEFINE_METHOD1(PyWGraph, resetConstraints, DOC_RESETCONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, registerConstraints, + DOC_REGISTERCONSTRAINTS) + + .def("createPlacementConstraint", &PyWGraph::createPlacementConstraint1, + DOC_CREATEPLACEMENTCONSTRAINT) + .def("createPlacementConstraint", &PyWGraph::createPlacementConstraint2, + DOC_CREATEPLACEMENTCONSTRAINT) + .def("createPrePlacementConstraint", + &PyWGraph::createPrePlacementConstraint1, + DOC_CREATEPREPLACEMENTCONSTRAINT) + .def("createPrePlacementConstraint", + &PyWGraph::createPrePlacementConstraint2, + DOC_CREATEPREPLACEMENTCONSTRAINT) + .def("createGraspConstraint", &PyWGraph::createGraspConstraint) + .def("createPreGraspConstraint", &PyWGraph::createPreGraspConstraint) + // Configuration error checking + .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForState, + DOC_GETCONFIGERRORFORSTATE) + .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransition, + DOC_GETCONFIGERRORFORTRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransitionLeaf, + DOC_GETCONFIGERRORFORTRANSITIONLEAF) + .PYHPP_DEFINE_METHOD1(PyWGraph, getConfigErrorForTransitionTarget, + DOC_GETCONFIGERRORFORTRANSITIONTARGET) + + // Constraint application + .PYHPP_DEFINE_METHOD1(PyWGraph, applyStateConstraints, + DOC_APPLYSTATECONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, applyLeafConstraints, + DOC_APPLYLEAFCONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, generateTargetConfig, + DOC_GENERATETARGETCONFIG) + + // Level set transitions + .PYHPP_DEFINE_METHOD1(PyWGraph, addLevelSetFoliation, + DOC_ADDLEVELSETFOLIATION) + + // Security margins and collision + .PYHPP_DEFINE_METHOD1(PyWGraph, getSecurityMarginMatrixForTransition, + DOC_GETSECURITYMARGINMATRIXFORTRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, setSecurityMarginForTransition, + DOC_SETSECURITYMARGINFORTRANSITION) + .PYHPP_DEFINE_METHOD1(PyWGraph, getRelativeMotionMatrix, + DOC_GETRELATIVEMOTIONMATRIX) + .PYHPP_DEFINE_METHOD1(PyWGraph, removeCollisionPairFromTransition, + DOC_REMOVECOLLISIONPAIRFROMTRANSITION) + + // Subgraph management + .PYHPP_DEFINE_METHOD1(PyWGraph, createSubGraph, DOC_CREATESUBGRAPH) + .PYHPP_DEFINE_METHOD1(PyWGraph, setTargetNodeList, DOC_SETTARGETNODELIST) + + // Display and debugging + .PYHPP_DEFINE_METHOD1(PyWGraph, displayStateConstraints, + DOC_DISPLAYSTATECONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, displayTransitionConstraints, + DOC_DISPLAYTRANSITIONCONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, displayTransitionTargetConstraints, + DOC_DISPLAYTRANSITIONTARGETCONSTRAINTS) + .PYHPP_DEFINE_METHOD1(PyWGraph, display, DOC_DISPLAY) + + // Initialization + .PYHPP_DEFINE_METHOD1(PyWGraph, initialize, DOC_INITIALIZE); } } // namespace manipulation diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index 553cfc91..e37140d8 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -60,6 +60,8 @@ struct PyWState { PyWState(const StatePtr_t& object); std::size_t id() const; std::string name() const; + + boost::python::list neighborEdges(); }; typedef std::shared_ptr PyWStatePtr_t; @@ -69,8 +71,10 @@ struct PyWEdge { 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; From e60042702ee20cd3585070f89e3e162dcf11728f Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 22 Apr 2026 10:18:57 +0200 Subject: [PATCH 6/6] Add readable constraint error reporting --- src/CMakeLists.txt | 1 + src/pyhpp/constraints/by-substitution.cc | 53 +++++++++++++++- .../constraints/differentiable-function.cc | 3 + .../constraints/explicit-constraint-set.cc | 3 +- src/pyhpp/constraints/iterative-solver.cc | 15 ++++- src/pyhpp/core/constraint.cc | 9 ++- src/pyhpp/manipulation/graph.cc | 5 ++ src/pyhpp/manipulation/graph.hh | 2 +- src/pyhpp/tools/__init__.py | 1 + src/pyhpp/tools/constraint_error.py | 60 +++++++++++++++++++ 10 files changed, 147 insertions(+), 5 deletions(-) create mode 100644 src/pyhpp/tools/constraint_error.py diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 747cd7cd..816a444a 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 277b3328..1db7f11d 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 683e5532..27eecfd7 100644 --- a/src/pyhpp/constraints/differentiable-function.cc +++ b/src/pyhpp/constraints/differentiable-function.cc @@ -131,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 d90e4af9..a7331415 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 187fee06..b9c3b3a4 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 b02d7447..35d4908e 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -276,6 +276,10 @@ boost::python::list PyWState::neighborEdges() { } } +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(); } @@ -1282,6 +1286,7 @@ void exposeGraph() { class_("State", no_init) .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) diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index e37140d8..02e7523e 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -60,8 +60,8 @@ struct PyWState { 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; diff --git a/src/pyhpp/tools/__init__.py b/src/pyhpp/tools/__init__.py index ce53a5ee..a776cf56 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 00000000..c4e5078d --- /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}" + )