From d4243797f73f4dc5fbc0f6bfdb1aad60fb87925f Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 26 Jul 2017 13:47:51 +0200
Subject: [PATCH] Add SplineGradientBased for manipulation path
---
CMakeLists.txt | 1 +
.../spline-gradient-based.hh | 75 +++++++++
src/CMakeLists.txt | 1 +
.../spline-gradient-based.cc | 148 ++++++++++++++++++
src/problem-solver.cc | 9 ++
5 files changed, 234 insertions(+)
create mode 100644 include/hpp/manipulation/path-optimization/spline-gradient-based.hh
create mode 100644 src/path-optimization/spline-gradient-based.cc
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 90672b7..2295f4f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -104,6 +104,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
+ include/hpp/manipulation/path-optimization/spline-gradient-based.hh
include/hpp/manipulation/problem-target/state.hh
)
diff --git a/include/hpp/manipulation/path-optimization/spline-gradient-based.hh b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
new file mode 100644
index 0000000..ec924bb
--- /dev/null
+++ b/include/hpp/manipulation/path-optimization/spline-gradient-based.hh
@@ -0,0 +1,75 @@
+// Copyright (c) 2017, 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_SPLINE_GRADIENT_BASED_HH
+# define HPP_MANIPULATION_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_HH
+
+#include <hpp/core/path-optimization/spline-gradient-based.hh>
+
+#include <hpp/manipulation/config.hh>
+#include <hpp/manipulation/problem.hh>
+
+namespace hpp {
+ namespace manipulation {
+ /// \addtogroup path_optimization
+ /// \{
+ namespace pathOptimization {
+ template <int _PolynomeBasis, int _SplineOrder>
+ class HPP_MANIPULATION_DLLAPI SplineGradientBased :
+ public core::pathOptimization::SplineGradientBased<_PolynomeBasis, _SplineOrder>
+ {
+ public:
+ enum {
+ PolynomeBasis = _PolynomeBasis,
+ SplineOrder = _SplineOrder
+ };
+ typedef core::pathOptimization::SplineGradientBased<PolynomeBasis, SplineOrder>
+ Parent_t;
+ typedef boost::shared_ptr<SplineGradientBased> Ptr_t;
+
+ typedef typename Parent_t::Spline Spline;
+ typedef typename Parent_t::SplinePtr_t SplinePtr_t;
+ typedef typename Parent_t::Splines_t Splines_t;
+
+ /// Return shared pointer to new object.
+ static Ptr_t create (const Problem& problem);
+
+ /// This is only for compatibility purpose (with ProblemSolver).
+ /// problem is statically casted to an object of type
+ /// const manipulation::Problem& and method create(const Problem&)
+ /// is called.
+ static Ptr_t createFromCore (const core::Problem& problem);
+
+ protected:
+ typedef typename Parent_t::LinearConstraint LinearConstraint;
+ typedef typename Parent_t::Solvers_t Solvers_t;
+
+ SplineGradientBased (const Problem& problem);
+
+ virtual void initializePathValidation(const Splines_t& splines);
+
+ virtual void addProblemConstraints (const core::PathVectorPtr_t& init,
+ const Splines_t& splines, LinearConstraint& lc, Solvers_t& ess) const;
+
+ virtual void constrainEndIntoState (const core::PathPtr_t& path,
+ const size_type& idxSpline, const SplinePtr_t& spline,
+ const graph::StatePtr_t state, LinearConstraint& lc) const;
+ }; // SplineGradientBased
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
+
+#endif // HPP_MANIPULATION_PATH_OPTIMIZATION_GRADIENT_BASED_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index d721213..b7810d4 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -50,6 +50,7 @@ SET(SOURCES
path-optimization/config-optimization.cc
path-optimization/keypoints.cc
+ path-optimization/spline-gradient-based.cc
problem-target/state.cc
)
diff --git a/src/path-optimization/spline-gradient-based.cc b/src/path-optimization/spline-gradient-based.cc
new file mode 100644
index 0000000..0e4c0dd
--- /dev/null
+++ b/src/path-optimization/spline-gradient-based.cc
@@ -0,0 +1,148 @@
+// Copyright (c) 2017, 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/spline-gradient-based.hh>
+
+#include <hpp/core/path-optimization/spline-gradient-based/linear-constraint.hh>
+
+#include <hpp/manipulation/constraint-set.hh>
+#include <hpp/manipulation/graph/state.hh>
+#include <hpp/manipulation/graph/edge.hh>
+
+namespace hpp {
+ namespace manipulation {
+ namespace pathOptimization {
+
+ template <int _PB, int _SO>
+ SplineGradientBased<_PB, _SO>::SplineGradientBased (const Problem& problem)
+ : Parent_t (problem)
+ {
+ this->checkOptimum_ = true;
+ }
+
+ // ----------- Convenience class -------------------------------------- //
+
+ // ----------- Resolution steps --------------------------------------- //
+
+ template <int _PB, int _SO>
+ typename SplineGradientBased<_PB, _SO>::Ptr_t SplineGradientBased<_PB, _SO>::create
+ (const Problem& problem)
+ {
+ SplineGradientBased* ptr = new SplineGradientBased (problem);
+ Ptr_t shPtr (ptr);
+ return shPtr;
+ }
+
+ template <int _PB, int _SO>
+ typename SplineGradientBased<_PB, _SO>::Ptr_t SplineGradientBased<_PB, _SO>::createFromCore
+ (const core::Problem& problem)
+ {
+ HPP_STATIC_CAST_REF_CHECK(const Problem, problem);
+ return create (static_cast<const Problem&>(problem));
+ }
+
+ template <int _PB, int _SO>
+ void SplineGradientBased<_PB, _SO>::initializePathValidation
+ (const Splines_t& splines)
+ {
+ this->validations_.resize(splines.size());
+ for (std::size_t i = 0; i < splines.size(); ++i) {
+ ConstraintSetPtr_t set = HPP_STATIC_PTR_CAST (ConstraintSet, splines[i]->constraints ());
+ if (set && set->edge())
+ this->validations_[i] = set->edge()->pathValidation();
+ else
+ this->validations_[i] = this->problem().pathValidation();
+ }
+ }
+
+ template <int _PB, int _SO>
+ void SplineGradientBased<_PB, _SO>::addProblemConstraints
+ (const core::PathVectorPtr_t& init, const Splines_t& splines, LinearConstraint& lc, Solvers_t& ss) const
+ {
+ assert (init->numberPaths() == splines.size() && ss.size() == splines.size());
+
+ const std::size_t last = splines.size() - 1;
+ for (std::size_t i = 0; i < last; ++i) {
+ core::PathPtr_t path = init->pathAtRank(i);
+ ConstraintSetPtr_t set = HPP_STATIC_PTR_CAST (ConstraintSet, splines[i]->constraints ());
+ if (!set || !set->edge())
+ std::invalid_argument ("Cannot optimize a path that has not been "
+ "generated with a graph.");
+ graph::EdgePtr_t transition = set->edge();
+
+ this->addProblemConstraintOnPath (path, i, splines[i], lc, ss[i]);
+
+ bool reversed = transition->direction (path);
+
+ // The path should always go through the start and end states of the
+ // transition.
+ if (reversed && transition->state() != from) {
+ // Do something different
+ constrainEndIntoState (path, i, splines[i], from, lc);
+ } else if (!reversed && transition->state() != transition->to()) {
+ // Do something different
+ constrainEndIntoState (path, i, splines[i], transition->to(), lc);
+ }
+ }
+ this->addProblemConstraintOnPath (init->pathAtRank(last), last, splines[last], lc, ss[last]);
+ }
+
+ template <int _PB, int _SO>
+ void SplineGradientBased<_PB, _SO>::constrainEndIntoState
+ (const core::PathPtr_t& path, const size_type& idxSpline,
+ const SplinePtr_t& spline, const graph::StatePtr_t state,
+ LinearConstraint& lc) const
+ {
+ typename Spline::BasisFunctionVector_t B1;
+ spline->basisFunctionDerivative(0, 1, B1);
+ // TODO Should we add zero velocity sometimes ?
+
+ ConstraintSetPtr_t set = state->configConstraint();
+ value_type guessThreshold = this->problem().getParameter ("SplineGradientBased/guessThreshold", value_type(-1));
+ Eigen::RowBlockIndexes select =
+ this->computeActiveParameters (path, set->configProjector()->solver(), guessThreshold);
+
+ const size_type rDof = this->robot_->numberDof(),
+ col = idxSpline * Spline::NbCoeffs * rDof,
+ row = lc.J.rows(),
+ nOutVar = select.nbIndexes();
+
+ // Add nOutVar constraints
+ lc.addRows(nOutVar);
+ matrix_t I = select.rview(matrix_t::Identity(rDof, rDof));
+ for (size_type k = 0; k < Spline::NbCoeffs; ++k)
+ lc.J.block (row, col + k * rDof, nOutVar, rDof) = B1(k) * I;
+ lc.b.segment(row, nOutVar) = select.rview(spline->parameters().transpose() * B1);
+
+ assert ((lc.J.block(row, col, nOutVar, rDof * Spline::NbCoeffs) * spline->rowParameters())
+ .isApprox(lc.b.segment(row, nOutVar)));
+ }
+
+ // ----------- Optimize ----------------------------------------------- //
+
+ // ----------- Convenience functions ---------------------------------- //
+
+ // ----------- Instanciate -------------------------------------------- //
+
+ template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>; // equivalent to StraightPath
+ template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>;
+ template class SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>;
+ template class SplineGradientBased<core::path::BernsteinBasis, 1>; // equivalent to StraightPath
+ template class SplineGradientBased<core::path::BernsteinBasis, 2>;
+ template class SplineGradientBased<core::path::BernsteinBasis, 3>;
+ } // namespace pathOptimization
+ } // namespace manipulation
+} // namespace hpp
diff --git a/src/problem-solver.cc b/src/problem-solver.cc
index 66c130e..7c7278c 100644
--- a/src/problem-solver.cc
+++ b/src/problem-solver.cc
@@ -48,6 +48,7 @@
#include "hpp/manipulation/graph-steering-method.hh"
#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/problem-target/state.hh"
#if HPP_MANIPULATION_HAS_WHOLEBODY_STEP
@@ -100,6 +101,14 @@ namespace hpp {
GraphConfigOptimizationTraits
<pathOptimization::ConfigOptimizationTraits>
>);
+
+ parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical1",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 1>::createFromCore);
+ parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical2",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 2>::createFromCore);
+ parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_cannonical3",pathOptimization::SplineGradientBased<core::path::CanonicalPolynomeBasis, 3>::createFromCore);
+ parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier1",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 1>::createFromCore);
+ parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier2",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 2>::createFromCore);
+ parent_t::add <PathOptimizerBuilder_t> ("SplineGradientBased_bezier3",pathOptimization::SplineGradientBased<core::path::BernsteinBasis, 3>::createFromCore);
+
using core::SteeringMethodBuilder_t;
parent_t::add <SteeringMethodBuilder_t> ("Graph-SteeringMethodStraight",
GraphSteeringMethod::create <core::SteeringMethodStraight>);
--
GitLab