Bind equivalent transitions in class Graph.

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
 

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,25 @@ 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 +1108,14 @@ 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 +1148,14 @@ 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

src/pyhpp/manipulation/graph.cc

@@ -30,6 +30,7 @@
 #include <../src/pyhpp/manipulation/device.hh>
 #include <../src/pyhpp/manipulation/graph.hh>
 #include <../src/pyhpp/manipulation/problem.hh>
+#include <boost/operators.hpp>
 #include <boost/python.hpp>
 #include <hpp/constraints/convex-shape-contact.hh>
 #include <hpp/constraints/differentiable-function.hh>
@@ -231,6 +232,48 @@ 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 +573,17 @@ 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<PyWEdge>(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 +1011,10 @@ pyhpp::core::PyWPathValidationPtr_t PyWEdge::pyPathValidation() const {
   return std::make_shared<pyhpp::core::PathValidation>(obj->pathValidation());
 }
 
+bool PyWEdge::operator==(const PyWEdge& other) const {
+  return obj == other.obj;
+}
+
 // =============================================================================
 // Subgraph management
 // =============================================================================
@@ -1311,7 +1369,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_<PyWGraph, PyWGraphPtr_t>(
@@ -1467,7 +1526,15 @@ 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<PyWEdgePtr_t (PyWGraph::*)(PyWEdgePtr_t)>(
+               &PyWGraph::equivalentTransition),
+           DOC_GET_EQUIVALENT_TRANSITION)
+      .def("equivalentTransition",
+           static_cast<void (PyWGraph::*)(PyWEdgePtr_t, PyWEdgePtr_t)>(
+               &PyWGraph::equivalentTransition),
+           DOC_SET_EQUIVALENT_TRANSITION);
 }
 
 }  // namespace manipulation

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<PyWEdge> 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<PyWGraph> PyWGraphPtr_t;