pgtypes.hh 9.44 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
/*
2
 * Copyright 2007, 2008, 2009, 2010,
Thomas Moulard's avatar
Thomas Moulard committed
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
 *
 * Mehdi    Benallegue
 * Andrei   Herdt
 * Francois Keith
 * Olivier  Stasse
 *
 * JRL, CNRS/AIST
 *
 * This file is part of walkGenJrl.
 * walkGenJrl is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * walkGenJrl is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Lesser Public License for more details.
 * You should have received a copy of the GNU Lesser General Public License
 * along with walkGenJrl.  If not, see <http://www.gnu.org/licenses/>.
 *
24
 *  Research carried out within the scope of the
Thomas Moulard's avatar
Thomas Moulard committed
25
26
27
 *  Joint Japanese-French Robotics Laboratory (JRL)
 */
/*! \file PGTypes.h
Olivier Stasse's avatar
Olivier Stasse committed
28
  \brief Defines basic types for the Humanoid Walking Pattern Generator.
Thomas Moulard's avatar
Thomas Moulard committed
29
30
31
32
33
34
*/
#ifndef _PATTERN_GENERATOR_TYPES_H_
#define  _PATTERN_GENERATOR_TYPES_H_

// For Windows compatibility.
#if defined (WIN32)
Francois Keith's avatar
Francois Keith committed
35
#  ifdef jrl_walkgen_EXPORTS
Thomas Moulard's avatar
Thomas Moulard committed
36
#    define WALK_GEN_JRL_EXPORT __declspec(dllexport)
37
#  else
Thomas Moulard's avatar
Thomas Moulard committed
38
#    define WALK_GEN_JRL_EXPORT __declspec(dllimport)
39
#  endif
Thomas Moulard's avatar
Thomas Moulard committed
40
41
42
#else
#  define WALK_GEN_JRL_EXPORT
#endif
olivier stasse's avatar
olivier stasse committed
43
44
#include <iostream>
#include <fstream>
45
46
47

#include <Eigen/Dense>
#include <vector>
Thomas Moulard's avatar
Thomas Moulard committed
48
49
50
51
52

namespace PatternGeneratorJRL
{
  struct COMState_s;

53
  /// Structure to store the COM position computed by the preview control.
Thomas Moulard's avatar
Thomas Moulard committed
54
55
  struct WALK_GEN_JRL_EXPORT COMPosition_s
  {
56
57
    double x[3],y[3];
    double z[3];
Thomas Moulard's avatar
Thomas Moulard committed
58
59
    double yaw; // aka theta
    double pitch; // aka omega
60
61
    double roll; // aka hip

Thomas Moulard's avatar
Thomas Moulard committed
62
    struct COMPosition_s & operator=(const COMState_s &aCS);
63

Thomas Moulard's avatar
Thomas Moulard committed
64
65
  };

66
67
68
69
  inline std::ostream & operator<<(std::ostream & os, const COMPosition_s & aCp)
  {
    for(size_t i = 0; i < 3; ++i)
    {
70
71
      os << "x[" << i << "] " << aCp.x[i] << " y[" << i << "] "
         << aCp.y[i] << " z[" << i << "] " << aCp.z[i] << std::endl;
72
73
74
75
76
    }
    os << "yaw " << aCp.yaw << " pitch " << aCp.pitch << " roll " << aCp.roll;
    return os;
  }

Thomas Moulard's avatar
Thomas Moulard committed
77
78
79
80
81
82
83
84
85
86
  typedef struct COMPosition_s COMPosition;
  typedef struct COMPosition_s WaistState;

  /// Structure to store the COM state computed by the preview control.
  struct WALK_GEN_JRL_EXPORT COMState_s
  {
    double x[3],y[3],z[3];
    double yaw[3]; // aka theta
    double pitch[3]; // aka omega
    double roll[3]; // aka hip
87

Thomas Moulard's avatar
Thomas Moulard committed
88
    struct COMState_s & operator=(const COMPosition_s &aCS);
89
90

    void reset();
Thomas Moulard's avatar
Thomas Moulard committed
91
92

    COMState_s();
olivier stasse's avatar
olivier stasse committed
93

94
95
    friend std::ostream & operator<<(std::ostream &os,
                                     const struct COMState_s & acs);
Thomas Moulard's avatar
Thomas Moulard committed
96
  };
97
98


Thomas Moulard's avatar
Thomas Moulard committed
99
100

  typedef struct COMState_s COMState;
101
102

