Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/hpp/rbprm/interpolation/spline/bezier-path.hh	Lines:	0	47	0.0 %
Date:	2024-02-02 12:21:48	Branches:	0	60	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_BEZIER_PATH_HH
#define HPP_RBPRM_BEZIER_PATH_HH

#include <ndcurves/bezier_curve.h>

#include <hpp/core/path.hh>
#include <map>
#include <vector>

namespace hpp {
namespace rbprm {

typedef ndcurves::bezier_curve<double, double, true, Eigen::Vector3d> bezier_t;
typedef shared_ptr<bezier_t> bezier_Ptr;
HPP_PREDEF_CLASS(BezierPath);
typedef shared_ptr<BezierPath> BezierPathPtr_t;
typedef shared_ptr<const BezierPath> BezierPathConstPtr_t;

/// \addtogroup path
/// \{
/// Bezier curve representation between two configurations
///
/// This class only implement the bezier curve of dimension 3, need to template
/// the dimension of the points Use the Bezier curve for the translation of the
/// root and standard linear interpolation for the other DoF

class BezierPath : public core::Path {
 public:
  typedef Path parent_t;
  /// Destructor
  virtual ~BezierPath() {}

  /// Create instance and return shared pointer
  /// \param device Robot corresponding to configurations
  /// \param curve the curve that define this path
  /// \param timeRange : the time definition of the curve.
  static BezierPathPtr_t create(const core::DevicePtr_t& device,
                                const bezier_Ptr& curve,
                                core::ConfigurationIn_t init,
                                core::ConfigurationIn_t end,
                                core::interval_t timeRange) {
    BezierPath* ptr = new BezierPath(device, curve, init, end, timeRange);
    BezierPathPtr_t shPtr(ptr);
    ptr->init(shPtr);
    ptr->checkPath();
    return shPtr;
  }

  /// Create instance and return shared pointer
  /// \param device Robot corresponding to configurations
  /// \param wpBegin iterator to the first waypoint
  /// \param wpEnd iterator to the last wp
  /// \param length Distance between the configurations.
  static BezierPathPtr_t create(
      const core::DevicePtr_t& device,
      std::vector<bezier_t::point_t>::const_iterator wpBegin,
      std::vector<bezier_t::point_t>::const_iterator wpEnd,
      core::ConfigurationIn_t init, core::ConfigurationIn_t end,
      core::interval_t timeRange) {
    BezierPath* ptr =
        new BezierPath(device, wpBegin, wpEnd, init, end, timeRange);
    BezierPathPtr_t shPtr(ptr);
    ptr->init(shPtr);
    ptr->checkPath();
    return shPtr;
  }

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

  /// Create copy and return shared pointer
  /// \param path path to copy
  /// \param constraints the path is subject to
  static BezierPathPtr_t createCopy(
      const BezierPathPtr_t& path,
      const core::ConstraintSetPtr_t& constraints) {
    BezierPath* ptr = new BezierPath(*path, constraints);
    BezierPathPtr_t shPtr(ptr);
    ptr->initCopy(shPtr);
    ptr->checkPath();
    return shPtr;
  }

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

  /// Get the initial configuration
  virtual core::Configuration_t initial() const;

  /// Get the final configuration
  virtual core::Configuration_t end() const;

  core::Configuration_t operator()(const core::value_type& t) const {
    core::Configuration_t result(outputSize());
    impl_compute(result, t);
    if (constraints()) {
      constraints()->apply(result);
    }
    return result;
  }

  bezier_Ptr getBezier() { return curve_; }

  bezier_t::t_point_t getWaypoints() { return curve_->waypoints(); }

 protected:
  /// Print path in a stream
  virtual std::ostream& print(std::ostream& os) const {
    os << "BezierPath:" << std::endl;
    os << "interval: [ " << timeRange().first << ", " << timeRange().second
       << " ]" << std::endl;
    os << "initial configuration: " << initial().transpose() << std::endl;
    os << "final configuration:   " << end().transpose() << std::endl;
    os << "Curve of degree :" << curve_->degree_ << std::endl;
    os << "waypoints = " << std::endl;
    for (bezier_t::cit_point_t wpit = curve_->waypoints().begin();
         wpit != curve_->waypoints().end(); ++wpit) {
      os << (*wpit).transpose() << std::endl;
    }
    return os;
  }

