hpp-rbprm  4.11.0
Implementation of RB-PRM planner using hpp.
bezier-path.hh
Go to the documentation of this file.
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 #include <hpp/core/path.hh>
24 #include <vector>
25 #include <map>
26 
27 namespace hpp {
28 namespace rbprm {
29 
30 typedef ndcurves::bezier_curve<double, double, true, Eigen::Vector3d> bezier_t;
31 typedef std::shared_ptr<bezier_t> bezier_Ptr;
33 typedef std::shared_ptr<BezierPath> BezierPathPtr_t;
34 typedef std::shared_ptr<const BezierPath> BezierPathConstPtr_t;
35 
42 
43 class BezierPath : public core::Path {
44  public:
45  typedef Path parent_t;
47  virtual ~BezierPath() {}
48 
53  static BezierPathPtr_t create(const core::DevicePtr_t& device, const bezier_Ptr& curve, core::ConfigurationIn_t init,
54  core::ConfigurationIn_t end, core::interval_t timeRange) {
55  BezierPath* ptr = new BezierPath(device, curve, init, end, timeRange);
56  BezierPathPtr_t shPtr(ptr);
57  ptr->init(shPtr);
58  ptr->checkPath();
59  return shPtr;
60  }
61 
67  static BezierPathPtr_t create(const core::DevicePtr_t& device,
68  std::vector<bezier_t::point_t>::const_iterator wpBegin,
69  std::vector<bezier_t::point_t>::const_iterator wpEnd, core::ConfigurationIn_t init,
70  core::ConfigurationIn_t end, core::interval_t timeRange) {
71  BezierPath* ptr = new BezierPath(device, wpBegin, wpEnd, init, end, timeRange);
72  BezierPathPtr_t shPtr(ptr);
73  ptr->init(shPtr);
74  ptr->checkPath();
75  return shPtr;
76  }
77 
80  static BezierPathPtr_t createCopy(const BezierPathPtr_t& path) {
81  BezierPath* ptr = new BezierPath(*path);
82  BezierPathPtr_t shPtr(ptr);
83  ptr->initCopy(shPtr);
84  return shPtr;
85  }
86 
90  static BezierPathPtr_t createCopy(const BezierPathPtr_t& path, const core::ConstraintSetPtr_t& constraints) {
91  BezierPath* ptr = new BezierPath(*path, constraints);
92  BezierPathPtr_t shPtr(ptr);
93  ptr->initCopy(shPtr);
94  ptr->checkPath();
95  return shPtr;
96  }
97 
102  virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
103 
108  virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t& constraints) const {
109  return createCopy(weak_.lock(), constraints);
110  }
111 
113  virtual core::Configuration_t initial() const;
114 
116  virtual core::Configuration_t end() const;
117 
118  core::Configuration_t operator()(const core::value_type& t) const {
119  core::Configuration_t result(outputSize());
120  impl_compute(result, t);
121  if (constraints()) {
122  constraints()->apply(result);
123  }
124  return result;
125  }
126 
127  bezier_Ptr getBezier() { return curve_; }
128 
129  bezier_t::t_point_t getWaypoints() { return curve_->waypoints(); }
130 
131  protected:
133  virtual std::ostream& print(std::ostream& os) const {
134  os << "BezierPath:" << std::endl;
135  os << "interval: [ " << timeRange().first << ", " << timeRange().second << " ]" << std::endl;
136  os << "initial configuration: " << initial().transpose() << std::endl;
137  os << "final configuration: " << end().transpose() << std::endl;
138  os << "Curve of degree :" << curve_->degree_ << std::endl;
139  os << "waypoints = " << std::endl;
140  for (bezier_t::cit_point_t wpit = curve_->waypoints().begin(); wpit != curve_->waypoints().end(); ++wpit) {
141  os << (*wpit).transpose() << std::endl;
142  }
143  return os;
144  }
145 
147  BezierPath(const core::DevicePtr_t& robot, const bezier_Ptr& curve, core::ConfigurationIn_t init,
148  core::ConfigurationIn_t end, core::interval_t timeRange);
149 
151  BezierPath(const core::DevicePtr_t& robot, std::vector<bezier_t::point_t>::const_iterator wpBegin,
152  std::vector<bezier_t::point_t>::const_iterator wpEnd, core::ConfigurationIn_t init,
153  core::ConfigurationIn_t end, core::interval_t timeRange);
154 
156  BezierPath(const BezierPath& path);
157 
159  BezierPath(const BezierPath& path, const core::ConstraintSetPtr_t& constraints);
160 
161  void init(BezierPathPtr_t self) {
162  parent_t::init(self);
163  weak_ = self;
164  checkPath();
165  }
166 
167  void initCopy(BezierPathPtr_t self) {
168  parent_t::init(self);
169  weak_ = self;
170  }
171 
172  virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const;
173 
174  /*
176  virtual void impl_derivative (core::vectorOut_t result, const core::value_type& t,
177  core::size_type order) const;
178  */
179 
180  private:
181  pinocchio::DevicePtr_t device_;
182  bezier_Ptr curve_;
183  core::Configuration_t initial_;
184  core::Configuration_t end_;
185  BezierPathWkPtr_t weak_;
186 
187 }; // class Bezier Path
188 } // namespace rbprm
189 } // namespace hpp
190 
191 #endif // HPP_RBPRM_BEZIER_PATH_HH
Definition: bezier-path.hh:43
ndcurves::bezier_curve< double, double, true, Eigen::Vector3d > bezier_t
Definition: bezier-path.hh:30
static BezierPathPtr_t createCopy(const BezierPathPtr_t &path, const core::ConstraintSetPtr_t &constraints)
Definition: bezier-path.hh:90
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: bezier-path.hh:133
Definition: algorithm.hh:27
bezier_Ptr getBezier()
Definition: bezier-path.hh:127
virtual core::Configuration_t end() const
Get the final configuration.
void init(BezierPathPtr_t self)
Definition: bezier-path.hh:161
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: bezier-path.hh:108
virtual core::Configuration_t initial() const
Get the initial configuration.
static BezierPathPtr_t createCopy(const BezierPathPtr_t &path)
Definition: bezier-path.hh:80
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)
Definition: bezier-path.hh:53
BezierPath(const core::DevicePtr_t &robot, const bezier_Ptr &curve, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::interval_t timeRange)
constructor with curve
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)
Definition: bezier-path.hh:67
std::shared_ptr< const BezierPath > BezierPathConstPtr_t
Definition: bezier-path.hh:34
HPP_PREDEF_CLASS(RbPrmFullBody)
bezier_t::t_point_t getWaypoints()
Definition: bezier-path.hh:129
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
Path parent_t
Definition: bezier-path.hh:45
virtual ~BezierPath()
Destructor.
Definition: bezier-path.hh:47
core::Configuration_t operator()(const core::value_type &t) const
Definition: bezier-path.hh:118
virtual core::PathPtr_t copy() const
Definition: bezier-path.hh:102
std::shared_ptr< BezierPath > BezierPathPtr_t
Definition: bezier-path.hh:33
void initCopy(BezierPathPtr_t self)
Definition: bezier-path.hh:167
std::shared_ptr< bezier_t > bezier_Ptr
Definition: bezier-path.hh:31