hpp-walkgen  4.10.1
Walk generator for hpp framework
foot-trajectory.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015 CNRS
3 // Authors: Florent Lamiraux
4 //
5 // This file is part of hpp-walkgen
6 // hpp-walkgen 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-walkgen 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-walkgen If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_WALKGEN_FOOT_TRAJECTORY_HH
20 # define HPP_WALKGEN_FOOT_TRAJECTORY_HH
21 
22 # include <stdexcept>
23 # include <hpp/core/path.hh>
24 # include <hpp/walkgen/foot-print.hh>
25 
26 namespace hpp {
27  namespace walkgen {
29 
33  class Step : public Path
34  {
35  public:
36  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38  static StepPtr_t create (const FootPrint& start, const FootPrint& end,
39  const value_type& stepLow,
40  const value_type& stepHigh,
41  const value_type& duration);
42  virtual PathPtr_t copy () const
43  {
44  return StepPtr_t (new Step (*this));
45  }
46 
47  virtual PathPtr_t copy (const ConstraintSetPtr_t&) const
48  {
49  throw std::logic_error ("Providing constraints to foot trajectory is "
50  "not supported.");
51  }
52 
53  virtual ~Step () throw () {}
54  protected:
55  Step (const FootPrint& start, const FootPrint& end,
56  const value_type& stepLow, const value_type& stepHigh,
57  const value_type& duration);
58 
59  Step (const Step& step) : Path (step),
60  h0_ (step.h0_), h1_ (step.h1_), h2_ (step.h2_), h3_ (step.h3_),
61  v0_ (step.v0_), v1_ (step.v1_), v2_ (step.v2_), v3_ (step.v3_),
62  initialOrientation_ (step.initialOrientation_), angle_ (step.angle_),
63  omega0_ (step.omega0_), omega1_ (step.omega1_), omega2_ (step.omega2_),
64  omega3_ (step.omega3_), initial_ (step.initial_), final_ (step.final_)
65 
66  {
67  }
68 
69  virtual Configuration_t initial () const
70  {
71  return initial_;
72  }
73 
74  virtual Configuration_t end () const
75  {
76  return final_;
77  }
78 
79  virtual bool impl_compute (ConfigurationOut_t configuration,
80  value_type t) const;
81 
82  virtual std::ostream& print (std::ostream &os) const
83  {
84  os << "Step between:" << std::endl;
85  os << " (" << h0_.transpose () << ") with orientation "
86  << atan2 (initialOrientation_ [1], initialOrientation_ [0])
87  << std::endl;
88  os << "and" << std::endl;
89  os << " (" << (h0_+h1_+h2_+h3_).transpose () << ") with orientation "
90  << atan2 (initialOrientation_ [1], initialOrientation_ [0]) + angle_
91  << std::endl;
92  os << " with height " << v0_+v1_+v2_+v3_ << std::endl;
93  return os;
94  }
95 
96  private:
97  value_type computeAngle (const FootPrint& start, const FootPrint& end)
98  const;
100  vector2_t h0_, h1_, h2_, h3_;
102  value_type v0_, v1_, v2_, v3_;
104  vector2_t initialOrientation_;
106  value_type angle_;
108  value_type omega0_, omega1_, omega2_, omega3_;
110  Configuration_t initial_;
112  Configuration_t final_;
113  }; // class Step
114 
118  class SupportFoot : public Path
119  {
120  public:
121  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
123  static SupportFootPtr_t create (const FootPrint& position,
124  const value_type& footHeight,
125  const value_type& duration);
126 
127  virtual PathPtr_t copy () const
128  {
129  return SupportFootPtr_t (new SupportFoot (*this));
130  }
131 
132  virtual PathPtr_t copy (const ConstraintSetPtr_t&) const
133  {
134  throw std::logic_error ("Providing constraints to foot trajectory is "
135  "not supported.");
136  }
137  virtual ~SupportFoot () throw () {}
138  protected:
139  SupportFoot (const FootPrint& position, const value_type& footHeight,
140  const value_type& duration);
141  SupportFoot (const SupportFoot& sp) : Path (sp),
142  h0_ (sp.h0_), v0_ (sp.v0_),
143  orientation_ (sp.orientation_),
144  configuration_ (sp.configuration_)
145  {
146  }
147  virtual Configuration_t initial () const
148  {
149  return configuration_;
150  }
151 
152  virtual Configuration_t end () const
153  {
154  return configuration_;
155  }
156 
157  virtual bool impl_compute (ConfigurationOut_t configuration,
158  value_type t) const;
159  virtual std::ostream& print (std::ostream &os) const
160  {
161  os << "Support foot:" << std::endl;
162  os << " (" << h0_.transpose () << ") with orientation "
163  << atan2 (orientation_ [1], orientation_ [0]) << std::endl;
164  return os;
165  }
166  private:
168  vector2_t h0_;
170  value_type v0_;
172  vector2_t orientation_;
174  Configuration_t configuration_;
175  }; // class SupportFoot
176 
177  } // namespace walkgen
178 } // namespace hpp
179 #endif // HPP_WALKGEN_FOOT_TRAJECTORY_HH
Position of a foot on the (horizontal) ground.
Definition: foot-print.hh:30
Eigen::Matrix< value_type, 2, 1 > vector2_t
Definition: fwd.hh:44
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW StepPtr_t create(const FootPrint &start, const FootPrint &end, const value_type &stepLow, const value_type &stepHigh, const value_type &duration)
Create a step and return a shared pointer.
Definition: foot-trajectory.cc:79
core::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:52
virtual PathPtr_t copy(const ConstraintSetPtr_t &) const
Definition: foot-trajectory.hh:47
virtual bool impl_compute(ConfigurationOut_t configuration, value_type t) const
Definition: foot-trajectory.cc:108
Definition: bspline-based.hh:28
virtual ~SupportFoot()
Definition: foot-trajectory.hh:137
double value_type
Definition: fwd.hh:37
virtual Configuration_t initial() const
Definition: foot-trajectory.hh:147
virtual std::ostream & print(std::ostream &os) const
Definition: foot-trajectory.hh:82
virtual std::ostream & print(std::ostream &os) const
Definition: foot-trajectory.hh:159
DevicePtr_t createFootDevice()
Definition: foot-trajectory.cc:40
virtual Configuration_t initial() const
Definition: foot-trajectory.hh:69
virtual Configuration_t end() const
Definition: foot-trajectory.hh:152
Definition: foot-trajectory.hh:33
Step(const FootPrint &start, const FootPrint &end, const value_type &stepLow, const value_type &stepHigh, const value_type &duration)
Definition: foot-trajectory.cc:87
Definition: foot-trajectory.hh:118
core::PathPtr_t PathPtr_t
Definition: fwd.hh:49
virtual PathPtr_t copy() const
Definition: foot-trajectory.hh:127
virtual PathPtr_t copy() const
Definition: foot-trajectory.hh:42
boost::shared_ptr< SupportFoot > SupportFootPtr_t
Definition: fwd.hh:36
virtual ~Step()
Definition: foot-trajectory.hh:53
core::Path Path
Definition: fwd.hh:48
core::DevicePtr_t DevicePtr_t
Definition: fwd.hh:55
boost::shared_ptr< Step > StepPtr_t
Definition: fwd.hh:34
core::ConstraintSetPtr_t ConstraintSetPtr_t
Definition: fwd.hh:54
Step(const Step &step)
Definition: foot-trajectory.hh:59
SupportFoot(const SupportFoot &sp)
Definition: foot-trajectory.hh:141
virtual Configuration_t end() const
Definition: foot-trajectory.hh:74
core::Configuration_t Configuration_t
Definition: fwd.hh:53
virtual PathPtr_t copy(const ConstraintSetPtr_t &) const
Definition: foot-trajectory.hh:132