hpp-rbprm  4.10.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 
20 #ifndef HPP_RBPRM_BEZIER_PATH_HH
21 #define HPP_RBPRM_BEZIER_PATH_HH
22 
23 # include <curves/bezier_curve.h>
24 # include <hpp/core/path.hh>
25 # include <vector>
26 # include <map>
27 
28 namespace hpp {
29 namespace rbprm {
30 
31 typedef curves::bezier_curve <double, double, true, Eigen::Vector3d > bezier_t;
32 typedef boost::shared_ptr<bezier_t> bezier_Ptr;
34 typedef boost::shared_ptr <BezierPath> BezierPathPtr_t;
35 typedef boost::shared_ptr <const BezierPath> BezierPathConstPtr_t;
36 
43 
44 class BezierPath : public core::Path
45 {
46 
47 public:
48  typedef Path parent_t;
50  virtual ~BezierPath () throw () {}
51 
56  static BezierPathPtr_t create (const core::DevicePtr_t& device,
57  const bezier_Ptr& curve,
58  core::ConfigurationIn_t init,
59  core::ConfigurationIn_t end,
60  core::interval_t timeRange)
61  {
62  BezierPath* ptr = new BezierPath (device, curve,init,end, timeRange);
63  BezierPathPtr_t shPtr (ptr);
64  ptr->init (shPtr);
65  ptr->checkPath ();
66  return shPtr;
67  }
68 
74  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,
75  core::ConfigurationIn_t init,
76  core::ConfigurationIn_t end,core::interval_t timeRange)
77  {
78  BezierPath* ptr = new BezierPath (device, wpBegin,wpEnd,init,end, timeRange);
79  BezierPathPtr_t shPtr (ptr);
80  ptr->init (shPtr);
81  ptr->checkPath ();
82  return shPtr;
83  }
84 
88  {
89  BezierPath* ptr = new BezierPath (*path);
90  BezierPathPtr_t shPtr (ptr);
91  ptr->initCopy (shPtr);
92  return shPtr;
93  }
94 
95 
100  (const BezierPathPtr_t& path, const core::ConstraintSetPtr_t& constraints)
101  {
102  BezierPath* ptr = new BezierPath (*path, constraints);
103  BezierPathPtr_t shPtr (ptr);
104  ptr->initCopy (shPtr);
105  ptr->checkPath ();
106  return shPtr;
107  }
108 
109 
114  virtual core::PathPtr_t copy () const
115  {
116  return createCopy (weak_.lock ());
117  }
118 
123  virtual core::PathPtr_t copy (const core::ConstraintSetPtr_t& constraints) const
124  {
125  return createCopy (weak_.lock (), constraints);
126  }
127 
128 
129 
130 
132  virtual core::Configuration_t initial () const;
133 
135  virtual core::Configuration_t end () const ;
136 
137 
138  core::Configuration_t operator () (const core::value_type& t) const
139  {
140  core::Configuration_t result (outputSize ());
141  impl_compute (result, t);
142  if (constraints()) {
143  constraints()->apply (result);
144  }
145  return result;
146  }
147 
148 
150  return curve_;
151  }
152 
153  bezier_t::t_point_t getWaypoints(){
154  return curve_->waypoints();
155  }
156 
157 
158 
159 protected :
161  virtual std::ostream& print (std::ostream &os) const
162  {
163  os << "BezierPath:" << std::endl;
164  os << "interval: [ " << timeRange ().first << ", "
165  << timeRange ().second << " ]" << std::endl;
166  os << "initial configuration: " << initial().transpose() << std::endl;
167  os << "final configuration: " << end().transpose () << std::endl;
168  os<<"Curve of degree :"<<curve_->degree_<<std::endl;
169  os<<"waypoints = "<<std::endl;
170  for(bezier_t::cit_point_t wpit = curve_->waypoints().begin() ; wpit != curve_->waypoints().end() ; ++wpit){
171  os<<(*wpit).transpose()<<std::endl;
172  }
173  return os;
174  }
175 
177  BezierPath (const core::DevicePtr_t& robot, const bezier_Ptr& curve,
178  core::ConfigurationIn_t init,
179  core::ConfigurationIn_t end,core::interval_t timeRange);
180 
182  BezierPath (const core::DevicePtr_t& robot, std::vector<bezier_t::point_t>::const_iterator wpBegin, std::vector<bezier_t::point_t>::const_iterator wpEnd,
183  core::ConfigurationIn_t init,
184  core::ConfigurationIn_t end, core::interval_t timeRange);
185 
186 
188  BezierPath (const BezierPath& path);
189 
190 
192  BezierPath (const BezierPath& path,
193  const core::ConstraintSetPtr_t& constraints);
194 
195 
196  void init (BezierPathPtr_t self)
197  {
198  parent_t::init (self);
199  weak_ = self;
200  checkPath ();
201  }
202 
204  {
205  parent_t::init (self);
206  weak_ = self;
207  }
208 
209  virtual bool impl_compute (core::ConfigurationOut_t result,
210  core::value_type param) const;
211 
212  /*
214  virtual void impl_derivative (core::vectorOut_t result, const core::value_type& t,
215  core::size_type order) const;
216  */
217 
218 
219 private:
220  pinocchio::DevicePtr_t device_;
221  bezier_Ptr curve_;
222  core::Configuration_t initial_;
223  core::Configuration_t end_;
224  BezierPathWkPtr_t weak_;
225 
226 };//class Bezier Path
227 }//rbprm
228 }//hpp
229 
230 #endif // HPP_RBPRM_BEZIER_PATH_HH
hpp::rbprm::BezierPath::end
virtual core::Configuration_t end() const
Get the final configuration.
hpp::rbprm::BezierPath::copy
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: bezier-path.hh:123
hpp::rbprm::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbPrmFullBody)
hpp::rbprm::BezierPath::init
void init(BezierPathPtr_t self)
Definition: bezier-path.hh:196
hpp::rbprm::BezierPathPtr_t
boost::shared_ptr< BezierPath > BezierPathPtr_t
Definition: bezier-path.hh:34
hpp::rbprm::BezierPath::create
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:74
hpp::rbprm::BezierPathConstPtr_t
boost::shared_ptr< const BezierPath > BezierPathConstPtr_t
Definition: bezier-path.hh:35
hpp::rbprm::BezierPath::getWaypoints
bezier_t::t_point_t getWaypoints()
Definition: bezier-path.hh:153
hpp::rbprm::BezierPath::createCopy
static BezierPathPtr_t createCopy(const BezierPathPtr_t &path)
Definition: bezier-path.hh:87
hpp::rbprm::BezierPath::getBezier
bezier_Ptr getBezier()
Definition: bezier-path.hh:149
hpp::rbprm::BezierPath::parent_t
Path parent_t
Definition: bezier-path.hh:48
hpp::rbprm::BezierPath
Definition: bezier-path.hh:44
hpp::rbprm::BezierPath::operator()
core::Configuration_t operator()(const core::value_type &t) const
Definition: bezier-path.hh:138
hpp::rbprm::BezierPath::create
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:56
hpp::rbprm::BezierPath::copy
virtual core::PathPtr_t copy() const
Definition: bezier-path.hh:114
hpp
Definition: algorithm.hh:27
hpp::rbprm::BezierPath::initial
virtual core::Configuration_t initial() const
Get the initial configuration.
hpp::rbprm::BezierPath::print
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: bezier-path.hh:161
hpp::rbprm::BezierPath::~BezierPath
virtual ~BezierPath()
Destructor.
Definition: bezier-path.hh:50
hpp::rbprm::BezierPath::impl_compute
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
hpp::rbprm::bezier_t
curves::bezier_curve< double, double, true, Eigen::Vector3d > bezier_t
Definition: bezier-path.hh:31
hpp::rbprm::BezierPath::BezierPath
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
hpp::rbprm::BezierPath::initCopy
void initCopy(BezierPathPtr_t self)
Definition: bezier-path.hh:203
hpp::rbprm::bezier_Ptr
boost::shared_ptr< bezier_t > bezier_Ptr
Definition: bezier-path.hh:32