hpp-rbprm  4.15.1
Implementation of RB-PRM planner using hpp.
polynom-trajectory.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
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_POLYNOM_TRAJECTORY_HH
20 #define HPP_RBPRM_POLYNOM_TRAJECTORY_HH
21 
22 #include <ndcurves/curve_abc.h>
23 
24 #include <hpp/core/config.hh>
25 #include <hpp/core/fwd.hh>
26 #include <hpp/core/path.hh>
28 
29 namespace hpp {
30 namespace rbprm {
31 namespace interpolation {
33 typedef shared_ptr<PolynomTrajectory> PolynomTrajectoryPtr_t;
34 typedef ndcurves::curve_abc<core::value_type, core::value_type, true,
35  Eigen::Vector3d>
37 typedef shared_ptr<Polynom> PolynomPtr_t;
47 class HPP_CORE_DLLAPI PolynomTrajectory : public core::Path {
48  public:
49  typedef Path parent_t;
51  virtual ~PolynomTrajectory() {}
52 
58  core::value_type subSetStart = 0,
59  core::value_type subSetEnd = 1) {
60  PolynomTrajectory* ptr =
61  new PolynomTrajectory(polynom, subSetStart, subSetEnd);
62  PolynomTrajectoryPtr_t shPtr(ptr);
63  ptr->init(shPtr);
64  ptr->checkPath();
65  return shPtr;
66  }
67 
71  PolynomTrajectory* ptr = new PolynomTrajectory(*path);
72  PolynomTrajectoryPtr_t shPtr(ptr);
73  ptr->init(shPtr);
74  ptr->checkPath();
75  return shPtr;
76  }
77 
82  virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
83 
88  virtual core::PathPtr_t extract(const core::interval_t& subInterval) const;
89 
91  core::Configuration_t initial() const {
92  return polynom_->operator()(subSetStart_);
93  }
94 
96  core::Configuration_t end() const { return polynom_->operator()(subSetEnd_); }
97 
98  virtual void checkPath() const {}
99 
100  protected:
102  virtual std::ostream& print(std::ostream& os) const {
103  os << "PolynomTrajectory:" << std::endl;
104  os << "interval: [ " << timeRange().first << ", " << timeRange().second
105  << " ]" << std::endl;
106  os << "initial configuration: " << initial() << std::endl;
107  os << "final configuration: " << end() << std::endl;
108  return os;
109  }
111  PolynomTrajectory(PolynomPtr_t polynom, core::value_type subSetStart,
112  core::value_type subSetEnd);
113 
116 
118  parent_t::init(self);
119  weak_ = self;
120  }
121 
122  virtual bool impl_compute(core::ConfigurationOut_t result,
123  core::value_type param) const;
124 
125  virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t&) const { throw; }
126 
127  public:
129  const core::value_type subSetStart_;
130  const core::value_type subSetEnd_;
131  const core::value_type length_;
132 
133  private:
134  PolynomTrajectoryWkPtr_t weak_;
135 }; // class ComTrajectory
136 } // namespace interpolation
137 } // namespace rbprm
138 } // namespace hpp
139 #endif // HPP_RBPRM_POLYNOM_TRAJECTORY_HH
Definition: polynom-trajectory.hh:47
virtual void checkPath() const
Definition: polynom-trajectory.hh:98
core::Configuration_t end() const
Get the final configuration.
Definition: polynom-trajectory.hh:96
PolynomTrajectory(PolynomPtr_t polynom, core::value_type subSetStart, core::value_type subSetEnd)
Constructor.
const core::value_type subSetEnd_
Definition: polynom-trajectory.hh:130
PolynomTrajectory(const PolynomTrajectory &path)
Copy constructor.
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &) const
Definition: polynom-trajectory.hh:125
const core::value_type length_
Definition: polynom-trajectory.hh:131
core::Configuration_t initial() const
Get the initial configuration.
Definition: polynom-trajectory.hh:91
Path parent_t
Definition: polynom-trajectory.hh:49
const PolynomPtr_t polynom_
Definition: polynom-trajectory.hh:128
const core::value_type subSetStart_
Definition: polynom-trajectory.hh:129
virtual core::PathPtr_t copy() const
Definition: polynom-trajectory.hh:82
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: polynom-trajectory.hh:102
static PolynomTrajectoryPtr_t createCopy(const PolynomTrajectoryPtr_t &path)
Definition: polynom-trajectory.hh:70
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
virtual core::PathPtr_t extract(const core::interval_t &subInterval) const
void init(PolynomTrajectoryPtr_t self)
Definition: polynom-trajectory.hh:117
static PolynomTrajectoryPtr_t create(PolynomPtr_t polynom, core::value_type subSetStart=0, core::value_type subSetEnd=1)
Definition: polynom-trajectory.hh:57
virtual ~PolynomTrajectory()
Destructor.
Definition: polynom-trajectory.hh:51
ndcurves::curve_abc< core::value_type, core::value_type, true, Eigen::Vector3d > Polynom
Definition: polynom-trajectory.hh:36
shared_ptr< Polynom > PolynomPtr_t
Definition: polynom-trajectory.hh:37
HPP_PREDEF_CLASS(ComTrajectory)
shared_ptr< PolynomTrajectory > PolynomTrajectoryPtr_t
Definition: polynom-trajectory.hh:33
Definition: algorithm.hh:26