From 2450d12f86c7b48d6d14b4c77a9e155e97720122 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 25 May 2016 20:55:45 +0200
Subject: [PATCH] Add KeypointsShortcut
---
CMakeLists.txt | 1 +
include/hpp/manipulation/fwd.hh | 2 +
.../path-optimization/keypoints.hh | 78 +++++++
src/CMakeLists.txt | 1 +
src/path-optimization/keypoints.cc | 211 ++++++++++++++++++
src/problem-solver.cc | 4 +
6 files changed, 297 insertions(+)
create mode 100644 include/hpp/manipulation/path-optimization/keypoints.hh
create mode 100644 src/path-optimization/keypoints.cc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 722cd6dc..defacb53 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -91,6 +91,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/keypoints.hh
)
ADD_SUBDIRECTORY(src)
diff --git a/include/hpp/manipulation/fwd.hh b/include/hpp/manipulation/fwd.hh
index 075a4011..6a5d3ff7 100644
--- a/include/hpp/manipulation/fwd.hh
+++ b/include/hpp/manipulation/fwd.hh
@@ -135,6 +135,8 @@ namespace hpp {
namespace pathOptimization {
HPP_PREDEF_CLASS (SmallSteps);
typedef boost::shared_ptr < SmallSteps > SmallStepsPtr_t;
+ HPP_PREDEF_CLASS (Keypoints);
+ typedef boost::shared_ptr < Keypoints > KeypointsPtr_t;
} // namespace pathOptimization
} // namespace manipulation
} // namespace hpp
diff --git a/include/hpp/manipulation/path-optimization/keypoints.hh b/include/hpp/manipulation/path-optimization/keypoints.hh
new file mode 100644
index 00000000..3bb13f5c
--- /dev/null
+++ b/include/hpp/manipulation/path-optimization/keypoints.hh
@@ -0,0 +1,78 @@
+// Copyright (c) 2016, 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_KEYPOINTS_HH
+#define HPP_MANIPULATION_PATHOPTIMIZATION_KEYPOINTS_HH
+
+# include <hpp/core/path-optimizer.hh>
+# include <hpp/core/path-optimizer.hh>
+
+# include <hpp/manipulation/fwd.hh>
+# include <hpp/manipulation/config.hh>
+# include <hpp/manipulation/problem.hh>
+# include <hpp/manipulation/graph/fwd.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;
+ /// \addtogroup path_optimization
+ /// \{
+
+ class HPP_MANIPULATION_DLLAPI Keypoints : public core::PathOptimizer
+ {
+ public:
+ static KeypointsPtr_t create (const core::Problem& problem) {
+ const Problem& p = dynamic_cast <const Problem&> (problem);
+ return KeypointsPtr_t (new Keypoints (p));
+ }
+
+ virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
+
+ protected:
+ /// Constructor
+ Keypoints (const Problem& problem) :
+ PathOptimizer (problem), problem_ (problem) {}
+
+ private:
+ const Problem& problem_;
+ struct InterKeypointPath {
+ PathVectorPtr_t path;
+ bool isShort;
+ graph::EdgePtr_t edge;
+ };
+ typedef std::vector<InterKeypointPath> IKPvector_t;
+
+ IKPvector_t split (PathVectorPtr_t path) const;
+
+ PathVectorPtr_t shorten (const IKPvector_t& paths,
+ std::size_t i1, std::size_t i2) const;
+
+ IKPvector_t replaceInPath(const IKPvector_t& input,
+ const PathVectorPtr_t& shortcut,
+ std::size_t i1, std::size_t i2) const;
+ };
+
+ /// \}
+
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATHOPTIMIZATION_KEYPOINTS_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 81ba4798..98ea6c21 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -49,6 +49,7 @@ SET(SOURCES
graph/dot.cc
path-optimization/config-optimization.cc
+ path-optimization/keypoints.cc
)
IF(HPP_MANIPULATION_HAS_WHOLEBODY_STEP)
diff --git a/src/path-optimization/keypoints.cc b/src/path-optimization/keypoints.cc
new file mode 100644
index 00000000..f0b16713
--- /dev/null
+++ b/src/path-optimization/keypoints.cc
@@ -0,0 +1,211 @@
+// Copyright (c) 2016, 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/keypoints.hh>
+
+#include <hpp/core/path.hh>
+#include <hpp/core/path-vector.hh>
+#include <hpp/core/path-projector.hh>
+#include <hpp/core/path-validation.hh>
+
+#include <hpp/manipulation/graph/edge.hh>
+#include <hpp/manipulation/graph/graph.hh>
+#include <hpp/manipulation/constraint-set.hh>
+#include <hpp/manipulation/graph-steering-method.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathOptimization {
+ using std::make_pair;
+
+ PathVectorPtr_t Keypoints::optimize (const PathVectorPtr_t& path)
+ {
+ PathVectorPtr_t input = PathVector::create (
+ path->outputSize(), path->outputDerivativeSize());
+ PathVectorPtr_t output = path;
+ path->flatten (input);
+ // Step 1: find the key waypoints
+ // paths is a vector of
+ IKPvector_t paths = split (input);
+
+ // Step 2: for each couple of consecutive path between keywaypoints,
+ // try to find a better intermediate key waypoint.
+ std::size_t i1 = 0;
+ bool success = false;
+ for (std::size_t i2 = 1; i2 < paths.size(); ++i2) {
+ if (paths[i2].isShort) continue; // So that i2 = i1 + 2
+ // Try to generate a new key waypoint
+ PathVectorPtr_t shortcut = shorten (paths, i1, i2);
+ if (shortcut) {
+ paths = replaceInPath (paths, shortcut, i1, i2);
+ success = true;
+ while (i2 < paths.size() && !paths[i2].isShort) ++i2;
+ }
+ i1 = i2;
+ }
+
+ if (success) {
+ // Build path
+ PathVectorPtr_t output = PathVector::create (path->outputSize (),
+ path->outputDerivativeSize ());
+ for (std::size_t j = 0; j < paths.size(); ++j)
+ output->concatenate (*paths[j].path);
+ }
+ return output;
+ }
+
+ Keypoints::IKPvector_t Keypoints::split (PathVectorPtr_t input) const
+ {
+ IKPvector_t paths;
+
+ ConstraintSetPtr_t c;
+ for (std::size_t i_s = 0; i_s < input->numberPaths ();) {
+ InterKeypointPath ikpp;
+ ikpp.path = PathVector::create (
+ input->outputSize(), input->outputDerivativeSize());
+ PathPtr_t current = input->pathAtRank (i_s);
+ ikpp.path->appendPath (current);
+ c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
+ if (c) ikpp.edge = c->edge ();
+ ikpp.isShort = ikpp.edge && ikpp.edge->isShort();
+ std::size_t i_e = i_s + 1;
+ for (; i_e < input->numberPaths (); ++i_e) {
+ current = input->pathAtRank (i_e);
+ c = HPP_DYNAMIC_PTR_CAST (ConstraintSet, current->constraints ());
+ if (!c && ikpp.edge) {
+ hppDout(info, "No manipulation::ConstraintSet");
+ break;
+ }
+ if (c && ikpp.edge->node() != c->edge ()->node()) break;
+ if (ikpp.isShort != c->edge()->isShort()) // We do not optimize edges marked as short
+ break;
+ ikpp.path->appendPath (current);
+ }
+ hppDout(info, "Edge name: " << ikpp.edge->name());
+ i_s = i_e;
+ paths.push_back (ikpp);
+ }
+ return paths;
+ }
+
+ PathVectorPtr_t Keypoints::shorten (const IKPvector_t& paths,
+ std::size_t i1, std::size_t i2) const
+ {
+ PathVectorPtr_t result;
+ const std::size_t maxTrials = 10;
+ value_type cur_length = 0;
+ for (std::size_t i = i1; i < i2 + 1; ++i)
+ cur_length += paths[i].path->length();
+
+ const core::SteeringMethodPtr_t& sm (problem_.steeringMethod ());
+ PathProjectorPtr_t pathProjector (problem().pathProjector ());
+ core::PathValidationPtr_t pathValidation (problem ().pathValidation ());
+
+ // Get all the possible edges of the graph.
+ const InterKeypointPath& ikpp1 = paths[i1];
+ const InterKeypointPath& ikpp2 = paths[i2];
+ graph::Edges_t edges;
+ graph::Graph& graph = *problem_.constraintGraph ();
+ try {
+ edges = graph.getEdges (ikpp1.edge->node(), ikpp2.edge->node());
+ } catch (const std::logic_error& e) {
+ hppDout (error, e.what ());
+ return PathVectorPtr_t ();
+ }
+
+ value_type t0, t1;
+ Configuration_t q (ikpp1.path->outputSize()),
+ q1 (ikpp1.path->outputSize()),
+ q4 (ikpp1.path->outputSize());
+
+ // Unused variables
+ PathValidationReportPtr_t report;
+ PathPtr_t unusedP;
+
+ for (std::size_t n1 = 0; n1 < maxTrials; ++n1) {
+ // Generate a random parameter on p1.
+ t0 = ikpp1.path->timeRange().first;
+ t1 = t0 + ikpp1.path->timeRange().second * rand ()/RAND_MAX;
+ // Get the configuration and project it.
+ if (!(*ikpp1.path) (q1, t1)) continue;
+ for (graph::Edges_t::const_iterator _edges = edges.begin();
+ _edges != edges.end(); ++_edges) {
+ // TODO: For level set edges, something different should be done.
+ // Maybe, we must add a virtual generateInIntersection (qfrom, qto, qrand)
+ // that would be specialized for each edge.
+ q = q1;
+ if ((*_edges)->applyConstraints (q1,q)) {
+ PathPtr_t short1, proj1;
+ if ((*_edges)->build (short1, q1, q)) {
+ if (!short1 || !pathProjector->apply(short1, proj1)) continue;
+ if (!pathValidation->validate (proj1, false, unusedP, report)) continue;
+
+ // short1 is a valid path leading to the target node.
+ // Try to connect this to ikpp2.path
+ for (std::size_t n2 = 0; n2 < maxTrials; ++n2) {
+ value_type t3, t4, t5;
+ t3 = ikpp2.path->timeRange().first;
+ t4 = t3 + ikpp2.path->timeRange().second * rand ()/RAND_MAX;
+ t5 = t3 + ikpp2.path->timeRange().second;
+ if (!(*ikpp2.path) (q4, t4)) continue;
+ PathPtr_t short2, proj2;
+ short2 = (*sm)(q, q4);
+ if (!short2 || !pathProjector->apply(short2, proj2)) continue;
+ if (!pathValidation->validate (proj2, false, unusedP, report)) continue;
+ // short2 is possible. Build the path and check it is shorter.
+
+ PathVectorPtr_t tmp = PathVector::create (ikpp1.path->outputSize (),
+ ikpp1.path->outputDerivativeSize ());
+ tmp->concatenate (*(ikpp1.path->extract
+ (make_pair (t0, t1))-> as <PathVector> ()));
+ tmp->appendPath (short1);
+ tmp->appendPath (short2);
+ tmp->concatenate (*(ikpp2.path->extract
+ (make_pair (t4, t5))-> as <PathVector> ()));
+
+ if (cur_length >= tmp->length()) {
+ cur_length = tmp->length();
+ result = tmp;
+ }
+ }
+ }
+ }
+ }
+ }
+ return result;
+ }
+
+ Keypoints::IKPvector_t Keypoints::replaceInPath(const IKPvector_t& input,
+ const PathVectorPtr_t& shortcut,
+ std::size_t i1, std::size_t i2) const
+ {
+ PathVectorPtr_t flat = PathVector::create (shortcut->outputSize (),
+ shortcut->outputDerivativeSize ());
+ shortcut->flatten(flat);
+
+ PathVectorPtr_t output = PathVector::create (shortcut->outputSize (),
+ shortcut->outputDerivativeSize ());
+ for (std::size_t j = 0; j < i1; ++j)
+ output->concatenate (*input[j].path);
+ output->concatenate (*flat);
+ for (std::size_t j = i2 + 1; j < input.size(); ++j)
+ output->concatenate (*input[j].path);
+
+ return split (output);
+ }
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index cc3a711b..cd90779e 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -47,6 +47,7 @@
#include "hpp/manipulation/graph-node-optimizer.hh"
#include "hpp/manipulation/graph-steering-method.hh"
#include "hpp/manipulation/path-optimization/config-optimization.hh"
+#include "hpp/manipulation/path-optimization/keypoints.hh"
#if HPP_MANIPULATION_HAS_WHOLEBODY_STEP
#include <hpp/wholebody-step/small-steps.hh>
@@ -102,6 +103,9 @@ namespace hpp {
parent_t::add <SteeringMethodBuilder_t> ("Graph-SteeringMethodStraight",
GraphSteeringMethod::create <core::SteeringMethodStraight>);
+ parent_t::add <PathOptimizerBuilder_t> ("KeypointsShortcut",
+ pathOptimization::Keypoints::create);
+
#ifdef HPP_MANIPULATION_HAS_WHOLEBODY_STEP
parent_t::add <PathOptimizerBuilder_t>
("Walkgen", wholebodyStep::SmallSteps::create);
--
GitLab