<commit_before>// Copyright (c) 2016, LAAS-CNRS
// Authors: <NAME> (<EMAIL>)
//
// 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_KINODYNAMIC_PATH_HH
# define HPP_CORE_KINODYNAMIC_PATH_HH

# include <hpp/core/fwd.hh>
# include <hpp/core/config.hh>
# include <hpp/core/straight-path.hh>
# include <hpp/model/configuration.hh>

namespace hpp {
  namespace core {
    /// Path with 2 segment of constant acceleration or 3 segments with a constant velocity segment
    ///
    /// Degrees of freedom are interpolated depending on the type of
    /// \link hpp::model::Joint joint \endlink
    /// they parameterize:
    ///   \li linear interpolation for translation joints, bounded rotation
    ///       joints, and translation part of freeflyer joints,
    ///   \li angular interpolation for unbounded rotation joints,
    ///   \li constant angular velocity for SO(3) part of freeflyer joints.
    class HPP_CORE_DLLAPI KinodynamicPath : public StraightPath
    {
    public:
      typedef StraightPath parent_t;
      /// Destructor
      virtual ~KinodynamicPath () throw () {}
      
      /// Create instance and return shared pointer
      /// \param device Robot corresponding to configurations
      /// \param init, end Start and end configurations of the path
      /// \param length Distance between the configurations.
      static KinodynamicPathPtr_t create (const DevicePtr_t& device,
                                          ConfigurationIn_t init,
                                          ConfigurationIn_t end,
                                          value_type length,ConfigurationIn_t a1,ConfigurationIn_t t1,ConfigurationIn_t tv,ConfigurationIn_t t2,ConfigurationIn_t vLim)
      {
        KinodynamicPath* ptr = new KinodynamicPath (device, init, end, length,a1,t1,tv,t2,vLim);
        KinodynamicPathPtr_t shPtr (ptr);
        ptr->init (shPtr);
        ptr->checkPath ();
        return shPtr;
      }
      
      /// Create instance and return shared pointer
      /// \param device Robot corresponding to configurations
      /// \param init, end Start and end configurations of the path
      /// \param length Distance between the configurations.
      /// \param constraints the path is subject to
      static KinodynamicPathPtr_t create (const DevicePtr_t& device,
                                          ConfigurationIn_t init,
                                          ConfigurationIn_t end,
                                          value_type length,ConfigurationIn_t a1,ConfigurationIn_t t1,ConfigurationIn_t tv,ConfigurationIn_t t2,ConfigurationIn_t vLim,
                                          ConstraintSetPtr_t constraints)
      {
        KinodynamicPath* ptr = new KinodynamicPath (device, init, end, length,a1,t1,tv,t2,vLim,
                                                    constraints);
        KinodynamicPathPtr_t shPtr (ptr);
        ptr->init (shPtr);
        ptr->checkPath ();
        return shPtr;
      }
      
      /// Create copy and return shared pointer
      /// \param path path to copy
      static KinodynamicPathPtr_t createCopy (const KinodynamicPathPtr_t& path)
      {
        KinodynamicPath* ptr = new KinodynamicPath (*path);
        KinodynamicPathPtr_t shPtr (ptr);
        ptr->init (shPtr);
        ptr->checkPath ();
        return shPtr;
      }
      
      /// Create copy and return shared pointer
      /// \param path path to copy
      /// \param constraints the path is subject to
      static KinodynamicPathPtr_t createCopy
      (const KinodynamicPathPtr_t& path, const ConstraintSetPtr_t& constraints)
      {
        KinodynamicPath* ptr = new KinodynamicPath (*path, constraints);
        KinodynamicPathPtr_t shPtr (ptr);
        ptr->init (shPtr);
        ptr->checkPath ();
        return shPtr;
      }
      
      /// Return a shared pointer to this
      ///
      /// As StaightPath are immutable, and refered to by shared pointers,
      /// they do not need to be copied.
      virtual PathPtr_t copy () const
      {
        return createCopy (weak_.lock ());
      }
      
