Head

GCC Code Coverage Report

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


//
// Copyright (c) 2015 CNRS
// Authors: Mylene Campana
//
// 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_PARABOLA_PATH_HH
#define HPP_RBPRM_PARABOLA_PATH_HH

#include <hpp/core/config.hh>
#include <hpp/core/fwd.hh>
#include <hpp/core/path.hh>

namespace hpp {
namespace rbprm {
using core::size_type;

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

/// Linear interpolation between two configurations
///
/// Degrees of freedom are interpolated depending on the type of
/// \link hpp::pinocchio::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 ParabolaPath : public core::Path {
 public:
  typedef Path parent_t;
  /// Destructor
  virtual ~ParabolaPath() {}

  /// 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 ParabolaPathPtr_t create(const core::DevicePtr_t& device,
                                  core::ConfigurationIn_t init,
                                  core::ConfigurationIn_t end,
                                  core::value_type length,
                                  core::vector_t coefficients) {
    ParabolaPath* ptr =
        new ParabolaPath(device, init, end, length, coefficients);
    ParabolaPathPtr_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 ParabolaPathPtr_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) {
    ParabolaPath* ptr =
        new ParabolaPath(device, init, end, length, coefficients, V0, Vimp,
                         initialROMnames, endROMnames);
    ParabolaPathPtr_t shPtr(ptr);
    ptr->init(shPtr);
    return shPtr;
  }