  /// constructor with curve
  BezierPath(const core::DevicePtr_t& robot, const bezier_Ptr& curve,
             core::ConfigurationIn_t init, core::ConfigurationIn_t end,
             core::interval_t timeRange);

  /// constructor with waypoints
  BezierPath(const core::DevicePtr_t& robot,
             std::vector<bezier_t::point_t>::const_iterator wpBegin,
             std::vector<bezier_t::point_t>::const_iterator wpEnd,
             core::ConfigurationIn_t init, core::ConfigurationIn_t end,
             core::interval_t timeRange);

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

  /// Copy constructor with constraints
  BezierPath(const BezierPath& path,
             const core::ConstraintSetPtr_t& constraints);

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

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

  virtual bool impl_compute(core::ConfigurationOut_t result,
                            core::value_type param) const;

  /*
  /// Virtual implementation of derivative
  virtual void impl_derivative (core::vectorOut_t result, const
  core::value_type& t, core::size_type order) const;
   */

 private:
  pinocchio::DevicePtr_t device_;
  bezier_Ptr curve_;
  core::Configuration_t initial_;
  core::Configuration_t end_;
  BezierPathWkPtr_t weak_;

};  // class Bezier Path
}  // namespace rbprm
}  // namespace hpp

#endif  // HPP_RBPRM_BEZIER_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_BEZIER_PATH_HH
20			#define HPP_RBPRM_BEZIER_PATH_HH
21
22			#include <ndcurves/bezier_curve.h>
23
24			#include <hpp/core/path.hh>
25			#include <map>
26			#include <vector>
27
28			namespace hpp {
29			namespace rbprm {
30
31			typedef ndcurves::bezier_curve<double, double, true, Eigen::Vector3d> bezier_t;
32			typedef shared_ptr<bezier_t> bezier_Ptr;
33			HPP_PREDEF_CLASS(BezierPath);
34			typedef shared_ptr<BezierPath> BezierPathPtr_t;
35			typedef shared_ptr<const BezierPath> BezierPathConstPtr_t;
36
37			/// \addtogroup path
38			/// \{
39			/// Bezier curve representation between two configurations
40			///
41			/// This class only implement the bezier curve of dimension 3, need to template
42			/// the dimension of the points Use the Bezier curve for the translation of the
43			/// root and standard linear interpolation for the other DoF
44
45			class BezierPath : public core::Path {
46			public:
47			typedef Path parent_t;
48			/// Destructor
49			virtual ~BezierPath() {}
50
51			/// Create instance and return shared pointer
52			/// \param device Robot corresponding to configurations
53			/// \param curve the curve that define this path
54			/// \param timeRange : the time definition of the curve.
55			static BezierPathPtr_t create(const core::DevicePtr_t& device,
56			const bezier_Ptr& curve,
57			core::ConfigurationIn_t init,
58			core::ConfigurationIn_t end,
59			core::interval_t timeRange) {
60			BezierPath* ptr = new BezierPath(device, curve, init, end, timeRange);
61			BezierPathPtr_t shPtr(ptr);
62			ptr->init(shPtr);
63			ptr->checkPath();
64			return shPtr;
65			}
66
67			/// Create instance and return shared pointer
68			/// \param device Robot corresponding to configurations
69			/// \param wpBegin iterator to the first waypoint
70			/// \param wpEnd iterator to the last wp
71			/// \param length Distance between the configurations.
72			static BezierPathPtr_t create(
73			const core::DevicePtr_t& device,
74			std::vector<bezier_t::point_t>::const_iterator wpBegin,
75			std::vector<bezier_t::point_t>::const_iterator wpEnd,
76			core::ConfigurationIn_t init, core::ConfigurationIn_t end,
77			core::interval_t timeRange) {
78			BezierPath* ptr =
79			new BezierPath(device, wpBegin, wpEnd, init, end, timeRange);
80			BezierPathPtr_t shPtr(ptr);
81			ptr->init(shPtr);
82			ptr->checkPath();
83			return shPtr;
84			}
85
86			/// Create copy and return shared pointer
87			/// \param path path to copy
88			static BezierPathPtr_t createCopy(const BezierPathPtr_t& path) {
89			BezierPath* ptr = new BezierPath(*path);
90			BezierPathPtr_t shPtr(ptr);
91			ptr->initCopy(shPtr);
92			return shPtr;
93			}
94
95			/// Create copy and return shared pointer
96			/// \param path path to copy
97			/// \param constraints the path is subject to
98			static BezierPathPtr_t createCopy(
99			const BezierPathPtr_t& path,
100			const core::ConstraintSetPtr_t& constraints) {
101			BezierPath* ptr = new BezierPath(*path, constraints);
102			BezierPathPtr_t shPtr(ptr);
103			ptr->initCopy(shPtr);
104			ptr->checkPath();
105			return shPtr;
106			}
107
108			/// Return a shared pointer to this
109			///
110			/// As BezierPath are immutable, and refered to by shared pointers,
111			/// they do not need to be copied.
112			virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
113
114			/// Return a shared pointer to a copy of this and set constraints
115			///
116			/// \param constraints constraints to apply to the copy
117			/// \precond *this should not have constraints.
118			virtual core::PathPtr_t copy(
119			const core::ConstraintSetPtr_t& constraints) const {
120			return createCopy(weak_.lock(), constraints);
121			}
122
123			/// Get the initial configuration
124			virtual core::Configuration_t initial() const;
125
126			/// Get the final configuration
127			virtual core::Configuration_t end() const;
128
129			core::Configuration_t operator()(const core::value_type& t) const {
130			core::Configuration_t result(outputSize());
131			impl_compute(result, t);
132			if (constraints()) {
133			constraints()->apply(result);
134			}
135			return result;
136			}
137
138			bezier_Ptr getBezier() { return curve_; }
139
140			bezier_t::t_point_t getWaypoints() { return curve_->waypoints(); }
141
142			protected:
143			/// Print path in a stream
144			virtual std::ostream& print(std::ostream& os) const {
145			os << "BezierPath:" << std::endl;
146			os << "interval: [ " << timeRange().first << ", " << timeRange().second
147			<< " ]" << std::endl;
148			os << "initial configuration: " << initial().transpose() << std::endl;
149			os << "final configuration: " << end().transpose() << std::endl;
150			os << "Curve of degree :" << curve_->degree_ << std::endl;
151			os << "waypoints = " << std::endl;
152			for (bezier_t::cit_point_t wpit = curve_->waypoints().begin();
153			wpit != curve_->waypoints().end(); ++wpit) {
154			os << (*wpit).transpose() << std::endl;
155			}
156			return os;
157			}
158
159			/// constructor with curve
160			BezierPath(const core::DevicePtr_t& robot, const bezier_Ptr& curve,
161			core::ConfigurationIn_t init, core::ConfigurationIn_t end,
162			core::interval_t timeRange);
163
164			/// constructor with waypoints
165			BezierPath(const core::DevicePtr_t& robot,
166			std::vector<bezier_t::point_t>::const_iterator wpBegin,
167			std::vector<bezier_t::point_t>::const_iterator wpEnd,
168			core::ConfigurationIn_t init, core::ConfigurationIn_t end,
169			core::interval_t timeRange);
170
171			/// Copy constructor
172			BezierPath(const BezierPath& path);
173
174			/// Copy constructor with constraints
175			BezierPath(const BezierPath& path,
176			const core::ConstraintSetPtr_t& constraints);
177
178			void init(BezierPathPtr_t self) {
179			parent_t::init(self);
180			weak_ = self;
181			checkPath();
182			}
183
184			void initCopy(BezierPathPtr_t self) {
185			parent_t::init(self);
186			weak_ = self;
187			}
188
189			virtual bool impl_compute(core::ConfigurationOut_t result,
190			core::value_type param) const;
191
192			/*
193			/// Virtual implementation of derivative
194			virtual void impl_derivative (core::vectorOut_t result, const
195			core::value_type& t, core::size_type order) const;
196			*/
197
198			private:
199			pinocchio::DevicePtr_t device_;
200			bezier_Ptr curve_;
201			core::Configuration_t initial_;
202			core::Configuration_t end_;
203			BezierPathWkPtr_t weak_;
204
205			}; // class Bezier Path
206			} // namespace rbprm
207			} // namespace hpp
208
209			#endif // HPP_RBPRM_BEZIER_PATH_HH