Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/hpp/rbprm/planner/timed-parabola-path.hh	Lines:	0	32	0.0 %
Date:	2024-02-02 12:21:48	Branches:	0	30	0.0 %


//
// Copyright (c) 2017 CNRS
// Authors: Pierre Fernbach
//
// 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_RBPRM_TIMED_PARABOLA_PATH_HH
#define HPP_RBPRM_TIMED_PARABOLA_PATH_HH

#include <hpp/rbprm/planner/parabola-path.hh>

namespace hpp {
namespace rbprm {

// forward declaration of class
HPP_PREDEF_CLASS(TimedParabolaPath);
// Planner objects are manipulated only via shared pointers
typedef shared_ptr<TimedParabolaPath> TimedParabolaPathPtr_t;

/// ballistic path between 2 configurations
///
/// call parabola-path but work with the time as parameter instead of x_theta
class TimedParabolaPath : public ParabolaPath {
 public:
  typedef ParabolaPath parent_t;
  /// Destructor
  virtual ~TimedParabolaPath() {}

  /// Create instance and return shared pointer
  /// \param device Robot corresponding to configurations
  /// \param init, end Start and end configurations of the path
  /// \param parabolaPath : the path used to compute the position at given time
  static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
                                       core::ConfigurationIn_t init,
                                       core::ConfigurationIn_t end,
                                       ParabolaPathPtr_t parabolaPath) {
    TimedParabolaPath* ptr =
        new TimedParabolaPath(device, init, end, parabolaPath);
    TimedParabolaPathPtr_t shPtr(ptr);
    ptr->init(shPtr);
    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.
  static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
                                       core::ConfigurationIn_t init,
                                       core::ConfigurationIn_t end,
                                       core::value_type length,
                                       core::vector_t coefficients) {
    TimedParabolaPath* ptr =
        new TimedParabolaPath(device, init, end, length, coefficients);
    TimedParabolaPathPtr_t shPtr(ptr);
    ptr->init(shPtr);
    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 V0, Vimp initial and final velocity vectors
  /// \param initialROMnames, endROMnames initial and final ROM names
  static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
                                       core::ConfigurationIn_t init,
                                       core::ConfigurationIn_t end,
                                       core::value_type length,
                                       core::vector_t coefficients,
                                       core::vector_t V0, core::vector_t Vimp,
                                       std::vector<std::string> initialROMnames,
                                       std::vector<std::string> endROMnames) {
    TimedParabolaPath* ptr =
        new TimedParabolaPath(device, init, end, length, coefficients, V0, Vimp,
                              initialROMnames, endROMnames);
    TimedParabolaPathPtr_t shPtr(ptr);
    ptr->init(shPtr);
    return shPtr;
  }

  /// Create copy and return shared pointer
  /// \param path path to copy
  static TimedParabolaPathPtr_t createCopy(const TimedParabolaPathPtr_t& path) {
    TimedParabolaPath* ptr = new TimedParabolaPath(*path);
    TimedParabolaPathPtr_t shPtr(ptr);
    ptr->init(shPtr);
    return shPtr;
  }

  /// Create copy and return shared pointer
  /// \param path path to copy
  /// \param constraints the path is subject to
  /// <!> constraints part NOT IMPLEMENTED YET
  static TimedParabolaPathPtr_t createCopy(
      const TimedParabolaPathPtr_t& path,
      const core::ConstraintSetPtr_t& /*constraints*/) {
    // TimedParabolaPath* ptr = new TimedParabolaPath (*path, constraints);
    TimedParabolaPath* ptr = new TimedParabolaPath(*path);
    TimedParabolaPathPtr_t shPtr(ptr);
    ptr->init(shPtr);
    return shPtr;
  }

