steering-kinodynamic.hh

Go to the documentation of this file.

// Copyright (c) 2016, LAAS-CNRS
// Authors: Pierre Fernbach (pierre.fernbach@laas.fr)
//
// 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_KINODYNAMIC_HH
# define HPP_CORE_STEERING_METHOD_KINODYNAMIC_HH

# include <hpp/util/debug.hh>
# include <hpp/util/pointer.hh>

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

namespace hpp {
  namespace core {
    namespace steeringMethod {

      typedef Eigen::Matrix <value_type, 3, 1>   Vector3;


      
      class HPP_CORE_DLLAPI Kinodynamic : public SteeringMethod
      {
      public:
        
        
        static KinodynamicPtr_t create (const Problem& problem)
        {
          Kinodynamic* ptr = new Kinodynamic (problem);
          KinodynamicPtr_t shPtr (ptr);
          ptr->init (shPtr);
          return shPtr;
        }
        
        static KinodynamicPtr_t createCopy
        (const KinodynamicPtr_t& other)
        {
          Kinodynamic* ptr = new Kinodynamic (*other);
          KinodynamicPtr_t shPtr (ptr);
          ptr->init (shPtr);
          return shPtr;
        }
        
        virtual SteeringMethodPtr_t copy () const
        {
          return createCopy (weak_.lock ());
        }
        
        virtual PathPtr_t impl_compute (ConfigurationIn_t q1,
                                        ConfigurationIn_t q2) const;
        
        double computeMinTime(int index,double p1,double p2, double v1, double v2,interval_t *infInterval) const;
        
        virtual void fixedTimeTrajectory(int index,double T, double p1, double p2, double v1, double v2, double *a1,double *t0, double* t1, double* tv, double* t2,double *vLim) const;
        

        void setAmax(Vector3 aMax){aMax_ = aMax;}

        void setVmax(Vector3 vMax){vMax_ = vMax;}

        
      protected:
        
        Kinodynamic (const Problem& problem);
        
        Kinodynamic (const Kinodynamic& other);
        
        void init (KinodynamicWkPtr_t weak)
        {
          core::SteeringMethod::init (weak);
          weak_ = weak;
        }

        Vector3 aMax_;
        Vector3 vMax_;
        double aMaxFixed_;
        double aMaxFixed_Z_;
        bool synchronizeVerticalAxis_;
        bool orientedPath_;
        bool orientationIgnoreZValue_;
      private:
        DeviceWkPtr_t device_;
        KinodynamicWkPtr_t weak_;
      }; // Kinodynamic
    } // namespace steeringMethod
  } // namespace core
} // namespace hpp
#endif // HPP_CORE_STEERING_METHOD_KINODYNAMIC_HH