hpp-wholebody-step  4.12.0
Path planning for humanoid robot by sliding on the groud.
time-dependant-path.hh
Go to the documentation of this file.
1 // Copyright (c) 2015 CNRS
2 // Authors: Joseph Mirabel
3 //
4 //
5 // This file is part of hpp-wholebody-step.
6 // hpp-wholebody-step-planner 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-wholebody-step-planner 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-wholebody-step-planner. If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_WHOLEBODY_STEP_TIME_DEPENDANT_PATH_HH
20 # define HPP_WHOLEBODY_STEP_TIME_DEPENDANT_PATH_HH
21 
22 # include <hpp/core/path.hh>
25 
26 namespace hpp {
27  namespace wholebodyStep {
31  {
32  public:
34  {
36  ptr->init (ptr);
37  return ptr;
38  }
39 
41  const ConstraintSetPtr_t& c)
42  {
43  TimeDependantPathPtr_t ptr (new TimeDependantPath (path, c));
44  ptr->init (ptr);
45  return ptr;
46  }
47 
49  {
50  TimeDependantPathPtr_t ptr (new TimeDependantPath (other));
51  ptr->init (ptr);
52  return ptr;
53  }
54 
56  const ConstraintSetPtr_t& c)
57  {
58  TimeDependantPathPtr_t ptr (new TimeDependantPath (other, c));
59  ptr->init (ptr);
60  return ptr;
61  }
62 
63  void add (const TimeDependant& td)
64  {
65  tds_.push_back (td);
66  }
67 
68  virtual ~TimeDependantPath () throw () {}
69 
71  return path_->initial ();
72  }
73 
74  Configuration_t end () const {
75  return path_->end ();
76  }
77 
78  core::PathPtr_t copy () const {
79  return createCopy (*this);
80  }
81 
83  return createCopy (*this, c);
84  }
85 
86  void setAffineTransform (const value_type& a, const value_type& b) {
87  a_ = a;
88  b_ = b;
89  }
90 
91  void updateAbscissa (value_type t) const
92  {
93  const value_type y = a_*t + b_;
95  if (constraints()) cp = constraints()->configProjector ();
96  for (TimeDependants_t::const_iterator it = tds_.begin ();
97  it != tds_.end (); ++it) {
98  it->rhsAbscissa (y);
99  ImplicitPtr_t nm = HPP_DYNAMIC_PTR_CAST(Implicit, it->eq_);
100  if (cp && nm) {
101  cp->rightHandSide(nm, nm->rightHandSide());
102  }
103  }
104  }
105 
106  protected:
108  Path (path->timeRange (), path->outputSize (),
109  path->outputDerivativeSize ()),
110  path_ (path->copy ()), a_ (1), b_(0)
111  {}
112 
114  Path (path->timeRange (), path->outputSize (),
115  path->outputDerivativeSize ()),
116  path_ (path->copy ()), a_ (1), b_(0)
117  {
118  constraints (c);
119  }
120 
122  Path (other), path_ (other.path_->copy ()),
123  tds_ (other.tds_), a_ (other.a_), b_(other.b_)
124  {}
125 
127  Path (other), path_ (other.path_->copy ()),
128  a_ (other.a_), b_(other.b_)
129  {
130  constraints (c);
131  }
132 
133  virtual bool impl_compute (ConfigurationOut_t config, value_type t) const
134  {
135  updateAbscissa (t);
136  return (*path_) (config, t);
137  }
138 
139  void init (const TimeDependantPathPtr_t& self)
140  {
141  Path::init (self);
142  }
143 
144  virtual std::ostream& print (std::ostream& os) const {
145  return os << "TimeDependantPath: " << *path_;
146  }
147 
148  private:
149  typedef std::vector <TimeDependant> TimeDependants_t;
150 
151  core::PathPtr_t path_;
152  TimeDependants_t tds_;
153  value_type a_, b_;
154  }; // class TimeDependantPath
155  } // namespace wholebodyStep
156 } // namespace hpp
157 #endif // HPP_WHOLEBODY_STEP_TIME_DEPENDANT_PATH_HH
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: static-stability-constraint.hh:31
void init(const TimeDependantPathPtr_t &self)
Definition: time-dependant-path.hh:139
core::Path Path
Definition: fwd.hh:51
static TimeDependantPathPtr_t create(const core::PathPtr_t path, const ConstraintSetPtr_t &c)
Definition: time-dependant-path.hh:40
Definition: fwd.hh:28
core::PathPtr_t copy() const
Definition: time-dependant-path.hh:78
Definition: time-dependant.hh:35
constraints::Implicit Implicit
Definition: static-stability-constraint.hh:30
virtual bool impl_compute(ConfigurationOut_t config, value_type t) const
Definition: time-dependant-path.hh:133
core::value_type value_type
Definition: fwd.hh:76
static TimeDependantPathPtr_t createCopy(const TimeDependantPath &other)
Definition: time-dependant-path.hh:48
TimeDependantPath(const core::PathPtr_t path)
Definition: time-dependant-path.hh:107
void updateAbscissa(value_type t) const
Definition: time-dependant-path.hh:91
virtual std::ostream & print(std::ostream &os) const
Definition: time-dependant-path.hh:144
Configuration_t initial() const
Definition: time-dependant-path.hh:70
core::PathPtr_t PathPtr_t
Definition: fwd.hh:52
std::shared_ptr< TimeDependantPath > TimeDependantPathPtr_t
Definition: fwd.hh:87
core::ConfigProjectorPtr_t ConfigProjectorPtr_t
Definition: fwd.hh:35
TimeDependantPath(const core::PathPtr_t path, const ConstraintSetPtr_t &c)
Definition: time-dependant-path.hh:113
Configuration_t end() const
Definition: time-dependant-path.hh:74
virtual ~TimeDependantPath()
Definition: time-dependant-path.hh:68
Definition: time-dependant-path.hh:30
core::ConstraintSetPtr_t ConstraintSetPtr_t
Definition: fwd.hh:33
void add(const TimeDependant &td)
Definition: time-dependant-path.hh:63
core::PathPtr_t copy(const ConstraintSetPtr_t &c) const
Definition: time-dependant-path.hh:82
core::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:70
TimeDependantPath(const TimeDependantPath &other, const ConstraintSetPtr_t &c)
Definition: time-dependant-path.hh:126
core::Configuration_t Configuration_t
Definition: fwd.hh:68
#define HPP_WHOLEBODY_STEP_DLLAPI
Definition: config.hh:64
static TimeDependantPathPtr_t create(const core::PathPtr_t path)
Definition: time-dependant-path.hh:33
TimeDependantPath(const TimeDependantPath &other)
Definition: time-dependant-path.hh:121
static TimeDependantPathPtr_t createCopy(const TimeDependantPath &other, const ConstraintSetPtr_t &c)
Definition: time-dependant-path.hh:55
void setAffineTransform(const value_type &a, const value_type &b)
Definition: time-dependant-path.hh:86