From ade78b38a1ff2612ea7a383dae4fd8d4aebded18 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 4 Jul 2018 11:18:30 +0200
Subject: [PATCH] Add path optimization EnforceTransitionSemantic
---
CMakeLists.txt | 1 +
.../enforce-transition-semantic.hh | 60 +++++++
src/CMakeLists.txt | 1 +
.../enforce-transition-semantic.cc | 164 ++++++++++++++++++
src/problem-solver.cc | 3 +
5 files changed, 229 insertions(+)
create mode 100644 include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
create mode 100644 src/path-optimization/enforce-transition-semantic.cc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 52942a00..764cdc2f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -101,6 +101,7 @@ SET (${PROJECT_NAME}_HEADERS
include/hpp/manipulation/path-optimization/small-steps.hh
include/hpp/manipulation/path-optimization/config-optimization.hh
+ include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
include/hpp/manipulation/path-optimization/keypoints.hh
include/hpp/manipulation/path-optimization/spline-gradient-based.hh
diff --git a/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh b/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
new file mode 100644
index 00000000..de218025
--- /dev/null
+++ b/include/hpp/manipulation/path-optimization/enforce-transition-semantic.hh
@@ -0,0 +1,60 @@
+// Copyright (c) 2018, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_MANIPULATION_PATHOPTIMIZATION_ENFORCE_TRANSITION_SEMANTIC_HH
+#define HPP_MANIPULATION_PATHOPTIMIZATION_ENFORCE_TRANSITION_SEMANTIC_HH
+
+# include <hpp/core/path-optimizer.hh>
+
+# include <hpp/manipulation/problem.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathOptimization {
+ using hpp::core::Path;
+ using hpp::core::PathPtr_t;
+ using hpp::core::PathVector;
+ /// \addtogroup path_optimization
+ /// \{
+
+ class HPP_MANIPULATION_DLLAPI EnforceTransitionSemantic : public core::PathOptimizer
+ {
+ public:
+ typedef hpp::core::PathVectorPtr_t PathVectorPtr_t;
+ typedef boost::shared_ptr<EnforceTransitionSemantic> Ptr_t;
+
+ static Ptr_t create (const core::Problem& problem) {
+ const Problem& p = dynamic_cast <const Problem&> (problem);
+ return Ptr_t (new EnforceTransitionSemantic (p));
+ }
+
+ virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
+
+ protected:
+ /// Constructor
+ EnforceTransitionSemantic (const Problem& problem) :
+ PathOptimizer (problem), problem_ (problem) {}
+
+ private:
+ const Problem& problem_;
+ };
+
+ /// \}
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATHOPTIMIZATION_ENFORCE_TRANSITION_SEMANTIC_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 05380d4c..d66da53e 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -49,6 +49,7 @@ SET(SOURCES
path-optimization/config-optimization.cc
path-optimization/keypoints.cc
path-optimization/spline-gradient-based.cc
+ path-optimization/enforce-transition-semantic.cc
problem-target/state.cc
diff --git a/src/path-optimization/enforce-transition-semantic.cc b/src/path-optimization/enforce-transition-semantic.cc
new file mode 100644
index 00000000..14221df9
--- /dev/null
+++ b/src/path-optimization/enforce-transition-semantic.cc
@@ -0,0 +1,164 @@
+// Copyright (c) 2018, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-manipulation.
+// hpp-manipulation is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-manipulation is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
+
+#include <hpp/manipulation/path-optimization/enforce-transition-semantic.hh>
+
+#include <hpp/pinocchio/util.hh>
+#include <hpp/core/path-vector.hh>
+
+#include <hpp/manipulation/constraint-set.hh>
+#include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/graph/state.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathOptimization {
+ using hpp::core::Path;
+ using hpp::core::PathPtr_t;
+ using hpp::core::PathVector;
+ using hpp::core::PathVectorPtr_t;
+ using graph::Edges_t;
+ using graph::StatePtr_t;
+
+
+ Edges_t getAllEdges (const StatePtr_t& from, const StatePtr_t& to)
+ {
+ Edges_t edges;
+ for (graph::Neighbors_t::const_iterator it = from->neighbors ().begin ();
+ it != from->neighbors ().end (); ++it) {
+ if (it->second->to () == to)
+ edges.push_back (it->second);
+ }
+ for (Edges_t::const_iterator it = from->hiddenNeighbors ().begin ();
+ it != from->hiddenNeighbors ().end (); ++it) {
+ if ((*it)->to () == to)
+ edges.push_back (*it);
+ }
+ return edges;
+ }
+
+ PathVectorPtr_t EnforceTransitionSemantic::optimize (const PathVectorPtr_t& path)
+ {
+ PathVectorPtr_t input = PathVector::create (
+ path->outputSize(), path->outputDerivativeSize());
+ PathVectorPtr_t output = PathVector::create (
+ path->outputSize(), path->outputDerivativeSize());
+ path->flatten (input);
+
+ ConstraintSetPtr_t c;
+ for (std::size_t i = 0; i < input->numberPaths(); ++i) {
+ PathPtr_t current = input->pathAtRank (i);
+ output->appendPath(current);
+ c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
+ if (!c) {
+ hppDout(info, "No manipulation::ConstraintSet");
+ break;
+ }
+ Configuration_t q0 = current->initial();
+ Configuration_t q1 = current->end ();
+ StatePtr_t src = c->edge()->from();
+ StatePtr_t dst = c->edge()->to ();
+ if (src == dst) continue;
+
+ bool q0_in_src = src->contains(q0);
+ bool q1_in_src = src->contains(q1);
+ bool q0_in_dst = dst->contains(q0);
+ bool q1_in_dst = dst->contains(q1);
+
+ if (q0_in_src && q1_in_dst) // Nominal case
+ continue;
+ hppDout (warning, "Transition " << i << ". "
+ "\nsrc=" << src->name() <<
+ "\ndst=" << dst->name() <<
+ "\nq0_in_src=" << q0_in_src <<
+ "\nq1_in_src=" << q1_in_src <<
+ "\nq0_in_dst=" << q0_in_dst <<
+ "\nq1_in_dst=" << q1_in_dst <<
+ setpyformat <<
+ "\nq0=" << one_line(q0) <<
+ "\nq1=" << one_line(q1) <<
+ unsetpyformat <<
+ "\nTrying with state.");
+
+ StatePtr_t from, to;
+ if (q0_in_dst && q1_in_src) { // Reversed from nominal case
+ from = dst;
+ to = src;
+ } else if (q0_in_dst && q1_in_dst) {
+ from = dst;
+ to = dst;
+ } else if (q0_in_src && q1_in_src) {
+ from = src;
+ to = src;
+ } else if (q0_in_src) { // Keep same transition
+ continue;
+ } else if (q0_in_dst) { // Reverse current transition
+ from = dst;
+ to = src;
+ } else if (q1_in_src) { // Keep same transition
+ continue;
+ } else if (q1_in_dst) { // Reverse current transition
+ from = dst;
+ to = src;
+ }
+ if (from && to) {
+ // Check that a path from dst to to exists.
+ Edges_t transitions = getAllEdges(from, to);
+ if (transitions.size() >= 1)
+ {
+ if (transitions.size() > 1)
+ {
+ hppDout (info, "More than one transition...");
+ }
+ c->edge(transitions[0]);
+ continue;
+ }
+ }
+ hppDout (warning, "Enable to find a suitable transition for " << i << ". "
+ "\nsrc=" << src->name() <<
+ "\ndst=" << dst->name() <<
+ "\nq0_in_src=" << q0_in_src <<
+ "\nq1_in_src=" << q1_in_src <<
+ "\nq0_in_dst=" << q0_in_dst <<
+ "\nq1_in_dst=" << q1_in_dst <<
+ setpyformat <<
+ "\nq0=" << one_line(q0) <<
+ "\nq1=" << one_line(q1) <<
+ unsetpyformat <<
+ "\nTrying with state.");
+
+ StatePtr_t state = c->edge()->state();
+ // Check that a path from dst to to exists.
+ Edges_t transitions = getAllEdges(state, state);
+ if (transitions.size() >= 1)
+ {
+ if (transitions.size() > 1)
+ {
+ hppDout (info, "More than one transition...");
+ }
+ c->edge(transitions[0]);
+ continue;
+ } else {
+ hppDout (error, "Enable to find a suitable transition for " << *current);
+ }
+ }
+ return output;
+ }
+
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index ffde8570..49e99c38 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -55,6 +55,7 @@
#include "hpp/manipulation/path-optimization/config-optimization.hh"
#include "hpp/manipulation/path-optimization/keypoints.hh"
#include "hpp/manipulation/path-optimization/spline-gradient-based.hh"
+#include "hpp/manipulation/path-optimization/enforce-transition-semantic.hh"
#include "hpp/manipulation/problem-target/state.hh"
#include "hpp/manipulation/steering-method/cross-state-optimization.hh"
#include "hpp/manipulation/steering-method/graph.hh"
@@ -133,6 +134,8 @@ namespace hpp {
GraphConfigOptimizationTraits
<pathOptimization::ConfigOptimizationTraits>
>);
+ pathOptimizers.add ("EnforceTransitionSemantic",
+ pathOptimization::EnforceTransitionSemantic::create);
pathProjectors.add ("Progressive",
createPathProjector <core::pathProjector::Progressive>);
--
GitLab