  /// Return a shared pointer to this
  ///
  /// As TimedParabolaPath are immutable, and refered to by shared pointers,
  /// they do not need to be copied.
  virtual core::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 core::PathPtr_t copy(
      const core::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 core::PathPtr_t extract(const core::interval_t& subInterval) const;

  /// Reversion of a path
  /// \return a new path that is this one reversed.
  virtual core::PathPtr_t reverse() const;

  /// Modify initial configuration
  /// \param initial new initial configuration
  /// \pre input configuration should be of the same size as current initial
  /// configuration
  void initialConfig(core::ConfigurationIn_t initial) {
    assert(initial.size() == initial_.size());
    initial_ = initial;
  }

  /// Modify end configuration
  /// \param end new end configuration
  /// \pre input configuration should be of the same size as current end
  /// configuration
  void endConfig(core::ConfigurationIn_t end) {
    assert(end.size() == end_.size());
    end_ = end;
  }

  /// Return the internal robot.
  core::DevicePtr_t device() const;

  /// Get the initial configuration
  core::Configuration_t initial() const { return initial_; }

  /// Get the final configuration
  core::Configuration_t end() const { return end_; }

  /// Get previously computed length
  virtual core::value_type length() const { return length_; }

 protected:
  /// Constructor
  TimedParabolaPath(const core::DevicePtr_t& robot,
                    core::ConfigurationIn_t init, core::ConfigurationIn_t end,
                    ParabolaPathPtr_t parabolaPath);

  /// Constructor
  TimedParabolaPath(const core::DevicePtr_t& robot,
                    core::ConfigurationIn_t init, core::ConfigurationIn_t end,
                    core::value_type length, core::vector_t coefficients);

  /// Constructor with velocities and ROMnames
  TimedParabolaPath(const core::DevicePtr_t& robot,
                    core::ConfigurationIn_t init, core::ConfigurationIn_t end,
                    core::value_type length, core::vector_t coefs,
                    core::vector_t V0, core::vector_t Vimp,
                    std::vector<std::string> initialROMnames,
                    std::vector<std::string> endROMnames);

  /// Copy constructor
  TimedParabolaPath(const TimedParabolaPath& path);

  void init(TimedParabolaPathPtr_t self) {
    parent_t::init(self);
    weak_ = self;
  }

  /// Param is the time
  virtual bool impl_compute(core::ConfigurationOut_t result,
                            core::value_type t) const;

  virtual double computeTimedLength(double x_theta, double v0, double alpha0);
  virtual double computeTimedLength(ParabolaPathPtr_t parabolaPath);

  /// Print path in a stream
  virtual std::ostream& print(std::ostream& os) const {
    os << "TimedParabolaPath:" << std::endl;
    os << "interval: [ " << timeRange().first << ", " << timeRange().second
       << " ]" << std::endl;
    os << "initial configuration: " << initial_.transpose() << std::endl;
    os << "final configuration:   " << end_.transpose() << std::endl;
    return os;
  }

 private:
  core::DevicePtr_t device_;
  core::Configuration_t initial_;
  core::Configuration_t end_;
  TimedParabolaPathWkPtr_t weak_;
  ParabolaPathPtr_t parabolaPath_;
  mutable core::value_type length_;

};  // class TimedParabolaPath
}  //   namespace rbprm
}  // namespace hpp