  /** Structure to store each foot position when the user is specifying
Thomas Moulard's avatar
Thomas Moulard committed
103
104
      a sequence of relative positions. */
  struct RelativeFootPosition_s
105
  {
student's avatar
student committed
106
    double sx,sy,sz,theta;
olivier-stasse's avatar
olivier-stasse committed
107
108
    double SStime;
    double DStime;
Francois Keith's avatar
Francois Keith committed
109
    int stepType;     //1:normal walking 2:one step before obstacle
110
111
                      //3:first leg over obstacle 4:second leg over obstacle
    // 5:one step after obstacle 6 :stepping stair
112
    double DeviationHipHeight;
113
    RelativeFootPosition_s();
Olivier Stasse's avatar
Olivier Stasse committed
114
  };
Thomas Moulard's avatar
Thomas Moulard committed
115
116
  typedef struct RelativeFootPosition_s RelativeFootPosition;

117
118
  inline std::ostream & operator<<
  (std::ostream & os, const RelativeFootPosition_s & rfp)
119
  {
120
121
122
123
124
125
126
127
    os << "sx " << rfp.sx
       << " sy "<< rfp.sy
       << " sz "<< rfp.sz 
       << " theta " << rfp.theta << std::endl;
    os << "SStime " << rfp.SStime
       << " DStime " << rfp.DStime
       << " stepType " << rfp.stepType
       << " DeviationHipHeight " << rfp.DeviationHipHeight;
128
129
130
    return os;
  }

Thomas Moulard's avatar
Thomas Moulard committed
131
132
133
  /** Structure to store each of the ZMP value, with a given
      direction at a certain time. */
  struct ZMPPosition_s
134
  {
Thomas Moulard's avatar
Thomas Moulard committed
135
136
137
    double px,py,pz;
    double theta;//For COM
    double time;
Francois Keith's avatar
Francois Keith committed
138
    int stepType;     //1:normal walking 2:one step before obstacle
139
140
141
142
                      //3:first leg over obstacle 4:second leg over
    // obstacle 5:one step after obstacle
    // +10 if double support phase
    // *(-1) if right foot stance else left foot stance
Thomas Moulard's avatar
Thomas Moulard committed
143
144
145
  };
  typedef struct ZMPPosition_s ZMPPosition;

146
147
  inline std::ostream & operator<<(std::ostream & os, const ZMPPosition_s & zmp)
  {
148
149
150
151
152
153
154
    os << "px " << zmp.px
       << " py " << zmp.pz
       << " pz " << zmp.pz
       << " theta " << zmp.theta
       << std::endl;
    os << "time " << zmp.time
       << " stepType " << zmp.stepType;
155
156
157
    return os;
  }

Thomas Moulard's avatar
Thomas Moulard committed
158
159
  /// Structure to store the absolute foot position.
  struct FootAbsolutePosition_t
160
  {
Thomas Moulard's avatar
Thomas Moulard committed
161
    /*! px, py in meters, theta in DEGREES. */
162
    double x,y,z, theta, omega, omega2;
Thomas Moulard's avatar
Thomas Moulard committed
163
    /*! Speed of the foot. */
164
    double dx,dy,dz, dtheta, domega, domega2;
Andrei's avatar
Andrei committed
165
166
    /*! Acceleration of the foot. */
    double ddx,ddy,ddz, ddtheta, ddomega, ddomega2;
167
168
    /*! Jerk of the foot. */
    double dddx,dddy,dddz, dddtheta, dddomega, dddomega2;
Thomas Moulard's avatar
Thomas Moulard committed
169
170
    /*! Time at which this position should be reached. */
    double time;
Francois Keith's avatar
Francois Keith committed
171
    /*! 1:normal walking 2:one step before obstacle
172
173
      3:first leg over obstacle 4:second leg over obstacle 
      5:one step after  obstacle
Thomas Moulard's avatar
Thomas Moulard committed
174
175
      +10 if double support phase
      (-1) if support foot  */
176
    int stepType;
Thomas Moulard's avatar
Thomas Moulard committed
177
178
179
  };
  typedef struct FootAbsolutePosition_t FootAbsolutePosition;

180
181
  inline std::ostream & operator<<(std::ostream & os,
                                   const FootAbsolutePosition & fap)
182
  {
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
    os << "x " << fap.x
       << " y " << fap.y
       << " z " << fap.z
       << " theta " << fap.theta
       << " omega " << fap.omega
       << " omega2 " << fap.omega2 << std::endl;
    os << "dx " << fap.dx
       << " dy " << fap.dy
       << " dz " << fap.dz
       << " dtheta " << fap.dtheta
       << " domega " << fap.domega
       << " domega2 " << fap.domega2 << std::endl;
    os << "ddx " << fap.ddx
       << " ddy " << fap.ddy
       << " ddz " << fap.ddz
       << " ddtheta " << fap.ddtheta
       << " ddomega " << fap.ddomega
       << " ddomega2 " << fap.ddomega2 << std::endl;
    os << "time " << fap.time
       << " stepType " << fap.stepType;
203
204
205
    return os;
  }

206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
  /// Structure to store the absolute foot position.
  struct HandAbsolutePosition_t
  {
    /*! x, y, z in meters, theta in DEGREES. */
    double x,y,z, theta, omega, omega2;
    /*! Speed of the foot. */
    double dx,dy,dz, dtheta, domega, domega2;
    /*! Acceleration of the foot. */
    double ddx,ddy,ddz, ddtheta, ddomega, ddomega2;
    /*! Jerk of the hand. */
    double dddx,dddy,dddz, dddtheta, dddomega, dddomega2;
    /*! Time at which this position should be reached. */
    double time;
    /*! -1 : contact
     *   1 : no contact
     */
    int stepType;
  };
  typedef struct HandAbsolutePosition_t HandAbsolutePosition;

226
227
  inline std::ostream & operator<<(std::ostream & os,
                                   const HandAbsolutePosition& hap)
228
  {
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
    os << "x " << hap.x
       << " y " << hap.y
       << " z " << hap.z
       << " theta " << hap.theta
       << " omega " << hap.omega
       << " omega2 " << hap.omega2
       << std::endl;
    os << "dx " << hap.dx
       << " dy " << hap.dy
       << " dz " << hap.dz
       << " dtheta " << hap.dtheta
       << " domega " << hap.domega
       << " domega2 " << hap.domega2
       << std::endl;
    os << "ddx " << hap.ddx
       << " ddy " << hap.ddy
       << " ddz " << hap.ddz
       << " ddtheta " << hap.ddtheta
       << " ddomega " << hap.ddomega
       << " ddomega2 " << hap.ddomega2 << std::endl;
    os << "time " << hap.time
       << " stepType " << hap.stepType;
251
252
253
    return os;
  }

Thomas Moulard's avatar
Thomas Moulard committed
254
255
256
  // Linear constraint.
  struct LinearConstraintInequality_s
  {
257
258
259
    Eigen::MatrixXd A;
    Eigen::MatrixXd B;
    Eigen::VectorXd Center;
Thomas Moulard's avatar
Thomas Moulard committed
260
261
262
    std::vector<int> SimilarConstraints;
    double StartingTime, EndingTime;
  };
263
  typedef struct LinearConstraintInequality_s
Thomas Moulard's avatar
Thomas Moulard committed
264
    LinearConstraintInequality_t;
265

266
267
268
  /// Linear constraints with variable feet placement.
  struct LinearConstraintInequalityFreeFeet_s
  {
269
270
    Eigen::MatrixXd D;
    Eigen::MatrixXd Dc;
271
272
273
274
    int StepNumber;
  };
  typedef struct LinearConstraintInequalityFreeFeet_s
    LinearConstraintInequalityFreeFeet_t;
Thomas Moulard's avatar
Thomas Moulard committed
275

276
277
278
279
280
281
282
283
284
  //State of the feet on the ground
  struct SupportFeet_s
  {
    double x,y,theta,StartTime;
    int SupportFoot;
  };
  typedef struct SupportFeet_s
    SupportFeet_t;

285
286
287
288
289
290
291
  inline std::ostream & operator<<(std::ostream & os, const SupportFeet_s & sf)
  {
    os << "x " << sf.x << " y " << sf.y << " theta " << sf.theta << std::endl;
    os << " StartTime " << sf.StartTime << " SupportFoot " << sf.SupportFoot;
    return os;
  }

olivier-stasse's avatar
olivier-stasse committed
292
  /// Structure to store the absolute reference.
293
294
  struct ReferenceAbsoluteVelocity_t
  {
olivier-stasse's avatar
olivier-stasse committed
295
296
    /*! m/sec or degrees/sec */
    double x,y,z, dYaw;
297

olivier-stasse's avatar
olivier-stasse committed
298
    /*! reference values for the whole preview window */
299
300
301
    Eigen::VectorXd RefVectorX;
    Eigen::VectorXd RefVectorY;
    Eigen::VectorXd RefVectorTheta;
302
303
304
  };
  typedef struct ReferenceAbsoluteVelocity_t ReferenceAbsoluteVelocity;

305
306
307
  inline std::ostream & operator<<
  (std::ostream & os,
   const ReferenceAbsoluteVelocity_t & rav)
308
  {
309
310
311
312
    os << "x " << rav.x
       << " y " << rav.y
       << " z " << rav.z
       << " dYaw " << rav.dYaw;
313
314
315
    return os;
  }

316
317
318
319
320
321
322
323
324
325
  /// Structure to model a circle (e.g : a stricly convex obstable)
  struct Circle_t
  {
    double x_0 ;
    double y_0 ;
    double r ;
    double margin ;
  };
  typedef struct Circle_t Circle ;

mnaveau's avatar
mnaveau committed
326
327
328
329
330
331
  inline std::ostream & operator<<(std::ostream & os, const Circle_t & circle)
  {
    os << "x_0 " << circle.x_0 << " y_0 " << circle.y_0 << " R " << circle.r ;
    return os;
  }

olivier-stasse's avatar
olivier-stasse committed
332
}
Thomas Moulard's avatar
Thomas Moulard committed
333
#endif