interpolated-path.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_INTERPOLATED_PATH_HH
# define HPP_CORE_INTERPOLATED_PATH_HH

# include <hpp/core/fwd.hh>
# include <hpp/core/config.hh>
# include <hpp/core/path.hh>

namespace hpp {
  namespace core {

    class HPP_CORE_DLLAPI InterpolatedPath : public Path
    {
    public:
      typedef std::pair <const value_type, Configuration_t> InterpolationPoint_t;
      typedef std::map <value_type, Configuration_t, std::less <value_type>,
        Eigen::aligned_allocator <InterpolationPoint_t> > InterpolationPoints_t;
      typedef Path parent_t;

      virtual ~InterpolatedPath () {}

      static InterpolatedPathPtr_t create (const DevicePtr_t& device,
                       ConfigurationIn_t init,
                       ConfigurationIn_t end,
                                       interval_t timeRange)
      {
    InterpolatedPath* ptr = new InterpolatedPath (device, init, end,
            timeRange);
    InterpolatedPathPtr_t shPtr (ptr);
    ptr->init (shPtr);
    return shPtr;
      }

      static InterpolatedPathPtr_t create (const DevicePtr_t& device,
                       ConfigurationIn_t init,
                       ConfigurationIn_t end,
                       interval_t timeRange,
                       ConstraintSetPtr_t constraints)
      {
    InterpolatedPath* ptr = new InterpolatedPath (device, init, end,
            timeRange, constraints);
    InterpolatedPathPtr_t shPtr (ptr);
    ptr->init (shPtr);
    return shPtr;
      }

      static InterpolatedPathPtr_t create (const DevicePtr_t& device,
                       ConfigurationIn_t init,
                       ConfigurationIn_t end,
                       value_type length)
      {
    return create (device, init, end, interval_t(0, length));
      }

      static InterpolatedPathPtr_t create (const DevicePtr_t& device,
                       ConfigurationIn_t init,
                       ConfigurationIn_t end,
                       value_type length,
                       ConstraintSetPtr_t constraints)
      {
        return create (device, init, end, interval_t(0, length), constraints);
      }

      static InterpolatedPathPtr_t createCopy (const InterpolatedPathPtr_t& path)
      {
    InterpolatedPath* ptr = new InterpolatedPath (*path);
    InterpolatedPathPtr_t shPtr (ptr);
    ptr->initCopy (shPtr);
    return shPtr;
      }

      static InterpolatedPathPtr_t create (const PathPtr_t& path,
          const DevicePtr_t& device, const std::size_t& nbSamples);

      static InterpolatedPathPtr_t createCopy
    (const InterpolatedPathPtr_t& path, const ConstraintSetPtr_t& constraints)
      {
    InterpolatedPath* ptr = new InterpolatedPath (*path, constraints);
    InterpolatedPathPtr_t shPtr (ptr);
    ptr->initCopy (shPtr);
    return shPtr;
      }

      virtual PathPtr_t copy () const
      {
    return createCopy (weak_.lock ());
      }

      virtual PathPtr_t copy (const ConstraintSetPtr_t& constraints) const
      {
    return createCopy (weak_.lock (), constraints);
      }

      virtual PathPtr_t reverse () const;

      DevicePtr_t device () const;

      void insert (const value_type& time, ConfigurationIn_t config)
      {
        configs_.insert (InterpolationPoint_t (time, config));
      }

      Configuration_t initial () const
      {
        return configs_.begin ()->second;
      }

      Configuration_t end () const
      {
        return configs_.rbegin ()->second;
      }

      const InterpolationPoints_t& interpolationPoints () const
      {
        return configs_;
      }

    protected:
      virtual std::ostream& print (std::ostream &os) const
      {
    os << "InterpolatedPath:" << std::endl;
        Path::print (os);
    os << "initial configuration: " << initial().transpose () << std::endl;
    os << "final configuration:   " << end().transpose () << std::endl;
    return os;
      }

      InterpolatedPath (const DevicePtr_t& robot, ConfigurationIn_t init,
            ConfigurationIn_t end, interval_t timeRange);

      InterpolatedPath (const DevicePtr_t& robot, ConfigurationIn_t init,
            ConfigurationIn_t end, interval_t timeRange,
            ConstraintSetPtr_t constraints);

      InterpolatedPath (const PathPtr_t& path, const DevicePtr_t& device,
                        const std::size_t& nbSamples);

      InterpolatedPath (const InterpolatedPath& path);

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

      void init (InterpolatedPathPtr_t self);

      void initCopy (InterpolatedPathPtr_t self);

      virtual bool impl_compute (ConfigurationOut_t result,
                 value_type param) const;
      virtual void impl_derivative (vectorOut_t result, const value_type& t,
                    size_type order) const;
      virtual void impl_velocityBound (vectorOut_t result,
          const value_type& t0, const value_type& t1) const;

      PathPtr_t impl_extract (const interval_t& subInterval) const;
    private:
      DevicePtr_t device_;
      InterpolationPoints_t configs_;
      InterpolatedPathWkPtr_t weak_;
    }; // class InterpolatedPath
  } //   namespace core
} // namespace hpp
#endif // HPP_CORE_INTERPOLATED_PATH_HH