constant-curvature.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_STEERING_METHOD_CONSTANT_CURVATURE_HH
# define HPP_CORE_STEERING_METHOD_CONSTANT_CURVATURE_HH

# include <hpp/core/fwd.hh>
# include <hpp/core/path.hh>
# include <hpp/core/steering-method/fwd.hh>

namespace hpp {
  namespace core {
    namespace steeringMethod {
      class ConstantCurvature : public Path {
      public:
        typedef Path parent_t;
        virtual ~ConstantCurvature () {}
        static ConstantCurvaturePtr_t create
        (const DevicePtr_t& robot, ConfigurationIn_t init,
         ConfigurationIn_t end,
         value_type curveLength, value_type pathLength,
         value_type curvature, size_type xyId, size_type rzId,
         const JointPtr_t rz, const std::vector<JointPtr_t> wheels,
         const ConstraintSetPtr_t& constraints);

        static ConstantCurvaturePtr_t createCopy
        (const ConstantCurvaturePtr_t& other);
 
        static ConstantCurvaturePtr_t createCopy
        (const ConstantCurvaturePtr_t& other,
         const ConstraintSetPtr_t& constraints);
 
        virtual PathPtr_t copy () const;

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

        inline Configuration_t end () const
        {
          return end_;
        }

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

        ConstantCurvature (const DevicePtr_t& robot, ConfigurationIn_t init,
                           ConfigurationIn_t end,
                           value_type curveLength, value_type pathLength,
                           value_type curvature, size_type xyId,
                           size_type rzId, const JointPtr_t rz,
                           const std::vector<JointPtr_t> wheels);

        ConstantCurvature (const DevicePtr_t& robot, ConfigurationIn_t init,
                           ConfigurationIn_t end,
                           value_type curveLength, value_type pathLength,
                           value_type curvature, size_type xyId, size_type rzId,
                           const JointPtr_t rz,
                           const std::vector<JointPtr_t> wheels,
                           ConstraintSetPtr_t constraints);

        ConstantCurvature (const ConstantCurvature& other);

        ConstantCurvature (const ConstantCurvature& other,
                           const ConstraintSetPtr_t& constraints);

        virtual bool impl_compute (ConfigurationOut_t result,
                                   value_type param) const;
        virtual void impl_derivative
        (vectorOut_t result, const value_type& param, size_type order) const;
        virtual void impl_velocityBound (vectorOut_t bound,
                                         const value_type& param0,
                                         const value_type& param1) const;

        virtual PathPtr_t impl_extract (const interval_t& paramInterval) const;

        void init (const ConstantCurvatureWkPtr_t& weak)
        {
          parent_t::init (weak);
          weak_ = weak;
        }
    private:
        void setWheelJoints (const JointPtr_t rz,
                             const std::vector<JointPtr_t> wheels);

        const DevicePtr_t robot_;
        Configuration_t initial_;
        Configuration_t end_;
        value_type curveLength_;
        const value_type curvature_;
        const size_type xyId_,rzId_;
        size_type dxyId_,drzId_;
        value_type forward_;
        struct Wheels_t {
          value_type value; // Constant value of the wheel angle
          JointPtr_t j;
          Wheels_t () : j () {}
        };
        std::vector<Wheels_t> wheels_;
        ConstantCurvatureWkPtr_t weak_;
      }; // class ConstantCurvature
    } // namespace steeringMethod
  } // namespace core
} // namespace hpp

#endif // HPP_CORE_STEERING_METHOD_CONSTANT_CURVATURE_HH