gradient-based.hh

Go to the documentation of this file.

//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// 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_GRADIENT_BASED_HH
# define HPP_CORE_PATH_OPTIMIZATION_GRADIENT_BASED_HH

#include <hpp/core/path-optimizer.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/steering-method/straight.hh>

namespace hpp {
  namespace core {
    namespace pathOptimization {
      class CollisionConstraintsResult;
      typedef std::vector <CollisionConstraintsResult>
      CollisionConstraintsResults_t;

      class HPP_CORE_DLLAPI GradientBased : public PathOptimizer
      {
      public:
    static GradientBasedPtr_t create (const Problem& problem);

    virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);

      protected:
    GradientBased (const Problem& problem);

      private:
    typedef Eigen::JacobiSVD < matrix_t > Jacobi_t;

    void compressHessian (matrixIn_t normal, matrixOut_t small) const;
    void compressVector (vectorIn_t normal, vectorOut_t small) const;
    void uncompressVector (vectorIn_t small, vectorOut_t normal) const;

    void initialize (const PathVectorPtr_t& path);

    void pathToVector (const PathVectorPtr_t& path, vectorOut_t x) const {
      size_type index = 0;
      for (std::size_t i=0; i < path->numberPaths () - 1; ++i) {
        const PathPtr_t& localPath = path->pathAtRank (i);
        value_type t1 = localPath->timeRange ().second;
        (*localPath) (x.segment (index, configSize_), t1);
        index += configSize_;
      }
      assert (index == x.size ());
    }

    void vectorToPath (vectorIn_t x, const PathVectorPtr_t& result) const {
      Configuration_t q0 = initial_, q1;
      size_type index = 0;
      while (index < x.size ()) {
        q1 = x.segment (index, configSize_);
        PathPtr_t p = (*steeringMethod_) (q0, q1);
        result->appendPath (p);
        q0 = q1;
        index += configSize_;
      }
      q1 = end_;
      PathPtr_t p = (*steeringMethod_) (q0, q1);
      result->appendPath (p);
    }

    void initializeProblemConstraints ();
    void updateProblemConstraints (vectorIn_t x);

    bool constraintsSatisfied
      (vectorOut_t& x, PathVectorPtr_t& path,
       CollisionConstraintsResults_t& collisionConstraints);

    bool solveConstraints (vectorOut_t x, PathVectorPtr_t& path,
                   CollisionConstraintsResults_t&
                   collisionConstraints);
    void integrate (vectorIn_t x0, vectorIn_t step, vectorOut_t x1) const;

    vector_t computeIterate (vectorIn_t x) const;

    void displayPath (vectorIn_t x, std::string
#ifdef HPP_DEBUG
              prefix
#endif
              ) const {
      hppDout(info, prefix);
      for (int i=0; i<x.size (); i++)
        hppDout(info, x[i]);
      hppDout(info, "finish path parsing");
    }

    void addCollisionConstraint (const CollisionConstraintsResult& ccr,
                     const PathVectorPtr_t& path) const;

    void updateRightHandSide (const CollisionConstraintsResults_t&
                  collisionConstraints,
                  const PathVectorPtr_t& path) const;

    bool getProblemConstraints ();

    mutable CostPtr_t cost_;
    DevicePtr_t robot_;
    size_type configSize_;
    size_type robotNumberDofs_;
    size_type robotNbNonLockedDofs_;
    size_type numberDofs_;
    size_type fSize_; // dimension of collision constraints
    mutable Configuration_t initial_;
    mutable Configuration_t end_;
    WeighedDistancePtr_t distance_;
        steeringMethod::StraightPtr_t steeringMethod_;
    mutable matrix_t H_, Hinverse_, J_;
    mutable matrix_t Hz_, gz_;
    mutable vector_t stepNormal_;
    mutable bool fullRank_;
    mutable size_type nbWaypoints_;
    mutable value_type alpha_;
    mutable rowvector_t rgrad_;
    mutable vector_t rhs_;
    mutable vector_t value_;
    mutable vector_t p_, p0_;
    value_type epsilon_;
    std::size_t iterMax_;
    value_type alphaInit_; // .1, .2, .4
    value_type alphaMax_;
      }; // GradientBased
    } // namespace pathOptimization
  }  // namespace core
} // namespace hpp
#endif // HPP_CORE_PATH_OPTIMIZATION_GRADIENT_BASED_HH