hpp-rbprm  4.10.0
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/fwd.hh>
23 # include <hpp/core/config.hh>
24 # include <hpp/core/path.hh>
26 
27 namespace hpp {
28 namespace rbprm {
29 namespace interpolation {
30  HPP_PREDEF_CLASS (TimeConstraintPath);
31  typedef boost::shared_ptr <TimeConstraintPath>
42  class HPP_CORE_DLLAPI TimeConstraintPath : public core::Path
43  {
44  public:
45  typedef Path parent_t;
47  virtual ~TimeConstraintPath () throw () {}
48 
53  static TimeConstraintPathPtr_t create (const core::DevicePtr_t& device,
54  core::ConfigurationIn_t init,
55  core::ConfigurationIn_t end,
56  core::value_type length,
57  const std::size_t pathDofRank,
58  const T_TimeDependant& tds)
59  {
60  TimeConstraintPath* ptr = new TimeConstraintPath (device, init, end, length, pathDofRank,tds);
61  TimeConstraintPathPtr_t shPtr (ptr);
62  ptr->init (shPtr);
63  ptr->checkPath ();
64  return shPtr;
65  }
66 
72  static TimeConstraintPathPtr_t create (const core::DevicePtr_t& device,
73  core::ConfigurationIn_t init,
74  core::ConfigurationIn_t end,
75  core::value_type length,
76  core::ConstraintSetPtr_t constraints,
77  const std::size_t pathDofRank,
78  const T_TimeDependant& tds)
79  {
80  TimeConstraintPath* ptr = new TimeConstraintPath (device, init, end,
81  length, constraints, pathDofRank,tds);
82  TimeConstraintPathPtr_t shPtr (ptr);
83  ptr->init (shPtr);
84  ptr->checkPath ();
85  return shPtr;
86  }
87 
91  {
92  TimeConstraintPath* ptr = new TimeConstraintPath (*path);
93  TimeConstraintPathPtr_t shPtr (ptr);
94  ptr->initCopy (shPtr);
95  ptr->checkPath ();
96  return shPtr;
97  }
98 
102  static TimeConstraintPathPtr_t createCopy
103  (const TimeConstraintPathPtr_t& path, const core::ConstraintSetPtr_t& constraints)
104  {
105  TimeConstraintPath* ptr = new TimeConstraintPath (*path, constraints);
106  TimeConstraintPathPtr_t shPtr (ptr);
107  ptr->initCopy (shPtr);
108  ptr->checkPath ();
109  return shPtr;
110  }
111 
116  virtual core::PathPtr_t copy () const
117  {
118  return createCopy (weak_.lock ());
119  }
120 
125  virtual core::PathPtr_t copy (const core::ConstraintSetPtr_t& constraints) const
126  {
127  return createCopy (weak_.lock (), constraints);
128  }
129 
130 
135  virtual core::PathPtr_t extract (const core::interval_t& subInterval) const
136  throw (core::projection_error);
137 
142  void initialConfig (core::ConfigurationIn_t initial)
143  {
144  assert (initial.size () == initial_.size ());
145  pinocchio::value_type dof = initial_[pathDofRank_];
146  initial_ = initial;
147  initial_[pathDofRank_] = dof;
148  }
149 
154  void endConfig (core::ConfigurationIn_t end)
155  {
156  assert (end.size () == end_.size ());
157  pinocchio::value_type dof = end_[pathDofRank_];
158  end_ = end;
159  end_[pathDofRank_] = dof;
160  }
161 
163  core::DevicePtr_t device () const;
164 
166  core::Configuration_t initial () const
167  {
168  return initial_;
169  }
170 
172  core::Configuration_t end () const
173  {
174  return end_;
175  }
176 
177  virtual void checkPath () const throw (core::projection_error);
178  protected:
180  virtual std::ostream& print (std::ostream &os) const
181  {
182  os << "TimeConstraintPath:" << std::endl;
183  os << "interval: [ " << timeRange ().first << ", "
184  << timeRange ().second << " ]" << std::endl;
185  os << "initial configuration: " << initial_.transpose () << std::endl;
186  os << "final configuration: " << end_.transpose () << std::endl;
187  return os;
188  }
190  TimeConstraintPath (const core::DevicePtr_t& robot, core::ConfigurationIn_t init,
191  core::ConfigurationIn_t end, core::value_type length,
192  const std::size_t pathDofRank,
193  const T_TimeDependant& tds);
194 
196  TimeConstraintPath (const core::DevicePtr_t& robot, core::ConfigurationIn_t init,
197  core::ConfigurationIn_t end, core::value_type length,
198  core::ConstraintSetPtr_t constraints, const std::size_t pathDofRank,
199  const T_TimeDependant& tds);
200 
203 
206  const core::ConstraintSetPtr_t& constraints);
207 
209  {
210  parent_t::init (self);
211  weak_ = self;
212  }
213 
215  {
216  parent_t::init (self);
217  weak_ = self;
218  }
219 
220  virtual bool impl_compute (core::ConfigurationOut_t result,
221  core::value_type param) const;
222 
223  private:
224  void updateConstraints(core::ConfigurationOut_t configuration) const;
225 
226  private:
227  core::DevicePtr_t device_;
228  core::Configuration_t initial_;
229  core::Configuration_t end_;
230 
231  public:
232  const std::size_t pathDofRank_;
234 
235  private:
236  TimeConstraintPathWkPtr_t weak_;
237  }; // class TimeConstraintPath
238 } // namespace interpolation
239 } // namespace rbprm
240 } // namespace hpp
241 #endif // HPP_RBPRM_TIMECONSTRAINT_PATH_HH
hpp::rbprm::interpolation::TimeConstraintPath::~TimeConstraintPath
virtual ~TimeConstraintPath()
Destructor.
Definition: time-constraint-path.hh:47
hpp::rbprm::interpolation::TimeConstraintPath::initialConfig
void initialConfig(core::ConfigurationIn_t initial)
Definition: time-constraint-path.hh:142
hpp::rbprm::interpolation::TimeConstraintPath::create
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:53
hpp::rbprm::interpolation::TimeConstraintPath::endConfig
void endConfig(core::ConfigurationIn_t end)
Definition: time-constraint-path.hh:154
hpp::rbprm::interpolation::TimeConstraintPath::pathDofRank_
const std::size_t pathDofRank_
Definition: time-constraint-path.hh:232
hpp::rbprm::interpolation::TimeConstraintPath::createCopy
static TimeConstraintPathPtr_t createCopy(const TimeConstraintPathPtr_t &path)
Definition: time-constraint-path.hh:90
hpp::rbprm::interpolation::T_TimeDependant
std::vector< TimeDependant > T_TimeDependant
Definition: time-dependant.hh:75
hpp::rbprm::interpolation::TimeConstraintPath::checkPath
virtual void checkPath() const
hpp::rbprm::interpolation::TimeConstraintPath::tds_
const T_TimeDependant tds_
Definition: time-constraint-path.hh:233
hpp::rbprm::interpolation::TimeConstraintPath::create
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:72
hpp::rbprm::interpolation::TimeConstraintPath::copy
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: time-constraint-path.hh:125
hpp::rbprm::interpolation::TimeConstraintPathPtr_t
boost::shared_ptr< TimeConstraintPath > TimeConstraintPathPtr_t
Definition: time-constraint-path.hh:32
hpp::rbprm::interpolation::TimeConstraintPath::copy
virtual core::PathPtr_t copy() const
Definition: time-constraint-path.hh:116
hpp::rbprm::interpolation::TimeConstraintPath::end
core::Configuration_t end() const
Get the final configuration.
Definition: time-constraint-path.hh:172
time-dependant.hh
hpp
Definition: algorithm.hh:27
hpp::rbprm::interpolation::TimeConstraintPath::initial
core::Configuration_t initial() const
Get the initial configuration.
Definition: time-constraint-path.hh:166
hpp::rbprm::interpolation::TimeConstraintPath
Definition: time-constraint-path.hh:42
hpp::rbprm::interpolation::TimeConstraintPath::init
void init(TimeConstraintPathPtr_t self)
Definition: time-constraint-path.hh:208
hpp::rbprm::interpolation::TimeConstraintPath::parent_t
Path parent_t
Definition: time-constraint-path.hh:45
hpp::rbprm::interpolation::TimeConstraintPath::initCopy
void initCopy(TimeConstraintPathPtr_t self)
Definition: time-constraint-path.hh:214
hpp::rbprm::interpolation::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(ComTrajectory)