reeds-shepp-path.hh

Go to the documentation of this file.

//
// Copyright (c) 2016 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_REEDS_SHEPP_PATH_HH
# define HPP_CORE_REEDS_SHEPP_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 HPP_CORE_DLLAPI ReedsSheppPath : public PathVector
    {
    public:
      typedef core::PathVector parent_t;

      virtual ~ReedsSheppPath () {}

      static ReedsSheppPathPtr_t create (const pinocchio::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 ReedsSheppPathPtr_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 ReedsSheppPathPtr_t createCopy (const ReedsSheppPathPtr_t& path)
      {
    ReedsSheppPath* ptr = new ReedsSheppPath (*path);
    ReedsSheppPathPtr_t shPtr (ptr);
    ptr->init (shPtr);
    return shPtr;
      }

      static ReedsSheppPathPtr_t createCopy
    (const ReedsSheppPathPtr_t& path, const ConstraintSetPtr_t& constraints)
      {
    ReedsSheppPath* ptr = new ReedsSheppPath (*path, constraints);
    ReedsSheppPathPtr_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;
      ReedsSheppPath (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);

      ReedsSheppPath (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);

      ReedsSheppPath (const ReedsSheppPath& path);

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

      void init (ReedsSheppPathPtr_t self);

      virtual void impl_derivative (vectorOut_t result, const value_type& t,
                    size_type order) const;

    private:
      typedef Eigen::Matrix<value_type, 5, 1> Lengths_t;

      // Compute path
      void buildReedsShepp (const JointPtr_t rz,
                            const std::vector<JointPtr_t> wheels);
      // Setup path
      void setupPath (const std::size_t& typeId, double t, double u=0.,
                      double v=0., double w=0., double x=0.);

      void CSC  (const vector2_t& xy, const vector2_t& csPhi, const double& phi);
      void CCC  (const vector2_t& xy, const vector2_t& csPhi, const double& phi);
      void CCCC (const vector2_t& xy, const vector2_t& csPhi, const double& phi);
      void CCSC (const vector2_t& xy, const vector2_t& csPhi, const double& phi);
      void CCSCC(const vector2_t& xy, const vector2_t& csPhi, const double& phi);


      DevicePtr_t device_;
      Configuration_t initial_;
      Configuration_t end_;
      const size_type xyId_,rzId_;
      size_type dxyId_,drzId_;
      std::size_t typeId_;
      Lengths_t lengths_;
      // Distance traveled by the center of the rear wheel axis
      value_type rsLength_;
      // Extra length to take into account the motion of other joints:
      // Length of path = rsLength_ + extraLength_
      value_type extraLength_;
      // Minimal radius of curvature
      value_type rho_;
      ReedsSheppPathWkPtr_t weak_;
    }; // class ReedsSheppPath
  } //   namespace core
} // namespace hpp
#endif // HPP_CORE_REEDS_SHEPP_PATH_HH