From bbb65424eb2256bb48d059e12d0b82553a394dc1 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 3 Dec 2018 15:06:24 +0100
Subject: [PATCH] =?UTF-8?q?[RandomShortcut]=C2=A0Do=20not=20sample=20time?=
=?UTF-8?q?=20in=20short=20transitions.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
CMakeLists.txt | 1 +
.../path-optimization/random-shortcut.hh | 61 ++++++++++++++++
.../spline-gradient-based.hh | 1 +
src/CMakeLists.txt | 1 +
src/path-optimization/random-shortcut.cc | 70 +++++++++++++++++++
src/problem-solver.cc | 7 +-
6 files changed, 139 insertions(+), 2 deletions(-)
create mode 100644 include/hpp/manipulation/path-optimization/random-shortcut.hh
create mode 100644 src/path-optimization/random-shortcut.cc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 08e33712..8bfd99b7 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/enforce-transition-semantic.hh
+ include/hpp/manipulation/path-optimization/random-shortcut.hh
include/hpp/manipulation/path-optimization/spline-gradient-based.hh
include/hpp/manipulation/problem-target/state.hh
diff --git a/include/hpp/manipulation/path-optimization/random-shortcut.hh b/include/hpp/manipulation/path-optimization/random-shortcut.hh
new file mode 100644
index 00000000..7b777894
--- /dev/null
+++ b/include/hpp/manipulation/path-optimization/random-shortcut.hh
@@ -0,0 +1,61 @@
+// 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_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH
+# define HPP_MANIPULATION_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH
+
+#include <hpp/core/path-optimization/random-shortcut.hh>
+
+#include <hpp/manipulation/fwd.hh>
+#include <hpp/manipulation/config.hh>
+
+namespace hpp {
+ namespace manipulation {
+ /// \addtogroup path_optimization
+ /// \{
+ namespace pathOptimization {
+ HPP_PREDEF_CLASS (RandomShortcut);
+ typedef boost::shared_ptr<RandomShortcut> RandomShortcutPtr_t;
+
+ class HPP_MANIPULATION_DLLAPI RandomShortcut :
+ public core::pathOptimization::RandomShortcut
+ {
+ public:
+ /// Return shared pointer to new object.
+ static RandomShortcutPtr_t create (const core::Problem& problem)
+ {
+ return RandomShortcutPtr_t (new RandomShortcut (problem));
+ }
+
+ protected:
+ RandomShortcut (const core::Problem& problem)
+ : core::pathOptimization::RandomShortcut (problem)
+ {}
+
+ /// Sample times along currentOpt.
+ /// t1 and t2 will not be on a path whose transition was short.
+ virtual bool shootTimes (const core::PathVectorPtr_t& currentOpt,
+ const value_type& t0,
+ value_type& t1,
+ value_type& t2,
+ const value_type& t3);
+ }; // class RandomShortcut
+ /// \}
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATH_OPTIMIZATION_RANDOM_SHORTCUT_HH
diff --git a/include/hpp/manipulation/path-optimization/spline-gradient-based.hh b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
index 320076d9..dea847e5 100644
--- a/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
+++ b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
@@ -82,6 +82,7 @@ namespace hpp {
const SplinePtr_t& spline, LinearConstraint& lc) const;
}; // SplineGradientBased
} // namespace pathOptimization
+ ///Â \}
} // namespace manipulation
} // namespace hpp
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e2d703b5..aca64838 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -46,6 +46,7 @@ SET(SOURCES
graph/dot.cc
+ path-optimization/random-shortcut.cc
path-optimization/spline-gradient-based.cc
path-optimization/enforce-transition-semantic.cc
diff --git a/src/path-optimization/random-shortcut.cc b/src/path-optimization/random-shortcut.cc
new file mode 100644
index 00000000..6d11ff37
--- /dev/null
+++ b/src/path-optimization/random-shortcut.cc
@@ -0,0 +1,70 @@
+// 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/random-shortcut.hh>
+
+#include <hpp/core/path.hh>
+#include <hpp/core/path-vector.hh>
+#include <hpp/manipulation/constraint-set.hh>
+#include <hpp/manipulation/graph/edge.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathOptimization {
+ using core::PathPtr_t;
+ using core::PathVector;
+ using core::PathVectorPtr_t;
+
+ bool isShort (const PathVectorPtr_t& path, const value_type& t)
+ {
+ value_type localT;
+ std::size_t i = path->rankAtParam (t, localT);
+ PathPtr_t p = path->pathAtRank (i);
+ PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST (PathVector, p);
+ if (pv) return isShort (pv, localT);
+ ConstraintSetPtr_t c = HPP_DYNAMIC_PTR_CAST(ConstraintSet, path->constraints());
+ if (c) return c->edge()->isShort();
+ return false;
+ }
+
+ bool RandomShortcut::shootTimes (const PathVectorPtr_t& currentOpt,
+ const value_type& t0,
+ value_type& t1,
+ value_type& t2,
+ const value_type& t3)
+ {
+ bool ok = false;
+ for (int i = 0; i < 5; ++i)
+ {
+ value_type u2 = (t3-t0) * rand ()/RAND_MAX;
+ value_type u1 = (t3-t0) * rand ()/RAND_MAX;
+ if (isShort (currentOpt, u1) || isShort (currentOpt, u2))
+ continue;
+ if (u1 < u2) {
+ t1 = t0 + u1;
+ t2 = t0 + u2;
+ } else {
+ t1 = t0 + u2;
+ t2 = t0 + u1;
+ }
+ ok = true;
+ break;
+ }
+ return ok;
+ }
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 171ac2b6..8f8d0955 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -26,7 +26,7 @@
#include <hpp/constraints/convex-shape-contact.hh>
-#include <hpp/core/random-shortcut.hh>
+#include <hpp/core/path-optimization/random-shortcut.hh>
#include <hpp/core/path-optimization/partial-shortcut.hh>
#include <hpp/core/path-projector/progressive.hh>
#include <hpp/core/path-projector/dichotomy.hh>
@@ -53,6 +53,7 @@
#include "hpp/manipulation/graph-path-validation.hh"
#include "hpp/manipulation/graph-node-optimizer.hh"
#include "hpp/manipulation/path-optimization/spline-gradient-based.hh"
+#include "hpp/manipulation/path-optimization/random-shortcut.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"
@@ -109,8 +110,10 @@ namespace hpp {
pathPlanners.add ("M-RRT", ManipulationPlanner::create);
pathPlanners.add ("SymbolicPlanner", SymbolicPlanner::create);
+ pathOptimizers.add ("RandomShortcut",
+ pathOptimization::RandomShortcut::create);
pathOptimizers.add ("Graph-RandomShortcut",
- GraphOptimizer::create <core::RandomShortcut>);
+ GraphOptimizer::create <core::pathOptimization::RandomShortcut>);
pathOptimizers.add ("PartialShortcut", core::pathOptimization::
PartialShortcut::createWithTraits <PartialShortcutTraits>);
pathOptimizers.add ("Graph-PartialShortcut",
--
GitLab