spline-gradient-based-abstract.hh

Go to the documentation of this file.

// Copyright (c) 2017 CNRS
// Authors: Joseph Mirabel
//
// This file is part of hpp-core
// hpp-core 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-core 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-core  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_ABSTRACT_HH
# define HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_ABSTRACT_HH

#include <hpp/constraints/explicit-constraint-set.hh>

#include <hpp/constraints/solver/by-substitution.hh>

#include <hpp/core/path-optimizer.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/path/spline.hh>

#include <hpp/core/steering-method/spline.hh>
#include <hpp/core/path-optimization/linear-constraint.hh>

namespace hpp {
  namespace core {
    namespace pathOptimization {
      template <int _PolynomeBasis, int _SplineOrder>
      class HPP_CORE_DLLAPI SplineGradientBasedAbstract : public PathOptimizer
      {
        public:
          enum {
            PolynomeBasis = _PolynomeBasis,
            SplineOrder = _SplineOrder
          };
          typedef path::Spline<PolynomeBasis, SplineOrder> Spline;
          typedef typename Spline::Ptr_t SplinePtr_t;
          typedef std::vector<SplinePtr_t> Splines_t;


          static void copy (const Splines_t& in, Splines_t& out);

          void updateSplines (Splines_t& spline, const vector_t& param) const;

          void updateParameters (vector_t& param, const Splines_t& spline) const;

          static void interpolate (const Splines_t& a, const Splines_t& b,
              const value_type& alpha, Splines_t& res);


        protected:

          SplineGradientBasedAbstract (const Problem& problem);


          static PathVectorPtr_t cleanInput (const PathVectorPtr_t& input);

          typedef steeringMethod::Spline<PolynomeBasis, SplineOrder> SSM_t;
          typename SSM_t::Ptr_t steeringMethod_;

          void appendEquivalentSpline (const StraightPathPtr_t& path, Splines_t& splines) const;

          void appendEquivalentSpline (const InterpolatedPathPtr_t& path, Splines_t& splines) const;

          void appendEquivalentSpline (const PathVectorPtr_t& path, Splines_t& splines) const;



          std::vector<PathValidationPtr_t> validations_;

          virtual void initializePathValidation(const Splines_t& splines);

          typedef std::vector <std::pair <PathValidationReportPtr_t,
                  std::size_t> > Reports_t;
          Reports_t validatePath (const Splines_t& splines,
              std::vector<std::size_t>& reordering,
              bool stopAtFirst,
              bool reorder) const;



          typedef Eigen::RowBlockIndices RowBlockIndices;
          typedef std::vector <bool> Bools_t;
          typedef std::vector <size_type> Indices_t;
          struct SplineOptimizationData {
            SplineOptimizationData () {}
            SplineOptimizationData (size_type rDof)
            { activeParameters.addRow (0, rDof); }

            ConstraintSetPtr_t set;
            boost::shared_ptr<constraints::ExplicitConstraintSet> es;

            RowBlockIndices activeParameters;
          };
          typedef std::vector <SplineOptimizationData> SplineOptimizationDatas_t;

          void jointBoundConstraint (const Splines_t& splines, LinearConstraint& lc) const;

          std::size_t addBoundConstraints (const Indices_t& bci, const LinearConstraint& bc,
              Bools_t& activeConstraint, LinearConstraint& constraint) const;

          Indices_t validateBounds (const Splines_t& splines, const LinearConstraint& lc) const;

          void addContinuityConstraints (const Splines_t& splines, const size_type maxOrder, const SplineOptimizationDatas_t& ess, LinearConstraint& continuity);



          PathVectorPtr_t buildPathVector (const Splines_t& splines) const;

          DevicePtr_t robot_;

        private:
          // void addCollisionConstraint (const std::size_t idxSpline,
              // const SplinePtr_t& spline, const SplinePtr_t& nextSpline,
              // const SolverPtr_t& solver,
              // const CollisionPathValidationReportPtr_t& report,
              // LinearConstraint& collision, CollisionFunctions& functions) const;
      }; // SplineGradientBasedAbstract
    } // namespace pathOptimization
  }  // namespace core
} // namespace hpp

#endif // HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_ABSTRACT_HH