hpp-rbprm  4.13.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 
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;
34 typedef shared_ptr<BezierPath> BezierPathPtr_t;
35 typedef shared_ptr<const BezierPath> BezierPathConstPtr_t;
36 
44 
45 class BezierPath : public core::Path {
46  public:
47  typedef Path parent_t;
49  virtual ~BezierPath() {}
50 
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 
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 
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 
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 
112  virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
113 
118  virtual core::PathPtr_t copy(
119  const core::ConstraintSetPtr_t& constraints) const {
120  return createCopy(weak_.lock(), constraints);
121  }
122 
124  virtual core::Configuration_t initial() const;
125 
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:
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 
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 
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 
172  BezierPath(const BezierPath& path);
173 
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  /*
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
Definition: bezier-path.hh:45
ndcurves::bezier_curve< double, double, true, Eigen::Vector3d > bezier_t
Definition: bezier-path.hh:31
static BezierPathPtr_t createCopy(const BezierPathPtr_t &path, const core::ConstraintSetPtr_t &constraints)
Definition: bezier-path.hh:98
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: bezier-path.hh:144
shared_ptr< BezierPath > BezierPathPtr_t
Definition: bezier-path.hh:34
Definition: algorithm.hh:26
bezier_Ptr getBezier()
Definition: bezier-path.hh:138
virtual core::Configuration_t end() const
Get the final configuration.
void init(BezierPathPtr_t self)
Definition: bezier-path.hh:178
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: bezier-path.hh:118
virtual core::Configuration_t initial() const
Get the initial configuration.
static BezierPathPtr_t createCopy(const BezierPathPtr_t &path)
Definition: bezier-path.hh:88
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:55
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:72
HPP_PREDEF_CLASS(RbPrmFullBody)
bezier_t::t_point_t getWaypoints()
Definition: bezier-path.hh:140
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
Path parent_t
Definition: bezier-path.hh:47
virtual ~BezierPath()
Destructor.
Definition: bezier-path.hh:49
core::Configuration_t operator()(const core::value_type &t) const
Definition: bezier-path.hh:129
virtual core::PathPtr_t copy() const
Definition: bezier-path.hh:112
shared_ptr< const BezierPath > BezierPathConstPtr_t
Definition: bezier-path.hh:35
shared_ptr< bezier_t > bezier_Ptr
Definition: bezier-path.hh:32
void initCopy(BezierPathPtr_t self)
Definition: bezier-path.hh:184