Add optimizer ConfigOptimization

CMakeLists.txt

@@ -81,6 +81,8 @@ SET (${PROJECT_NAME}_HEADERS
   include/hpp/manipulation/graph/graph-component.hh
   include/hpp/manipulation/graph/fwd.hh
   include/hpp/manipulation/graph/dot.hh
+
+  include/hpp/manipulation/path-optimization/config-optimization.hh
   )
 
 # Add dependency toward hpp-model library in pkg-config file.

include/hpp/manipulation/path-optimization/config-optimization.hh

+//
+// Copyright (c) 2015 CNRS
+// Authors: Joseph Mirabel
+//
+// 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_PATH_OPTIMIZATION_CONFIG_OPTIMIZATION_HH
+# define HPP_MANIPULATION_PATH_OPTIMIZATION_CONFIG_OPTIMIZATION_HH
+
+# include <hpp/core/path-optimization/config-optimization.hh>
+
+# include <hpp/manipulation/fwd.hh>
+
+namespace hpp {
+  namespace manipulation {
+    namespace pathOptimization {
+      /// \addtogroup path_optimization
+      /// \{
+
+      /// Optimize the waypoints of the path and optionally add the
+      /// constraint::ConfigurationConstraint to the ConfigProjector of the
+      /// path.
+      ///
+      /// See Parameters for information on how to tune the algorithm.
+      ///
+      /// \note The optimizer assumes that the input path is a vector of optimal
+      ///       paths for the distance function.
+      struct ConfigOptimizationTraits
+        : core::pathOptimization::ConfigOptimizationTraits {
+          typedef core::PathPtr_t PathPtr_t;
+
+          static size_type numberOfPass () { return 10; }
+
+          static size_type numberOfIterations () { return 1; }
+
+          static ConfigProjectorPtr_t getConfigProjector
+            (const PathPtr_t& before, const PathPtr_t& after, bool& reverse);
+        };
+      /// \}
+    } // namespace pathOptimization
+  } // namespace manipulation
+} // namespace hpp
+#endif // HPP_MANIPULATION_PATH_OPTIMIZATION_CONFIG_OPTIMIZATION_HH

src/CMakeLists.txt

@@ -41,6 +41,8 @@ ADD_LIBRARY(${LIBRARY_NAME} SHARED
   graph/statistics.cc
 
   graph/dot.cc
+
+  path-optimization/config-optimization.cc
 )
 
 PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-core)

src/path-optimization/config-optimization.cc

+// Copyright (c) 2015, 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/config-optimization.hh>
+
+#include <hpp/util/pointer.hh>
+
+#include <hpp/core/path.hh>
+#include <hpp/core/config-projector.hh>
+#include <hpp/manipulation/device.hh>
+#include <hpp/manipulation/graph/node.hh>
+#include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/constraint-set.hh>
+
+namespace hpp {
+  namespace manipulation {
+    namespace pathOptimization {
+      ConfigProjectorPtr_t ConfigOptimizationTraits::getConfigProjector
+        (const PathPtr_t& before, const PathPtr_t& after, bool& reverse)
+      {
+        ConstraintSetPtr_t setB =
+          HPP_STATIC_PTR_CAST (ConstraintSet, before->constraints ());
+        ConstraintSetPtr_t setA =
+          HPP_STATIC_PTR_CAST (ConstraintSet, after->constraints ());
+        assert (setA->edge () && setB->edge());
+        graph::GraphPtr_t graph = setA->edge()->parentGraph ();
+        graph::NodePtr_t n0 = graph->getNode (before->initial ()),
+                         n1 = graph->getNode (before->end ()),
+                         n2 = graph->getNode (after->initial ()),
+                         n3 = graph->getNode (after->end ());
+        /// Find if path were computed from init or goal config
+        assert ((n0 == setB->edge()->from () && n1 == setB->edge()->to ())
+                || (n1 == setB->edge()->from () && n0 == setB->edge()->to ()));
+        assert ((n2 == setA->edge()->from () && n3 == setA->edge()->to ())
+                || (n3 == setA->edge()->from () && n2 == setA->edge()->to ()));
+        bool reverseB =
+          (n1 == setB->edge()->from () && n0 == setB->edge()->to ());
+        bool reverseA =
+          (n3 == setA->edge()->from () && n2 == setA->edge()->to ());
+
+        reverse = reverseB;
+
+        ConfigProjectorPtr_t p = ConfigProjector::create (graph->robot(),
+            "intersect_" + setB->edge()->name() + "_" + setA->edge()->name(),
+            graph->errorThreshold (), graph->maxIterations ());
+
+        graph->insertNumericalConstraints (p);
+        // TODO: Is reverse case different ?
+        bool nodeB_Eq_nodeA = (setB->edge()->node() == setA->edge()->node());
+
+        setB->edge()->insertNumericalConstraints (p);
+        setB->edge()->node ()->insertNumericalConstraints (p);
+
+        graph->insertLockedJoints (p);
+        setB->edge()->insertLockedJoints (p);
+        setB->edge()->node ()->insertLockedJoints (p);
+
+        vector_t rhsB = p->rightHandSideFromConfig (before->initial ());
+
+        setA->edge()->insertNumericalConstraints (p);
+        if (nodeB_Eq_nodeA)
+          setA->edge()->node()->insertNumericalConstraints (p);
+        setA->edge()->insertLockedJoints (p);
+        if (nodeB_Eq_nodeA)
+          setA->edge()->node()->insertLockedJoints (p);
+
+        p->rightHandSideFromConfig (before->end ());
+        return p;
+      }
+    } // namespace pathOptimization
+  } // namespace manipulation
+} // namespace hpp

src/problem-solver.cc

@@ -36,6 +36,7 @@
 #include "hpp/manipulation/graph-optimizer.hh"
 #include "hpp/manipulation/graph-path-validation.hh"
 #include "hpp/manipulation/graph-node-optimizer.hh"
+#include "hpp/manipulation/path-optimization/config-optimization.hh"
 
 namespace hpp {
   namespace manipulation {
@@ -63,6 +64,9 @@ namespace hpp {
           PartialShortcut::createWithTraits <PartialShortcutTraits>);
       addPathOptimizerType ("Graph-PartialShortcut",
           GraphOptimizer::create <core::pathOptimization::PartialShortcut>);
+      addPathOptimizerType ("ConfigOptimization",
+          core::pathOptimization::ConfigOptimization::createWithTraits
+          <pathOptimization::ConfigOptimizationTraits>);
       pathPlannerType ("M-RRT");
       pathValidationType ("Graph-discretized", 0.05);
     }