From bff2f35cdfad8ee4e42b9d5d608e67ded335bf42 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 23 Oct 2026 10:44:57 +0200 Subject: [PATCH 1/3] [manipulation] Bind equivalent transitions in class Graph. --- src/pyhpp/manipulation/graph.cc | 52 +++++++++++++++++++++++++++++++-- src/pyhpp/manipulation/graph.hh | 4 +++ 2 files changed, 54 insertions(+), 2 deletions(-) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index a123c68..5a23652 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -31,6 +31,7 @@ #include <../src/pyhpp/manipulation/graph.hh> #include <../src/pyhpp/manipulation/problem.hh> #include +#include #include #include #include @@ -231,6 +232,34 @@ const char* DOC_CREATEPREPLACEMENTCONSTRAINT = const char* DOC_NEIGHBOREDGES = "Get the list of edges connected to this state."; +const char* DOC_GET_EQUIVALENT_TRANSITION = + "Get equivalent transition" + "" + " param transition: transition" + " return a weak pointer to the equivalent transition" + "" + "Transitions linking a regular node to a pregrasp node are equivalent to the loop transition of" + "the state except for the target state. A motion leading to a pregrasp is usually composed of" + "a sequence of local paths belonging to the loop transition, and a last local path belonging" + "to the transition leading to the pregrasp state. When time parameterizing this sequence of" + "local paths, the last one is considered as different and the time parameterization stops" + "before it. This method enable to correct this issue by returning the loop transition when given" + "the transition of the last local path. For any other transition, it returns the input."; + +const char* DOC_SET_EQUIVALENT_TRANSITION = + "Set equivalent transition" + "" + " param transition: input transition," + " equivalent: transition that is equivalent" + " return a weak pointer to the equivalent transition" + "" + "Transitions linking a regular node to a pregrasp node are equivalent to the loop transition of" + "the state except for the target state. A motion leading to a pregrasp is usually composed of" + "a sequence of local paths belonging to the loop transition, and a last local path belonging" + "to the transition leading to the pregrasp state. When time parameterizing this sequence of" + "local paths, the last one is considered as different and the time parameterization stops" + "before it. This method enable to correct this issue by returning the loop transition when given" + "the transition of the last local path. For any other transition, it returns the input."; } // namespace namespace pyhpp { @@ -530,6 +559,16 @@ PyWEdgePtr_t PyWGraph::getTransition(const std::string& edgeName) { } } +PyWEdgePtr_t PyWGraph::equivalentTransition(PyWEdgePtr_t edge) { + using namespace hpp::manipulation::graph; + EdgePtr_t e = obj->equivalentEdge(edge->obj); + return std::shared_ptr(new PyWEdge(e)); +} + +void PyWGraph::equivalentTransition(PyWEdgePtr_t edge, PyWEdgePtr_t equivalent) { + obj->equivalentEdge(edge->obj, equivalent->obj); +} + PyWStatePtr_t PyWGraph::getState(const std::string& stateName) { try { using namespace hpp::manipulation::graph; @@ -957,6 +996,10 @@ pyhpp::core::PyWPathValidationPtr_t PyWEdge::pyPathValidation() const { return std::make_shared(obj->pathValidation()); } +bool PyWEdge::operator==(const PyWEdge& other) const { + return obj == other.obj; +} + // ============================================================================= // Subgraph management // ============================================================================= @@ -1311,7 +1354,8 @@ void exposeGraph() { .def("waypoint", &PyWEdge::waypoint, DocClassMethod(waypoint)) .def("nbWaypoints", &PyWEdge::nbWaypoints, DocClassMethod(nbWaypoints)) .def("pathValidation", &PyWEdge::pyPathValidation, - DocClassMethod(pathValidation)); + DocClassMethod(pathValidation)) + .def(self == self); // DocClass(Graph) class_( @@ -1467,7 +1511,11 @@ void exposeGraph() { .def("transitionAtParam", &PyWGraph::transitionAtParam, "Return the transition used at a given parameter on a path (static " "method).") - .staticmethod("transitionAtParam"); + .staticmethod("transitionAtParam") + .def("equivalentTransition", static_cast( + &PyWGraph::equivalentTransition), DOC_GET_EQUIVALENT_TRANSITION) + .def("equivalentTransition", static_cast( + &PyWGraph::equivalentTransition), DOC_SET_EQUIVALENT_TRANSITION); } } // namespace manipulation diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index f7d6221..d70c409 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -78,6 +78,7 @@ struct PyWEdge { PyWEdge waypoint(int index) const; PathValidationPtr_t pathValidation() const; pyhpp::core::PyWPathValidationPtr_t pyPathValidation() const; + bool operator==(const PyWEdge& other) const; }; typedef std::shared_ptr PyWEdgePtr_t; @@ -239,6 +240,9 @@ struct PyWGraph { // Get transition at given parameter of a PathVector static PyWEdgePtr_t transitionAtParam(hpp::core::PathVectorPtr_t path, value_type param); + //Equivalent transition + PyWEdgePtr_t equivalentTransition(PyWEdgePtr_t edge); + void equivalentTransition(PyWEdgePtr_t edge, PyWEdgePtr_t equivalent); }; typedef std::shared_ptr PyWGraphPtr_t; From cf24eef25f3eb6d5fd36be1ba643c27fa865a15b Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 23 Oct 2026 11:03:30 +0200 Subject: [PATCH 2/3] [ConstraintGraphFactory] Register information about equivalent transitions when building the constraint graph. --- CHANGELOG.md | 2 ++ .../manipulation/constraint_graph_factory.py | 31 +++++++++++++++++++ 2 files changed, 33 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1ef583a..c1ee2dc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,8 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] +- [ConstraintGraphFactory] Register equivalent transitions +- [manipulation] Bind equivalent transition - [manipulation] Remove class ManipulationSpline - [path-validation] Create NoValidation without a ProblemSolver diff --git a/src/pyhpp/manipulation/constraint_graph_factory.py b/src/pyhpp/manipulation/constraint_graph_factory.py index 1f2ace4..a24d906 100644 --- a/src/pyhpp/manipulation/constraint_graph_factory.py +++ b/src/pyhpp/manipulation/constraint_graph_factory.py @@ -904,6 +904,8 @@ def __init__(self, graph, constraints=None): self.state_objects = {} self.edge_objects = {} self.preplaceDistance = {} + # Store loop transition by state + self.loopTransition = dict() # # \name Default functions # \{ @@ -925,6 +927,8 @@ def makeLoopTransition(self, state): ) self.edge_objects[n] = loop_edge self._add_constraints_to_transition(loop_edge, state.foliation) + # store loop transition by state + self.loopTransition[state.state_obj] = loop_edge def makeTransition(self, stateFrom, stateTo, ig): """ @@ -1068,6 +1072,21 @@ def _createWaypointState(name, constraints): ) self.edge_objects[nf] = forward_trans self.edge_objects[nb] = backward_trans + # Register equivalent transitions + if i == 0: + # First waypoint transition is equivalent to loop transition of + # source state + self.graph.equivalentTransition(forward_trans, + self.loopTransition[sf.state_obj]) + self.graph.equivalentTransition(backward_trans, + self.loopTransition[sf.state_obj]) + if i == nTransitions - 1 and nTransitions == 4: + # Last waypoint transition is equivalent to loop transition of + # target state + self.graph.equivalentTransition(forward_trans, + self.loopTransition[st.state_obj]) + self.graph.equivalentTransition(backward_trans, + self.loopTransition[st.state_obj]) if crossedFoliation: nf_ls = nf @@ -1085,6 +1104,12 @@ def _createWaypointState(name, constraints): containingState, ) self.edge_objects[edgeName] = level_set_edge_forward + # Register equivalent transitions + # First level set transition is equivalent to loop transition of + # source state + if nTransitions == 4: + self.graph.equivalentTransition(level_set_edge_forward, + self.loopTransition[sf.state_obj]) paramNC_constraints = self._get_constraint_objects( (st.foliation - sf.foliation).numConstraints @@ -1117,6 +1142,12 @@ def _createWaypointState(name, constraints): ) ) self.edge_objects[edgeName] = level_set_edge_backward + # Register equivalent transitions + # Last level set transition is equivalent to loop transition of + # target state + if nTransitions == 4: + self.graph.equivalentTransition(level_set_edge_backward, + self.loopTransition[st.state_obj]) pNC_constraints = self._get_constraint_objects( (sf.foliation - st.foliation).numConstraints From e7403896a9067d1c1fbfc0b67671a4600c1b0a60 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 26 Oct 2026 14:12:00 +0000 Subject: [PATCH 3/3] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .../manipulation/constraint_graph_factory.py | 32 ++++--- src/pyhpp/manipulation/graph.cc | 83 ++++++++++++------- src/pyhpp/manipulation/graph.hh | 2 +- 3 files changed, 72 insertions(+), 45 deletions(-) diff --git a/src/pyhpp/manipulation/constraint_graph_factory.py b/src/pyhpp/manipulation/constraint_graph_factory.py index a24d906..9b9af2b 100644 --- a/src/pyhpp/manipulation/constraint_graph_factory.py +++ b/src/pyhpp/manipulation/constraint_graph_factory.py @@ -1076,17 +1076,21 @@ def _createWaypointState(name, constraints): if i == 0: # First waypoint transition is equivalent to loop transition of # source state - self.graph.equivalentTransition(forward_trans, - self.loopTransition[sf.state_obj]) - self.graph.equivalentTransition(backward_trans, - self.loopTransition[sf.state_obj]) + self.graph.equivalentTransition( + forward_trans, self.loopTransition[sf.state_obj] + ) + self.graph.equivalentTransition( + backward_trans, self.loopTransition[sf.state_obj] + ) if i == nTransitions - 1 and nTransitions == 4: # Last waypoint transition is equivalent to loop transition of # target state - self.graph.equivalentTransition(forward_trans, - self.loopTransition[st.state_obj]) - self.graph.equivalentTransition(backward_trans, - self.loopTransition[st.state_obj]) + self.graph.equivalentTransition( + forward_trans, self.loopTransition[st.state_obj] + ) + self.graph.equivalentTransition( + backward_trans, self.loopTransition[st.state_obj] + ) if crossedFoliation: nf_ls = nf @@ -1108,8 +1112,10 @@ def _createWaypointState(name, constraints): # First level set transition is equivalent to loop transition of # source state if nTransitions == 4: - self.graph.equivalentTransition(level_set_edge_forward, - self.loopTransition[sf.state_obj]) + self.graph.equivalentTransition( + level_set_edge_forward, + self.loopTransition[sf.state_obj], + ) paramNC_constraints = self._get_constraint_objects( (st.foliation - sf.foliation).numConstraints @@ -1146,8 +1152,10 @@ def _createWaypointState(name, constraints): # Last level set transition is equivalent to loop transition of # target state if nTransitions == 4: - self.graph.equivalentTransition(level_set_edge_backward, - self.loopTransition[st.state_obj]) + self.graph.equivalentTransition( + level_set_edge_backward, + self.loopTransition[st.state_obj], + ) pNC_constraints = self._get_constraint_objects( (sf.foliation - st.foliation).numConstraints diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index 5a23652..2b85b7d 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -30,8 +30,8 @@ #include <../src/pyhpp/manipulation/device.hh> #include <../src/pyhpp/manipulation/graph.hh> #include <../src/pyhpp/manipulation/problem.hh> -#include #include +#include #include #include #include @@ -233,33 +233,47 @@ const char* DOC_NEIGHBOREDGES = "Get the list of edges connected to this state."; const char* DOC_GET_EQUIVALENT_TRANSITION = - "Get equivalent transition" - "" - " param transition: transition" - " return a weak pointer to the equivalent transition" - "" - "Transitions linking a regular node to a pregrasp node are equivalent to the loop transition of" - "the state except for the target state. A motion leading to a pregrasp is usually composed of" - "a sequence of local paths belonging to the loop transition, and a last local path belonging" - "to the transition leading to the pregrasp state. When time parameterizing this sequence of" - "local paths, the last one is considered as different and the time parameterization stops" - "before it. This method enable to correct this issue by returning the loop transition when given" - "the transition of the last local path. For any other transition, it returns the input."; + "Get equivalent transition" + "" + " param transition: transition" + " return a weak pointer to the equivalent transition" + "" + "Transitions linking a regular node to a pregrasp node are equivalent to " + "the loop transition of" + "the state except for the target state. A motion leading to a pregrasp is " + "usually composed of" + "a sequence of local paths belonging to the loop transition, and a last " + "local path belonging" + "to the transition leading to the pregrasp state. When time parameterizing " + "this sequence of" + "local paths, the last one is considered as different and the time " + "parameterization stops" + "before it. This method enable to correct this issue by returning the loop " + "transition when given" + "the transition of the last local path. For any other transition, it " + "returns the input."; const char* DOC_SET_EQUIVALENT_TRANSITION = - "Set equivalent transition" - "" - " param transition: input transition," - " equivalent: transition that is equivalent" - " return a weak pointer to the equivalent transition" - "" - "Transitions linking a regular node to a pregrasp node are equivalent to the loop transition of" - "the state except for the target state. A motion leading to a pregrasp is usually composed of" - "a sequence of local paths belonging to the loop transition, and a last local path belonging" - "to the transition leading to the pregrasp state. When time parameterizing this sequence of" - "local paths, the last one is considered as different and the time parameterization stops" - "before it. This method enable to correct this issue by returning the loop transition when given" - "the transition of the last local path. For any other transition, it returns the input."; + "Set equivalent transition" + "" + " param transition: input transition," + " equivalent: transition that is equivalent" + " return a weak pointer to the equivalent transition" + "" + "Transitions linking a regular node to a pregrasp node are equivalent to " + "the loop transition of" + "the state except for the target state. A motion leading to a pregrasp is " + "usually composed of" + "a sequence of local paths belonging to the loop transition, and a last " + "local path belonging" + "to the transition leading to the pregrasp state. When time parameterizing " + "this sequence of" + "local paths, the last one is considered as different and the time " + "parameterization stops" + "before it. This method enable to correct this issue by returning the loop " + "transition when given" + "the transition of the last local path. For any other transition, it " + "returns the input."; } // namespace namespace pyhpp { @@ -565,7 +579,8 @@ PyWEdgePtr_t PyWGraph::equivalentTransition(PyWEdgePtr_t edge) { return std::shared_ptr(new PyWEdge(e)); } -void PyWGraph::equivalentTransition(PyWEdgePtr_t edge, PyWEdgePtr_t equivalent) { +void PyWGraph::equivalentTransition(PyWEdgePtr_t edge, + PyWEdgePtr_t equivalent) { obj->equivalentEdge(edge->obj, equivalent->obj); } @@ -1355,7 +1370,7 @@ void exposeGraph() { .def("nbWaypoints", &PyWEdge::nbWaypoints, DocClassMethod(nbWaypoints)) .def("pathValidation", &PyWEdge::pyPathValidation, DocClassMethod(pathValidation)) - .def(self == self); + .def(self == self); // DocClass(Graph) class_( @@ -1512,10 +1527,14 @@ void exposeGraph() { "Return the transition used at a given parameter on a path (static " "method).") .staticmethod("transitionAtParam") - .def("equivalentTransition", static_cast( - &PyWGraph::equivalentTransition), DOC_GET_EQUIVALENT_TRANSITION) - .def("equivalentTransition", static_cast( - &PyWGraph::equivalentTransition), DOC_SET_EQUIVALENT_TRANSITION); + .def("equivalentTransition", + static_cast( + &PyWGraph::equivalentTransition), + DOC_GET_EQUIVALENT_TRANSITION) + .def("equivalentTransition", + static_cast( + &PyWGraph::equivalentTransition), + DOC_SET_EQUIVALENT_TRANSITION); } } // namespace manipulation diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index d70c409..b1ce5ee 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -240,7 +240,7 @@ struct PyWGraph { // Get transition at given parameter of a PathVector static PyWEdgePtr_t transitionAtParam(hpp::core::PathVectorPtr_t path, value_type param); - //Equivalent transition + // Equivalent transition PyWEdgePtr_t equivalentTransition(PyWEdgePtr_t edge); void equivalentTransition(PyWEdgePtr_t edge, PyWEdgePtr_t equivalent); };