hpp-rbprm  4.15.1
Implementation of RB-PRM planner using hpp.
time-constraint-path.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_TIMECONSTRAINT_PATH_HH
20 #define HPP_RBPRM_TIMECONSTRAINT_PATH_HH
21 
22 #include <hpp/core/config.hh>
23 #include <hpp/core/fwd.hh>
24 #include <hpp/core/path.hh>
26 
27 namespace hpp {
28 namespace rbprm {
29 namespace interpolation {
30 HPP_PREDEF_CLASS(TimeConstraintPath);
31 typedef shared_ptr<TimeConstraintPath> TimeConstraintPathPtr_t;
41 class HPP_CORE_DLLAPI TimeConstraintPath : public core::Path {
42  public:
43  typedef Path parent_t;
45  virtual ~TimeConstraintPath() {}
46 
51  static TimeConstraintPathPtr_t create(const core::DevicePtr_t& device,
52  core::ConfigurationIn_t init,
53  core::ConfigurationIn_t end,
54  core::value_type length,
55  const std::size_t pathDofRank,
56  const T_TimeDependant& tds) {
57  TimeConstraintPath* ptr =
58  new TimeConstraintPath(device, init, end, length, pathDofRank, tds);
59  TimeConstraintPathPtr_t shPtr(ptr);
60  ptr->init(shPtr);
61  ptr->checkPath();
62  return shPtr;
63  }
64 
70  static TimeConstraintPathPtr_t create(const core::DevicePtr_t& device,
71  core::ConfigurationIn_t init,
72  core::ConfigurationIn_t end,
73  core::value_type length,
74  core::ConstraintSetPtr_t constraints,
75  const std::size_t pathDofRank,
76  const T_TimeDependant& tds) {
78  device, init, end, length, constraints, pathDofRank, tds);
79  TimeConstraintPathPtr_t shPtr(ptr);
80  ptr->init(shPtr);
81  ptr->checkPath();
82  return shPtr;
83  }
84 
87  static TimeConstraintPathPtr_t createCopy(
88  const TimeConstraintPathPtr_t& path) {
89  TimeConstraintPath* ptr = new TimeConstraintPath(*path);
90  TimeConstraintPathPtr_t shPtr(ptr);
91  ptr->initCopy(shPtr);
92  ptr->checkPath();
93  return shPtr;
94  }
95 
99  static TimeConstraintPathPtr_t createCopy(
100  const TimeConstraintPathPtr_t& path,
101  const core::ConstraintSetPtr_t& constraints) {
102  TimeConstraintPath* ptr = new TimeConstraintPath(*path, constraints);
103  TimeConstraintPathPtr_t shPtr(ptr);
104  ptr->initCopy(shPtr);
105  ptr->checkPath();
106  return shPtr;
107  }
108 
113  virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
114 
119  virtual core::PathPtr_t copy(
120  const core::ConstraintSetPtr_t& constraints) const {
121  return createCopy(weak_.lock(), constraints);
122  }
123 
128  virtual core::PathPtr_t extract(const core::interval_t& subInterval) const;
129 
134  void initialConfig(core::ConfigurationIn_t initial) {
135  assert(initial.size() == initial_.size());
136  pinocchio::value_type dof = initial_[pathDofRank_];
137  initial_ = initial;
138  initial_[pathDofRank_] = dof;
139  }
140 
145  void endConfig(core::ConfigurationIn_t end) {
146  assert(end.size() == end_.size());
147  pinocchio::value_type dof = end_[pathDofRank_];
148  end_ = end;
149  end_[pathDofRank_] = dof;
150  }
151 
153  core::DevicePtr_t device() const;
154 
156  core::Configuration_t initial() const { return initial_; }
157 
159  core::Configuration_t end() const { return end_; }
160 
161  virtual void checkPath() const;
162 
163  protected:
165  virtual std::ostream& print(std::ostream& os) const {
166  os << "TimeConstraintPath:" << std::endl;
167  os << "interval: [ " << timeRange().first << ", " << timeRange().second
168  << " ]" << std::endl;
169  os << "initial configuration: " << initial_.transpose() << std::endl;
170  os << "final configuration: " << end_.transpose() << std::endl;
171  return os;
172  }
174  TimeConstraintPath(const core::DevicePtr_t& robot,
175  core::ConfigurationIn_t init, core::ConfigurationIn_t end,
176  core::value_type length, const std::size_t pathDofRank,
177  const T_TimeDependant& tds);
178 
180  TimeConstraintPath(const core::DevicePtr_t& robot,
181  core::ConfigurationIn_t init, core::ConfigurationIn_t end,
182  core::value_type length,
183  core::ConstraintSetPtr_t constraints,
184  const std::size_t pathDofRank, const T_TimeDependant& tds);
185 
188 
191  const core::ConstraintSetPtr_t& constraints);
192 
193  void init(TimeConstraintPathPtr_t self) {
194  parent_t::init(self);
195  weak_ = self;
196  }
197 
198  void initCopy(TimeConstraintPathPtr_t self) {
199  parent_t::init(self);
200  weak_ = self;
201  }
202 
203  virtual bool impl_compute(core::ConfigurationOut_t result,
204  core::value_type param) const;
205 
206  private:
207  void updateConstraints(core::ConfigurationOut_t configuration) const;
208 
209  private:
210  core::DevicePtr_t device_;
211  core::Configuration_t initial_;
212  core::Configuration_t end_;
213 
214  public:
215  const std::size_t pathDofRank_;
217 
218  private:
219  TimeConstraintPathWkPtr_t weak_;
220 }; // class TimeConstraintPath
221 } // namespace interpolation
222 } // namespace rbprm
223 } // namespace hpp
224 #endif // HPP_RBPRM_TIMECONSTRAINT_PATH_HH
static TimeConstraintPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, const std::size_t pathDofRank, const T_TimeDependant &tds)
Definition: time-constraint-path.hh:51
virtual ~TimeConstraintPath()
Destructor.
Definition: time-constraint-path.hh:45
void initialConfig(core::ConfigurationIn_t initial)
Definition: time-constraint-path.hh:134
Definition: time-constraint-path.hh:41
Definition: algorithm.hh:26
const std::size_t pathDofRank_
Definition: time-constraint-path.hh:215
shared_ptr< TimeConstraintPath > TimeConstraintPathPtr_t
Definition: time-constraint-path.hh:31
Path parent_t
Definition: time-constraint-path.hh:43
void endConfig(core::ConfigurationIn_t end)
Definition: time-constraint-path.hh:145
virtual core::PathPtr_t copy() const
Definition: time-constraint-path.hh:113
void initCopy(TimeConstraintPathPtr_t self)
Definition: time-constraint-path.hh:198
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: time-constraint-path.hh:119
const T_TimeDependant tds_
Definition: time-constraint-path.hh:216
static TimeConstraintPathPtr_t createCopy(const TimeConstraintPathPtr_t &path, const core::ConstraintSetPtr_t &constraints)
Definition: time-constraint-path.hh:99
static TimeConstraintPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::ConstraintSetPtr_t constraints, const std::size_t pathDofRank, const T_TimeDependant &tds)
Definition: time-constraint-path.hh:70
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: time-constraint-path.hh:165
void init(TimeConstraintPathPtr_t self)
Definition: time-constraint-path.hh:193
core::Configuration_t end() const
Get the final configuration.
Definition: time-constraint-path.hh:159
std::vector< TimeDependant > T_TimeDependant
Definition: time-dependant.hh:73
HPP_PREDEF_CLASS(ComTrajectory)
core::Configuration_t initial() const
Get the initial configuration.
Definition: time-constraint-path.hh:156
static TimeConstraintPathPtr_t createCopy(const TimeConstraintPathPtr_t &path)
Definition: time-constraint-path.hh:87