dubins-path.hh

Go to the documentation of this file.

//
// Copyright (c) 2017 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_DUBINS_PATH_HH
# define HPP_CORE_DUBINS_PATH_HH

# include <hpp/pinocchio/device.hh>

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

namespace hpp {
  namespace core {

    class DubinsPath : public PathVector
    {
    public:
      typedef core::PathVector parent_t;

      virtual ~DubinsPath () {}

      static DubinsPathPtr_t create (const DevicePtr_t& device,
                     ConfigurationIn_t init,
                     ConfigurationIn_t end,
                                     value_type extraLength,
                     value_type rho,
                     size_type xyId, size_type rzId,
                                     const std::vector<JointPtr_t> wheels);
      static DubinsPathPtr_t create (const DevicePtr_t& device,
                     ConfigurationIn_t init,
                     ConfigurationIn_t end,
                                     value_type extraLength,
                     value_type rho,
                     size_type xyId, size_type rzId,
                                     const std::vector<JointPtr_t> wheels,
                     ConstraintSetPtr_t constraints);

      static DubinsPathPtr_t createCopy (const DubinsPathPtr_t& path)
      {
    DubinsPath* ptr = new DubinsPath (*path);
    DubinsPathPtr_t shPtr (ptr);
    ptr->init (shPtr);
    return shPtr;
      }

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

      inline DevicePtr_t device () const
      {
        return device_;
      }

      inline Configuration_t initial () const
      {
        return initial_;
      }

      inline Configuration_t end () const
      {
        return end_;
      }

    protected:
      virtual std::ostream& print (std::ostream &os) const
      {
    os << "DubinsPath:" << std::endl;
        Path::print (os);
    os << "initial configuration: " << initial_.transpose () << std::endl;
    os << "final configuration:   " << end_.transpose () << std::endl;
    return os;
      }
      DubinsPath (const DevicePtr_t& robot, ConfigurationIn_t init,
          ConfigurationIn_t end, value_type extraLength, value_type rho,
          size_type xyId, size_type rzId,
                  const std::vector<JointPtr_t> wheels);

      DubinsPath (const DevicePtr_t& robot, ConfigurationIn_t init,
          ConfigurationIn_t end, value_type extraLength, value_type rho,
          size_type xyId, size_type rzId,
                  const std::vector<JointPtr_t> wheels,
          ConstraintSetPtr_t constraints);

      DubinsPath (const DubinsPath& path);

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

      void init (DubinsPathPtr_t self);

    private:
      void dubins_init_normalised (double alpha, double beta, double d);
      void dubins_init (vector3_t q0, vector3_t q1);
      typedef Eigen::Matrix<value_type, 3, 1> Lengths_t;

      DevicePtr_t device_;
      Configuration_t initial_;
      Configuration_t end_;
      const size_type xyId_,rzId_;
      size_type dxyId_,drzId_;
      std::vector<JointPtr_t> wheels_;
      std::size_t typeId_;
      Lengths_t lengths_;
      value_type extraLength_, rho_;

      vector3_t qi_;       // the initial configuration
      DubinsPathWkPtr_t weak_;

    }; // class DubinsPath

  } //   namespace core
} // namespace hpp
#endif // HPP_CORE_DUBINS_PATH_HH