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..9b9af2b 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,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 diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index a123c68..2b85b7d 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/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 #include #include #include @@ -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(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(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_( @@ -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( + &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..b1ce5ee 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;