From 077f2f79d2b70b6c5939726991a0a1bffcd4f59f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 7 Oct 2015 11:04:50 +0200
Subject: [PATCH] Add optimizer ConfigOptimization
---
CMakeLists.txt | 2 +
.../path-optimization/config-optimization.hh | 55 ++++++++++++
src/CMakeLists.txt | 2 +
src/path-optimization/config-optimization.cc | 85 +++++++++++++++++++
src/problem-solver.cc | 4 +
5 files changed, 148 insertions(+)
create mode 100644 include/hpp/manipulation/path-optimization/config-optimization.hh
create mode 100644 src/path-optimization/config-optimization.cc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a40f409..16e200e 100644
--- a/CMakeLists.txt
+++ b/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.
diff --git a/include/hpp/manipulation/path-optimization/config-optimization.hh b/include/hpp/manipulation/path-optimization/config-optimization.hh
new file mode 100644
index 0000000..c634737
--- /dev/null
+++ b/include/hpp/manipulation/path-optimization/config-optimization.hh
@@ -0,0 +1,55 @@
+//
+// 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
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 26021e7..b15985e 100644
--- a/src/CMakeLists.txt
+++ b/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)
diff --git a/src/path-optimization/config-optimization.cc b/src/path-optimization/config-optimization.cc
new file mode 100644
index 0000000..a654d84
--- /dev/null
+++ b/src/path-optimization/config-optimization.cc
@@ -0,0 +1,85 @@
+// 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
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 4572755..d902052 100644
--- a/src/problem-solver.cc
+++ b/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);
}
--
GitLab