straight-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_STRAIGHT_PATH_HH
# define HPP_CORE_STRAIGHT_PATH_HH

# include <hpp/pinocchio/liegroup-element.hh>

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

namespace hpp {
  namespace core {

    class HPP_CORE_DLLAPI StraightPath : public Path
    {
    public:
      typedef Path parent_t;
      virtual ~StraightPath () {}

      static StraightPathPtr_t create (LiegroupSpacePtr_t space,
                                       vectorIn_t init,
                       vectorIn_t end,
                       interval_t interval,
                                       ConstraintSetPtr_t constraints = ConstraintSetPtr_t())
      {
    StraightPath* ptr;
        if (constraints)
          ptr = new StraightPath (space, init, end, interval, constraints);
        else
          ptr = new StraightPath (space, init, end, interval);
    StraightPathPtr_t shPtr (ptr);
    ptr->init (shPtr);
        ptr->checkPath ();
        return shPtr;
      }

      static StraightPathPtr_t create (LiegroupElementConstRef init,
                       LiegroupElementConstRef end,
                       interval_t interval,
                                       ConstraintSetPtr_t constraints = ConstraintSetPtr_t())
      {
        assert (init.space() == end.space());
    return create (init.space(), init.vector(), end.vector(), interval, constraints);
      }

      static StraightPathPtr_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 StraightPathPtr_t create (const DevicePtr_t& device,
                       ConfigurationIn_t init,
                       ConfigurationIn_t end,
                       interval_t interval)
      {
    StraightPath* ptr = new StraightPath (device, init, end, interval);
    StraightPathPtr_t shPtr (ptr);
    ptr->init (shPtr);
        ptr->checkPath ();
    return shPtr;
      }

      static StraightPathPtr_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 StraightPathPtr_t create (const DevicePtr_t& device,
                       ConfigurationIn_t init,
                       ConfigurationIn_t end,
                                       interval_t interval,
                       ConstraintSetPtr_t constraints)
      {
    StraightPath* ptr = new StraightPath (device, init, end, interval,
                          constraints);
    StraightPathPtr_t shPtr (ptr);
    ptr->init (shPtr);
        ptr->checkPath ();
    return shPtr;
      }

      static StraightPathPtr_t createCopy (const StraightPathPtr_t& path)
      {
    StraightPath* ptr = new StraightPath (*path);
    StraightPathPtr_t shPtr (ptr);
    ptr->init (shPtr);
        ptr->checkPath ();
    return shPtr;
      }

      static StraightPathPtr_t createCopy
    (const StraightPathPtr_t& path, const ConstraintSetPtr_t& constraints)
      {
    StraightPath* ptr = new StraightPath (*path, constraints);
    StraightPathPtr_t shPtr (ptr);
    ptr->init (shPtr);
        ptr->checkPath ();
    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);
      }

      void initialConfig (ConfigurationIn_t initial)
      {
    assert (initial.size () == initial_.size ());
    initial_ = initial;
      }

      void endConfig (ConfigurationIn_t end)
      {
    assert (end.size () == end_.size ());
    end_ = end;
      }
      
      DevicePtr_t device () const HPP_CORE_DEPRECATED;

      Configuration_t initial () const
      {
        return initial_;
      }

      Configuration_t end () const
      {
        return end_;
      }

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

      StraightPath (LiegroupSpacePtr_t space, vectorIn_t init, vectorIn_t end,
          interval_t interval);

      StraightPath (LiegroupSpacePtr_t space, vectorIn_t init, vectorIn_t end,
          interval_t interval, ConstraintSetPtr_t constraints);

      StraightPath (const DevicePtr_t& robot, ConfigurationIn_t init,
            ConfigurationIn_t end, interval_t interval);

      StraightPath (const DevicePtr_t& robot, ConfigurationIn_t init,
            ConfigurationIn_t end, value_type length,
            ConstraintSetPtr_t constraints);

      StraightPath (const DevicePtr_t& robot, ConfigurationIn_t init,
            ConfigurationIn_t end, interval_t interval,
            ConstraintSetPtr_t constraints);

      StraightPath (const StraightPath& path);

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

      void init (StraightPathPtr_t self)
      {
    parent_t::init (self);
    weak_ = self;
        checkPath ();
      }

      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&, const value_type&) const;

      PathPtr_t impl_extract (const interval_t& subInterval) const;

    protected:
      DevicePtr_t device_;
      LiegroupSpacePtr_t space_;
      Configuration_t initial_;
      Configuration_t end_;
    private:
      StraightPathWkPtr_t weak_;
    }; // class StraightPath
  } //   namespace core
} // namespace hpp
#endif // HPP_CORE_STRAIGHT_PATH_HH