#endif  // HPP_RBPRM_TIMED_PARABOLA_PATH_HH


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2017 CNRS
3			// Authors: Pierre Fernbach
4			//
5			// This file is part of hpp-core
6			// hpp-core is free software: you can redistribute it
7			// and/or modify it under the terms of the GNU Lesser General Public
8			// License as published by the Free Software Foundation, either version
9			// 3 of the License, or (at your option) any later version.
10			//
11			// hpp-core is distributed in the hope that it will be
12			// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13			// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14			// General Lesser Public License for more details. You should have
15			// received a copy of the GNU Lesser General Public License along with
16			// hpp-core If not, see
17			// <http://www.gnu.org/licenses/>.
18
19			#ifndef HPP_RBPRM_TIMED_PARABOLA_PATH_HH
20			#define HPP_RBPRM_TIMED_PARABOLA_PATH_HH
21
22			#include <hpp/rbprm/planner/parabola-path.hh>
23
24			namespace hpp {
25			namespace rbprm {
26
27			// forward declaration of class
28			HPP_PREDEF_CLASS(TimedParabolaPath);
29			// Planner objects are manipulated only via shared pointers
30			typedef shared_ptr<TimedParabolaPath> TimedParabolaPathPtr_t;
31
32			/// ballistic path between 2 configurations
33			///
34			/// call parabola-path but work with the time as parameter instead of x_theta
35			class TimedParabolaPath : public ParabolaPath {
36			public:
37			typedef ParabolaPath parent_t;
38			/// Destructor
39			virtual ~TimedParabolaPath() {}
40
41			/// Create instance and return shared pointer
42			/// \param device Robot corresponding to configurations
43			/// \param init, end Start and end configurations of the path
44			/// \param parabolaPath : the path used to compute the position at given time
45			static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
46			core::ConfigurationIn_t init,
47			core::ConfigurationIn_t end,
48			ParabolaPathPtr_t parabolaPath) {
49			TimedParabolaPath* ptr =
50			new TimedParabolaPath(device, init, end, parabolaPath);
51			TimedParabolaPathPtr_t shPtr(ptr);
52			ptr->init(shPtr);
53			return shPtr;
54			}
55
56			/// Create instance and return shared pointer
57			/// \param device Robot corresponding to configurations
58			/// \param init, end Start and end configurations of the path
59			/// \param length Distance between the configurations.
60			static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
61			core::ConfigurationIn_t init,
62			core::ConfigurationIn_t end,
63			core::value_type length,
64			core::vector_t coefficients) {
65			TimedParabolaPath* ptr =
66			new TimedParabolaPath(device, init, end, length, coefficients);
67			TimedParabolaPathPtr_t shPtr(ptr);
68			ptr->init(shPtr);
69			return shPtr;
70			}
71
72			/// Create instance and return shared pointer
73			/// \param device Robot corresponding to configurations
74			/// \param init, end Start and end configurations of the path
75			/// \param length Distance between the configurations.
76			/// \param V0, Vimp initial and final velocity vectors
77			/// \param initialROMnames, endROMnames initial and final ROM names
78			static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
79			core::ConfigurationIn_t init,
80			core::ConfigurationIn_t end,
81			core::value_type length,
82			core::vector_t coefficients,
83			core::vector_t V0, core::vector_t Vimp,
84			std::vector<std::string> initialROMnames,
85			std::vector<std::string> endROMnames) {
86			TimedParabolaPath* ptr =
87			new TimedParabolaPath(device, init, end, length, coefficients, V0, Vimp,
88			initialROMnames, endROMnames);
89			TimedParabolaPathPtr_t shPtr(ptr);
90			ptr->init(shPtr);
91			return shPtr;
92			}
93
94			/// Create copy and return shared pointer
95			/// \param path path to copy
96			static TimedParabolaPathPtr_t createCopy(const TimedParabolaPathPtr_t& path) {
97			TimedParabolaPath* ptr = new TimedParabolaPath(*path);
98			TimedParabolaPathPtr_t shPtr(ptr);
99			ptr->init(shPtr);
100			return shPtr;
101			}
102
103			/// Create copy and return shared pointer
104			/// \param path path to copy
105			/// \param constraints the path is subject to
106			/// <!> constraints part NOT IMPLEMENTED YET
107			static TimedParabolaPathPtr_t createCopy(
108			const TimedParabolaPathPtr_t& path,
109			const core::ConstraintSetPtr_t& /constraints/) {
110			// TimedParabolaPath* ptr = new TimedParabolaPath (*path, constraints);
111			TimedParabolaPath* ptr = new TimedParabolaPath(*path);
112			TimedParabolaPathPtr_t shPtr(ptr);
113			ptr->init(shPtr);
114			return shPtr;
115			}
116
117			/// Return a shared pointer to this
118			///
119			/// As TimedParabolaPath are immutable, and refered to by shared pointers,
120			/// they do not need to be copied.
121			virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
122
123			/// Return a shared pointer to a copy of this and set constraints
124			///
125			/// \param constraints constraints to apply to the copy
126			/// \precond *this should not have constraints.
127			virtual core::PathPtr_t copy(
128			const core::ConstraintSetPtr_t& constraints) const {
129			return createCopy(weak_.lock(), constraints);
130			}
131
132			/// Extraction/Reversion of a sub-path
133			/// \param subInterval interval of definition of the extract path
134			/// If upper bound of subInterval is smaller than lower bound,
135			/// result is reversed.
136			virtual core::PathPtr_t extract(const core::interval_t& subInterval) const;
137
138			/// Reversion of a path
139			/// \return a new path that is this one reversed.
140			virtual core::PathPtr_t reverse() const;
141
142			/// Modify initial configuration
143			/// \param initial new initial configuration
144			/// \pre input configuration should be of the same size as current initial
145			/// configuration
146			void initialConfig(core::ConfigurationIn_t initial) {
147			assert(initial.size() == initial_.size());
148			initial_ = initial;
149			}
150
151			/// Modify end configuration
152			/// \param end new end configuration
153			/// \pre input configuration should be of the same size as current end
154			/// configuration
155			void endConfig(core::ConfigurationIn_t end) {
156			assert(end.size() == end_.size());
157			end_ = end;
158			}
159
160			/// Return the internal robot.
161			core::DevicePtr_t device() const;
162
163			/// Get the initial configuration
164			core::Configuration_t initial() const { return initial_; }
165
166			/// Get the final configuration
167			core::Configuration_t end() const { return end_; }
168
169			/// Get previously computed length
170			virtual core::value_type length() const { return length_; }
171
172			protected:
173			/// Constructor
174			TimedParabolaPath(const core::DevicePtr_t& robot,
175			core::ConfigurationIn_t init, core::ConfigurationIn_t end,
176			ParabolaPathPtr_t parabolaPath);
177
178			/// Constructor
179			TimedParabolaPath(const core::DevicePtr_t& robot,
180			core::ConfigurationIn_t init, core::ConfigurationIn_t end,
181			core::value_type length, core::vector_t coefficients);
182
183			/// Constructor with velocities and ROMnames
184			TimedParabolaPath(const core::DevicePtr_t& robot,
185			core::ConfigurationIn_t init, core::ConfigurationIn_t end,
186			core::value_type length, core::vector_t coefs,
187			core::vector_t V0, core::vector_t Vimp,
188			std::vector<std::string> initialROMnames,
189			std::vector<std::string> endROMnames);
190
191			/// Copy constructor
192			TimedParabolaPath(const TimedParabolaPath& path);
193
194			void init(TimedParabolaPathPtr_t self) {
195			parent_t::init(self);
196			weak_ = self;
197			}
198
199			/// Param is the time
200			virtual bool impl_compute(core::ConfigurationOut_t result,
201			core::value_type t) const;
202
203			virtual double computeTimedLength(double x_theta, double v0, double alpha0);
204			virtual double computeTimedLength(ParabolaPathPtr_t parabolaPath);
205
206			/// Print path in a stream
207			virtual std::ostream& print(std::ostream& os) const {
208			os << "TimedParabolaPath:" << std::endl;
209			os << "interval: [ " << timeRange().first << ", " << timeRange().second
210			<< " ]" << std::endl;
211			os << "initial configuration: " << initial_.transpose() << std::endl;
212			os << "final configuration: " << end_.transpose() << std::endl;
213			return os;
214			}
215
216			private:
217			core::DevicePtr_t device_;
218			core::Configuration_t initial_;
219			core::Configuration_t end_;
220			TimedParabolaPathWkPtr_t weak_;
221			ParabolaPathPtr_t parabolaPath_;
222			mutable core::value_type length_;
223
224			}; // class TimedParabolaPath
225			} // namespace rbprm
226			} // namespace hpp
227
228			#endif // HPP_RBPRM_TIMED_PARABOLA_PATH_HH