  /// Create copy and return shared pointer
  /// \param path path to copy
  static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t& path) {
    ParabolaPath* ptr = new ParabolaPath(*path);
    ParabolaPathPtr_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 ParabolaPathPtr_t createCopy(
      const ParabolaPathPtr_t& path,
      const core::ConstraintSetPtr_t& /*constraints*/) {
    // ParabolaPath* ptr = new ParabolaPath (*path, constraints);
    ParabolaPath* ptr = new ParabolaPath(*path);
    ParabolaPathPtr_t shPtr(ptr);
    ptr->init(shPtr);
    return shPtr;
  }

  /// Return a shared pointer to this
  ///
  /// As ParabolaPath 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_; }

  /// Set the three parabola coefficients
  void coefficients(core::vector_t coefs) const {
    for (size_type i = 0; i < coefs.size(); i++) coefficients_(i) = coefs(i);
  }

  /// Get path coefficients
  core::vector_t coefficients() const { return coefficients_; }

  virtual core::value_type computeLength(
      const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2) const;

  /// Evaluate velocity vector at path abcissa t
  core::vector_t evaluateVelocity(const core::value_type t) const;

  core::value_type alpha_;     // chosen alpha in interval
  core::value_type alphaMin_;  // min bound of alpha interval
  core::value_type alphaMax_;  // max bound of alpha interval
  core::value_type Xtheta_;
  core::value_type Z_;
  core::vector_t V0_;                         // initial velocity
  core::vector_t Vimp_;                       // final velocity
  std::vector<std::string> initialROMnames_;  // active ROM list at begining
  std::vector<std::string> endROMnames_;      // active ROM list at end

 protected:
  /// Print path in a stream
  virtual std::ostream& print(std::ostream& os) const {
    os << "ParabolaPath:" << 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;
  }
  /// Constructor
  ParabolaPath(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
  ParabolaPath(const core::DevicePtr_t& device, 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
  ParabolaPath(const ParabolaPath& path);

  core::value_type lengthFunction(const core::value_type x) const;

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

  /*void initCopy (ParabolaPathPtr_t self)
  {
    parent_t::initCopy (self);
    weak_ = self;
  }*/

  /// Param is the curvilinear abcissa \in [0 : pathLength]
  /// The pathLength can be computed as long as the coefficients_ are known
  /// Finally:
  /// config(0) = x(param) = (1 - param/length)*x1 + param/length*x2
  /// config(1) = coefs(0)*x(param)^2 + coefs(1)*x(param) + coefs(2)
  virtual bool impl_compute(core::ConfigurationOut_t result,
                            core::value_type param) const;

 private:
  core::DevicePtr_t device_;
  core::Configuration_t initial_;
  core::Configuration_t end_;
  ParabolaPathWkPtr_t weak_;
  mutable core::vector_t coefficients_;  // parabola coefficients
  mutable core::value_type length_;
};  // class ParabolaPath
}  //   namespace rbprm
}  // namespace hpp
#endif  // HPP_CORE_PARABOLA_PATH_HH


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015 CNRS
3			// Authors: Mylene Campana
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_PARABOLA_PATH_HH
20			#define HPP_RBPRM_PARABOLA_PATH_HH
21
22			#include <hpp/core/config.hh>
23			#include <hpp/core/fwd.hh>
24			#include <hpp/core/path.hh>
25
26			namespace hpp {
27			namespace rbprm {
28			using core::size_type;
29
30			// forward declaration of class
31			HPP_PREDEF_CLASS(ParabolaPath);
32			// Planner objects are manipulated only via shared pointers
33			typedef shared_ptr<ParabolaPath> ParabolaPathPtr_t;
34
35			/// Linear interpolation between two configurations
36			///
37			/// Degrees of freedom are interpolated depending on the type of
38			/// \link hpp::pinocchio::Joint joint \endlink
39			/// they parameterize:
40			/// \li linear interpolation for translation joints, bounded rotation
41			/// joints, and translation part of freeflyer joints,
42			/// \li angular interpolation for unbounded rotation joints,
43			/// \li constant angular velocity for SO(3) part of freeflyer joints.
44			class ParabolaPath : public core::Path {
45			public:
46			typedef Path parent_t;
47			/// Destructor
48			virtual ~ParabolaPath() {}
49
50			/// Create instance and return shared pointer
51			/// \param device Robot corresponding to configurations
52			/// \param init, end Start and end configurations of the path
53			/// \param length Distance between the configurations.
54			static ParabolaPathPtr_t create(const core::DevicePtr_t& device,
55			core::ConfigurationIn_t init,
56			core::ConfigurationIn_t end,
57			core::value_type length,
58			core::vector_t coefficients) {
59			ParabolaPath* ptr =
60			new ParabolaPath(device, init, end, length, coefficients);
61			ParabolaPathPtr_t shPtr(ptr);
62			ptr->init(shPtr);
63			return shPtr;
64			}
65
66			/// Create instance and return shared pointer
67			/// \param device Robot corresponding to configurations
68			/// \param init, end Start and end configurations of the path
69			/// \param length Distance between the configurations.
70			/// \param V0, Vimp initial and final velocity vectors
71			/// \param initialROMnames, endROMnames initial and final ROM names
72			static ParabolaPathPtr_t create(const core::DevicePtr_t& device,
73			core::ConfigurationIn_t init,
74			core::ConfigurationIn_t end,
75			core::value_type length,
76			core::vector_t coefficients,
77			core::vector_t V0, core::vector_t Vimp,
78			std::vector<std::string> initialROMnames,
79			std::vector<std::string> endROMnames) {
80			ParabolaPath* ptr =
81			new ParabolaPath(device, init, end, length, coefficients, V0, Vimp,
82			initialROMnames, endROMnames);
83			ParabolaPathPtr_t shPtr(ptr);
84			ptr->init(shPtr);
85			return shPtr;
86			}
87
88			/// Create copy and return shared pointer
89			/// \param path path to copy
90			static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t& path) {
91			ParabolaPath* ptr = new ParabolaPath(*path);
92			ParabolaPathPtr_t shPtr(ptr);
93			ptr->init(shPtr);
94			return shPtr;
95			}
96
97			/// Create copy and return shared pointer
98			/// \param path path to copy
99			/// \param constraints the path is subject to
100			/// <!> constraints part NOT IMPLEMENTED YET
101			static ParabolaPathPtr_t createCopy(
102			const ParabolaPathPtr_t& path,
103			const core::ConstraintSetPtr_t& /constraints/) {
104			// ParabolaPath* ptr = new ParabolaPath (*path, constraints);
105			ParabolaPath* ptr = new ParabolaPath(*path);
106			ParabolaPathPtr_t shPtr(ptr);
107			ptr->init(shPtr);
108			return shPtr;
109			}
110
111			/// Return a shared pointer to this
112			///
113			/// As ParabolaPath are immutable, and refered to by shared pointers,
114			/// they do not need to be copied.
115			virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
116
117			/// Return a shared pointer to a copy of this and set constraints
118			///
119			/// \param constraints constraints to apply to the copy
120			/// \precond *this should not have constraints.
121			virtual core::PathPtr_t copy(
122			const core::ConstraintSetPtr_t& constraints) const {
123			return createCopy(weak_.lock(), constraints);
124			}
125
126			/// Extraction/Reversion of a sub-path
127			/// \param subInterval interval of definition of the extract path
128			/// If upper bound of subInterval is smaller than lower bound,
129			/// result is reversed.
130			virtual core::PathPtr_t extract(const core::interval_t& subInterval) const;
131
132			/// Reversion of a path
133			/// \return a new path that is this one reversed.
134			virtual core::PathPtr_t reverse() const;
135
136			/// Modify initial configuration
137			/// \param initial new initial configuration
138			/// \pre input configuration should be of the same size as current initial
139			/// configuration
140			void initialConfig(core::ConfigurationIn_t initial) {
141			assert(initial.size() == initial_.size());
142			initial_ = initial;
143			}
144
145			/// Modify end configuration
146			/// \param end new end configuration
147			/// \pre input configuration should be of the same size as current end
148			/// configuration
149			void endConfig(core::ConfigurationIn_t end) {
150			assert(end.size() == end_.size());
151			end_ = end;
152			}
153
154			/// Return the internal robot.
155			core::DevicePtr_t device() const;
156
157			/// Get the initial configuration
158			core::Configuration_t initial() const { return initial_; }
159
160			/// Get the final configuration
161			core::Configuration_t end() const { return end_; }
162
163			/// Get previously computed length
164			virtual core::value_type length() const { return length_; }
165
166			/// Set the three parabola coefficients
167			void coefficients(core::vector_t coefs) const {
168			for (size_type i = 0; i < coefs.size(); i++) coefficients_(i) = coefs(i);
169			}
170
171			/// Get path coefficients
172			core::vector_t coefficients() const { return coefficients_; }
173
174			virtual core::value_type computeLength(
175			const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2) const;
176
177			/// Evaluate velocity vector at path abcissa t
178			core::vector_t evaluateVelocity(const core::value_type t) const;
179
180			core::value_type alpha_; // chosen alpha in interval
181			core::value_type alphaMin_; // min bound of alpha interval
182			core::value_type alphaMax_; // max bound of alpha interval
183			core::value_type Xtheta_;
184			core::value_type Z_;
185			core::vector_t V0_; // initial velocity
186			core::vector_t Vimp_; // final velocity
187			std::vector<std::string> initialROMnames_; // active ROM list at begining
188			std::vector<std::string> endROMnames_; // active ROM list at end
189
190			protected:
191			/// Print path in a stream
192			virtual std::ostream& print(std::ostream& os) const {
193			os << "ParabolaPath:" << std::endl;
194			os << "interval: [ " << timeRange().first << ", " << timeRange().second
195			<< " ]" << std::endl;
196			os << "initial configuration: " << initial_.transpose() << std::endl;
197			os << "final configuration: " << end_.transpose() << std::endl;
198			return os;
199			}
200			/// Constructor
201			ParabolaPath(const core::DevicePtr_t& robot, core::ConfigurationIn_t init,
202			core::ConfigurationIn_t end, core::value_type length,
203			core::vector_t coefficients);
204
205			/// Constructor with velocities and ROMnames
206			ParabolaPath(const core::DevicePtr_t& device, core::ConfigurationIn_t init,
207			core::ConfigurationIn_t end, core::value_type length,
208			core::vector_t coefs, core::vector_t V0_, core::vector_t Vimp,
209			std::vector<std::string> initialROMnames,
210			std::vector<std::string> endROMnames);
211
212			/// Copy constructor
213			ParabolaPath(const ParabolaPath& path);
214
215			core::value_type lengthFunction(const core::value_type x) const;
216
217			void init(ParabolaPathPtr_t self) {
218			parent_t::init(self);
219			weak_ = self;
220			}
221
222			/*void initCopy (ParabolaPathPtr_t self)
223			{
224			parent_t::initCopy (self);
225			weak_ = self;
226			}*/
227
228			/// Param is the curvilinear abcissa \in [0 : pathLength]
229			/// The pathLength can be computed as long as the coefficients_ are known
230			/// Finally:
231			/// config(0) = x(param) = (1 - param/length)x1 + param/lengthx2
232			/// config(1) = coefs(0)x(param)^2 + coefs(1)x(param) + coefs(2)
233			virtual bool impl_compute(core::ConfigurationOut_t result,
234			core::value_type param) const;
235
236			private:
237			core::DevicePtr_t device_;
238			core::Configuration_t initial_;
239			core::Configuration_t end_;
240			ParabolaPathWkPtr_t weak_;
241			mutable core::vector_t coefficients_; // parabola coefficients
242			mutable core::value_type length_;
243			}; // class ParabolaPath
244			} // namespace rbprm
245			} // namespace hpp
246			#endif // HPP_CORE_PARABOLA_PATH_HH