path.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_HH
# define HPP_CORE_PATH_HH

# include <boost/concept_check.hpp>
# include <hpp/util/exception.hh>
# include <hpp/core/fwd.hh>
# include <hpp/core/config.hh>
# include <hpp/core/constraint-set.hh>
# include <hpp/core/deprecated.hh>
# include <hpp/core/projection-error.hh>
# include <hpp/core/time-parameterization.hh>

namespace hpp {
  namespace core {

    class HPP_CORE_DLLAPI Path
    {
    public:

      virtual ~Path () {}

      virtual PathPtr_t copy () const = 0;

      virtual PathPtr_t copy (const ConstraintSetPtr_t& constraints) const = 0;

      template <class T> boost::shared_ptr<T> as (void)
      {
    assert (HPP_DYNAMIC_PTR_CAST (T, weak_.lock ()));
    return HPP_STATIC_PTR_CAST (T, weak_.lock ());
      }

      template <class T> boost::shared_ptr<const T> as (void) const
      {
    assert (HPP_DYNAMIC_PTR_CAST (const T, weak_.lock ()));
    return HPP_STATIC_PTR_CAST (const T, weak_.lock ());
      }



      PathPtr_t extract (const interval_t& subInterval) const;

      PathPtr_t extract (const value_type& tmin, const value_type& tmax) const
      {
        return extract (std::make_pair(tmin, tmax));
      }

      virtual PathPtr_t reverse () const;



      Configuration_t operator () (const value_type& time) const
        HPP_CORE_DEPRECATED
      {
        bool unused;
        return (*this) (time, unused);
      }

      Configuration_t operator () (const value_type& time, bool& success) const
      {
        return configAtParam (paramAtTime(time), success);
      }

      bool operator () (ConfigurationOut_t result, const value_type& time)
       const
      {
        value_type s = paramAtTime (time);
    bool success = impl_compute (result, s);
    if (!success) return false;
        return applyConstraints (result, s);
      }

      Configuration_t eval (const value_type& time, bool& success) const
      {
        return this->operator() (time, success);
      }

      bool eval (ConfigurationOut_t result, const value_type& time)
       const
      {
        return this->operator() (result, time);
      }

      bool at (const value_type& time, ConfigurationOut_t result) const
      {
        return impl_compute (result, paramAtTime(time));
      }

      void derivative (vectorOut_t result, const value_type& time,
          size_type order) const;

      void velocityBound (vectorOut_t result, const value_type& t0, const value_type& t1) const
      {
        assert(result.size() == outputDerivativeSize());
        assert(t0 <= t1);
    impl_velocityBound (result,
            paramAtTime (std::max(t0, timeRange().first )),
            paramAtTime (std::min(t1, timeRange().second)));
        if (timeParam_)
          result *= timeParam_->derivativeBound (t0, t1);
      }


      size_type outputSize () const
      {
    return outputSize_;
      }

      size_type outputDerivativeSize () const
      {
    return outputDerivativeSize_;
      }

      const interval_t& timeRange () const
      {
    return timeRange_;
      }

      virtual value_type length () const
      {
    return timeRange_.second - timeRange_.first;
      }

      virtual Configuration_t initial () const = 0;

      virtual Configuration_t end () const = 0;

      const ConstraintSetPtr_t& constraints () const
      {
    return constraints_;
      }



      const interval_t& paramRange () const
      {
    return paramRange_;
      }

      void timeParameterization (const TimeParameterizationPtr_t& tp,
          const interval_t& tr)
      {
        timeParam_ = tp;
        timeRange (tr);
      }


    protected:
      virtual std::ostream& print (std::ostream &os) const;

      Path (const interval_t& interval, size_type outputSize,
        size_type outputDerivativeSize,
        const ConstraintSetPtr_t& constraints);

      Path (const interval_t& interval, size_type outputSize,
        size_type outputDerivativeSize);

      Path (const Path& path);

      Path (const Path& path, const ConstraintSetPtr_t& constraints);

      void init (const PathWkPtr_t& self);

      interval_t paramRange_;

      void constraints (const ConstraintSetPtr_t& constraint) {
        constraints_ = constraint;
      }

      virtual void checkPath () const;

      void timeRange (const interval_t& timeRange)
      {
        timeRange_ = timeRange;
        if (timeParam_) {
          paramRange_.first  = timeParam_->value(timeRange_.first );
          paramRange_.second = timeParam_->value(timeRange_.second);
        } else
          paramRange_ = timeRange_;
      }

      const TimeParameterizationPtr_t& timeParameterization() const
      {
        return timeParam_;
      }

      value_type paramLength() const
      {
        return paramRange_.second - paramRange_.first;
      }

      Configuration_t configAtParam (const value_type& param, bool& success) const
      {
    Configuration_t result (outputSize ());
    success = impl_compute (result, param);
    if (!success) return result;
        success = applyConstraints (result, param);
    return result;
      }

      virtual bool impl_compute (ConfigurationOut_t configuration,
                 value_type param) const = 0;

      virtual void impl_derivative (vectorOut_t derivative,
                                    const value_type& param,
                    size_type order) const
      {
        (void) derivative;
        (void) param;
        (void) order;
    HPP_THROW_EXCEPTION (hpp::Exception, "not implemented");
      }

      virtual void impl_velocityBound (vectorOut_t bound,
                                       const value_type& param0,
                                       const value_type& param1) const
      {
        (void) bound;
        (void) param0;
        (void) param1;
    HPP_THROW_EXCEPTION (hpp::Exception, "not implemented");
      }

      virtual PathPtr_t impl_extract (const interval_t& paramInterval) const;

    private:
      interval_t timeRange_;

      value_type paramAtTime (const value_type& time) const
      {
        if (timeParam_) {
          return timeParam_->value (time);
        }
        return time;
      }

      bool applyConstraints (ConfigurationOut_t result, const value_type& param) const;

      size_type outputSize_;
      size_type outputDerivativeSize_;
      ConstraintSetPtr_t constraints_;
      TimeParameterizationPtr_t timeParam_;
      PathWkPtr_t weak_;
      friend std::ostream& operator<< (std::ostream& os, const Path& path);
      friend class ExtractedPath;
    }; // class Path
    inline std::ostream& operator<< (std::ostream& os, const Path& path)
    {
      return path.print (os);
    }

  } //   namespace core
} // namespace hpp
#endif // HPP_CORE_PATH_HH