hpp-walkgen  4.12.0
Walk generator for hpp framework
bspline-based.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_SPLINE_BASED_HH
20 # define HPP_WALKGEN_SPLINE_BASED_HH
21 
22 # include<Eigen/StdVector>
23 
24 # include <pinocchio/fwd.hpp>
26 # include <hpp/walkgen/foot-print.hh>
27 
28 namespace hpp {
29  namespace walkgen {
30 
32  typedef
33 #ifdef ROBOPTIM_TRAJECTORY_32
34  roboptim::trajectory::CubicBSpline::basisPolynomialsVector_t
35 #else
37 #endif
39  typedef
40 #ifdef ROBOPTIM_TRAJECTORY_32
41  roboptim::trajectory::CubicBSpline::basisPolynomials_t
42 #else
44 #endif
46 
47  typedef Eigen::Matrix <value_type, 7, 1> vector7_t;
48 
55  {
56  public:
57  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63  PiecewisePoly3 (const value_type& tmin, const value_type& tmax,
64  const Polynomial3& polynomial) : lower (tmin),
65  upper (tmax)
66  {
67  value_type delta = (upper-lower)/6;
68  values_ [0] = polynomial (lower);
69  values_ [6] = polynomial (upper);
70  value_type t = lower + delta;
71  for (unsigned int i=1; i<6; ++i) {
72  values_ [i] = polynomial (t);
73  t += delta;
74  }
75  }
80  PiecewisePoly3 (const value_type& tmin, const value_type& tmax,
81  const value_type& v0, const value_type& v1) :
82  lower (tmin), upper (tmax)
83  {
84  value_type delta_p = (upper-lower)/6;
85  value_type delta_v = (v1-v0)/6;
86  values_ [0] = v0;
87  values_ [6] = v1;
88  value_type t = lower + delta_p;
89  value_type v = v0 + delta_v;
90  for (unsigned int i=1; i<6; ++i) {
91  values_ [i] = v;
92  t += delta_p;
93  v += delta_v;
94  }
95  }
97  PiecewisePoly3 (const value_type& tmin, const value_type& tmax,
98  const vector7_t& values) : lower (tmin), upper (tmax),
99  values_ (values)
100  {
101  }
104  PiecewisePoly3 () : lower (sqrt (-1.)), upper (sqrt (-1.))
105  {
106  values_.fill (sqrt (-1.));
107  }
111  const value_type& operator[] (const size_type& index) const
112  {
113  return values_ [index];
114  }
117  private:
118  vector7_t values_;
119  }; // class PiecewisePoly3
120 
121 
130  {
131  public:
132  typedef std::vector <PiecewisePoly3> ZmpTrajectory_t;
133  typedef std::vector <ZmpTrajectory_t> ZmpTrajectories_t;
134  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
136  static size_type l;
138  {
139  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
140  BoundaryCondition (const value_type& time, const vector2_t& pos,
141  const vector2_t& vel) : t (time),
142  position (pos),
143  velocity (vel)
144  {
145  }
149  }; // struct BoundaryCondition
150  typedef std::vector <BoundaryCondition,
151  Eigen::aligned_allocator <BoundaryCondition> >
153  typedef std::vector <value_type> StepHeights_t;
157  static SplineBasedPtr_t create (const value_type& height);
158 
162  void timeSequence (const Times_t& times);
163 
167  const Times_t& timeSequence () const
168  {
169  return tau_;
170  }
171 
181  void footPrintSequence (const FootPrints_t& footPrints);
182 
185  {
186  return footPrints_;
187  }
188 
191  void defaultStepHeight (const value_type& stepHeight)
192  {
193  defaultStepHeight_ = stepHeight;
194  }
198  {
199  return defaultStepHeight_;
200  }
201 
207  void stepHeights (const StepHeights_t& stepHeights)
208  {
209  stepHeights_ = stepHeights;
210  }
212  const StepHeights_t& stepHeights () const
213  {
214  return stepHeights_;
215  }
216 
224  const vector2_t& end)
225  {
226  zmpRefInit_ = init;
227  zmpRefEnd_ = end;
228  }
234  void add (const BoundaryCondition& boundaryCondition);
235 
239  void setInitialComState (const vector2_t& position,
240  const vector2_t& velocity);
246  void setEndComState (const vector2_t& position,
247  const vector2_t& velocity);
248 
251  CubicBSplinePtr_t solve () const;
252 
257  {
258  return leftFootTraj_;
259  }
260 
265  {
266  return rightFootTraj_;
267  }
268 
270  {
271  return leftFoot_;
272  }
273 
275  {
276  return rightFoot_;
277  }
278 
280  const ZmpTrajectory_t& zmpRefx () const
281  {
282  return zmpRef0_;
283  }
285  const ZmpTrajectory_t& zmpRefy () const
286  {
287  return zmpRef1_;
288  }
290  value_type cost (const vector_t& controlPoints);
291 
293  value_type cost () const;
294 
295  static value_type integral (value_type lower,value_type upper,
296  const PiecewisePoly3& P1,
297  const PiecewisePoly3& P2);
298 
300  const ZmpTrajectories_t& zmpBasisFunctions () const
301  {
302  return Z_;
303  }
304  protected:
308  SplineBased (const value_type& height);
311  void init (const SplineBasedWkPtr_t& self);
313  void computeFootTrajectory () const;
314  private:
315  void defineProblem () const;
316  void buildPolynomialVector () const;
317  value_type height_;
318  SplineBasedWkPtr_t weakPtr_;
319  mutable size_type m_;
320  Times_t tau_;
321  FootPrints_t footPrints_;
322  StepHeights_t stepHeights_;
323  value_type defaultStepHeight_;
324  BoundaryConditions_t boundaryConditions_;
325  vector2_t zmpRefInit_;
326  vector2_t zmpRefEnd_;
327  DevicePtr_t leftFoot_;
328  DevicePtr_t rightFoot_;
329  mutable CubicBSplinePtr_t comTrajectory_;
330  mutable matrix_t H0_;
331  mutable vector_t b0_, b1_;
332  mutable matrix_t A0_;
333  mutable vector_t c0_, c1_;
334  mutable ZmpTrajectories_t Z_;
335  mutable ZmpTrajectory_t zmpRef0_;
336  mutable ZmpTrajectory_t zmpRef1_;
337  mutable PathVectorPtr_t leftFootTraj_;
338  mutable PathVectorPtr_t rightFootTraj_;
339  }; // class SplineBased
340  } // namespace walkgen
341 } // namespace hpp
342 #endif // HPP_WALKGEN_SPLINE_BASED_HH
const ZmpTrajectories_t & zmpBasisFunctions() const
Get base functions of Zmp.
Definition: bspline-based.hh:300
value_type upper
Definition: bspline-based.hh:116
Definition: bspline-based.hh:54
const value_type & defaultStepHeight() const
Definition: bspline-based.hh:197
Eigen::Matrix< value_type, 2, 1 > vector2_t
Definition: fwd.hh:44
matrix_t::Index size_type
Definition: fwd.hh:42
DevicePtr_t leftFoot() const
Definition: bspline-based.hh:269
Definition: bspline-based.hh:28
PiecewisePoly3(const value_type &tmin, const value_type &tmax, const vector7_t &values)
Constructor with 7 values.
Definition: bspline-based.hh:97
const PathVectorPtr_t & rightFootTrajectory() const
Definition: bspline-based.hh:264
double value_type
Definition: fwd.hh:37
#define HPP_WALKGEN_DLLAPI
Definition: config.hh:64
Definition: bspline-based.hh:137
const StepHeights_t & stepHeights() const
Get sequence of step heights.
Definition: bspline-based.hh:212
std::vector< ZmpTrajectory_t > ZmpTrajectories_t
Definition: bspline-based.hh:133
std::shared_ptr< CubicBSpline > CubicBSplinePtr_t
Definition: fwd.hh:30
Definition: bspline-based.hh:129
const ZmpTrajectory_t & zmpRefx() const
Getter to representation of zmp ref abscissa.
Definition: bspline-based.hh:280
std::vector< value_type > StepHeights_t
Definition: bspline-based.hh:153
vector2_t velocity
Definition: bspline-based.hh:148
std::vector< Polynomial3, Eigen::aligned_allocator< Polynomial3 > > polynomials3vector_t
std::shared_ptr< SplineBased > SplineBasedPtr_t
Definition: fwd.hh:32
roboptim::trajectory::CubicBSpline::polynomials3vector_t polynomials3vector_t
Definition: bspline-based.hh:45
const value_type & operator[](const size_type &index) const
Definition: bspline-based.hh:111
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoundaryCondition(const value_type &time, const vector2_t &pos, const vector2_t &vel)
Definition: bspline-based.hh:140
std::vector< BoundaryCondition, Eigen::aligned_allocator< BoundaryCondition > > BoundaryConditions_t
Definition: bspline-based.hh:152
roboptim::trajectory::Polynomial3 Polynomial3
Definition: bspline-based.hh:31
static size_type l
Definition: bspline-based.hh:136
roboptim::trajectory::CubicBSpline::polynomials3vectors_t polynomials3vectors_t
Definition: bspline-based.hh:38
const FootPrints_t & footPrintSequence() const
get sequence of foot prints
Definition: bspline-based.hh:184
Eigen::Matrix< value_type, 7, 1 > vector7_t
Definition: bspline-based.hh:47
std::vector< PiecewisePoly3 > ZmpTrajectory_t
Definition: bspline-based.hh:132
value_type t
Definition: bspline-based.hh:146
core::PathVectorPtr_t PathVectorPtr_t
Definition: fwd.hh:50
const ZmpTrajectory_t & zmpRefy() const
Getter to representation of zmp ref abscissa.
Definition: bspline-based.hh:285
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > vector_t
Definition: fwd.hh:40
PiecewisePoly3()
Definition: bspline-based.hh:104
DevicePtr_t rightFoot() const
Definition: bspline-based.hh:274
void defaultStepHeight(const value_type &stepHeight)
Definition: bspline-based.hh:191
core::DevicePtr_t DevicePtr_t
Definition: fwd.hh:55
std::vector< FootPrint, Eigen::aligned_allocator< FootPrint > > FootPrints_t
Definition: foot-print.hh:50
value_type lower
Definition: bspline-based.hh:115
std::vector< value_type > Times_t
Definition: fwd.hh:43
std::vector< polynomials3vector_t > polynomials3vectors_t
const PathVectorPtr_t & leftFootTrajectory() const
Definition: bspline-based.hh:256
vector2_t position
Definition: bspline-based.hh:147
static EIGEN_MAKE_ALIGNED_OPERATOR_NEW value_type gravity
Definition: bspline-based.hh:135
void zmpRefBoundaryConditions(const vector2_t &init, const vector2_t &end)
Definition: bspline-based.hh:223
PiecewisePoly3(const value_type &tmin, const value_type &tmax, const value_type &v0, const value_type &v1)
Definition: bspline-based.hh:80
void stepHeights(const StepHeights_t &stepHeights)
Definition: bspline-based.hh:207
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > matrix_t
Definition: fwd.hh:41
const Times_t & timeSequence() const
Definition: bspline-based.hh:167
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PiecewisePoly3(const value_type &tmin, const value_type &tmax, const Polynomial3 &polynomial)
Definition: bspline-based.hh:63