      /// Return a shared pointer to a copy of this and set constraints
      ///
      /// \param constraints constraints to apply to the copy
      /// \precond *this should not have constraints.
      virtual PathPtr_t copy (const ConstraintSetPtr_t& constraints) const
      {
        return createCopy (weak_.lock (), constraints);
      }
      
      
      /// Extraction/Reversion of a sub-path
      /// \param subInterval interval of definition of the extract path
      /// If upper bound of subInterval is smaller than lower bound,
      /// result is reversed.
      virtual PathPtr_t extract (const interval_t& subInterval) const
      throw (projection_error);
      

      
    protected:
      /// Print path in a stream
      virtual std::ostream& print (std::ostream &os) const
      {
        os << "KinodynamicPath:" << std::endl;
        os << "interval: [ " << timeRange ().first << ", "
           << timeRange ().second << " ]" << std::endl;
        os << "initial configuration: " << model::displayConfig(initial_ )<< std::endl;
        os << "final configuration:   " << model::displayConfig(end_) << std::endl;<commit_msg>add accesseur to switch times in kinodynamic-path<commit_after>// Copyright (c) 2016, LAAS-CNRS
// Authors: <NAME> (<EMAIL>)
//
// 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_KINODYNAMIC_PATH_HH
# define HPP_CORE_KINODYNAMIC_PATH_HH

# include <hpp/core/fwd.hh>
# include <hpp/core/config.hh>
# include <hpp/core/straight-path.hh>
# include <hpp/model/configuration.hh>

namespace hpp {
  namespace core {
    /// Path with 2 segment of constant acceleration or 3 segments with a constant velocity segment
    ///
    /// Degrees of freedom are interpolated depending on the type of
    /// \link hpp::model::Joint joint \endlink
    /// they parameterize:
    ///   \li linear interpolation for translation joints, bounded rotation
    ///       joints, and translation part of freeflyer joints,
    ///   \li angular interpolation for unbounded rotation joints,
    ///   \li constant angular velocity for SO(3) part of freeflyer joints.
    class HPP_CORE_DLLAPI KinodynamicPath : public StraightPath
    {
    public:
      typedef StraightPath parent_t;
      /// Destructor
      virtual ~KinodynamicPath () throw () {}
      
      /// Create instance and return shared pointer
      /// \param device Robot corresponding to configurations
      /// \param init, end Start and end configurations of the path
      /// \param length Distance between the configurations.
      static KinodynamicPathPtr_t create (const DevicePtr_t& device,
                                          ConfigurationIn_t init,
                                          ConfigurationIn_t end,
                                          value_type length,ConfigurationIn_t a1,ConfigurationIn_t t1,ConfigurationIn_t tv,ConfigurationIn_t t2,ConfigurationIn_t vLim)
      {
        KinodynamicPath* ptr = new KinodynamicPath (device, init, end, length,a1,t1,tv,t2,vLim);
        KinodynamicPathPtr_t shPtr (ptr);
        ptr->init (shPtr);
        ptr->checkPath ();
        return shPtr;
      }
      
      /// Create instance and return shared pointer
      /// \param device Robot corresponding to configurations
      /// \param init, end Start and end configurations of the path
      /// \param length Distance between the configurations.
      /// \param constraints the path is subject to
      static KinodynamicPathPtr_t create (const DevicePtr_t& device,
                                          ConfigurationIn_t init,
                                          ConfigurationIn_t end,
                                          value_type length,ConfigurationIn_t a1,ConfigurationIn_t t1,ConfigurationIn_t tv,ConfigurationIn_t t2,ConfigurationIn_t vLim,
                                          ConstraintSetPtr_t constraints)
      {
        KinodynamicPath* ptr = new KinodynamicPath (device, init, end, length,a1,t1,tv,t2,vLim,
                                                    constraints);
        KinodynamicPathPtr_t shPtr (ptr);
        ptr->init (shPtr);
        ptr->checkPath ();
        return shPtr;
      }
      
      /// Create copy and return shared pointer
      /// \param path path to copy
      static KinodynamicPathPtr_t createCopy (const KinodynamicPathPtr_t& path)
      {
        KinodynamicPath* ptr = new KinodynamicPath (*path);
        KinodynamicPathPtr_t shPtr (ptr);
        ptr->init (shPtr);
        ptr->checkPath ();
        return shPtr;
      }
      
      /// Create copy and return shared pointer
      /// \param path path to copy
      /// \param constraints the path is subject to
      static KinodynamicPathPtr_t createCopy
      (const KinodynamicPathPtr_t& path, const ConstraintSetPtr_t& constraints)
      {
        KinodynamicPath* ptr = new KinodynamicPath (*path, constraints);
        KinodynamicPathPtr_t shPtr (ptr);
        ptr->init (shPtr);
        ptr->checkPath ();
        return shPtr;
      }
      
      /// Return a shared pointer to this
      ///
      /// As StaightPath are immutable, and refered to by shared pointers,
      /// they do not need to be copied.
      virtual PathPtr_t copy () const
      {
        return createCopy (weak_.lock ());
      }
      
      /// Return a shared pointer to a copy of this and set constraints
      ///
      /// \param constraints constraints to apply to the copy
      /// \precond *this should not have constraints.
      virtual PathPtr_t copy (const ConstraintSetPtr_t& constraints) const
      {
        return createCopy (weak_.lock (), constraints);
      }
      
      
      /// Extraction/Reversion of a sub-path
      /// \param subInterval interval of definition of the extract path
      /// If upper bound of subInterval is smaller than lower bound,
      /// result is reversed.
      virtual PathPtr_t extract (const interval_t& subInterval) const
      throw (projection_error);
      

      vector_t getT1(){return t1_;}

      vector_t getT2(){return t2_;}

      vector_t getTv(){return tv_;}

      
    protected:
      /// Print path in a stream
      virtual std::ostream& print (std::ostream &os) const
      {
        os << "KinodynamicPath:" << std::endl;
        os << "interval: [ " << timeRange ().first << ", "
           << timeRange ().second << " ]" << std::endl;
        os << "initial configuration: " << model::displayConfig(initial_ )<< std::endl;
        os << "final configuration:   " << model::displayConfig(end_) << std::endl;
{'fraction_non_alphanumeric': 0.08712406802379158, 'fraction_numerical': 0.003686018262545028, 'mean_word_length': 2.1284067085953877, 'pattern_counts': {'":': 0, '<': 73, '<?xml version=': 0, '>': 33, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 32, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '111998', 'n_tokens_mistral': 3117, 'n_tokens_neox': 2999, 'n_words': 1098}