partial-shortcut.hh

Go to the documentation of this file.

//
// Copyright (c) 2015 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_PARTIAL_SHORTCUT_HH
# define HPP_CORE_PATH_OPTIMIZATION_PARTIAL_SHORTCUT_HH

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

namespace hpp {
  namespace core {
    namespace pathOptimization {

    typedef std::vector <JointConstPtr_t> JointStdVector_t;


      struct PartialShortcutTraits {
        static bool        removeLockedJoints ()
        { return true; }
        static bool        onlyFullShortcut ()
        { return false; }
        static std::size_t numberOfConsecutiveFailurePerJoints ()
        { return 5; }
        static value_type  progressionMargin ()
        { return 1e-3; }
      };

      class HPP_CORE_DLLAPI PartialShortcut : public PathOptimizer
      {
        public:
          template < typename Traits > static
            PartialShortcutPtr_t createWithTraits (const Problem& problem);

          static PartialShortcutPtr_t create (const Problem& problem);

          virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);

          struct Parameters {
            bool removeLockedJoints;

            bool onlyFullShortcut;

            std::size_t numberOfConsecutiveFailurePerJoints;

            value_type progressionMargin;

            Parameters ();
          } parameters;

        protected:
          PartialShortcut (const Problem& problem);

        private:
          PathVectorPtr_t generatePath (PathVectorPtr_t path, JointConstPtr_t joint,
              const value_type t1, ConfigurationIn_t q1,
              const value_type t2, ConfigurationIn_t q2) const;

          JointStdVector_t generateJointVector(const PathVectorPtr_t& pv) const;

          PathVectorPtr_t optimizeFullPath (const PathVectorPtr_t& pv,
              const JointStdVector_t &jvIn, JointStdVector_t &jvOut) const;

          PathVectorPtr_t optimizeRandom (const PathVectorPtr_t& pv,
              const JointStdVector_t &jv) const;
      }; // class RandomShortcut

      template < typename Traits > PartialShortcutPtr_t
        PartialShortcut::createWithTraits (const Problem& problem)
      {
        PartialShortcut* ptr = new PartialShortcut (problem);
        ptr->parameters.removeLockedJoints = Traits::removeLockedJoints();
        ptr->parameters.onlyFullShortcut   = Traits::onlyFullShortcut();
        ptr->parameters.progressionMargin  = Traits::progressionMargin ();
        ptr->parameters.numberOfConsecutiveFailurePerJoints =
          Traits::numberOfConsecutiveFailurePerJoints();
        return PartialShortcutPtr_t (ptr);
      }
    } // namespace pathOptimization
  } // namespace core
} // namespace hpp
#endif // HPP_CORE_PATH_OPTIMIZATION_PARTIAL_SHORTCUT_HH